1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
|
#include <nuttx/config.h>
#include <geo/geo.h>
#include <math.h>
#include <mathlib/math/filter/LowPassFilter.hpp>
#include <poll.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/types.h>
#include <systemlib/err.h>
#include <systemlib/param/param.h>
#include <systemlib/systemlib.h>
#include <uORB/uORB.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/trajectory.h>
#include <uORB/topics/vehicle_local_position.h>
namespace trajectory_calculator {
// Global variables, controlling threaded execution
static bool g_thread_continue = false;
static bool g_thread_running = false;
// Helper class for position analysis.
// TODO! Conisider migrating to function
class PointAnalyzer {
private:
// TODO! Consider migrating to a vector filtering
math::LowPassFilter<float> x_filter, y_filter;
double radius;
float center_x, center_y;
public:
PointAnalyzer(float filter_cutoff, float init_radius) :
x_filter(filter_cutoff),
y_filter(filter_cutoff),
radius(init_radius),
center_x(0.0f/0.0f),
center_y(center_x) {
}
~PointAnalyzer() {}
// Analyze provided point, filter its x and y, update center, if needed and return point type
uint8_t analyze(vehicle_local_position_s &point) {
double x, y;
point.x = x_filter.apply(point.timestamp, point.x);
point.y = y_filter.apply(point.timestamp, point.y);
if (!isfinite(center_x) || !isfinite(center_y)) {
center_x = point.x;
center_y = point.y;
}
x = point.x - center_x;
y = point.y - center_y;
// TODO! Consider variable radius based on speed
if (sqrt(x*x + y*y) < (double) radius) {
return 0; // still point
}
else {
center_x = point.x;
center_y = point.y;
return 1; // moving point
}
}
};
// Thread entry point. Calculates trajectory points and publishes them
int calculate(int argc, char *argv[]) {
// TODO! Parameters instead of constants
float cutoff, radius;
if (param_get(param_find("AIRD_TRAJ_CUT"), &cutoff) != 0 ||
param_get(param_find("AIRD_TRAJ_RAD"), &radius) != 0) {
warnx("Could not read parameters!");
return -1;
}
// Prepare pollers
int pos_topic = orb_subscribe(ORB_ID(vehicle_local_position));
pollfd poll_data;
poll_data.fd = pos_topic;
poll_data.events = POLLIN;
int home_topic = orb_subscribe(ORB_ID(home_position));
int res;
orb_advert_t trajectory_pub = -1;
trajectory_s trajectory_data;
// TODO! We might be to the party too early - consider waiting for the first publication
vehicle_local_position_s local_position = {};
home_position_s home;
bool updated;
// We consider home position secondary, thus we need to initialize it with correct data
orb_copy(ORB_ID(home_position), home_topic, &home);
// Projection vars
struct map_projection_reference_s ref_point;
double est_lat, est_lon;
// Latitude and longitude in ref_point are converted, but we need to compare with local refs
double ref_lat = 0.0f/0.0f, ref_lon = ref_lat;
// Filtering object
PointAnalyzer analyzer(cutoff, radius);
g_thread_running = true;
while (g_thread_continue) {
// pace the daemon by global position topic, 100 HZ with a bit of overhead
res = poll(&poll_data, 1, 15);
if (res == 1) {
orb_copy(ORB_ID(vehicle_local_position), pos_topic, &local_position);
orb_check(home_topic, &updated);
if (updated) {
orb_copy(ORB_ID(home_position), home_topic, &home);
}
// TODO! Currently we trust local_position topic to not have incorrect refs
// TODO! Use xy_global to check for valid refs
if (local_position.ref_lat != ref_lat || local_position.ref_lon != ref_lon) {
map_projection_init(&ref_point, local_position.ref_lat, local_position.ref_lon);
ref_lat = local_position.ref_lat;
ref_lon = local_position.ref_lon;
}
// Filters and modifies local position while he's at it
trajectory_data.point_type = analyzer.analyze(local_position);
map_projection_reproject(&ref_point, local_position.x, local_position.y, &est_lat, &est_lon);
trajectory_data.alt = local_position.ref_alt - local_position.z;
trajectory_data.lat = est_lat;
trajectory_data.lon = est_lon;
trajectory_data.vel_d = local_position.vz;
trajectory_data.vel_e = local_position.vy;
trajectory_data.vel_n = local_position.vx;
trajectory_data.relative_alt = home.alt - trajectory_data.alt;
trajectory_data.timestamp = local_position.timestamp;
trajectory_data.heading = _wrap_2pi(local_position.yaw);
if (trajectory_pub < 0) {
trajectory_pub = orb_advertise(ORB_ID(trajectory), &trajectory_data);
} else {
orb_publish(ORB_ID(trajectory), trajectory_pub, &trajectory_data);
}
}
else {
warnx("Poll error!");
}
}
close(pos_topic);
close(home_topic);
close(trajectory_pub);
g_thread_running = false;
return 0;
}
// Helper function to start the thread
int start() {
if (g_thread_running) {
return -1;
}
g_thread_continue = true;
// Beware of the stack size. With 500 bytes the process just hangs!
if (0 >= task_spawn_cmd("trajectory_daemon",
SCHED_DEFAULT,SCHED_PRIORITY_DEFAULT, 2000,
calculate,
(const char**) NULL)){
g_thread_continue = false;
return -2;
}
// TODO! Consider checking if the thread successfully started
return 0;
}
// Helper function to stop the thread
int stop() {
int i = 0;
if (!g_thread_running) {
return -1;
}
g_thread_continue = false;
// Wait for the thread to actually stop, pausing the caller
while (g_thread_running) {
// Wait max of 10 seconds
if (i++ > 10) {
return -2;
}
sleep(1);
}
return 0;
}
} // End trajectory_calculator namespace
inline void usage(const char *progname) {
printf("Usage: %s {start|stop|status}\n", progname);
}
extern "C" __EXPORT int trajectory_calculator_main(int argc, char *argv[]) {
using namespace trajectory_calculator;
if (argc != 2) {
usage(argv[0]);
return 1;
}
if (strcmp(argv[1], "start") == 0) {
switch (start()) {
case 0:
warnx("Started.");
break;
case -1:
errx(1, "Already running!");
break;
default:
errx(1, "Failed to start the calculator!");
break;
}
}
else if (strcmp(argv[1], "stop") == 0) {
switch (stop()) {
case 0:
warnx("Stopped.");
break;
case -1:
errx(1, "Not running!");
break;
default:
errx(1, "Failed to stop the calculator!");
break;
}
}
else if (strcmp(argv[1], "status") == 0) {
if (g_thread_running == 0) {
warnx("Not running.");
}
else {
warnx("Started.");
}
}
else {
warnx("Unknown argument: '%s'", argv[1]);
usage(argv[0]);
return 1;
}
return 0;
}
|