Paparazzi UAS v7.0_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 */
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
105
109
112
113 // Only move if we had a previous location
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;
128 wp_new_enu.z = wp_enu->z;
129
130 // Rotate the waypoint
135
136 // Update the waypoint
138
139 // Send to the GCS that the waypoint has been moved
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
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
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)
216 // Filter the cosine and sine of the ground course to avoid wrapping
219
220 // Add an advance and the difference in measured time multiplied by the speed
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
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;
261
262 // Send to the GCS that the waypoint has been moved
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
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
float y
Definition common_nav.h:41
float x
Definition common_nav.h:40
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 lat
in degrees*1e7
int32_t alt
in millimeters above WGS84 reference ellipsoid
int32_t lon
in degrees*1e7
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
bool ned_initialized_i
true if local int coordinate frame is initialsed
Definition state.h:199
static struct LtpDef_i * stateGetNedOrigin_i(void)
Get the coordinate NED frame origin (int)
Definition state.h:556
uint16_t foo
Definition main_demo5.c:58
struct EnuCoor_f * waypoint_get_enu_f(uint8_t wp_id)
Get ENU coordinates (float)
Definition waypoints.c:387
void waypoint_set_enu(uint8_t wp_id, struct EnuCoor_f *enu)
Set local ENU waypoint coordinates.
Definition waypoints.c:177
float x
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.