Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
follow_me.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2019 Freek van Tienen <freek.v.tienen@gmail.com>
3 *
4 * This file is part of paparazzi
5 *
6 * paparazzi is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2, or (at your option)
9 * any later version.
10 *
11 * paparazzi is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 * GNU General Public License for more details.
15 *
16 * You should have received a copy of the GNU General Public License
17 * along with paparazzi; see the file COPYING. If not, see
18 * <http://www.gnu.org/licenses/>.
19 */
25
26#include "follow_me.h"
27
29#include "generated/modules.h"
30#include "generated/flight_plan.h"
31
32// Distance to the target to hover from is by default 45 meters
33#ifndef FOLLOW_ME_DISTANCE
34#define FOLLOW_ME_DISTANCE 45
35#endif
36
37// Height difference between te target be default 60 meters
38#ifndef FOLLOW_ME_HEIGHT
39#define FOLLOW_ME_HEIGHT 60
40#endif
41
42// The relative position GPS timeout in ms
43#ifndef FOLLOW_ME_GPS_TIMEOUT
44#define FOLLOW_ME_GPS_TIMEOUT 5000
45#endif
46
47// The timeout when receiving GPS messages from the ground in ms
48#ifndef FOLLOW_ME_GROUND_TIMEOUT
49#define FOLLOW_ME_GROUND_TIMEOUT 5000
50#endif
51
52// The default heading sin/cos filter value (higher is harder filtering)
53#ifndef FOLLOW_ME_FILT
54#define FOLLOW_ME_FILT 0.9
55#endif
56
57// By default no moving waypoints
58#ifndef FOLLOW_ME_MOVING_WPS
59#define FOLLOW_ME_MOVING_WPS
60#endif
61
64float follow_me_heading = 180.;
72
74static bool ground_set = false;
75static struct LlaCoor_i ground_lla;
76static float ground_speed;
77static float ground_climb;
78static float ground_course;
79static float ground_heading;
80
85static bool last_targetpos_valid = false;
86
88{
89 ground_set = false;
92 moving_wps_cnt = sizeof(moving_wps) / sizeof(uint8_t);
93}
94
96{
97 if(!ground_set) {
98 return;
99 }
100
101 // Calculate the difference to move the waypoints
102 struct EnuCoor_i target_pos_cm;
103 struct EnuCoor_f cur_targetpos, diff_targetpos;
104 float cur_targetpos_heading, diff_targetpos_heading;
105
107 VECT3_FLOAT_OF_CM(cur_targetpos, target_pos_cm);
108 VECT3_DIFF(diff_targetpos, cur_targetpos, last_targetpos);
109
110 cur_targetpos_heading = ground_heading;
111 diff_targetpos_heading = cur_targetpos_heading - last_targetpos_heading;
112
113 // Only move if we had a previous location
114 VECT3_COPY(last_targetpos, cur_targetpos);
115 last_targetpos_heading = cur_targetpos_heading;
118 return;
119 }
120
121 // Go through all waypoints
122 for(uint8_t i = 0; i < moving_wps_cnt; i++) {
123 uint8_t wp_id = moving_wps[i];
124 struct EnuCoor_f *wp_enu = waypoint_get_enu_f(wp_id);
125 struct EnuCoor_f wp_new_enu;
126 wp_new_enu.x = wp_enu->x + diff_targetpos.x;
127 wp_new_enu.y = wp_enu->y + diff_targetpos.y;
128 wp_new_enu.z = wp_enu->z;
129
130 // Rotate the waypoint
131 float cos_heading = cosf(diff_targetpos_heading/180.*M_PI);
132 float sin_heading = sinf(diff_targetpos_heading/180.*M_PI);
133 wp_new_enu.x = ((wp_new_enu.x - cur_targetpos.x) * cos_heading) + ((wp_new_enu.y - cur_targetpos.y) * sin_heading) + cur_targetpos.x;
134 wp_new_enu.y = (-(wp_new_enu.x - cur_targetpos.x) * sin_heading) + ((wp_new_enu.y - cur_targetpos.y) * cos_heading) + cur_targetpos.y;
135
136 // Update the waypoint
137 waypoint_set_enu(wp_id, &wp_new_enu);
138
139 // Send to the GCS that the waypoint has been moved
140 DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id,
141 &waypoints[wp_id].enu_i.x,
142 &waypoints[wp_id].enu_i.y,
143 &waypoints[wp_id].enu_i.z);
144 }
145}
146
148{
149 if(DL_TARGET_POS_ac_id(buf) != AC_ID)
150 return;
151
152 // Save the received values
154 ground_lla.lat = DL_TARGET_POS_lat(buf);
155 ground_lla.lon = DL_TARGET_POS_lon(buf);
156 ground_lla.alt = DL_TARGET_POS_alt(buf);
157 ground_speed = DL_TARGET_POS_speed(buf);
158 ground_climb = DL_TARGET_POS_climb(buf);
159 ground_course = DL_TARGET_POS_course(buf);
160 ground_heading = DL_TARGET_POS_heading(buf);
161 if(ground_heading > 360.f) {
162 // Ground heading is invalid
163 ground_set = false;
164 return;
165 }
166
167 ground_set = true;
168}
169
170void follow_me_set_wp(uint8_t wp_id, float speed)
171{
172 bool target_valid = true;
173 struct NedCoor_f target_pos;
174 float diff_time_ms = 0;
175
176 // Check if we got a valid relative position which didn't timeout (FIXME)
177 /*if(bit_is_set(gps.valid_fields, GPS_VALID_RELPOS_BIT) && gps.relpos_tow+FOLLOW_ME_GPS_TIMEOUT > gps_tow_from_sys_ticks(sys_time.nb_tick)) {
178 static struct NedCoor_f cur_pos;
179 static uint32_t last_relpos_tow = 0;
180
181 // Make sure to only use the current state from the receive of the GPS message (FIXME overflow at sunday)
182 if(last_relpos_tow < gps.relpos_tow) {
183 cur_pos = *stateGetPositionNed_f();
184 last_relpos_tow = gps.relpos_tow;
185 }
186
187 // Set the target position
188 target_pos.x = cur_pos.x - gps.relpos_ned.x / 100.0f;
189 target_pos.y = cur_pos.y - gps.relpos_ned.y / 100.0f;
190 target_pos.z = cur_pos.z - gps.relpos_ned.z / 100.0f;
191
192 // Calculate the difference in time from measurement
193 diff_time_ms = gps_tow_from_sys_ticks(sys_time.nb_tick) - gps.relpos_tow + follow_me_gps_delay;
194 if(diff_time_ms < 0) diff_time_ms += (1000*60*60*24*7); //msec of a week
195 }
196 // Check if we got a position from the ground which didn't timeout and local NED is initialized
197 else*/ if(ground_set && state.ned_initialized_i && ground_time_msec+FOLLOW_ME_GROUND_TIMEOUT > get_sys_time_msec()) {
198 struct NedCoor_i target_pos_cm;
200 target_pos.x = target_pos_cm.x / 100.;
201 target_pos.y = target_pos_cm.y / 100.;
202 target_pos.z = target_pos_cm.z / 100.;
203
204 // Calculate the difference in time from the measurement
206 }
207 // No target found
208 else {
209 target_valid = false;
210 }
211
212 static float gc_cos_filt = 0, gc_sin_filt = 0;
213
214 // Integrate NE over the time (only if information from the ground is valid)
215 if(target_valid && ground_set && ground_time_msec+FOLLOW_ME_GROUND_TIMEOUT > get_sys_time_msec() && (diff_time_ms > 0 || follow_me_advance_ms > 0)) {
216 // Filter the cosine and sine of the ground course to avoid wrapping
217 gc_cos_filt = gc_cos_filt * follow_me_filt + cosf(ground_course/180.*M_PI) * (1 - follow_me_filt);
218 gc_sin_filt = gc_sin_filt * follow_me_filt + sinf(ground_course/180.*M_PI) * (1 - follow_me_filt);
219
220 // Add an advance and the difference in measured time multiplied by the speed
221 float int_dist_m = (follow_me_advance_ms + diff_time_ms) / 1000.f * ground_speed;
222 target_pos.x += int_dist_m * gc_cos_filt;
223 target_pos.y += int_dist_m * gc_sin_filt;
224 }
225
226 static uint32_t last_time_ms = 0;
227 static float dist = FOLLOW_ME_DISTANCE;
228 static float height = FOLLOW_ME_HEIGHT;
229 static float fmh_cos_filt = 0, fmh_sin_filt = 0;
230
231 // Update the waypoint only when target is valid
232 if(target_valid) {
233 // Move the distance and height according to the given speed
234 if(last_time_ms != 0 && speed != 0) {
235 float time_diff = (get_sys_time_msec() - last_time_ms) / 1000.f;
236 dist -= speed * time_diff;
237 height -= speed * time_diff;
238
239 if(dist <= follow_me_min_dist) dist = follow_me_min_dist;
240 if(height <= follow_me_min_height) height = follow_me_min_height;
241 }
242 // Reset distance and height if speed is 0
243 else if(speed == 0) {
244 dist = follow_me_distance;
245 height = follow_me_height;
246 }
247
248 // Filter the cosine and sine of the follow me heading to avoid wrapping
249 fmh_cos_filt = fmh_cos_filt * follow_me_filt + cosf((ground_heading+follow_me_heading)/180.*M_PI) * (1 - follow_me_filt);
250 fmh_sin_filt = fmh_sin_filt * follow_me_filt + sinf((ground_heading+follow_me_heading)/180.*M_PI) * (1 - follow_me_filt);
251
252 // Add the target distance in the direction of the follow me heading
253 target_pos.x += dist * fmh_cos_filt;
254 target_pos.y += dist * fmh_sin_filt;
255 target_pos.z -= height; // Target is in NED
256
257 // Update the waypoint
258 struct EnuCoor_f target_enu;
259 ENU_OF_TO_NED(target_enu, target_pos);
260 waypoint_set_enu(wp_id, &target_enu);
261
262 // Send to the GCS that the waypoint has been moved
263 DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id,
264 &waypoints[wp_id].enu_i.x,
265 &waypoints[wp_id].enu_i.y,
266 &waypoints[wp_id].enu_i.z);
267 }
268
269 // Allways update the time to avoid big jumps in distance and height
270 last_time_ms = get_sys_time_msec();
271}
uint32_t get_sys_time_msec(void)
Get the time in milliseconds since startup.
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition common_nav.c:44
static float ground_course
Definition follow_me.c:78
void follow_me_parse_target_pos(uint8_t *buf)
on receiving a TARGET_POS message
Definition follow_me.c:147
float follow_me_min_height
Follow me minimum height in meters when approaching with a speed.
Definition follow_me.c:71
float follow_me_min_dist
Follow me minimum distance in meters when trying to approach with a certain speed.
Definition follow_me.c:70
void follow_me_periodic(void)
periodic function
Definition follow_me.c:95
float follow_me_height
height from the ground gps
Definition follow_me.c:63
static struct LlaCoor_i ground_lla
Definition follow_me.c:75
float follow_me_distance
distance from the ground gps
Definition follow_me.c:62
#define FOLLOW_ME_HEIGHT
Definition follow_me.c:39
float follow_me_heading
heading direction in which to hover from (automatically set if ground is exceeding speed)
Definition follow_me.c:64
void follow_me_set_wp(uint8_t wp_id, float speed)
run function
Definition follow_me.c:170
float follow_me_gps_delay
Follow me GPS delay from the relative positionb packet (in ms).
Definition follow_me.c:67
static uint8_t moving_wps_cnt
Definition follow_me.c:82
static uint32_t ground_time_msec
Definition follow_me.c:73
static uint8_t moving_wps[]
Definition follow_me.c:81
#define FOLLOW_ME_FILT
Definition follow_me.c:54
static float ground_speed
Definition follow_me.c:76
void follow_me_init(void)
init function
Definition follow_me.c:87
#define FOLLOW_ME_MOVING_WPS
Definition follow_me.c:59
float follow_me_advance_ms
Follow me waypoint advance time in ms (multiplied by the ground speed).
Definition follow_me.c:69
#define FOLLOW_ME_DISTANCE
Definition follow_me.c:34
#define FOLLOW_ME_GROUND_TIMEOUT
Definition follow_me.c:49
static struct EnuCoor_f last_targetpos
Definition follow_me.c:83
static bool ground_set
Definition follow_me.c:74
static bool last_targetpos_valid
Definition follow_me.c:85
float follow_me_datalink_delay
Follow me datalink delay from the ground GPS packet (in ms).
Definition follow_me.c:68
float follow_me_diag_speed
Diagonal speed for follow me.
Definition follow_me.c:66
static float last_targetpos_heading
Definition follow_me.c:84
static float ground_heading
Definition follow_me.c:79
static float ground_climb
Definition follow_me.c:77
float follow_me_filt
Follow me course sin/cos filter value (higher is harder filter).
Definition follow_me.c:65
#define VECT3_COPY(_a, _b)
#define VECT3_DIFF(_c, _a, _b)
#define ENU_OF_TO_NED(_po, _pi)
int32_t z
Down.
int32_t y
East.
int32_t x
North.
void ned_of_lla_point_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct LlaCoor_i *lla)
Convert a point from LLA to local NED.
void enu_of_lla_point_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct LlaCoor_i *lla)
Convert a point from LLA to local ENU.
#define VECT3_FLOAT_OF_CM(_o, _i)
vector in East North Up coordinates
vector in Latitude, Longitude and Altitude
vector in North East Down coordinates
struct State state
Definition state.c:36
static struct LtpDef_i * stateGetNedOrigin_i(void)
Get the coordinate NED frame origin (int).
Definition state.h:567
struct EnuCoor_f * waypoint_get_enu_f(uint8_t wp_id)
Get ENU coordinates (float).
Definition waypoints.c:406
void waypoint_set_enu(uint8_t wp_id, struct EnuCoor_f *enu)
Set local ENU waypoint coordinates.
Definition waypoints.c:204
float y
in meters
float x
in meters
float z
in meters
float x
in meters
float z
in meters
float y
in meters
vector in East North Up coordinates Units: meters
vector in North East Down coordinates Units: meters
Periodic telemetry system header (includes downlink utility and generated code).
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.