Paparazzi UAS  v6.3_unstable
Paparazzi is a free software Unmanned Aircraft System.
navigation.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2011 The Paparazzi Team
3  * Copyright (C) 2022 Gautier Hattenberger <gautier.hattenberger@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 
28 #ifndef NAVIGATION_H
29 #define NAVIGATION_H
30 
31 #include "std.h"
32 #include "math/pprz_geodetic_int.h"
34 #include "state.h"
35 #include "modules/nav/waypoints.h"
37 #include "autopilot.h"
38 #include "generated/airframe.h"
39 
41 #ifndef DEFAULT_CIRCLE_RADIUS
42 #define DEFAULT_CIRCLE_RADIUS 5.f
43 #endif
44 
45 #ifndef NAV_CLIMB_VSPEED
46 #define NAV_CLIMB_VSPEED 0.5f
47 #endif
48 
49 #ifndef NAV_DESCEND_VSPEED
50 #define NAV_DESCEND_VSPEED -0.8f
51 #endif
52 
54 #ifndef ARRIVED_AT_WAYPOINT
55 #define ARRIVED_AT_WAYPOINT 3.0f
56 #endif
57 
59 #ifndef FAILSAFE_MODE_DISTANCE
60 #define FAILSAFE_MODE_DISTANCE (1.2*MAX_DIST_FROM_HOME)
61 #endif
62 
64 #ifndef NAV_CARROT_DIST
65 #define NAV_CARROT_DIST 12
66 #endif
67 
69 #ifndef CARROT
70 #define CARROT 0
71 #endif
72 
74 #ifndef NAVIGATION_FREQUENCY
75 #if PERIODIC_FREQUENCY == 512
76 #define NAVIGATION_FREQUENCY 16
77 #else // if not 512, assume a multiple of 20 (e.g. 200, 500, 1000, ...)
78 #define NAVIGATION_FREQUENCY 20
79 #endif
80 #endif
81 
85 #define NAV_HORIZONTAL_MODE_WAYPOINT 0
86 #define NAV_HORIZONTAL_MODE_ROUTE 1
87 #define NAV_HORIZONTAL_MODE_CIRCLE 2
88 #define NAV_HORIZONTAL_MODE_ATTITUDE 3
89 #define NAV_HORIZONTAL_MODE_MANUAL 4
90 #define NAV_HORIZONTAL_MODE_GUIDED 5
91 
92 #define NAV_VERTICAL_MODE_MANUAL 0
93 #define NAV_VERTICAL_MODE_CLIMB 1
94 #define NAV_VERTICAL_MODE_ALT 2
95 #define NAV_VERTICAL_MODE_GUIDED 3
96 
100 #define NAV_SETPOINT_MODE_POS 0
101 #define NAV_SETPOINT_MODE_SPEED 1
102 #define NAV_SETPOINT_MODE_ACCEL 2
103 #define NAV_SETPOINT_MODE_ATTITUDE 3 // attitude defined by roll, pitch and heading
104 #define NAV_SETPOINT_MODE_QUAT 4 // attitude defined by unit quaternion
105 #define NAV_SETPOINT_MODE_RATE 5
106 #define NAV_SETPOINT_MODE_MANUAL 6
107 
108 typedef void (*navigation_stage_init)(void);
109 typedef void (*navigation_goto)(struct EnuCoor_f *wp);
110 typedef void (*navigation_route)(struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_end);
111 typedef bool (*navigation_approaching)(struct EnuCoor_f *wp_to, struct EnuCoor_f *wp_from, float approaching_time);
112 typedef void (*navigation_circle)(struct EnuCoor_f *wp_center, float radius);
113 typedef void (*navigation_oval_init)(void);
114 typedef void (*navigation_oval)(struct EnuCoor_f *wp1, struct EnuCoor_f *wp2, float radius);
115 
116 
120  // mode
121  uint8_t horizontal_mode; // nav horizontal mode
122  uint8_t vertical_mode; // nav vertical mode
123  uint8_t setpoint_mode; // nav setpoint mode
124 
125  // commands
126  struct EnuCoor_f target;
127  struct EnuCoor_f carrot;
128  struct EnuCoor_f speed;
129  struct EnuCoor_f accel;
134  float roll;
135  float pitch;
136  float heading;
137  struct FloatQuat quat;
138  struct FloatRates rates;
139  float radius;
140  float climb;
141  float fp_altitude;
142  float nav_altitude;
143 
144  // misc
148  float climb_vspeed;
150 
151  // pointers to basic nav functions
159 };
160 
161 extern struct RotorcraftNavigation nav;
162 
169 // TODO: eight, survey
170 
171 
172 // flight altitude setting
173 extern float flight_altitude; // hmsl flight altitude in meters
174 
175 
176 /*****************************************************************
177  * macros to ensure compatibility between fixedwing and rotorcraft
178  *****************************************************************/
179 
181 #define GetPosX() (stateGetPositionEnu_f()->x)
183 #define GetPosY() (stateGetPositionEnu_f()->y)
185 #define GetPosAlt() (stateGetPositionEnu_f()->z+state.ned_origin_f.hmsl)
187 #define GetPosHeight() (stateGetPositionEnu_f()->z)
194 #define GetAltRef() (state.ned_origin_f.hmsl)
195 
196 
197 extern void nav_init(void);
198 extern void nav_run(void);
199 extern void nav_parse_BLOCK(uint8_t *buf);
200 extern void nav_parse_MOVE_WP(uint8_t *buf);
201 
202 extern float get_dist2_to_waypoint(uint8_t wp_id);
203 extern float get_dist2_to_point(struct EnuCoor_f *p);
204 extern void compute_dist2_to_home(void);
205 extern void nav_home(void);
206 extern void nav_set_manual(int32_t roll, int32_t pitch, int32_t yaw);
207 
208 extern void nav_reset_reference(void) __attribute__((unused));
209 extern void nav_reset_alt(void) __attribute__((unused));
210 extern void nav_periodic_task(void);
211 
212 extern bool nav_is_in_flight(void);
213 
214 extern void nav_glide_points(struct EnuCoor_f *start_point, struct EnuCoor_f *end_point);
215 
217 extern void nav_set_heading_rad(float rad);
218 extern void nav_set_heading_deg(float deg);
219 extern void nav_set_heading_towards(float x, float y);
221 extern void nav_set_heading_towards_target(void);
222 extern void nav_set_heading_current(void);
223 
224 extern void nav_set_failsafe(void);
225 
226 /* ground detection */
227 extern bool nav_detect_ground(void);
228 #define NavStartDetectGround() ({ autopilot.detect_ground_once = true; false; })
229 #define NavDetectGround() nav_detect_ground()
230 
231 /* switching motors on/off */
232 static inline void NavKillThrottle(void)
233 {
235 }
236 static inline void NavResurrect(void)
237 {
239 }
240 
241 
242 #define NavSetManual nav_set_manual
243 #define NavSetFailsafe nav_set_failsafe
244 
245 #define NavSetGroundReferenceHere nav_reset_reference
246 #define NavSetAltitudeReferenceHere nav_reset_alt
247 
248 #define NavSetWaypointHere waypoint_set_here_2d
249 #define NavCopyWaypoint waypoint_copy
250 #define NavCopyWaypointPositionOnly waypoint_position_copy
251 
253 bool nav_check_wp_time(struct EnuCoor_f *wp, uint16_t stay_time);
254 #define NavCheckWaypointTime(wp, time) nav_check_wp_time(&waypoints[wp].enu_f, time)
255 
256 
257 /***********************************************************
258  * macros used by flight plan to set different modes
259  **********************************************************/
260 
261 #define NavAttitude(_roll) { \
262  nav.horizontal_mode = NAV_HORIZONTAL_MODE_ATTITUDE; \
263  nav.setpoint_mode = NAV_SETPOINT_MODE_ATTITUDE; \
264  nav.roll = _roll; \
265  }
266 
269 #define NavVerticalAutoThrottleMode(_pitch) { \
270  nav.pitch = _pitch; \
271  }
272 
275 #define NavVerticalAltitudeMode(_alt, _pre_climb) { \
276  nav.vertical_mode = NAV_VERTICAL_MODE_ALT; \
277  nav.fp_altitude = _alt; \
278  }
279 
281 #define NavVerticalClimbMode(_climb) { \
282  nav.vertical_mode = NAV_VERTICAL_MODE_CLIMB; \
283  nav.climb = _climb; \
284  nav.speed.z = _climb; \
285  }
286 
288 #define NavVerticalThrottleMode(_throttle) { \
289  nav.vertical_mode = NAV_VERTICAL_MODE_MANUAL; \
290  nav.throttle = _throttle; \
291  }
292 
294 #define NavHeading nav_set_heading_rad
295 
296 
297 /***********************************************************
298  * built in navigation routines
299  **********************************************************/
300 
301 /*********** Navigation to waypoint *************************************/
302 static inline void NavGotoWaypoint(uint8_t wp)
303 {
304  struct EnuCoor_f * _wp = waypoint_get_enu_f(wp);
305  if (_wp != NULL) {
306  nav.nav_goto(_wp);
307  }
308 }
309 
310 /*********** Navigation along a line *************************************/
311 static inline void NavSegment(uint8_t wp_start, uint8_t wp_end)
312 {
313  struct EnuCoor_f * _wp_start = waypoint_get_enu_f(wp_start);
314  struct EnuCoor_f * _wp_end = waypoint_get_enu_f(wp_end);
315  if (_wp_start != NULL && _wp_end != NULL) {
316  nav.nav_route(_wp_start, _wp_end);
317  }
318 }
319 
320 static inline bool NavApproaching(uint8_t wp, float approaching_time)
321 {
322  struct EnuCoor_f * _wp = waypoint_get_enu_f(wp);
323  if (_wp != NULL) {
324  return nav.nav_approaching(_wp, NULL, approaching_time);
325  }
326  return true; // not valid, end now
327 }
328 
329 static inline bool NavApproachingFrom(uint8_t to, uint8_t from, float approaching_time)
330 {
331  struct EnuCoor_f * _to = waypoint_get_enu_f(to);
332  struct EnuCoor_f * _from = waypoint_get_enu_f(from);
333  if (_to != NULL && _from != NULL) {
334  return nav.nav_approaching(_to, _from, approaching_time);
335  }
336  return true; // not valid, end now
337 }
338 
339 /*********** Navigation on a circle **************************************/
340 static inline void NavCircleWaypoint(uint8_t wp_center, float radius)
341 {
342  struct EnuCoor_f * _wp_center = waypoint_get_enu_f(wp_center);
343  if (_wp_center != NULL) {
344  nav.nav_circle(_wp_center, radius);
345  }
346 }
347 
348 
349 /*********** Navigation along an oval *************************************/
350 static inline void nav_oval_init(void)
351 {
352  nav.nav_oval_init();
353 }
354 
355 static inline void Oval(uint8_t wp1, uint8_t wp2, float radius)
356 {
357  struct EnuCoor_f * _wp1 = waypoint_get_enu_f(wp1);
358  struct EnuCoor_f * _wp2 = waypoint_get_enu_f(wp2);
359  if (_wp1 != NULL && _wp2 != NULL) {
360  nav.nav_oval(_wp1, _wp2, radius);
361  }
362 }
363 
364 
367 static inline void NavGlide(uint8_t wp_start, uint8_t wp_end)
368 {
369  struct EnuCoor_f * _wp_start = waypoint_get_enu_f(wp_start);
370  struct EnuCoor_f * _wp_end = waypoint_get_enu_f(wp_end);
371  if (_wp_start != NULL && _wp_end != NULL) {
372  nav_glide_points(_wp_start, _wp_end);
373  }
374 }
375 
376 
377 /***********************************************************
378  * settings handlers
379  **********************************************************/
380 #define nav_IncreaseShift(x) {}
381 #define nav_SetNavRadius(x) {}
382 #define navigation_SetFlightAltitude(x) { \
383  flight_altitude = x; \
384  nav.nav_altitude = flight_altitude - state.ned_origin_f.hmsl; \
385  }
386 
390 #define NavVerticalAutoPitchMode(_throttle) {}
391 
392 /* follow another aircraft not implemented with this function
393  * see dedicated nav modules
394  */
395 #define NavFollow(_i, _d, _h) {}
396 
397 #endif /* NAVIGATION_H */
uint8_t autopilot_get_mode(void)
get autopilot mode
Definition: autopilot.c:216
void autopilot_set_motors_on(bool motors_on)
turn motors on/off, eventually depending of the current mode set kill_throttle accordingly FIXME is i...
Definition: autopilot.c:232
Core autopilot interface common to all firmwares.
Common flight_plan functions shared between fixedwing and rotorcraft.
Roation quaternion.
angular rates
static float p[2][2]
struct EnuCoor_f * waypoint_get_enu_f(uint8_t wp_id)
Get ENU coordinates (float)
Definition: waypoints.c:379
void nav_oval(uint8_t p1, uint8_t p2, float radius)
Definition: nav.c:763
static void nav_goto(struct EnuCoor_f *wp)
static void _nav_oval_init(void)
static void nav_route(struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_end)
static void nav_circle(struct EnuCoor_f *wp_center, float radius)
static bool nav_approaching(struct EnuCoor_f *wp, struct EnuCoor_f *from, float approaching_time)
static void nav_stage_init(void)
Implement basic nav function.
Paparazzi floating point math for geodetic calculations.
vector in East North Up coordinates Units: meters
Paparazzi fixed point math for geodetic calculations.
#define AP_MODE_NAV
struct EnuCoor_f speed
speed setpoint (in m/s)
Definition: navigation.h:128
static void NavGlide(uint8_t wp_start, uint8_t wp_end)
Nav glide routine.
Definition: navigation.h:367
void nav_home(void)
Home mode navigation (circle around HOME)
Definition: nav.c:424
int32_t cmd_pitch
pitch command (in pprz_t)
Definition: navigation.h:132
static void NavGotoWaypoint(uint8_t wp)
Definition: navigation.h:302
float flight_altitude
Dynamically adjustable, reset to nav_altitude when it is changing.
Definition: nav.c:74
void nav_reset_reference(void)
Reset the geographic reference to the current GPS fix.
Definition: navigation.c:238
void nav_register_oval(navigation_oval_init _nav_oval_init, navigation_oval nav_oval)
Definition: navigation.c:424
struct FloatRates rates
rates setpoint (in rad/s)
Definition: navigation.h:138
void nav_set_heading_current(void)
Set heading to the current yaw angle.
Definition: navigation.c:395
int32_t cmd_roll
roll command (in pprz_t)
Definition: navigation.h:131
uint32_t throttle
throttle command (in pprz_t)
Definition: navigation.h:130
void nav_set_manual(int32_t roll, int32_t pitch, int32_t yaw)
Set manual roll, pitch and yaw without stabilization.
Definition: navigation.c:318
void nav_reset_alt(void)
Reset the altitude reference to the current GPS alt.
Definition: navigation.c:245
float climb
climb setpoint (in m/s)
Definition: navigation.h:140
void nav_set_heading_towards_waypoint(uint8_t wp)
Set heading in the direction of a waypoint.
Definition: navigation.c:383
static void Oval(uint8_t wp1, uint8_t wp2, float radius)
Definition: navigation.h:355
void compute_dist2_to_home(void)
Computes squared distance to the HOME waypoint potentially sets too_far_from_home.
Definition: navigation.c:346
bool nav_is_in_flight(void)
Definition: navigation.c:278
void nav_run(void)
Definition: navigation.c:169
void nav_set_heading_towards(float x, float y)
Set heading to point towards x,y position in local coordinates.
Definition: navigation.c:370
struct RotorcraftNavigation nav
Definition: navigation.c:51
float dist2_to_home
squared distance to home waypoint
Definition: navigation.h:145
void(* navigation_goto)(struct EnuCoor_f *wp)
Definition: navigation.h:109
bool(* navigation_approaching)(struct EnuCoor_f *wp_to, struct EnuCoor_f *wp_from, float approaching_time)
Definition: navigation.h:111
float failsafe_mode_dist2
maximum squared distance to home wp before going to failsafe mode
Definition: navigation.h:147
void(* navigation_oval)(struct EnuCoor_f *wp1, struct EnuCoor_f *wp2, float radius)
Definition: navigation.h:114
void nav_init(void)
Navigation Initialisation.
Definition: nav.c:532
int32_t cmd_yaw
yaw command (in pprz_t)
Definition: navigation.h:133
static bool NavApproachingFrom(uint8_t to, uint8_t from, float approaching_time)
Definition: navigation.h:329
void nav_parse_MOVE_WP(uint8_t *buf)
Definition: navigation.c:125
navigation_stage_init nav_stage_init
Definition: navigation.h:152
void nav_register_circle(navigation_circle nav_circle)
Definition: navigation.c:419
void nav_register_goto_wp(navigation_goto nav_goto, navigation_route nav_route, navigation_approaching nav_approaching)
Register functions.
Definition: navigation.c:412
navigation_oval nav_oval
Definition: navigation.h:158
navigation_approaching nav_approaching
Definition: navigation.h:155
struct FloatQuat quat
quaternion setpoint
Definition: navigation.h:137
void nav_parse_BLOCK(uint8_t *buf)
Definition: navigation.c:119
navigation_circle nav_circle
Definition: navigation.h:156
static void NavSegment(uint8_t wp_start, uint8_t wp_end)
Definition: navigation.h:311
void(* navigation_oval_init)(void)
Definition: navigation.h:113
struct EnuCoor_f accel
accel setpoint (in m/s)
Definition: navigation.h:129
void nav_set_heading_towards_target(void)
Set heading in the direction of the target.
Definition: navigation.c:389
static void nav_oval_init(void)
Definition: navigation.h:350
float nav_altitude
current altitude setpoint (in meters): might differ from fp_altitude depending on altitude shift from...
Definition: navigation.h:142
float radius
radius setpoint (in meters)
Definition: navigation.h:139
float descend_vspeed
descend speed setting, mostly used in flight plans
Definition: navigation.h:149
float get_dist2_to_point(struct EnuCoor_f *p)
Returns squared horizontal distance to given point.
Definition: navigation.c:328
navigation_oval_init nav_oval_init
Definition: navigation.h:157
void nav_register_stage_init(navigation_stage_init nav_stage_init)
Registering functions.
Definition: navigation.c:407
void(* navigation_route)(struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_end)
Definition: navigation.h:110
float pitch
pitch angle (in radians)
Definition: navigation.h:135
void nav_glide_points(struct EnuCoor_f *start_point, struct EnuCoor_f *end_point)
Definition: navigation.c:283
void(* navigation_circle)(struct EnuCoor_f *wp_center, float radius)
Definition: navigation.h:112
float heading
heading setpoint (in radians)
Definition: navigation.h:136
struct EnuCoor_f carrot
carrot position (also used for GCS display)
Definition: navigation.h:127
static void NavKillThrottle(void)
Definition: navigation.h:232
navigation_goto nav_goto
Definition: navigation.h:153
void nav_set_heading_rad(float rad)
heading utility functions
Definition: navigation.c:357
static void NavResurrect(void)
Definition: navigation.h:236
bool nav_detect_ground(void)
Definition: navigation.c:271
void nav_set_heading_deg(float deg)
Set nav_heading in degrees.
Definition: navigation.c:364
navigation_route nav_route
Definition: navigation.h:154
static void NavCircleWaypoint(uint8_t wp_center, float radius)
Definition: navigation.h:340
bool nav_check_wp_time(struct EnuCoor_f *wp, uint16_t stay_time)
Check the time spent in a radius of 'ARRIVED_AT_WAYPOINT' around a wp
Definition: navigation.c:184
void nav_periodic_task(void)
Navigation main: call to the code generated from the XML flight plan.
Definition: nav.c:445
float get_dist2_to_waypoint(uint8_t wp_id)
Returns squared horizontal distance to given waypoint.
Definition: navigation.c:338
float fp_altitude
altitude setpoint from flight plan (in meters)
Definition: navigation.h:141
void(* navigation_stage_init)(void)
Definition: navigation.h:108
struct EnuCoor_f target
final target position (in meters)
Definition: navigation.h:126
static bool NavApproaching(uint8_t wp, float approaching_time)
Definition: navigation.h:320
bool too_far_from_home
too_far flag
Definition: navigation.h:146
float climb_vspeed
climb speed setting, mostly used in flight plans
Definition: navigation.h:148
float roll
roll angle (in radians)
Definition: navigation.h:134
void nav_set_failsafe(void)
Definition: navigation.c:400
General Navigation structure.
Definition: navigation.h:119
API to get/set the generic vehicle states.
#define TRUE
Definition: std.h:4
#define FALSE
Definition: std.h:5
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
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