Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
nav_rosette.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2018-2019 Gautier Hattenberger <gautier.hattenberger@enac.fr>
3  * Titouan Verdu <titouan.verdu@enac.fr>
4  *
5  * This file is part of paparazzi.
6  *
7  * paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, see
19  * <http://www.gnu.org/licenses/>.
20  */
21 
29 
31 #include "state.h"
32 #include "autopilot.h"
33 #include "generated/flight_plan.h"
34 #include "modules/core/abi.h"
35 
36 #ifndef NAV_ROSETTE_RECOVER_MAX_TURN
37 #define NAV_ROSETTE_RECOVER_MAX_TURN 1.5f
38 #endif
39 
40 #ifndef NAV_ROSETTE_BORDER_FILTER
41 #define NAV_ROSETTE_BORDER_FILTER 60.f
42 #endif
43 
53 };
54 
58 };
59 
60 struct NavRosette {
61  enum RosetteStatus status;
62  enum RotationDir rotation;
64  struct EnuCoor_f actual;
65  struct EnuCoor_f target;
66  struct EnuCoor_f circle;
67  struct EnuCoor_f estim_border;
69  struct EnuCoor_f * first;
70  struct FloatVect3 pos_incr;
71  float direction;
72  float radius;
73  float radius_sign;
77 };
78 
79 static struct NavRosette nav_rosette;
80 
81 static const float nav_dt = 1.f / NAVIGATION_FREQUENCY;
82 
83 static float change_rep(float dir)
84 {
85  return M_PI_2 - dir;
86 }
87 
88 static void update_barycenter(struct EnuCoor_f *bary, struct EnuCoor_f *border, struct EnuCoor_f *first, float alt_sp, float dt, float tau)
89 {
90  if (first != NULL) {
91  bary->x = (border->x + first->x) / 2.f;
92  bary->y = (border->y + first->y) / 2.f;
93  bary->z = alt_sp;
94  }
95  else {
96  if (tau > 1e-4) {
97  float alpha = dt / (dt + tau);
98  bary->x = (border->x * alpha) + (bary->x * (1.f - alpha));
99  bary->y = (border->y * alpha) + (bary->y * (1.f - alpha));
100  bary->z = alt_sp;
101  }
102  }
103 }
104 
105 static struct EnuCoor_f process_new_point_rosette(struct EnuCoor_f *position, float uav_direction)
106 {
107  struct EnuCoor_f new_point;
108  float rot_angle;
109 
111  rot_angle = -M_PI_2;
112  } else {
113  rot_angle = M_PI_2;
114  }
115 
116  if (nav_rosette.inside_cloud == true) {
117  new_point.x = nav_rosette.target.x;
118  new_point.y = nav_rosette.target.y;
119  new_point.z = nav_rosette.target.z;
120  }
121  else if (nav_rosette.inside_cloud == false) {
122  new_point.x = position->x + (cosf(rot_angle + uav_direction) * nav_rosette.radius);
123  new_point.y = position->y + (sinf(rot_angle + uav_direction) * nav_rosette.radius);
124  new_point.z = nav_rosette.target.z;
125  }
126 
127  return new_point;
128 }
129 
130 #if USE_MISSION
132 
133 static bool nav_rosette_mission(uint8_t nb, float *params, enum MissionRunFlag flag)
134 {
135  if (flag == MissionInit && nb == 8) {
136  float start_x = params[0];
137  float start_y = params[1];
138  float start_z = params[2];
139  int first_turn = params[3];
140  float circle_radius = params[4];
141  float vx = params[5];
142  float vy = params[6];
143  float vz = params[7];
144  nav_rosette_setup(start_x, start_y, start_z, first_turn, circle_radius, vx, vy, vz);
145  return true;
146  }
147  else if (flag == MissionUpdate && nb == 3) {
148  // update target 3D position (ENU frame, above ground alt)
149  float x = params[0];
150  float y = params[1];
151  float z = params[2];
153  return true;
154  }
155  else if (flag == MissionUpdate && nb == 2) {
156  // update horizontal speed
157  float vx = params[0];
158  float vy = params[1];
159  nav_rosette.pos_incr.x = vx * nav_dt;
160  nav_rosette.pos_incr.y = vy * nav_dt;
161  return true;
162  }
163  else if (flag == MissionUpdate && nb == 1) {
164  // update vertical speed
165  float vz = params[0];
166  nav_rosette.pos_incr.z = vz * nav_dt;
167  return true;
168  }
169  else if (flag == MissionRun) {
170  return nav_rosette_run();
171  }
172  return false; // not a valid case
173 }
174 #endif
175 
176 // ABI message
177 
178 #ifndef NAV_ROSETTE_LWC_ID
179 #define NAV_ROSETTE_LWC_ID ABI_BROADCAST
180 #endif
181 
183 
184 static void lwc_cb(uint8_t sender_id UNUSED, uint32_t stamp UNUSED, int32_t data_type, uint32_t size, uint8_t * data)
185 {
186  if (data_type == 1 && size == 1) {
187  nav_rosette.inside_cloud = (bool) data[0];
188  }
189 }
190 
192 {
197  nav_rosette.inside_cloud = false;
199 
200  AbiBindMsgPAYLOAD_DATA(NAV_ROSETTE_LWC_ID, &lwc_ev, lwc_cb);
201 
202 #if USE_MISSION
203  mission_register(nav_rosette_mission, "RSTT");
204 #endif
205 }
206 
207 void nav_rosette_setup(float init_x, float init_y, float init_z,
208  int turn, float desired_radius,
209  float vx, float vy, float vz)
210 {
211  struct EnuCoor_f start = {init_x, init_y, init_z};
212  // increment based on speed
214 
215  nav_rosette.target = start;
217  nav_rosette.inside_cloud = false;
218  nav_rosette.radius = desired_radius;
219  nav_rosette.first = NULL;
220 
221  if (turn == 1) {
223  nav_rosette.radius_sign = 1.0f;
224  } else {
226  nav_rosette.radius_sign = -1.0f;
227  }
228 
231 }
232 
233 bool nav_rosette_run(void)
234 {
235  float pre_climb = 0.f;
236  float time = get_sys_time_float();
237 
238  NavVerticalAutoThrottleMode(0.f); /* No Pitch */
239 
240  switch (nav_rosette.status) {
241  case RSTT_ENTER:
242  // init stage
243  nav_init_stage();
244  // reach target point
247 
249  // found border or already inside
251  }
252  break;
253  case RSTT_CROSSING_FIRST:
254  // prepare first crossing
259  // init stage
260  nav_init_stage();
261  // cross
263  break;
264  case RSTT_CROSSING_START:
265  // prepare crossing
271  // init stage
272  nav_init_stage();
273  // cross
275  break;
276  case RSTT_CROSSING:
277  // cross towards target
278  pre_climb = nav_rosette.pos_incr.z / nav_dt;
281 
282  if (!nav_rosette.inside_cloud) {
283  // found a border, starting turning back inside
284  // note that you don't need a recover as you always expect to go out after some time
286  }
287  break;
288  case RSTT_TURNING_START:
289  // update target
295  nav_rosette.first = NULL;
296  // prepare next circle
299  // init stage
300  nav_init_stage();
301  // turn
303  break;
304  case RSTT_TURNING:
305  // update circle center
307  pre_climb = nav_rosette.pos_incr.z / nav_dt;
310 
312  // found a border, cross again
314  }
316  // most likely lost outside
318  }
319  break;
320  case RSTT_RECOVER_START:
321  // prepare recovery circle
324  // initial recovery radius
327  // init stage
328  nav_init_stage();
329  // recover
331  break;
333  // increment center position
336  // increment recover circle radius
339  }
340  // found a new border
343  }
344  break;
345  default:
346  // error, leaving
347  return false;
348  }
349  // update target and border positions
352 #ifdef WP_TARGET
354  RunOnceEvery(10, nav_send_waypoint(WP_TARGET));
355 #endif
356 
357  return true;
358 }
Main include for ABI (AirBorneInterface).
Event structure to store callbacks in a linked list.
Definition: abi_common.h:67
Core autopilot interface common to all firmwares.
uint8_t last_wp UNUSED
void nav_send_waypoint(uint8_t wp_id)
Send a waypoint throught default telemetry channel.
Definition: common_nav.c:210
float ground_alt
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:41
void nav_move_waypoint_enu(uint8_t wp_id, float x, float y, float alt)
Move a waypoint in local frame.
Definition: common_nav.c:184
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:125
#define VECT3_ADD(_a, _b)
Definition: pprz_algebra.h:147
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:944
bool mission_register(mission_custom_cb cb, char *type)
Register a new navigation or action callback function.
mission planner library
MissionRunFlag
@ MissionInit
first exec
@ MissionRun
normal run
@ MissionUpdate
param update
void nav_init_stage(void)
needs to be implemented by fixedwing and rotorcraft seperately
Definition: nav.c:92
void nav_route_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_y)
Computes the carrot position along the desired segment.
Definition: nav.c:382
void nav_circle_XY(float x, float y, float radius)
Navigates around (x, y).
Definition: nav.c:108
Fixedwing Navigation library.
#define NavCircleCountNoRewind()
Definition: nav.h:154
#define NAVIGATION_FREQUENCY
Default fixedwing navigation frequency.
Definition: nav.h:49
#define NavVerticalAltitudeMode(_alt, _pre_climb)
Set the vertical mode to altitude control with the specified altitude setpoint and climb pre-command.
Definition: nav.h:191
#define NavVerticalAutoThrottleMode(_pitch)
Set the climb control to auto-throttle with the specified pitch pre-command.
Definition: nav.h:177
RotationDir
Definition: nav_lace.c:56
float radius_sign
Definition: nav_rosette.c:73
static void lwc_cb(uint8_t sender_id UNUSED, uint32_t stamp UNUSED, int32_t data_type, uint32_t size, uint8_t *data)
Definition: nav_rosette.c:184
#define NAV_ROSETTE_BORDER_FILTER
Definition: nav_rosette.c:41
struct EnuCoor_f actual
Definition: nav_rosette.c:64
RosetteStatus
Definition: nav_rosette.c:44
@ RSTT_TURNING_START
Definition: nav_rosette.c:49
@ RSTT_CROSSING_FIRST
Definition: nav_rosette.c:46
@ RSTT_RECOVER_OUTSIDE
Definition: nav_rosette.c:52
@ RSTT_TURNING
Definition: nav_rosette.c:50
@ RSTT_ENTER
Definition: nav_rosette.c:45
@ RSTT_CROSSING
Definition: nav_rosette.c:48
@ RSTT_CROSSING_START
Definition: nav_rosette.c:47
@ RSTT_RECOVER_START
Definition: nav_rosette.c:51
static struct NavRosette nav_rosette
Definition: nav_rosette.c:79
void nav_rosette_setup(float init_x, float init_y, float init_z, int turn, float desired_radius, float vx, float vy, float vz)
Initialized the exploration with a first target point inside the cloud Called from flight plan or wit...
Definition: nav_rosette.c:207
void nav_rosette_init(void)
Init function called by modules init.
Definition: nav_rosette.c:191
float radius
Definition: nav_rosette.c:72
struct EnuCoor_f * first
Definition: nav_rosette.c:69
float recover_radius
Definition: nav_rosette.c:75
float direction
Definition: nav_rosette.c:71
enum RosetteStatus status
Definition: nav_rosette.c:61
#define NAV_ROSETTE_RECOVER_MAX_TURN
Definition: nav_rosette.c:37
struct FloatVect3 pos_incr
Definition: nav_rosette.c:70
struct EnuCoor_f recover_circle
Definition: nav_rosette.c:68
struct EnuCoor_f circle
Definition: nav_rosette.c:66
#define NAV_ROSETTE_LWC_ID
Definition: nav_rosette.c:179
struct EnuCoor_f estim_border
Definition: nav_rosette.c:67
static void update_barycenter(struct EnuCoor_f *bary, struct EnuCoor_f *border, struct EnuCoor_f *first, float alt_sp, float dt, float tau)
Definition: nav_rosette.c:88
struct EnuCoor_f target
Definition: nav_rosette.c:65
bool nav_rosette_run(void)
Navigation function Called by flight plan or mission run function.
Definition: nav_rosette.c:233
static const float nav_dt
Definition: nav_rosette.c:81
@ RSTT_LEFT
Definition: nav_rosette.c:56
@ RSTT_RIGHT
Definition: nav_rosette.c:57
enum RotationDir rotation
Definition: nav_rosette.c:62
float last_border_time
Definition: nav_rosette.c:74
static float change_rep(float dir)
Definition: nav_rosette.c:83
float max_recover_radius
Definition: nav_rosette.c:76
static abi_event lwc_ev
Definition: nav_rosette.c:182
static struct EnuCoor_f process_new_point_rosette(struct EnuCoor_f *position, float uav_direction)
Definition: nav_rosette.c:105
bool inside_cloud
Definition: nav_rosette.c:63
Adaptive flower pattern for cloud exploration Can be used in mission mode with custom pattern and ID ...
float y
in meters
float x
in meters
float z
in meters
vector in East North Up coordinates Units: meters
#define DEFAULT_CIRCLE_RADIUS
default nav_circle_radius in meters
Definition: navigation.h:42
static const float dir[]
API to get/set the generic vehicle states.
static float get_sys_time_float(void)
Get the time in seconds since startup.
Definition: sys_time.h:138
float alpha
Definition: textons.c:133
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
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