Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
navigation.h
Go to the documentation of this file.
1/*
2 * Copyright (C) 2018 Gautier Hattenberger <gautier.hattenberger@enac.fr>
3 *
4 * This file is part of paparazzi.
5 *
6 * paparazzi is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2, or (at your option)
9 * any later version.
10 *
11 * paparazzi is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 * GNU General Public License for more details.
15 *
16 * You should have received a copy of the GNU General Public License
17 * along with paparazzi; see the file COPYING. If not, see
18 * <http://www.gnu.org/licenses/>.
19 */
20
27#ifndef NAVIGATION_H
28#define NAVIGATION_H
29
30#include "std.h"
32#include "state.h"
35#include "autopilot.h"
36
// FIXME
38#ifndef CARROT
39#define CARROT 0.f
40#endif
41
42#ifndef CARROT_DIST
43#define CARROT_DIST 5.f
44#endif
45
47#ifndef NAVIGATION_FREQUENCY
48#if PERIODIC_FREQUENCY == 512
49#define NAVIGATION_FREQUENCY 16
50#else // if not 512, assume a multiple of 20 (e.g. 200, 500, 1000, ...)
51#define NAVIGATION_FREQUENCY 20
52#endif
53#endif
54
55
57#ifndef DEFAULT_CIRCLE_RADIUS
58#define DEFAULT_CIRCLE_RADIUS 6.0f
59#endif
60
62#ifndef ARRIVED_AT_WAYPOINT
63#define ARRIVED_AT_WAYPOINT 3.0f
64#endif
65
67#ifndef FAILSAFE_MODE_DISTANCE
68#define FAILSAFE_MODE_DISTANCE (1.2*MAX_DIST_FROM_HOME)
69#endif
70
72#define NAV_MODE_WAYPOINT 0
73#define NAV_MODE_ROUTE 1
74#define NAV_MODE_CIRCLE 2
75#define NAV_MODE_HEADING 3
76#define NAV_MODE_MANUAL 4
77
79typedef void (*nav_rover_goto)(struct EnuCoor_f *wp);
82typedef void (*nav_rover_circle)(struct EnuCoor_f *wp_center, float radius);
84typedef void (*nav_rover_oval)(struct EnuCoor_f *wp1, struct EnuCoor_f *wp2, float radius);
85
86
118
119extern struct RoverNavigation nav;
120
129// TODO: eight, survey
130
131
132/*****************************************************************
133 * macros to ensure compatibility between fixedwing and rotorcraft
134 *****************************************************************/
135
137#define GetPosX() (stateGetPositionEnu_f()->x)
139#define GetPosY() (stateGetPositionEnu_f()->y)
141#define GetPosAlt() (stateGetPositionEnu_f()->z+stateGetHmslOrigin_f())
143#define GetPosHeight() (stateGetPositionEnu_f()->z)
150#define GetAltRef() (stateGetHmslOrigin_f())
151
152
154#define NormCourse(x) { \
155 while (x < 0) x += 360; \
156 while (x >= 360) x -= 360; \
157 }
159#define NormCourseRad(x) { \
160 while (x < 0) x += 2*M_PI; \
161 while (x >= 2*M_PI) x -= 2*M_PI; \
162 }
163
164extern void nav_init(void);
165extern void nav_run(void);
166extern void nav_parse_BLOCK(uint8_t *buf);
167extern void nav_parse_MOVE_WP(uint8_t *buf);
168
170
171extern float get_dist2_to_waypoint(uint8_t wp_id);
172extern float get_dist2_to_point(struct EnuCoor_f *p);
173extern void compute_dist2_to_home(void);
174extern void nav_home(void);
175extern void nav_set_manual(float speed, float turn);
176
177extern void nav_reset_reference(void) __attribute__((unused));
178extern void nav_reset_alt(void) __attribute__((unused));
179extern void nav_periodic_task(void);
180
181extern bool nav_is_in_flight(void);
182
184extern void nav_set_heading_rad(float rad);
185extern void nav_set_heading_deg(float deg);
186extern void nav_set_heading_towards(float x, float y);
188extern void nav_set_heading_towards_target(void);
189extern void nav_set_heading_current(void);
190
191extern void nav_set_failsafe(void);
192
193/* switching motors on/off */
194static inline void NavKillThrottle(void)
195{
197}
198static inline void NavResurrect(void)
199{
201}
202
203#define NavSetFailsafe nav_set_failsafe
204
205#define NavSetGroundReferenceHere nav_reset_reference
206#define NavSetAltitudeReferenceHere nav_reset_alt
207
208#define NavSetWaypointHere waypoint_set_here_2d
209#define NavCopyWaypoint waypoint_copy
210#define NavCopyWaypointPositionOnly waypoint_position_copy
211
212
214bool nav_check_wp_time(struct EnuCoor_f *wp, float stay_time);
215#define NavCheckWaypointTime(wp, time) nav_check_wp_time(&waypoints[wp].enu_f, time)
216
217
218/***********************************************************
219 * macros used by flight plan to set different modes
220 **********************************************************/
221
222// command direct turn rate from FP roll [-1.; 1.]
223#define NavAttitude(_roll) { \
224 nav.mode = NAV_MODE_MANUAL; \
225 nav.turn = _roll; \
226 BoundAbs(nav.turn, 1.f); \
227}
228
229// command direct motor speed from FP throttle [-1.; 1.]
230#define NavVerticalThrottleMode(_speed) { \
231 nav.speed = _speed; \
232 BoundAbs(nav.speed, 1.f); \
233}
234
236#define NavHeading nav_set_heading_rad
237
238#define NavSetMaxSpeed(_speed) {} // not used
239
240
241/***********************************************************
242 * built in navigation routines
243 **********************************************************/
244
245/*********** Navigation to waypoint *************************************/
246static inline void NavGotoWaypoint(uint8_t wp)
247{
248 nav.mode = NAV_MODE_WAYPOINT;
249 if (nav.nav_goto) {
250 nav.nav_goto(&waypoints[wp].enu_f);
251 }
252}
253
254/*********** Navigation along a line *************************************/
256{
257 nav.mode = NAV_MODE_ROUTE;
258 if (nav.nav_route) {
260 }
261}
262
263static inline bool NavApproaching(uint8_t wp, float approaching_time)
264{
265 if (nav.nav_approaching) {
267 }
268 else {
269 return true;
270 }
271}
272
273static inline bool NavApproachingFrom(uint8_t to, uint8_t from, float approaching_time)
274{
275 if (nav.nav_approaching) {
276 return nav.nav_approaching(&waypoints[to].enu_f, &waypoints[from].enu_f, approaching_time);
277 }
278 else {
279 return true;
280 }
281}
282
283/*********** Navigation on a circle **************************************/
284static inline void NavCircleWaypoint(uint8_t wp_center, float radius)
285{
286 nav.mode = NAV_MODE_CIRCLE;
287 if (nav.nav_circle) {
288 nav.nav_circle(&waypoints[wp_center].enu_f, radius);
289 }
290}
291
292/*********** Navigation along an oval *************************************/
293static inline void nav_oval_init(void)
294{
295 if (nav.nav_oval_init) {
297 }
298}
299
300static inline void Oval(uint8_t wp1, uint8_t wp2, float radius)
301{
302 if (nav.nav_oval) {
303 nav.nav_oval(&waypoints[wp1].enu_f, &waypoints[wp2].enu_f, radius);
304 }
305}
306
307
310#define navigation_IncreaseShift(x) { if (x==0) nav.shift = 0; else nav.shift += x; }
311
312#define navigation_SetNavRadius(x) { if (x==1) nav.radius = DEFAULT_CIRCLE_RADIUS; else if (x==-1) nav.radius = -DEFAULT_CIRCLE_RADIUS; else nav.radius = x; }
313
314
315/* follow another aircraft TODO */
316#define NavFollow nav_follow(_id, _dist, _height) {}
317
318
322#define NavGlide(_start_wp, _wp) {}
323#define NavVerticalAutoThrottleMode(_pitch) {}
324#define NavVerticalAutoPitchMode(_throttle) {}
325#define NavVerticalAltitudeMode(_alt, _pre_climb) {}
326#define NavVerticalClimbMode(_climb) {}
327
328
329#endif /* NAVIGATION_H */
uint8_t autopilot_get_mode(void)
get autopilot mode
Definition autopilot.c:222
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:250
Core autopilot interface common to all firmwares.
Common flight_plan functions shared between fixedwing and rotorcraft.
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition common_nav.c:44
static float p[2][2]
uint16_t foo
Definition main_demo5.c:58
void nav_oval(uint8_t p1, uint8_t p2, float radius)
Definition nav.c:762
#define NavApproaching(wp, time)
Definition nav.h:171
#define NavCircleWaypoint(wp, radius)
Definition nav.h:151
#define Oval(a, b, c)
Definition nav.h:114
#define NavGotoWaypoint(_wp)
Definition nav.h:96
#define NavApproachingFrom(wp, from, time)
Definition nav.h:172
#define NavSegment(_start, _end)
Definition nav.h:167
static void nav_goto(struct EnuCoor_f *wp)
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
#define AP_MODE_NAV
void nav_home(void)
Home mode navigation (circle around HOME)
Definition nav.c:424
void nav_reset_reference(void)
Reset the geographic reference to the current GPS fix.
Definition navigation.c:234
void nav_register_oval(navigation_oval_init _nav_oval_init, navigation_oval nav_oval)
Definition navigation.c:404
void nav_set_heading_current(void)
Set heading to the current yaw angle.
Definition navigation.c:375
void nav_reset_alt(void)
Reset the altitude reference to the current GPS alt.
Definition navigation.c:241
void nav_set_heading_towards_waypoint(uint8_t wp)
Set heading in the direction of a waypoint.
Definition navigation.c:363
void compute_dist2_to_home(void)
Computes squared distance to the HOME waypoint potentially sets too_far_from_home.
Definition navigation.c:326
bool nav_is_in_flight(void)
Definition navigation.c:277
void nav_run(void)
Definition navigation.c:165
void nav_set_heading_towards(float x, float y)
Set heading to point towards x,y position in local coordinates.
Definition navigation.c:350
struct RotorcraftNavigation nav
Definition navigation.c:51
void nav_init(void)
Navigation Initialisation.
Definition nav.c:532
void nav_parse_MOVE_WP(uint8_t *buf)
Definition navigation.c:121
void nav_register_circle(navigation_circle nav_circle)
Definition navigation.c:399
void nav_register_goto_wp(navigation_goto nav_goto, navigation_route nav_route, navigation_approaching nav_approaching)
Definition navigation.c:392
navigation_oval nav_oval
Definition navigation.h:156
navigation_approaching nav_approaching
Definition navigation.h:153
void nav_parse_BLOCK(uint8_t *buf)
Definition navigation.c:115
navigation_circle nav_circle
Definition navigation.h:154
void nav_set_heading_towards_target(void)
Set heading in the direction of the target.
Definition navigation.c:369
static void nav_oval_init(void)
Definition navigation.h:350
float get_dist2_to_point(struct EnuCoor_f *p)
Returns squared horizontal distance to given point.
Definition navigation.c:308
navigation_oval_init nav_oval_init
Definition navigation.h:155
void nav_register_stage_init(navigation_stage_init nav_stage_init)
Registering functions.
Definition navigation.c:387
static void NavKillThrottle(void)
Definition navigation.h:229
navigation_goto nav_goto
Definition navigation.h:151
void nav_set_heading_rad(float rad)
heading utility functions
Definition navigation.c:337
static void NavResurrect(void)
Definition navigation.h:233
void nav_set_heading_deg(float deg)
Set nav_heading in degrees.
Definition navigation.c:344
navigation_route nav_route
Definition navigation.h:152
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:180
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:318
void nav_set_failsafe(void)
Definition navigation.c:380
float failsafe_mode_dist2
maximum squared distance to home wp before going to failsafe mode
Definition navigation.h:105
float turn
turn rate setpoint
Definition navigation.h:99
void(* nav_rover_oval_init)(void)
Definition navigation.h:83
struct EnuCoor_f carrot
carrot position
Definition navigation.h:95
#define NAV_MODE_CIRCLE
Definition navigation.h:74
float shift
lateral shift (in meters)
Definition navigation.h:100
#define NAV_MODE_ROUTE
Definition navigation.h:73
float speed
speed setpoint
Definition navigation.h:98
float dist2_to_home
squared distance to home waypoint
Definition navigation.h:103
nav_rover_route nav_route
Definition navigation.h:112
struct EnuCoor_f target
final target
Definition navigation.h:94
bool too_far_from_home
too_far flag
Definition navigation.h:104
void(* nav_rover_goto)(struct EnuCoor_f *wp)
Definition navigation.h:79
void(* nav_rover_circle)(struct EnuCoor_f *wp_center, float radius)
Definition navigation.h:82
void nav_set_manual(float speed, float turn)
struct EnuCoor_f last_pos
last stage position
Definition navigation.h:107
nav_rover_circle nav_circle
Definition navigation.h:114
nav_rover_oval nav_oval
Definition navigation.h:116
nav_rover_goto nav_goto
Definition navigation.h:111
void(* nav_rover_stage_init)(void)
Definition navigation.h:78
float heading
heading setpoint (in radians)
Definition navigation.h:96
#define NAV_MODE_WAYPOINT
Nav modes.
Definition navigation.h:72
nav_rover_oval_init nav_oval_init
Definition navigation.h:115
void(* nav_rover_route)(struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_end)
Definition navigation.h:80
void(* nav_rover_oval)(struct EnuCoor_f *wp1, struct EnuCoor_f *wp2, float radius)
Definition navigation.h:84
nav_rover_approaching nav_approaching
Definition navigation.h:113
bool(* nav_rover_approaching)(struct EnuCoor_f *wp_to, struct EnuCoor_f *wp_from, float approaching_time)
Definition navigation.h:81
nav_rover_stage_init nav_stage_init
Definition navigation.h:110
bool exception_flag[10]
array of flags that might be used in flight plans
Definition navigation.h:106
void set_exception_flag(uint8_t flag_num)
Definition navigation.c:57
float radius
radius setpoint
Definition navigation.h:97
General Navigation structure.
Definition navigation.h:89
API to get/set the generic vehicle states.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.