Paparazzi UAS  v6.1.0_stable
Paparazzi is a free software Unmanned Aircraft System.
nav_trinity.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 
31 #include "state.h"
32 #include "autopilot.h"
33 #include "generated/flight_plan.h"
34 #include "modules/core/abi.h"
35 
36 #ifndef NAV_TRINITY_RECOVER_MAX_TURN
37 #define NAV_TRINITY_RECOVER_MAX_TURN 1.5f
38 #endif
39 
40 #ifndef NAV_TRINITY_BORDER_FILTER
41 #define NAV_TRINITY_BORDER_FILTER 20.f
42 #endif
43 
53 };
54 
58 };
59 
60 struct NavTrinity {
64  struct EnuCoor_f actual;
65  struct EnuCoor_f target;
66  struct EnuCoor_f circle;
70  float direction;
71  float radius;
72  float radius_sign;
76 };
77 
78 static struct NavTrinity nav_trinity;
79 
80 static const float nav_dt = 1.f / NAVIGATION_FREQUENCY;
81 
82 static float change_rep(float dir)
83 {
84  return M_PI_2 - dir;
85 }
86 
87 static struct EnuCoor_f process_new_point_trinity(struct EnuCoor_f *position, float alt_sp, float uav_direction)
88 {
89  struct EnuCoor_f new_point;
90  float rot_angle;
91 
93  rot_angle = -M_PI_2;
94  } else{
95  rot_angle = M_PI_2;
96  }
97 
98  new_point.x = position->x + (cosf(rot_angle + uav_direction) * nav_trinity.radius);
99  new_point.y = position->y + (sinf(rot_angle + uav_direction) * nav_trinity.radius);
100  new_point.z = alt_sp;
101 
102  return new_point;
103 }
104 
105 static struct EnuCoor_f process_first_point_trinity(struct EnuCoor_f *position, float alt_sp, float uav_direction)
106 {
109  } else{
111  }
112 
113  return process_new_point_trinity(position, alt_sp, uav_direction);
114 }
115 
116 static void update_target_point(struct EnuCoor_f *target, struct EnuCoor_f *border, float dt, float tau)
117 {
118  // with a proper discrete transform, coeff should be exp(-dt/tau) and (1-exp(-dt/tau))
119  // but to make it simpler with the same behavior at the limits, we use tau/(dt+tau) and dt/(dt+tau)
120  // with a positive value for tau
121  if (tau > 1e-4) {
122  float alpha = dt / (dt + tau);
123  target->x = (border->x * alpha) + (target->x * (1.f - alpha));
124  target->y = (border->y * alpha) + (target->y * (1.f - alpha));
125  }
126 }
127 
128 
129 #if USE_MISSION
131 
132 static bool nav_trinity_mission(uint8_t nb, float *params, enum MissionRunFlag flag)
133 {
134  if (flag == MissionInit && nb == 8) {
135  float start_x = params[0];
136  float start_y = params[1];
137  float start_z = params[2];
138  int first_turn = params[3];
139  float circle_radius = params[4];
140  float vx = params[5];
141  float vy = params[6];
142  float vz = params[7];
143  nav_trinity_setup(start_x, start_y, start_z, first_turn, circle_radius, vx, vy, vz);
144  return true;
145  }
146  else if (flag == MissionUpdate && nb == 3) {
147  // update target 3D position (ENU frame, above ground alt)
148  float x = params[0];
149  float y = params[1];
150  float z = params[2];
152  return true;
153  }
154  else if (flag == MissionUpdate && nb == 2) {
155  // update horizontal speed
156  float vx = params[0];
157  float vy = params[1];
158  nav_trinity.pos_incr.x = vx * nav_dt;
159  nav_trinity.pos_incr.y = vy * nav_dt;
160  return true;
161  }
162  else if (flag == MissionUpdate && nb == 1) {
163  // update vertical speed
164  float vz = params[0];
165  nav_trinity.pos_incr.z = vz * nav_dt;
166  return true;
167  }
168  else if (flag == MissionRun) {
169  return nav_trinity_run();
170  }
171  return false; // not a valid case
172 }
173 #endif
174 
175 // ABI message
176 
177 #ifndef NAV_TRINITY_LWC_ID
178 #define NAV_TRINITY_LWC_ID ABI_BROADCAST
179 #endif
180 
182 
183 static void lwc_cb(uint8_t sender_id UNUSED, uint32_t stamp UNUSED, int32_t data_type, uint32_t size, uint8_t * data) {
184  if (data_type == 1 && size == 1) {
185  nav_trinity.inside_cloud = (bool) data[0];
186  }
187 }
188 
190 {
196  nav_trinity.inside_cloud = false;
197 
198  AbiBindMsgPAYLOAD_DATA(NAV_TRINITY_LWC_ID, &lwc_ev, lwc_cb);
199 
200 #if USE_MISSION
201  mission_register(nav_trinity_mission, "TRNTY");
202 #endif
203 }
204 
205 void nav_trinity_setup(float init_x, float init_y,
206  float init_z, int turn,
207  float desired_radius, float vx,
208  float vy, float vz)
209 {
210  struct EnuCoor_f start = {init_x, init_y, init_z};
211  // increment based on speed
213 
214  nav_trinity.target = start;
216  nav_trinity.inside_cloud = false;
217  nav_trinity.radius = desired_radius;
218 
219  if (turn == 1) {
221  nav_trinity.radius_sign = 1.0f;
222  } else {
224  nav_trinity.radius_sign = -1.0f;
225  }
226 
228 }
229 
230 bool nav_trinity_run(void)
231 {
232  float pre_climb = 0.f;
233  float time = get_sys_time_float();
234 
235  NavVerticalAutoThrottleMode(0.f); /* No pitch */
236 
237  switch (nav_trinity.status) {
238  case TRINITY_ENTER:
239  // init stage
240  nav_init_stage();
241  // reach target point
244 
247  }
248  break;
250  // prepare inside circle
254  // init stage
255  nav_init_stage();
256  // update border and target for recover
260  // fly inside
262  break;
263  case TRINITY_INSIDE:
264  // increment center position
267  pre_climb = nav_trinity.pos_incr.z / nav_dt;
269 
270  if (!nav_trinity.inside_cloud) {
271  // found border, start outside
273  }
275  // most likely lost inside
277  }
278  break;
280  // prepare outside circle
284  // init stage
285  nav_init_stage();
286  // upadte border and target for recover
290  // fly outside
292  break;
293  case TRINITY_OUTSIDE:
294  // increment center position
296  pre_climb = nav_trinity.pos_incr.z / nav_dt;
299 
301  // found border, continue inside
303  }
305  // most likely lost outside
307  }
308  break;
310  // prepare recovery circle or line
313  // initial recovery radius
316  // init stage
317  nav_init_stage();
320  }
321  else {
323  }
324  break;
326  // increment border position
327  // fly in opposite direction to cover more space
329  if (!nav_trinity.inside_cloud) {
331  }
332  break;
334  // increment center position
337  // increment recover circle radius
340  }
341  // found a new border
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 }
NavTrinity::status
enum TrinityStatus status
Definition: nav_trinity.c:61
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
TRINITY_RECOVER_OUTSIDE
@ TRINITY_RECOVER_OUTSIDE
Definition: nav_trinity.c:52
nav_dt
static const float nav_dt
Definition: nav_trinity.c:80
nav_send_waypoint
void nav_send_waypoint(uint8_t wp_id)
Send a waypoint throught default telemetry channel.
Definition: common_nav.c:210
change_rep
static float change_rep(float dir)
Definition: nav_trinity.c:82
abi.h
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_trinity.c:183
get_sys_time_float
static float get_sys_time_float(void)
Get the time in seconds since startup.
Definition: sys_time.h:134
NavTrinity::radius_sign
float radius_sign
Definition: nav_trinity.c:72
MissionInit
@ MissionInit
first exec
Definition: mission_common.h:55
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:65
nav_circle_XY
void nav_circle_XY(float x, float y, float radius)
Navigates around (x, y).
Definition: nav.c:109
UNUSED
uint8_t last_wp UNUSED
Definition: navigation.c:97
alpha
float alpha
Definition: textons.c:107
TRINITY_LEFT
@ TRINITY_LEFT
Definition: nav_trinity.c:56
VECT3_ADD
#define VECT3_ADD(_a, _b)
Definition: pprz_algebra.h:147
TRINITY_OUTSIDE_START
@ TRINITY_OUTSIDE_START
Definition: nav_trinity.c:48
NavTrinity::last_border_time
float last_border_time
Definition: nav_trinity.c:73
nav_trinity_init
void nav_trinity_init(void)
Init function called by modules init.
Definition: nav_trinity.c:189
EnuCoor_f::y
float y
in meters
Definition: pprz_geodetic_float.h:74
NavTrinity::pos_incr
struct FloatVect3 pos_incr
Definition: nav_trinity.c:69
EnuCoor_f::z
float z
in meters
Definition: pprz_geodetic_float.h:75
NavTrinity::target
struct EnuCoor_f target
Definition: nav_trinity.c:65
NavVerticalAutoThrottleMode
#define NavVerticalAutoThrottleMode(_pitch)
Set the climb control to auto-throttle with the specified pitch pre-command.
Definition: nav.h:176
NavCircleCountNoRewind
#define NavCircleCountNoRewind()
Definition: nav.h:153
NavTrinity::max_recover_radius
float max_recover_radius
Definition: nav_trinity.c:75
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:381
dir
static const float dir[]
Definition: shift_tracking.c:91
process_first_point_trinity
static struct EnuCoor_f process_first_point_trinity(struct EnuCoor_f *position, float alt_sp, float uav_direction)
Definition: nav_trinity.c:105
DEFAULT_CIRCLE_RADIUS
#define DEFAULT_CIRCLE_RADIUS
default nav_circle_radius in meters
Definition: navigation.c:56
update_target_point
static void update_target_point(struct EnuCoor_f *target, struct EnuCoor_f *border, float dt, float tau)
Definition: nav_trinity.c:116
TRINITY_ENTER
@ TRINITY_ENTER
Definition: nav_trinity.c:45
nav_init_stage
void nav_init_stage(void)
needs to be implemented by fixedwing and rotorcraft seperately
Definition: nav.c:93
TrinityStatus
TrinityStatus
Definition: nav_trinity.c:44
NAVIGATION_FREQUENCY
#define NAVIGATION_FREQUENCY
Default fixedwing navigation frequency.
Definition: nav.h:47
nav_trinity_run
bool nav_trinity_run(void)
Navigation function Called by flight plan or mission run function.
Definition: nav_trinity.c:230
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
TRINITY_RIGHT
@ TRINITY_RIGHT
Definition: nav_trinity.c:57
NavTrinity::direction
float direction
Definition: nav_trinity.c:70
NAV_TRINITY_BORDER_FILTER
#define NAV_TRINITY_BORDER_FILTER
Definition: nav_trinity.c:41
lwc_ev
static abi_event lwc_ev
Definition: nav_trinity.c:181
autopilot.h
NavTrinity::estim_border
struct EnuCoor_f estim_border
Definition: nav_trinity.c:67
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
NavTrinity::rotation
enum RotationDir rotation
Definition: nav_trinity.c:62
f
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)
Definition: wedgebug.c:204
NavTrinity::recover_circle
struct EnuCoor_f recover_circle
Definition: nav_trinity.c:68
FloatVect3::y
float y
Definition: pprz_algebra_float.h:56
FloatVect2::y
float y
Definition: pprz_algebra_float.h:51
NAV_TRINITY_RECOVER_MAX_TURN
#define NAV_TRINITY_RECOVER_MAX_TURN
Definition: nav_trinity.c:37
RotationDir
RotationDir
Definition: nav_lace.c:56
NavTrinity::recover_radius
float recover_radius
Definition: nav_trinity.c:74
nav.h
int32_t
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
mission_common.h
mission planner library
ground_alt
float ground_alt
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:41
FloatVect3::x
float x
Definition: pprz_algebra_float.h:55
MissionUpdate
@ MissionUpdate
param update
Definition: mission_common.h:56
nav_trinity_setup
void nav_trinity_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_trinity.c:205
nav_trinity
static struct NavTrinity nav_trinity
Definition: nav_trinity.c:78
mission_register
bool mission_register(mission_custom_cb cb, char *type)
Register a new navigation or action callback function.
Definition: mission_common.c:115
NAV_TRINITY_LWC_ID
#define NAV_TRINITY_LWC_ID
Definition: nav_trinity.c:178
process_new_point_trinity
static struct EnuCoor_f process_new_point_trinity(struct EnuCoor_f *position, float alt_sp, float uav_direction)
Definition: nav_trinity.c:87
NavTrinity::actual
struct EnuCoor_f actual
Definition: nav_trinity.c:64
TRINITY_OUTSIDE
@ TRINITY_OUTSIDE
Definition: nav_trinity.c:49
FloatVect3::z
float z
Definition: pprz_algebra_float.h:57
FloatVect2::x
float x
Definition: pprz_algebra_float.h:50
TRINITY_RECOVER_START
@ TRINITY_RECOVER_START
Definition: nav_trinity.c:50
EnuCoor_f::x
float x
in meters
Definition: pprz_geodetic_float.h:73
NavTrinity
Definition: nav_trinity.c:60
state.h
VECT3_ASSIGN
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:125
TRINITY_RECOVER_INSIDE
@ TRINITY_RECOVER_INSIDE
Definition: nav_trinity.c:51
MissionRunFlag
MissionRunFlag
Definition: mission_common.h:53
NavTrinity::inside_cloud
bool inside_cloud
Definition: nav_trinity.c:63
TRINITY_INSIDE_FIRST
@ TRINITY_INSIDE_FIRST
Definition: nav_trinity.c:46
NavTrinity::circle
struct EnuCoor_f circle
Definition: nav_trinity.c:66
target
struct FloatVect2 target
Definition: obstacle_avoidance.c:78
NavTrinity::radius
float radius
Definition: nav_trinity.c:71
TRINITY_INSIDE
@ TRINITY_INSIDE
Definition: nav_trinity.c:47
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:190
nav_trinity.h