Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 
64 float follow_me_heading = 180.;
67 float follow_me_gps_delay = 200;
72 
74 static bool ground_set = false;
75 static struct LlaCoor_i ground_lla;
76 static float ground_speed;
77 static float ground_climb;
78 static float ground_course;
79 static float ground_heading;
80 
83 static struct EnuCoor_f last_targetpos;
85 static bool last_targetpos_valid = false;
86 
87 void follow_me_init(void)
88 {
89  ground_set = false;
90  ground_time_msec = 0;
91  last_targetpos_valid = 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 
106  enu_of_lla_point_i(&target_pos_cm, &state.ned_origin_i, &ground_lla);
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;
116  if(!last_targetpos_valid) {
117  last_targetpos_valid = true;
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 
170 void 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
198  struct NedCoor_i target_pos_cm;
199  ned_of_lla_point_i(&target_pos_cm, &state.ned_origin_i, &ground_lla);
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.
Definition: sys_time_arch.c:98
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:39
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)
Definition: pprz_algebra.h:140
#define VECT3_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:182
#define ENU_OF_TO_NED(_po, _pi)
Definition: pprz_geodetic.h:41
int32_t lat
in degrees*1e7
int32_t alt
in millimeters above WGS84 reference ellipsoid
int32_t z
Down.
int32_t y
East.
int32_t lon
in degrees*1e7
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
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:166
bool ned_initialized_i
true if local int coordinate frame is initialsed
Definition: state.h:171
struct EnuCoor_f * waypoint_get_enu_f(uint8_t wp_id)
Get ENU coordinates (float)
Definition: waypoints.c:379
void waypoint_set_enu(uint8_t wp_id, struct EnuCoor_f *enu)
Set local ENU waypoint coordinates.
Definition: waypoints.c:169
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.
Definition: vl53l1_types.h:78
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98