Paparazzi UAS  v6.2_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 {
65  struct EnuCoor_f actual;
66  struct EnuCoor_f target;
67  struct EnuCoor_f circle;
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
255  nav_lace.last_border_time = time;
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
286  nav_lace.last_border_time = time;
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 }
NavLace::actual
struct EnuCoor_f actual
Definition: nav_lace.c:65
uint32_t
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
stateGetHorizontalSpeedDir_f
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:944
NAV_LACE_LWC_ID
#define NAV_LACE_LWC_ID
Definition: nav_lace.c:170
NavLace::status
enum LaceStatus status
Definition: nav_lace.c:62
change_rep
static float change_rep(float dir)
Definition: nav_lace.c:83
nav_send_waypoint
void nav_send_waypoint(uint8_t wp_id)
Send a waypoint throught default telemetry channel.
Definition: common_nav.c:210
NavLace::radius_sign
float radius_sign
Definition: nav_lace.c:73
LACE_RIGHT
@ LACE_RIGHT
Definition: nav_lace.c:58
abi.h
NavLace::radius
float radius
Definition: nav_lace.c:72
nav_lace.h
get_sys_time_float
static float get_sys_time_float(void)
Get the time in seconds since startup.
Definition: sys_time.h:138
MissionInit
@ MissionInit
first exec
Definition: mission_common.h:55
NavLace::recover_radius
float recover_radius
Definition: nav_lace.c:75
NavLace::estim_border
struct EnuCoor_f estim_border
Definition: nav_lace.c:68
stateGetPositionEnu_f
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
abi_struct
Event structure to store callbacks in a linked list.
Definition: abi_common.h:66
NavLace
Definition: nav_lace.c:61
nav_circle_XY
void nav_circle_XY(float x, float y, float radius)
Navigates around (x, y).
Definition: nav.c:111
UNUSED
uint8_t last_wp UNUSED
Definition: navigation.c:101
alpha
float alpha
Definition: textons.c:107
LACE_LEFT
@ LACE_LEFT
Definition: nav_lace.c:57
LACE_RECOVER_START
@ LACE_RECOVER_START
Definition: nav_lace.c:51
VECT3_ADD
#define VECT3_ADD(_a, _b)
Definition: pprz_algebra.h:147
NavLace::circle
struct EnuCoor_f circle
Definition: nav_lace.c:67
lwc_ev
static abi_event lwc_ev
Definition: nav_lace.c:173
LaceStatus
LaceStatus
Definition: nav_lace.c:45
NAV_LACE_RECOVER_MAX_TURN
#define NAV_LACE_RECOVER_MAX_TURN
Definition: nav_lace.c:38
EnuCoor_f::y
float y
in meters
Definition: pprz_geodetic_float.h:74
NavLace::rotation
enum RotationDir rotation
Definition: nav_lace.c:63
EnuCoor_f::z
float z
in meters
Definition: pprz_geodetic_float.h:75
NavVerticalAutoThrottleMode
#define NavVerticalAutoThrottleMode(_pitch)
Set the climb control to auto-throttle with the specified pitch pre-command.
Definition: nav.h:180
NavCircleCountNoRewind
#define NavCircleCountNoRewind()
Definition: nav.h:157
lwc_cb
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
FloatVect3
Definition: pprz_algebra_float.h:54
nav_route_xy
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:383
NavLace::direction
float direction
Definition: nav_lace.c:71
LACE_INSIDE
@ LACE_INSIDE
Definition: nav_lace.c:48
dir
static const float dir[]
Definition: shift_tracking.c:91
nav_lace
static struct NavLace nav_lace
Definition: nav_lace.c:79
DEFAULT_CIRCLE_RADIUS
#define DEFAULT_CIRCLE_RADIUS
default nav_circle_radius in meters
Definition: navigation.c:56
LACE_RECOVER_OUTSIDE
@ LACE_RECOVER_OUTSIDE
Definition: nav_lace.c:53
nav_init_stage
void nav_init_stage(void)
needs to be implemented by fixedwing and rotorcraft seperately
Definition: nav.c:95
NavLace::pos_incr
struct FloatVect3 pos_incr
Definition: nav_lace.c:70
NAVIGATION_FREQUENCY
#define NAVIGATION_FREQUENCY
Default fixedwing navigation frequency.
Definition: nav.h:49
NavLace::inside_cloud
bool inside_cloud
Definition: nav_lace.c:64
MissionRun
@ MissionRun
normal run
Definition: mission_common.h:54
EnuCoor_f
vector in East North Up coordinates Units: meters
Definition: pprz_geodetic_float.h:72
autopilot.h
nav_move_waypoint_enu
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
nav_lace_run
bool nav_lace_run(void)
Navigation function Called by flight plan or mission run function.
Definition: nav_lace.c:222
nav_lace_init
void nav_lace_init(void)
Init function called by modules init.
Definition: nav_lace.c:181
f
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)
Definition: wedgebug.c:204
LACE_OUTSIDE_START
@ LACE_OUTSIDE_START
Definition: nav_lace.c:49
FloatVect3::y
float y
Definition: pprz_algebra_float.h:56
FloatVect2::y
float y
Definition: pprz_algebra_float.h:51
RotationDir
RotationDir
Definition: nav_lace.c:56
nav.h
int32_t
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
mission_common.h
mission planner library
NAV_LACE_BORDER_FILTER
#define NAV_LACE_BORDER_FILTER
Definition: nav_lace.c:42
ground_alt
float ground_alt
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:41
NavLace::recover_circle
struct EnuCoor_f recover_circle
Definition: nav_lace.c:69
FloatVect3::x
float x
Definition: pprz_algebra_float.h:55
nav_dt
static const float nav_dt
Definition: nav_lace.c:81
MissionUpdate
@ MissionUpdate
param update
Definition: mission_common.h:56
update_target_point
static void update_target_point(struct EnuCoor_f *target, struct EnuCoor_f *border, float dt, float tau)
Definition: nav_lace.c:108
NavLace::max_recover_radius
float max_recover_radius
Definition: nav_lace.c:76
mission_register
bool mission_register(mission_custom_cb cb, char *type)
Register a new navigation or action callback function.
Definition: mission_common.c:115
process_new_point_lace
static struct EnuCoor_f process_new_point_lace(struct EnuCoor_f *position, float alt_sp, float uav_direction)
Definition: nav_lace.c:88
FloatVect3::z
float z
Definition: pprz_algebra_float.h:57
FloatVect2::x
float x
Definition: pprz_algebra_float.h:50
nav_lace_setup
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
LACE_RECOVER_INSIDE
@ LACE_RECOVER_INSIDE
Definition: nav_lace.c:52
EnuCoor_f::x
float x
in meters
Definition: pprz_geodetic_float.h:73
state.h
LACE_ENTER
@ LACE_ENTER
Definition: nav_lace.c:46
VECT3_ASSIGN
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:125
NavLace::last_border_time
float last_border_time
Definition: nav_lace.c:74
MissionRunFlag
MissionRunFlag
Definition: mission_common.h:53
target
struct FloatVect2 target
Definition: obstacle_avoidance.c:78
NavLace::target
struct EnuCoor_f target
Definition: nav_lace.c:66
NavVerticalAltitudeMode
#define NavVerticalAltitudeMode(_alt, _pre_climb)
Set the vertical mode to altitude control with the specified altitude setpoint and climb pre-command.
Definition: nav.h:194
LACE_OUTSIDE
@ LACE_OUTSIDE
Definition: nav_lace.c:50
LACE_INSIDE_START
@ LACE_INSIDE_START
Definition: nav_lace.c:47