Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
nav_lace.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2018-2021 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 #include "modules/nav/nav_lace.h"
30 
32 #include "state.h"
33 #include "autopilot.h"
34 #include "generated/flight_plan.h"
35 #include "modules/core/abi.h"
36 
37 #ifndef NAV_LACE_RECOVER_MAX_TURN
38 #define NAV_LACE_RECOVER_MAX_TURN 1.5f
39 #endif
40 
41 #ifndef NAV_LACE_BORDER_FILTER
42 #define NAV_LACE_BORDER_FILTER 20.f
43 #endif
44 
45 enum LaceStatus {
54 };
55 
59 };
60 
61 struct NavLace {
62  enum LaceStatus status;
63  enum RotationDir rotation;
65  struct EnuCoor_f actual;
66  struct EnuCoor_f target;
67  struct EnuCoor_f circle;
68  struct EnuCoor_f estim_border;
70  struct FloatVect3 pos_incr;
71  float direction;
72  float radius;
73  float radius_sign;
77 };
78 
79 static struct NavLace nav_lace;
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 struct EnuCoor_f process_new_point_lace(struct EnuCoor_f *position, float alt_sp, float uav_direction)
89 {
90  struct EnuCoor_f new_point;
91  float rot_angle;
92 
93  if (nav_lace.rotation == LACE_RIGHT) {
94  rot_angle = -M_PI_2;
96  } else {
97  rot_angle = M_PI_2;
99  }
100 
101  new_point.x = position->x + (cosf(rot_angle + uav_direction) * nav_lace.radius);
102  new_point.y = position->y + (sinf(rot_angle + uav_direction) * nav_lace.radius);
103  new_point.z = alt_sp;
104 
105  return new_point;
106 }
107 
108 static void update_target_point(struct EnuCoor_f *target, struct EnuCoor_f *border, float dt, float tau)
109 {
110  // with a proper discrete transform, coeff should be exp(-dt/tau) and (1-exp(-dt/tau))
111  // but to make it simpler with the same behavior at the limits, we use tau/(dt+tau) and dt/(dt+tau)
112  // with a positive value for tau
113  if (tau > 1e-4) {
114  float alpha = dt / (dt + tau);
115  target->x = (border->x * alpha) + (target->x * (1.f - alpha));
116  target->y = (border->y * alpha) + (target->y * (1.f - alpha));
117  }
118 }
119 
120 
121 #if USE_MISSION
123 
124 static bool nav_lace_mission(uint8_t nb, float *params, enum MissionRunFlag flag)
125 {
126  if (flag == MissionInit && nb == 8) {
127  float start_x = params[0];
128  float start_y = params[1];
129  float start_z = params[2];
130  int first_turn = params[3];
131  float circle_radius = params[4];
132  float vx = params[5];
133  float vy = params[6];
134  float vz = params[7];
135  nav_lace_setup(start_x, start_y, start_z, first_turn, circle_radius, vx, vy, vz);
136  return true;
137  }
138  else if (flag == MissionUpdate && nb == 3) {
139  // update target 3D position (ENU frame, above ground alt)
140  float x = params[0];
141  float y = params[1];
142  float z = params[2];
144  return true;
145  }
146  else if (flag == MissionUpdate && nb == 2) {
147  // update horizontal speed
148  float vx = params[0];
149  float vy = params[1];
150  nav_lace.pos_incr.x = vx * nav_dt;
151  nav_lace.pos_incr.y = vy * nav_dt;
152  return true;
153  }
154  else if (flag == MissionUpdate && nb == 1) {
155  // update vertical speed
156  float vz = params[0];
157  nav_lace.pos_incr.z = vz * nav_dt;
158  return true;
159  }
160  else if (flag == MissionRun) {
161  return nav_lace_run();
162  }
163  return false; // not a valid case
164 }
165 #endif
166 
167 // ABI message
168 
169 #ifndef NAV_LACE_LWC_ID
170 #define NAV_LACE_LWC_ID ABI_BROADCAST
171 #endif
172 
174 
175 static void lwc_cb(uint8_t sender_id UNUSED, uint32_t stamp UNUSED, int32_t data_type, uint32_t size, uint8_t * data) {
176  if (data_type == 1 && size == 1) {
177  nav_lace.inside_cloud = (bool) data[0];
178  }
179 }
180 
181 void nav_lace_init(void)
182 {
188  nav_lace.inside_cloud = false;
189 
190  AbiBindMsgPAYLOAD_DATA(NAV_LACE_LWC_ID, &lwc_ev, lwc_cb);
191 
192 #if USE_MISSION
193  mission_register(nav_lace_mission, "LACE");
194 #endif
195 }
196 
197 void nav_lace_setup(float init_x, float init_y,
198  float init_z, int turn,
199  float desired_radius, float vx,
200  float vy, float vz)
201 {
202  struct EnuCoor_f start = {init_x, init_y, init_z};
203  // increment based on speed
205 
206  nav_lace.target = start;
208  nav_lace.inside_cloud = false;
209  nav_lace.radius = desired_radius;
210 
211  if (turn == 1) {
213  nav_lace.radius_sign = 1.0f;
214  } else {
216  nav_lace.radius_sign = -1.0f;
217  }
218 
220 }
221 
222 bool nav_lace_run(void)
223 {
224  float pre_climb = 0.f;
225  float time = get_sys_time_float();
226 
227  NavVerticalAutoThrottleMode(0.f); /* No pitch */
228 
229  switch (nav_lace.status) {
230  case LACE_ENTER:
231  // init stage
232  nav_init_stage();
233  // reach target point
236 
237  if (nav_lace.inside_cloud) {
238  // found border or already inside
240  // update target for horizontal position
243  }
244  break;
245  case LACE_INSIDE_START:
246  // prepare inside circle
250  // init stage
251  nav_init_stage();
252  // update border and target for recover
256  // fly inside
258  break;
259  case LACE_INSIDE:
260  // increment center position
263  pre_climb = nav_lace.pos_incr.z / nav_dt;
265 
266  if (!nav_lace.inside_cloud) {
267  // found border, start outside
270  }
272  // most likely lost inside
274  }
275  break;
276  case LACE_OUTSIDE_START:
277  // prepare outside circle
281  // init stage
282  nav_init_stage();
283  // upadte border and target for recover
287  // fly outside
289  break;
290  case LACE_OUTSIDE:
291  // increment center position
293  pre_climb = nav_lace.pos_incr.z / nav_dt;
296 
297  if (nav_lace.inside_cloud) {
298  // found border, start inside
301  }
303  // most likely lost outside
305  }
306  break;
307  case LACE_RECOVER_START:
308  // prepare recovery circle or line
311  // initial recovery radius
314  // init stage
315  nav_init_stage();
316  if (nav_lace.inside_cloud) {
318  }
319  else {
321  }
322  break;
323  case LACE_RECOVER_INSIDE:
324  // increment border position
325  // fly in opposite direction to cover more space
327  if (!nav_lace.inside_cloud) {
330  }
331  break;
333  // increment center position
336  // increment recover circle radius
338  nav_lace.recover_radius += 0.5;
339  }
340  // found a new border
341  if (nav_lace.inside_cloud) {
344  }
345  break;
346  default:
347  // error, leaving
348  return false;
349  }
350  // increment border and target positions
353 #ifdef WP_TARGET
355  RunOnceEvery(10, nav_send_waypoint(WP_TARGET));
356 #endif
357 
358  return true;
359 }
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
#define NAV_LACE_LWC_ID
Definition: nav_lace.c:170
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_lace.c:175
#define NAV_LACE_BORDER_FILTER
Definition: nav_lace.c:42
struct EnuCoor_f target
Definition: nav_lace.c:66
float radius
Definition: nav_lace.c:72
enum RotationDir rotation
Definition: nav_lace.c:63
struct EnuCoor_f circle
Definition: nav_lace.c:67
#define NAV_LACE_RECOVER_MAX_TURN
Definition: nav_lace.c:38
enum LaceStatus status
Definition: nav_lace.c:62
bool nav_lace_run(void)
Navigation function Called by flight plan or mission run function.
Definition: nav_lace.c:222
static struct EnuCoor_f process_new_point_lace(struct EnuCoor_f *position, float alt_sp, float uav_direction)
Definition: nav_lace.c:88
float max_recover_radius
Definition: nav_lace.c:76
struct EnuCoor_f actual
Definition: nav_lace.c:65
struct EnuCoor_f recover_circle
Definition: nav_lace.c:69
bool inside_cloud
Definition: nav_lace.c:64
float radius_sign
Definition: nav_lace.c:73
float last_border_time
Definition: nav_lace.c:74
LaceStatus
Definition: nav_lace.c:45
@ LACE_OUTSIDE
Definition: nav_lace.c:50
@ LACE_INSIDE_START
Definition: nav_lace.c:47
@ LACE_RECOVER_INSIDE
Definition: nav_lace.c:52
@ LACE_OUTSIDE_START
Definition: nav_lace.c:49
@ LACE_ENTER
Definition: nav_lace.c:46
@ LACE_RECOVER_START
Definition: nav_lace.c:51
@ LACE_RECOVER_OUTSIDE
Definition: nav_lace.c:53
@ LACE_INSIDE
Definition: nav_lace.c:48
static void update_target_point(struct EnuCoor_f *target, struct EnuCoor_f *border, float dt, float tau)
Definition: nav_lace.c:108
static const float nav_dt
Definition: nav_lace.c:81
RotationDir
Definition: nav_lace.c:56
@ LACE_RIGHT
Definition: nav_lace.c:58
@ LACE_LEFT
Definition: nav_lace.c:57
void nav_lace_init(void)
Init function called by modules init.
Definition: nav_lace.c:181
static float change_rep(float dir)
Definition: nav_lace.c:83
static struct NavLace nav_lace
Definition: nav_lace.c:79
void nav_lace_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_lace.c:197
struct FloatVect3 pos_incr
Definition: nav_lace.c:70
struct EnuCoor_f estim_border
Definition: nav_lace.c:68
static abi_event lwc_ev
Definition: nav_lace.c:173
float recover_radius
Definition: nav_lace.c:75
float direction
Definition: nav_lace.c:71
Adaptive border 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
struct target_t target
Definition: target_pos.c:64
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