Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
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"
33 #include "modules/nav/waypoints.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 2.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 
78 typedef void (*nav_rover_goto)(struct EnuCoor_f *wp);
79 typedef void (*nav_rover_route)(struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_end);
80 typedef bool (*nav_rover_approaching)(struct EnuCoor_f *wp_to, struct EnuCoor_f *wp_from, float approaching_time);
81 typedef void (*nav_rover_circle)(struct EnuCoor_f *wp_center, float radius);
82 typedef void (*nav_rover_oval_init)(void);
83 typedef void (*nav_rover_oval)(struct EnuCoor_f *wp1, struct EnuCoor_f *wp2, float radius);
84 
85 
89  // mode
90  uint8_t mode; // nav mode
91 
92  // commands
93  struct EnuCoor_f target;
94  struct EnuCoor_f carrot;
95  float heading;
96  float radius;
97  float speed;
98  float turn;
99  float shift;
100 
101  // misc
105  bool exception_flag[10];
107 
108  // pointers to basic nav functions
115 };
116 
117 extern struct RoverNavigation nav;
118 
126 // TODO: eight, survey
127 
128 
129 /*****************************************************************
130  * macros to ensure compatibility between fixedwing and rotorcraft
131  *****************************************************************/
132 
134 #define GetPosX() (stateGetPositionEnu_f()->x)
135 #define GetPosY() (stateGetPositionEnu_f()->y)
137 #define GetPosAlt() (stateGetPositionEnu_f()->z+state.ned_origin_f.hmsl)
139 #define GetPosHeight() (stateGetPositionEnu_f()->z)
141 
147 #define GetAltRef() (state.ned_origin_f.hmsl)
148 
149 
151 #define NormCourse(x) { \
152  while (x < 0) x += 360; \
153  while (x >= 360) x -= 360; \
154  }
155 
156 #define NormCourseRad(x) { \
157  while (x < 0) x += 2*M_PI; \
158  while (x >= 2*M_PI) x -= 2*M_PI; \
159  }
160 
161 extern uint8_t last_wp __attribute__((unused));
162 
163 extern void nav_init(void);
164 extern void nav_run(void);
165 extern void nav_parse_BLOCK(uint8_t *buf);
166 extern void nav_parse_MOVE_WP(uint8_t *buf);
167 
168 extern void set_exception_flag(uint8_t flag_num);
169 
170 extern float get_dist2_to_waypoint(uint8_t wp_id);
171 extern float get_dist2_to_point(struct EnuCoor_f *p);
172 extern void compute_dist2_to_home(void);
173 extern void nav_home(void);
174 extern void nav_set_manual(float speed, float turn);
175 
176 extern void nav_reset_reference(void) __attribute__((unused));
177 extern void nav_reset_alt(void) __attribute__((unused));
178 extern void nav_periodic_task(void);
179 
180 extern bool nav_is_in_flight(void);
181 
183 extern void nav_set_heading_rad(float rad);
184 extern void nav_set_heading_deg(float deg);
185 extern void nav_set_heading_towards(float x, float y);
187 extern void nav_set_heading_towards_target(void);
188 extern void nav_set_heading_current(void);
189 
190 extern void nav_set_failsafe(void);
191 
192 /* switching motors on/off */
193 static inline void NavKillThrottle(void)
194 {
196 }
197 static inline void NavResurrect(void)
198 {
200 }
201 
202 
203 #define NavSetManual(_roll, _pitch, _yaw) _Pragma("GCC error \"Manual mode in flight plan for fixedwing is not available\"")
204 #define NavSetFailsafe nav_set_failsafe
205 
206 #define NavSetGroundReferenceHere nav_reset_reference
207 #define NavSetAltitudeReferenceHere nav_reset_alt
208 
209 #define NavSetWaypointHere waypoint_set_here_2d
210 #define NavCopyWaypoint waypoint_copy
211 #define NavCopyWaypointPositionOnly waypoint_position_copy
212 
213 
215 bool nav_check_wp_time(struct EnuCoor_f *wp, float stay_time);
216 #define NavCheckWaypointTime(wp, time) nav_check_wp_time(&waypoints[wp].enu_f, time)
217 
218 
219 /***********************************************************
220  * macros used by flight plan to set different modes
221  **********************************************************/
222 
223 // command direct turn rate from FP roll [-1.; 1.]
224 #define NavAttitude(_roll) { \
225  nav.mode = NAV_MODE_MANUAL; \
226  nav.turn = _roll; \
227  BoundAbs(nav.turn, 1.f); \
228 }
229 
230 // command direct motor speed from FP throttle [-1.; 1.]
231 #define NavVerticalThrottleMode(_speed) { \
232  nav.speed = _speed; \
233  BoundAbs(nav.speed, 1.f); \
234 }
235 
237 #define NavHeading nav_set_heading_rad
238 
239 
240 
241 /***********************************************************
242  * built in navigation routines
243  **********************************************************/
244 
245 /*********** Navigation to waypoint *************************************/
246 static inline void NavGotoWaypoint(uint8_t wp)
247 {
249  VECT3_COPY(nav.target, waypoints[wp].enu_f);
250  //nav.dist2_to_wp = get_dist2_to_waypoint(wp); FIXME
251 }
252 
253 /*********** Navigation along a line *************************************/
254 static inline void NavSegment(uint8_t wp_start, uint8_t wp_end)
255 {
257  if (nav.nav_route) {
258  nav.nav_route(&waypoints[wp_start].enu_f, &waypoints[wp_end].enu_f);
259  }
260 }
261 
262 static inline bool NavApproaching(uint8_t wp, float approaching_time)
263 {
264  if (nav.nav_approaching) {
265  return nav.nav_approaching(&waypoints[wp].enu_f, NULL, approaching_time);
266  }
267  else {
268  return true;
269  }
270 }
271 
272 static inline bool NavApproachingFrom(uint8_t to, uint8_t from, float approaching_time)
273 {
274  if (nav.nav_approaching) {
275  return nav.nav_approaching(&waypoints[to].enu_f, &waypoints[from].enu_f, approaching_time);
276  }
277  else {
278  return true;
279  }
280 }
281 
282 /*********** Navigation on a circle **************************************/
283 static inline void NavCircleWaypoint(uint8_t wp_center, float radius)
284 {
286  if (nav.nav_circle) {
287  nav.nav_circle(&waypoints[wp_center].enu_f, radius);
288  }
289 }
290 
291 /*********** Navigation along an oval *************************************/
292 static inline void nav_oval_init(void)
293 {
294  if (nav.nav_oval_init) {
295  nav.nav_oval_init();
296  }
297 }
298 
299 static inline void Oval(uint8_t wp1, uint8_t wp2, float radius)
300 {
301  if (nav.nav_oval) {
302  nav.nav_oval(&waypoints[wp1].enu_f, &waypoints[wp2].enu_f, radius);
303  }
304 }
305 
306 
309 #define navigation_IncreaseShift(x) { if (x==0) nav.shift = 0; else nav.shift += x; }
310 
311 #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; }
312 
313 
314 /* follow another aircraft TODO */
315 #define NavFollow nav_follow(_id, _dist, _height) {}
316 
317 
321 #define NavGlide(_start_wp, _wp) {}
322 #define NavVerticalAutoThrottleMode(_pitch) {}
323 #define NavVerticalAutoPitchMode(_throttle) {}
324 #define NavVerticalAltitudeMode(_alt, _pre_climb) {}
325 #define NavVerticalClimbMode(_climb) {}
326 
327 
328 #endif /* NAVIGATION_H */
NavSegment
static void NavSegment(uint8_t wp_start, uint8_t wp_end)
Definition: navigation.h:254
nav_rover_oval_init
void(* nav_rover_oval_init)(void)
Definition: navigation.h:82
nav_set_heading_towards_waypoint
void nav_set_heading_towards_waypoint(uint8_t wp)
Set heading in the direction of a waypoint.
Definition: navigation.c:520
NavGotoWaypoint
static void NavGotoWaypoint(uint8_t wp)
Definition: navigation.h:246
nav_route
void nav_route(struct EnuCoor_i *wp_start, struct EnuCoor_i *wp_end)
Definition: navigation.c:594
RoverNavigation::dist2_to_home
float dist2_to_home
squared distance to home waypoint
Definition: navigation.h:102
common_flight_plan.h
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
Oval
static void Oval(uint8_t wp1, uint8_t wp2, float radius)
Definition: navigation.h:299
RoverNavigation::too_far_from_home
bool too_far_from_home
too_far flag
Definition: navigation.h:103
RoverNavigation::nav_goto
nav_rover_goto nav_goto
Definition: navigation.h:109
nav_home
void nav_home(void)
Home mode navigation (circle around HOME)
Definition: nav.c:425
set_exception_flag
void set_exception_flag(uint8_t flag_num)
Definition: navigation.c:135
nav_parse_BLOCK
void nav_parse_BLOCK(uint8_t *buf)
Definition: navigation.c:231
RoverNavigation::carrot
struct EnuCoor_f carrot
carrot position
Definition: navigation.h:94
NavResurrect
static void NavResurrect(void)
Definition: navigation.h:197
nav_rover_goto
void(* nav_rover_goto)(struct EnuCoor_f *wp)
Definition: navigation.h:78
RoverNavigation::target
struct EnuCoor_f target
final target
Definition: navigation.h:93
NAV_MODE_WAYPOINT
#define NAV_MODE_WAYPOINT
Nav modes.
Definition: navigation.h:72
waypoints
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:39
nav_init
void nav_init(void)
Navigation Initialisation.
Definition: nav.c:533
NavApproaching
static bool NavApproaching(uint8_t wp, float approaching_time)
Definition: navigation.h:262
nav_reset_reference
void nav_reset_reference(void)
Reset the geographic reference to the current GPS fix.
Definition: navigation.c:382
std.h
pprz_geodetic_float.h
Paparazzi floating point math for geodetic calculations.
nav
struct RoverNavigation nav
Definition: navigation.c:50
AP_MODE_NAV
#define AP_MODE_NAV
Definition: autopilot_static.h:48
nav_set_heading_rad
void nav_set_heading_rad(float rad)
Set nav_heading in radians.
Definition: navigation.c:494
nav_register_goto_wp
void nav_register_goto_wp(nav_rover_goto nav_goto, nav_rover_route nav_route, nav_rover_approaching nav_approaching)
Registering functions.
Definition: navigation.c:314
nav_rover_route
void(* nav_rover_route)(struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_end)
Definition: navigation.h:79
nav_rover_oval
void(* nav_rover_oval)(struct EnuCoor_f *wp1, struct EnuCoor_f *wp2, float radius)
Definition: navigation.h:83
RoverNavigation::mode
uint8_t mode
Definition: navigation.h:90
get_dist2_to_point
float get_dist2_to_point(struct EnuCoor_i *p)
Returns squared horizontal distance to given point.
Definition: navigation.c:465
RoverNavigation::nav_route
nav_rover_route nav_route
Definition: navigation.h:110
last_wp
uint8_t last_wp
Index of last waypoint.
Definition: nav.c:50
nav_set_heading_current
void nav_set_heading_current(void)
Set heading to the current yaw angle.
Definition: navigation.c:533
nav_rover_circle
void(* nav_rover_circle)(struct EnuCoor_f *wp_center, float radius)
Definition: navigation.h:81
NavApproachingFrom
static bool NavApproachingFrom(uint8_t to, uint8_t from, float approaching_time)
Definition: navigation.h:272
nav_set_manual
void nav_set_manual(int32_t roll, int32_t pitch, int32_t yaw)
Set manual roll, pitch and yaw without stabilization.
Definition: navigation.c:456
NAV_MODE_ROUTE
#define NAV_MODE_ROUTE
Definition: navigation.h:73
NAV_MODE_CIRCLE
#define NAV_MODE_CIRCLE
Definition: navigation.h:74
EnuCoor_f
vector in East North Up coordinates Units: meters
Definition: pprz_geodetic_float.h:72
RoverNavigation::nav_circle
nav_rover_circle nav_circle
Definition: navigation.h:112
nav_run
void nav_run(void)
Definition: navigation.c:277
autopilot.h
get_dist2_to_waypoint
float get_dist2_to_waypoint(uint8_t wp_id)
Returns squared horizontal distance to given waypoint.
Definition: navigation.c:475
RoverNavigation
General Navigation structure.
Definition: navigation.h:88
RoverNavigation::nav_oval
nav_rover_oval nav_oval
Definition: navigation.h:114
nav_set_heading_towards_target
void nav_set_heading_towards_target(void)
Set heading in the direction of the target.
Definition: navigation.c:526
autopilot_set_motors_on
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
nav_reset_alt
void nav_reset_alt(void)
Reset the altitude reference to the current GPS alt.
Definition: navigation.c:389
nav_check_wp_time
bool nav_check_wp_time(struct EnuCoor_i *wp, uint16_t stay_time)
Check the time spent in a radius of 'ARRIVED_AT_WAYPOINT' around a wp
Definition: navigation.c:335
nav_circle
void nav_circle(struct EnuCoor_i *wp_center, int32_t radius)
Definition: navigation.c:548
nav_oval
void nav_oval(uint8_t, uint8_t, float)
Navigation along a figure O.
Definition: nav.c:764
NavKillThrottle
static void NavKillThrottle(void)
Definition: navigation.h:193
nav_is_in_flight
bool nav_is_in_flight(void)
Definition: navigation.c:425
nav_periodic_task
void nav_periodic_task(void)
Navigation main: call to the code generated from the XML flight plan.
Definition: nav.c:446
nav_rover_approaching
bool(* nav_rover_approaching)(struct EnuCoor_f *wp_to, struct EnuCoor_f *wp_from, float approaching_time)
Definition: navigation.h:80
RoverNavigation::speed
float speed
speed setpoint
Definition: navigation.h:97
nav_goto
static void nav_goto(struct EnuCoor_f *wp)
Implement basic nav function.
Definition: nav_rover_base.c:34
compute_dist2_to_home
void compute_dist2_to_home(void)
Computes squared distance to the HOME waypoint potentially sets too_far_from_home.
Definition: navigation.c:483
nav_set_heading_deg
void nav_set_heading_deg(float deg)
Set nav_heading in degrees.
Definition: navigation.c:501
nav_parse_MOVE_WP
void nav_parse_MOVE_WP(uint8_t *buf)
Definition: navigation.c:237
nav_register_circle
void nav_register_circle(nav_rover_circle nav_circle)
Definition: navigation.c:321
RoverNavigation::exception_flag
bool exception_flag[10]
array of flags that might be used in flight plans
Definition: navigation.h:105
autopilot_get_mode
uint8_t autopilot_get_mode(void)
get autopilot mode
Definition: autopilot.c:216
state.h
FALSE
#define FALSE
Definition: std.h:5
RoverNavigation::shift
float shift
lateral shift (in meters)
Definition: navigation.h:99
RoverNavigation::last_pos
struct EnuCoor_f last_pos
last stage position
Definition: navigation.h:106
waypoints.h
TRUE
#define TRUE
Definition: std.h:4
nav_register_oval
void nav_register_oval(nav_rover_oval_init nav_oval_init, nav_rover_oval nav_oval)
Definition: navigation.c:326
nav_set_heading_towards
void nav_set_heading_towards(float x, float y)
Set heading to point towards x,y position in local coordinates.
Definition: navigation.c:507
nav_oval_init
void nav_oval_init(void)
Definition: nav.c:758
RoverNavigation::heading
float heading
heading setpoint (in radians)
Definition: navigation.h:95
nav_set_failsafe
void nav_set_failsafe(void)
Definition: navigation.c:538
RoverNavigation::nav_oval_init
nav_rover_oval_init nav_oval_init
Definition: navigation.h:113
RoverNavigation::radius
float radius
radius setpoint
Definition: navigation.h:96
p
static float p[2][2]
Definition: ins_alt_float.c:268
VECT3_COPY
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
RoverNavigation::nav_approaching
nav_rover_approaching nav_approaching
Definition: navigation.h:111
NavCircleWaypoint
static void NavCircleWaypoint(uint8_t wp_center, float radius)
Definition: navigation.h:283
RoverNavigation::failsafe_mode_dist2
float failsafe_mode_dist2
maximum squared distance to home wp before going to failsafe mode
Definition: navigation.h:104
nav_approaching
static bool nav_approaching(struct EnuCoor_f *wp, struct EnuCoor_f *from, float approaching_time)
Definition: nav_rover_base.c:63
RoverNavigation::turn
float turn
turn rate setpoint
Definition: navigation.h:98