Paparazzi UAS  v6.0_unstable-53-gfe8bbd3-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 
31 // Distance to the target to hover from is by default 15 meters
32 #ifndef FOLLOW_ME_DISTANCE
33 #define FOLLOW_ME_DISTANCE 45
34 #endif
35 
36 // Height difference between te target be default 18 meters
37 #ifndef FOLLOW_ME_HEIGHT
38 #define FOLLOW_ME_HEIGHT 75
39 #endif
40 
41 // Minimum speed in m/s which the ground needs to have in order to update the heading
42 #ifndef FOLLOW_ME_MIN_SPEED
43 #define FOLLOW_ME_MIN_SPEED 1.0f
44 #endif
45 
46 // The relative position GPS timeout in ms
47 #ifndef FOLLOW_ME_GPS_TIMEOUT
48 #define FOLLOW_ME_GPS_TIMEOUT 5000
49 #endif
50 
51 // The timeout when receiving GPS messages from the ground in ms
52 #ifndef FOLLOW_ME_GROUND_TIMEOUT
53 #define FOLLOW_ME_GROUND_TIMEOUT 5000
54 #endif
55 
56 // The default course sin/cos filter value (higher is harder filtering)
57 #ifndef FOLLOW_ME_FILT
58 #define FOLLOW_ME_FILT 0.9
59 #endif
60 
63 float follow_me_heading = 0.;
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 
80 void follow_me_init(void)
81 {
82  ground_set = false;
83  ground_time_msec = 0;
84 }
85 
87 {
88  if(DL_TARGET_POS_ac_id(buf) != AC_ID)
89  return;
90 
91  // Save the received values
93  ground_lla.lat = DL_TARGET_POS_lat(buf);
94  ground_lla.lon = DL_TARGET_POS_lon(buf);
95  ground_lla.alt = DL_TARGET_POS_alt(buf);
96  ground_speed = DL_TARGET_POS_speed(buf);
97  ground_climb = DL_TARGET_POS_climb(buf);
98  ground_course = DL_TARGET_POS_course(buf);
99 
100  // Update the heading based on the course
103  if(follow_me_heading > 360.f) follow_me_heading -= 360.f;
104  }
105  ground_set = true;
106 }
107 
108 void follow_me_set_wp(uint8_t wp_id, float speed)
109 {
110  bool target_valid = true;
111  struct NedCoor_f target_pos;
112  float diff_time_ms = 0;
113 
114  // Check if we got a valid relative position which didn't timeout
115  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)) {
116  static struct NedCoor_f cur_pos;
117  static uint32_t last_relpos_tow = 0;
118 
119  // Make sure to only use the current state from the receive of the GPS message (FIXME overflow at sunday)
120  if(last_relpos_tow < gps.relpos_tow) {
121  cur_pos = *stateGetPositionNed_f();
122  last_relpos_tow = gps.relpos_tow;
123  }
124 
125  // Set the target position
126  target_pos.x = cur_pos.x - gps.relpos_ned.x / 100.0f;
127  target_pos.y = cur_pos.y - gps.relpos_ned.y / 100.0f;
128  target_pos.z = cur_pos.z - gps.relpos_ned.z / 100.0f;
129 
130  // Calculate the difference in time from measurement
131  diff_time_ms = gps_tow_from_sys_ticks(sys_time.nb_tick) - gps.relpos_tow + follow_me_gps_delay;
132  if(diff_time_ms < 0) diff_time_ms += (1000*60*60*24*7); //msec of a week
133  }
134  // Check if we got a position from the ground which didn't timeout and local NED is initialized
136  struct NedCoor_i target_pos_cm;
137  ned_of_lla_point_i(&target_pos_cm, &state.ned_origin_i, &ground_lla);
138  target_pos.x = target_pos_cm.x / 100.;
139  target_pos.y = target_pos_cm.y / 100.;
140  target_pos.z = target_pos_cm.z / 100.;
141 
142  // Calculate the difference in time from the measurement
144  }
145  // No target found
146  else {
147  target_valid = false;
148  }
149 
150  static float gc_cos_filt = 0, gc_sin_filt = 0;
151 
152  // Integrate NE over the time (only if information from the ground is valid)
153  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)) {
154  // Filter the cosine and sine of the ground course to avoid wrapping
155  gc_cos_filt = gc_cos_filt * follow_me_filt + cosf(ground_course/180.*M_PI) * (1 - follow_me_filt);
156  gc_sin_filt = gc_sin_filt * follow_me_filt + sinf(ground_course/180.*M_PI) * (1 - follow_me_filt);
157 
158  // Add an advance and the difference in measured time multiplied by the speed
159  float int_dist_m = (follow_me_advance_ms + diff_time_ms) / 1000.f * ground_speed;
160  target_pos.x += int_dist_m * gc_cos_filt;
161  target_pos.y += int_dist_m * gc_sin_filt;
162  }
163 
164  static uint32_t last_time_ms = 0;
165  static float dist = FOLLOW_ME_DISTANCE;
166  static float height = FOLLOW_ME_HEIGHT;
167  static float fmh_cos_filt = 0, fmh_sin_filt = 0;
168 
169  // Update the waypoint only when target is valid
170  if(target_valid) {
171  // Move the distance and height according to the given speed
172  if(last_time_ms != 0 && speed != 0) {
173  float time_diff = (get_sys_time_msec() - last_time_ms) / 1000.f;
174  dist -= speed * time_diff;
175  height -= speed * time_diff;
176 
177  if(dist <= follow_me_min_dist) dist = follow_me_min_dist;
178  if(height <= follow_me_min_height) height = follow_me_min_height;
179  }
180  // Reset distance and height if speed is 0
181  else if(speed == 0) {
182  dist = follow_me_distance;
183  height = follow_me_height;
184  }
185 
186  // Filter the cosine and sine of the follow me heading to avoid wrapping
187  fmh_cos_filt = fmh_cos_filt * follow_me_filt + cosf(follow_me_heading/180.*M_PI) * (1 - follow_me_filt);
188  fmh_sin_filt = fmh_sin_filt * follow_me_filt + sinf(follow_me_heading/180.*M_PI) * (1 - follow_me_filt);
189 
190  // Add the target distance in the direction of the follow me heading
191  target_pos.x += dist * fmh_cos_filt;
192  target_pos.y += dist * fmh_sin_filt;
193  target_pos.z -= height; // Target is in NED
194 
195  // Update the waypoint
196  struct EnuCoor_f target_enu;
197  ENU_OF_TO_NED(target_enu, target_pos);
198  waypoint_set_enu(wp_id, &target_enu);
199 
200  // Send to the GCS that the waypoint has been moved
201  DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id,
202  &waypoints[wp_id].enu_i.x,
203  &waypoints[wp_id].enu_i.y,
204  &waypoints[wp_id].enu_i.z);
205  }
206 
207  // Allways update the time to avoid big jumps in distance and height
208  last_time_ms = get_sys_time_msec();
209 }
210 
static bool ground_set
Definition: follow_me.c:74
float x
Definition: common_nav.h:40
float y
in meters
float follow_me_min_dist
Follow me minimum distance in meters when trying to approach with a certain speed.
Definition: follow_me.c:70
#define FOLLOW_ME_GPS_TIMEOUT
Definition: follow_me.c:48
#define FOLLOW_ME_DISTANCE
Definition: follow_me.c:33
Periodic telemetry system header (includes downlink utility and generated code).
static float ground_speed
Definition: follow_me.c:76
uint32_t get_sys_time_msec(void)
Get the time in milliseconds since startup.
Definition: sys_time_arch.c:78
uint8_t valid_fields
bitfield indicating valid fields (GPS_VALID_x_BIT)
Definition: gps.h:88
float follow_me_height
height from the ground gps
Definition: follow_me.c:62
vector in East North Up coordinates Units: meters
vector in Latitude, Longitude and Altitude
volatile uint32_t nb_tick
SYS_TIME_TICKS since startup.
Definition: sys_time.h:74
#define FOLLOW_ME_FILT
Definition: follow_me.c:58
int32_t z
Down.
int32_t alt
in millimeters above WGS84 reference ellipsoid
#define ENU_OF_TO_NED(_po, _pi)
Definition: pprz_geodetic.h:41
float follow_me_distance
distance from the ground gps
Definition: follow_me.c:61
float z
in meters
int32_t y
East.
void follow_me_set_wp(uint8_t wp_id, float speed)
run function
Definition: follow_me.c:108
float y
Definition: common_nav.h:41
static uint32_t ground_time_msec
Definition: follow_me.c:73
vector in North East Down coordinates Units: meters
float follow_me_min_speed
minimum speed in m/s which the ground needs to have in order to update the heading ...
Definition: follow_me.c:64
int32_t x
North.
static struct LlaCoor_i ground_lla
Definition: follow_me.c:75
static float ground_course
Definition: follow_me.c:78
int32_t lon
in degrees*1e7
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup) ...
Definition: wedgebug.c:204
bool ned_initialized_i
true if local int coordinate frame is initialsed
Definition: state.h:171
#define FOLLOW_ME_MIN_SPEED
Definition: follow_me.c:43
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:166
float follow_me_min_height
Follow me minimum height in meters when approaching with a speed.
Definition: follow_me.c:71
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.
vector in North East Down coordinates
float follow_me_gps_delay
Follow me GPS delay from the relative positionb packet (in ms)
Definition: follow_me.c:67
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:38
float follow_me_diag_speed
Diagonal speed for follow me.
Definition: follow_me.c:66
float follow_me_advance_ms
Follow me waypoint advance time in ms (multiplied by the ground speed)
Definition: follow_me.c:69
static float ground_climb
Definition: follow_me.c:77
uint32_t gps_tow_from_sys_ticks(uint32_t sys_ticks)
Convert time in sys_time ticks to GPS time of week.
Definition: gps.c:355
void follow_me_parse_target_pos(uint8_t *buf)
on receiving a TARGET_POS message
Definition: follow_me.c:86
void waypoint_set_enu(uint8_t wp_id, struct EnuCoor_f *enu)
Set local ENU waypoint coordinates.
Definition: waypoints.c:113
#define FOLLOW_ME_HEIGHT
Definition: follow_me.c:38
#define FOLLOW_ME_GROUND_TIMEOUT
Definition: follow_me.c:53
float follow_me_filt
Follow me course sin/cos filter value (higher is harder filter)
Definition: follow_me.c:65
int32_t lat
in degrees*1e7
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
Definition: state.h:710
float follow_me_datalink_delay
Follow me datalink delay from the ground GPS packet (in ms)
Definition: follow_me.c:68
struct GpsState gps
global GPS state
Definition: gps.c:69
struct State state
Definition: state.c:36
float follow_me_heading
heading direction in which to hover from (automatically set if ground is exceeding speed) ...
Definition: follow_me.c:63
float x
in meters
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
void follow_me_init(void)
init function
Definition: follow_me.c:80
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78