Paparazzi UAS  v7.0_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_NONE 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;
131  float roll;
132  float pitch;
133  float heading;
134  struct FloatQuat quat;
135  struct FloatRates rates;
136  float radius;
137  float climb;
138  float fp_altitude;
139  float nav_altitude;
140 
141  // misc
145  float climb_vspeed;
147 
148  // pointers to basic nav functions
156 };
157 
158 extern struct RotorcraftNavigation nav;
159 
166 // TODO: eight, survey
167 
168 
169 // flight altitude setting
170 extern float flight_altitude; // hmsl flight altitude in meters
171 
172 
173 /*****************************************************************
174  * macros to ensure compatibility between fixedwing and rotorcraft
175  *****************************************************************/
176 
178 #define GetPosX() (stateGetPositionEnu_f()->x)
180 #define GetPosY() (stateGetPositionEnu_f()->y)
182 #define GetPosAlt() (stateGetPositionEnu_f()->z+state.ned_origin_f.hmsl)
184 #define GetPosHeight() (stateGetPositionEnu_f()->z)
191 #define GetAltRef() (state.ned_origin_f.hmsl)
192 
193 
194 extern void nav_init(void);
195 extern void nav_run(void);
196 extern void nav_parse_BLOCK(uint8_t *buf);
197 extern void nav_parse_MOVE_WP(uint8_t *buf);
198 
199 extern float get_dist2_to_waypoint(uint8_t wp_id);
200 extern float get_dist2_to_point(struct EnuCoor_f *p);
201 extern void compute_dist2_to_home(void);
202 extern void nav_home(void);
203 
204 extern void nav_reset_reference(void) __attribute__((unused));
205 extern void nav_reset_alt(void) __attribute__((unused));
206 extern void nav_periodic_task(void);
207 
208 extern bool nav_is_in_flight(void);
209 
210 extern void nav_glide_points(struct EnuCoor_f *start_point, struct EnuCoor_f *end_point);
211 
213 extern void nav_set_heading_rad(float rad);
214 extern void nav_set_heading_deg(float deg);
215 extern void nav_set_heading_towards(float x, float y);
217 extern void nav_set_heading_towards_target(void);
218 extern void nav_set_heading_current(void);
219 
220 extern void nav_set_failsafe(void);
221 
222 /* ground detection */
223 extern bool nav_detect_ground(void);
224 #define NavStartDetectGround() ({ autopilot.detect_ground_once = true; false; })
225 #define NavDetectGround() nav_detect_ground()
226 
227 /* switching motors on/off */
228 static inline void NavKillThrottle(void)
229 {
231 }
232 static inline void NavResurrect(void)
233 {
235 }
236 
237 
238 #define NavSetFailsafe nav_set_failsafe
239 
240 #define NavSetGroundReferenceHere nav_reset_reference
241 #define NavSetAltitudeReferenceHere nav_reset_alt
242 
243 #define NavSetWaypointHere waypoint_set_here_2d
244 #define NavCopyWaypoint waypoint_copy
245 #define NavCopyWaypointPositionOnly waypoint_position_copy
246 
248 bool nav_check_wp_time(struct EnuCoor_f *wp, uint16_t stay_time);
249 #define NavCheckWaypointTime(wp, time) nav_check_wp_time(&waypoints[wp].enu_f, time)
250 
251 
252 /***********************************************************
253  * macros used by flight plan to set different modes
254  **********************************************************/
255 
256 #define NavAttitude(_roll) { \
257  nav.horizontal_mode = NAV_HORIZONTAL_MODE_ATTITUDE; \
258  nav.setpoint_mode = NAV_SETPOINT_MODE_ATTITUDE; \
259  nav.roll = _roll; \
260  }
261 
264 #define NavVerticalAutoThrottleMode(_pitch) { \
265  nav.pitch = _pitch; \
266  }
267 
270 #define NavVerticalAltitudeMode(_alt, _pre_climb) { \
271  nav.vertical_mode = NAV_VERTICAL_MODE_ALT; \
272  nav.fp_altitude = _alt; \
273  }
274 
276 #define NavVerticalClimbMode(_climb) { \
277  nav.vertical_mode = NAV_VERTICAL_MODE_CLIMB; \
278  nav.climb = _climb; \
279  nav.speed.z = _climb; \
280  }
281 
283 #define NavVerticalThrottleMode(_throttle) { \
284  nav.vertical_mode = NAV_VERTICAL_MODE_MANUAL; \
285  nav.throttle = _throttle; \
286  }
287 
289 #define NavHeading nav_set_heading_rad
290 
291 
292 /***********************************************************
293  * built in navigation routines
294  **********************************************************/
295 
296 /*********** Navigation to waypoint *************************************/
297 static inline void NavGotoWaypoint(uint8_t wp)
298 {
299  struct EnuCoor_f * _wp = waypoint_get_enu_f(wp);
300  if (_wp != NULL) {
301  nav.nav_goto(_wp);
302  }
303 }
304 
305 /*********** Navigation along a line *************************************/
306 static inline void NavSegment(uint8_t wp_start, uint8_t wp_end)
307 {
308  struct EnuCoor_f * _wp_start = waypoint_get_enu_f(wp_start);
309  struct EnuCoor_f * _wp_end = waypoint_get_enu_f(wp_end);
310  if (_wp_start != NULL && _wp_end != NULL) {
311  nav.nav_route(_wp_start, _wp_end);
312  }
313 }
314 
315 static inline bool NavApproaching(uint8_t wp, float approaching_time)
316 {
317  struct EnuCoor_f * _wp = waypoint_get_enu_f(wp);
318  if (_wp != NULL) {
319  return nav.nav_approaching(_wp, NULL, approaching_time);
320  }
321  return true; // not valid, end now
322 }
323 
324 static inline bool NavApproachingFrom(uint8_t to, uint8_t from, float approaching_time)
325 {
326  struct EnuCoor_f * _to = waypoint_get_enu_f(to);
327  struct EnuCoor_f * _from = waypoint_get_enu_f(from);
328  if (_to != NULL && _from != NULL) {
329  return nav.nav_approaching(_to, _from, approaching_time);
330  }
331  return true; // not valid, end now
332 }
333 
334 /*********** Navigation on a circle **************************************/
335 static inline void NavCircleWaypoint(uint8_t wp_center, float radius)
336 {
337  struct EnuCoor_f * _wp_center = waypoint_get_enu_f(wp_center);
338  if (_wp_center != NULL) {
339  nav.nav_circle(_wp_center, radius);
340  }
341 }
342 
343 
344 /*********** Navigation along an oval *************************************/
345 static inline void nav_oval_init(void)
346 {
347  nav.nav_oval_init();
348 }
349 
350 static inline void Oval(uint8_t wp1, uint8_t wp2, float radius)
351 {
352  struct EnuCoor_f * _wp1 = waypoint_get_enu_f(wp1);
353  struct EnuCoor_f * _wp2 = waypoint_get_enu_f(wp2);
354  if (_wp1 != NULL && _wp2 != NULL) {
355  nav.nav_oval(_wp1, _wp2, radius);
356  }
357 }
358 
359 
362 static inline void NavGlide(uint8_t wp_start, uint8_t wp_end)
363 {
364  struct EnuCoor_f * _wp_start = waypoint_get_enu_f(wp_start);
365  struct EnuCoor_f * _wp_end = waypoint_get_enu_f(wp_end);
366  if (_wp_start != NULL && _wp_end != NULL) {
367  nav_glide_points(_wp_start, _wp_end);
368  }
369 }
370 
371 
372 /***********************************************************
373  * settings handlers
374  **********************************************************/
375 #define nav_IncreaseShift(x) {}
376 #define nav_SetNavRadius(x) {}
377 #define navigation_SetFlightAltitude(x) { \
378  flight_altitude = x; \
379  nav.nav_altitude = flight_altitude - state.ned_origin_f.hmsl; \
380  }
381 
385 #define NavVerticalAutoPitchMode(_throttle) {}
386 
387 /* follow another aircraft not implemented with this function
388  * see dedicated nav modules
389  */
390 #define NavFollow(_i, _d, _h) {}
391 
392 #endif /* NAVIGATION_H */
uint8_t autopilot_get_mode(void)
get autopilot mode
Definition: autopilot.c:217
bool 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:245
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:387
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:362
void nav_home(void)
Home mode navigation (circle around HOME)
Definition: nav.c:424
static void NavGotoWaypoint(uint8_t wp)
Definition: navigation.h:297
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:235
void nav_register_oval(navigation_oval_init _nav_oval_init, navigation_oval nav_oval)
Definition: navigation.c:402
struct FloatRates rates
rates setpoint (in rad/s)
Definition: navigation.h:135
void nav_set_heading_current(void)
Set heading to the current yaw angle.
Definition: navigation.c:373
uint32_t throttle
throttle command (in pprz_t)
Definition: navigation.h:130
void nav_reset_alt(void)
Reset the altitude reference to the current GPS alt.
Definition: navigation.c:242
float climb
climb setpoint (in m/s)
Definition: navigation.h:137
void nav_set_heading_towards_waypoint(uint8_t wp)
Set heading in the direction of a waypoint.
Definition: navigation.c:361
static void Oval(uint8_t wp1, uint8_t wp2, float radius)
Definition: navigation.h:350
void compute_dist2_to_home(void)
Computes squared distance to the HOME waypoint potentially sets too_far_from_home.
Definition: navigation.c:324
bool nav_is_in_flight(void)
Definition: navigation.c:275
void nav_run(void)
Definition: navigation.c:166
void nav_set_heading_towards(float x, float y)
Set heading to point towards x,y position in local coordinates.
Definition: navigation.c:348
struct RotorcraftNavigation nav
Definition: navigation.c:51
float dist2_to_home
squared distance to home waypoint
Definition: navigation.h:142
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:144
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
static bool NavApproachingFrom(uint8_t to, uint8_t from, float approaching_time)
Definition: navigation.h:324
void nav_parse_MOVE_WP(uint8_t *buf)
Definition: navigation.c:122
navigation_stage_init nav_stage_init
Definition: navigation.h:149
void nav_register_circle(navigation_circle nav_circle)
Definition: navigation.c:397
void nav_register_goto_wp(navigation_goto nav_goto, navigation_route nav_route, navigation_approaching nav_approaching)
Register functions.
Definition: navigation.c:390
navigation_oval nav_oval
Definition: navigation.h:155
navigation_approaching nav_approaching
Definition: navigation.h:152
struct FloatQuat quat
quaternion setpoint
Definition: navigation.h:134
void nav_parse_BLOCK(uint8_t *buf)
Definition: navigation.c:116
navigation_circle nav_circle
Definition: navigation.h:153
static void NavSegment(uint8_t wp_start, uint8_t wp_end)
Definition: navigation.h:306
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:367
static void nav_oval_init(void)
Definition: navigation.h:345
float nav_altitude
current altitude setpoint (in meters): might differ from fp_altitude depending on altitude shift from...
Definition: navigation.h:139
float radius
radius setpoint (in meters)
Definition: navigation.h:136
float descend_vspeed
descend speed setting, mostly used in flight plans
Definition: navigation.h:146
float get_dist2_to_point(struct EnuCoor_f *p)
Returns squared horizontal distance to given point.
Definition: navigation.c:306
navigation_oval_init nav_oval_init
Definition: navigation.h:154
void nav_register_stage_init(navigation_stage_init nav_stage_init)
Registering functions.
Definition: navigation.c:385
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:132
void nav_glide_points(struct EnuCoor_f *start_point, struct EnuCoor_f *end_point)
Definition: navigation.c:280
void(* navigation_circle)(struct EnuCoor_f *wp_center, float radius)
Definition: navigation.h:112
float heading
heading setpoint (in radians)
Definition: navigation.h:133
struct EnuCoor_f carrot
carrot position (also used for GCS display)
Definition: navigation.h:127
static void NavKillThrottle(void)
Definition: navigation.h:228
navigation_goto nav_goto
Definition: navigation.h:150
void nav_set_heading_rad(float rad)
heading utility functions
Definition: navigation.c:335
static void NavResurrect(void)
Definition: navigation.h:232
bool nav_detect_ground(void)
Definition: navigation.c:268
void nav_set_heading_deg(float deg)
Set nav_heading in degrees.
Definition: navigation.c:342
navigation_route nav_route
Definition: navigation.h:151
static void NavCircleWaypoint(uint8_t wp_center, float radius)
Definition: navigation.h:335
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:181
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:316
float fp_altitude
altitude setpoint from flight plan (in meters)
Definition: navigation.h:138
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:315
bool too_far_from_home
too_far flag
Definition: navigation.h:143
float climb_vspeed
climb speed setting, mostly used in flight plans
Definition: navigation.h:145
float roll
roll angle (in radians)
Definition: navigation.h:131
void nav_set_failsafe(void)
Definition: navigation.c:378
General Navigation structure.
Definition: navigation.h:119
API to get/set the generic vehicle states.
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
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