Paparazzi UAS  v5.15_devel-112-g521f3cf
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 2.f
44 #endif
45 
46 #ifndef NAV_FREQ
47 #define NAV_FREQ 16
48 #endif
49 
50 
52 #ifndef DEFAULT_CIRCLE_RADIUS
53 #define DEFAULT_CIRCLE_RADIUS 6.0f
54 #endif
55 
57 #ifndef ARRIVED_AT_WAYPOINT
58 #define ARRIVED_AT_WAYPOINT 3.0f
59 #endif
60 
62 #ifndef FAILSAFE_MODE_DISTANCE
63 #define FAILSAFE_MODE_DISTANCE (1.2*MAX_DIST_FROM_HOME)
64 #endif
65 
67 #define NAV_MODE_WAYPOINT 0
68 #define NAV_MODE_ROUTE 1
69 #define NAV_MODE_CIRCLE 2
70 #define NAV_MODE_HEADING 3
71 #define NAV_MODE_MANUAL 4
72 
73 typedef void (*nav_rover_goto)(struct EnuCoor_f *wp);
74 typedef void (*nav_rover_route)(struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_end);
75 typedef bool (*nav_rover_approaching)(struct EnuCoor_f *wp_to, struct EnuCoor_f *wp_from, float approaching_time);
76 typedef void (*nav_rover_circle)(struct EnuCoor_f *wp_center, float radius);
77 typedef void (*nav_rover_oval_init)(void);
78 typedef void (*nav_rover_oval)(struct EnuCoor_f *wp1, struct EnuCoor_f *wp2, float radius);
79 
80 
84  // mode
85  uint8_t mode; // nav mode
86 
87  // commands
88  struct EnuCoor_f target;
89  struct EnuCoor_f carrot;
90  float heading;
91  float radius;
92  float speed;
93  float turn;
94  float shift;
95 
96  // misc
97  float dist2_to_home;
100  bool exception_flag[10];
102 
103  // pointers to basic nav functions
110 };
111 
112 extern struct RoverNavigation nav;
113 
121 // TODO: eight, survey
122 
123 
124 /*****************************************************************
125  * macros to ensure compatibility between fixedwing and rotorcraft
126  *****************************************************************/
127 
129 #define GetPosX() (stateGetPositionEnu_f()->x)
130 #define GetPosY() (stateGetPositionEnu_f()->y)
132 #define GetPosAlt() (stateGetPositionEnu_f()->z+state.ned_origin_f.hmsl)
134 
140 #define GetAltRef() (state.ned_origin_f.hmsl)
141 
142 
144 #define NormCourse(x) { \
145  while (x < 0) x += 360; \
146  while (x >= 360) x -= 360; \
147  }
148 
149 #define NormCourseRad(x) { \
150  while (x < 0) x += 2*M_PI; \
151  while (x >= 2*M_PI) x -= 2*M_PI; \
152  }
153 
154 extern uint8_t last_wp __attribute__((unused));
155 
156 extern void nav_init(void);
157 extern void nav_run(void);
158 
159 extern void set_exception_flag(uint8_t flag_num);
160 
161 extern float get_dist2_to_waypoint(uint8_t wp_id);
162 extern float get_dist2_to_point(struct EnuCoor_f *p);
163 extern void compute_dist2_to_home(void);
164 extern void nav_home(void);
165 extern void nav_set_manual(float speed, float turn);
166 
167 extern void nav_reset_reference(void) __attribute__((unused));
168 extern void nav_periodic_task(void);
169 
170 extern bool nav_is_in_flight(void);
171 
173 extern void nav_set_heading_rad(float rad);
174 extern void nav_set_heading_deg(float deg);
175 extern void nav_set_heading_towards(float x, float y);
177 extern void nav_set_heading_towards_target(void);
178 extern void nav_set_heading_current(void);
179 
180 extern void nav_set_failsafe(void);
181 
182 /* switching motors on/off */
183 static inline void NavKillThrottle(void)
184 {
186 }
187 static inline void NavResurrect(void)
188 {
190 }
191 
192 
193 #define NavSetManual(_roll, _pitch, _yaw) _Pragma("GCC error \"Manual mode in flight plan for fixedwing is not available\"")
194 #define NavSetFailsafe nav_set_failsafe
195 
196 #define NavSetGroundReferenceHere nav_reset_reference
197 #define NavSetAltitudeReferenceHere nav_reset_alt
198 
199 #define NavSetWaypointHere waypoint_set_here_2d
200 #define NavCopyWaypoint waypoint_copy
201 #define NavCopyWaypointPositionOnly waypoint_position_copy
202 
203 
205 bool nav_check_wp_time(struct EnuCoor_f *wp, float stay_time);
206 #define NavCheckWaypointTime(wp, time) nav_check_wp_time(&waypoints[wp].enu_f, time)
207 
208 
209 /***********************************************************
210  * macros used by flight plan to set different modes
211  **********************************************************/
212 
213 // command direct turn rate from FP roll [-1.; 1.]
214 #define NavAttitude(_roll) { \
215  nav.mode = NAV_MODE_MANUAL; \
216  nav.turn = _roll; \
217  BoundAbs(nav.turn, 1.f); \
218 }
219 
220 // command direct motor speed from FP throttle [-1.; 1.]
221 #define NavVerticalThrottleMode(_speed) { \
222  nav.speed = _speed; \
223  BoundAbs(nav.speed, 1.f); \
224 }
225 
227 #define NavHeading nav_set_heading_rad
228 
229 
230 
231 /***********************************************************
232  * built in navigation routines
233  **********************************************************/
234 
235 /*********** Navigation to waypoint *************************************/
236 static inline void NavGotoWaypoint(uint8_t wp)
237 {
239  VECT3_COPY(nav.target, waypoints[wp].enu_f);
240  //nav.dist2_to_wp = get_dist2_to_waypoint(wp); FIXME
241 }
242 
243 /*********** Navigation along a line *************************************/
244 static inline void NavSegment(uint8_t wp_start, uint8_t wp_end)
245 {
247  if (nav.nav_route) {
248  nav.nav_route(&waypoints[wp_start].enu_f, &waypoints[wp_end].enu_f);
249  }
250 }
251 
252 static inline bool NavApproaching(uint8_t wp, float approaching_time)
253 {
254  if (nav.nav_approaching) {
255  return nav.nav_approaching(&waypoints[wp].enu_f, NULL, approaching_time);
256  }
257  else {
258  return true;
259  }
260 }
261 
262 static inline bool NavApproachingFrom(uint8_t to, uint8_t from, float approaching_time)
263 {
264  if (nav.nav_approaching) {
265  return nav.nav_approaching(&waypoints[to].enu_f, &waypoints[from].enu_f, approaching_time);
266  }
267  else {
268  return true;
269  }
270 }
271 
272 /*********** Navigation on a circle **************************************/
273 static inline void NavCircleWaypoint(uint8_t wp_center, float radius)
274 {
276  if (nav.nav_circle) {
277  nav.nav_circle(&waypoints[wp_center].enu_f, radius);
278  }
279 }
280 
281 /*********** Navigation along an oval *************************************/
282 static inline void nav_oval_init(void)
283 {
284  if (nav.nav_oval_init) {
285  nav.nav_oval_init();
286  }
287 }
288 
289 static inline void Oval(uint8_t wp1, uint8_t wp2, float radius)
290 {
291  if (nav.nav_oval) {
292  nav.nav_oval(&waypoints[wp1].enu_f, &waypoints[wp2].enu_f, radius);
293  }
294 }
295 
296 
299 #define navigation_IncreaseShift(x) { if (x==0) nav.shift = 0; else nav.shift += x; }
300 
301 #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; }
302 
303 
304 /* follow another aircraft TODO */
305 #define NavFollow nav_follow(_id, _dist, _height) {}
306 
307 
311 #define NavGlide(_start_wp, _wp) {}
312 #define NavVerticalAutoThrottleMode(_pitch) {}
313 #define NavVerticalAutoPitchMode(_throttle) {}
314 #define NavVerticalAltitudeMode(_alt, _pre_climb) {}
315 #define NavVerticalClimbMode(_climb) {}
316 
317 
318 #endif /* NAVIGATION_H */
void nav_set_heading_rad(float rad)
Set nav_heading in radians.
Definition: navigation.c:484
static void NavResurrect(void)
Definition: navigation.h:187
float dist2_to_home
squared distance to home waypoint
Definition: navigation.h:97
uint8_t last_wp
Index of last waypoint.
Definition: nav.c:48
struct RoverNavigation nav
Definition: navigation.c:49
vector in East North Up coordinates Units: meters
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
void nav_reset_reference(void)
Reset the geographic reference to the current GPS fix.
Definition: navigation.c:348
struct EnuCoor_f target
final target
Definition: navigation.h:88
static bool NavApproaching(uint8_t wp, float approaching_time)
Definition: navigation.h:252
void(* nav_rover_circle)(struct EnuCoor_f *wp_center, float radius)
Definition: navigation.h:76
void nav_circle(struct EnuCoor_i *wp_center, int32_t radius)
Definition: navigation.c:538
void(* nav_rover_goto)(struct EnuCoor_f *wp)
Definition: navigation.h:73
void(* nav_rover_route)(struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_end)
Definition: navigation.h:74
void compute_dist2_to_home(void)
Computes squared distance to the HOME waypoint potentially sets too_far_from_home.
Definition: navigation.c:473
#define NAV_MODE_ROUTE
Definition: navigation.h:68
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:290
General Navigation structure.
Definition: navigation.h:83
float speed
speed setpoint
Definition: navigation.h:92
float failsafe_mode_dist2
maximum squared distance to home wp before going to failsafe mode
Definition: navigation.h:99
static void NavSegment(uint8_t wp_start, uint8_t wp_end)
Definition: navigation.h:244
#define FALSE
Definition: std.h:5
void nav_route(struct EnuCoor_i *wp_start, struct EnuCoor_i *wp_end)
Definition: navigation.c:584
void nav_periodic_task(void)
Navigation main: call to the code generated from the XML flight plan.
Definition: nav.c:443
Paparazzi floating point math for geodetic calculations.
bool nav_is_in_flight(void)
Definition: navigation.c:415
void nav_set_heading_current(void)
Set heading to the current yaw angle.
Definition: navigation.c:523
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:301
#define TRUE
Definition: std.h:4
bool(* nav_rover_approaching)(struct EnuCoor_f *wp_to, struct EnuCoor_f *wp_from, float approaching_time)
Definition: navigation.h:75
nav_rover_goto nav_goto
Definition: navigation.h:104
static bool NavApproachingFrom(uint8_t to, uint8_t from, float approaching_time)
Definition: navigation.h:262
float radius
radius setpoint
Definition: navigation.h:91
float get_dist2_to_point(struct EnuCoor_i *p)
Returns squared horizontal distance to given point.
Definition: navigation.c:455
static void NavKillThrottle(void)
Definition: navigation.h:183
static void NavGotoWaypoint(uint8_t wp)
Definition: navigation.h:236
#define NAV_MODE_WAYPOINT
Nav modes.
Definition: navigation.h:67
bool exception_flag[10]
array of flags that might be used in flight plans
Definition: navigation.h:100
uint8_t mode
Definition: navigation.h:85
void nav_oval(uint8_t, uint8_t, float)
Navigation along a figure O.
Definition: nav.c:733
float shift
lateral shift (in meters)
Definition: navigation.h:94
void nav_run(void)
Definition: navigation.c:243
void nav_set_heading_towards(float x, float y)
Set heading to point towards x,y position in local coordinates.
Definition: navigation.c:497
float heading
heading setpoint (in radians)
Definition: navigation.h:90
void set_exception_flag(uint8_t flag_num)
Definition: navigation.c:128
float turn
turn rate setpoint
Definition: navigation.h:93
Core autopilot interface common to all firmwares.
void nav_set_manual(int32_t roll, int32_t pitch, int32_t yaw)
Set manual roll, pitch and yaw without stabilization.
Definition: navigation.c:446
void nav_register_oval(nav_rover_oval_init nav_oval_init, nav_rover_oval nav_oval)
Definition: navigation.c:302
static void NavCircleWaypoint(uint8_t wp_center, float radius)
Definition: navigation.h:273
struct EnuCoor_f last_pos
last stage position
Definition: navigation.h:101
void(* nav_rover_oval_init)(void)
Definition: navigation.h:77
#define AP_MODE_NAV
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
nav_rover_oval nav_oval
Definition: navigation.h:109
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:38
void nav_home(void)
Home mode navigation (circle around HOME)
Definition: nav.c:422
nav_rover_circle nav_circle
Definition: navigation.h:107
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:200
Common flight_plan functions shared between fixedwing and rotorcraft.
void nav_register_circle(nav_rover_circle nav_circle)
Definition: navigation.c:297
nav_rover_approaching nav_approaching
Definition: navigation.h:106
void nav_oval_init(void)
Definition: nav.c:727
void nav_set_heading_deg(float deg)
Set nav_heading in degrees.
Definition: navigation.c:491
static float p[2][2]
bool too_far_from_home
too_far flag
Definition: navigation.h:98
void nav_init(void)
Navigation Initialisation.
Definition: nav.c:530
void nav_set_failsafe(void)
Definition: navigation.c:528
nav_rover_oval_init nav_oval_init
Definition: navigation.h:108
static void Oval(uint8_t wp1, uint8_t wp2, float radius)
Definition: navigation.h:289
struct EnuCoor_f carrot
carrot position
Definition: navigation.h:89
nav_rover_route nav_route
Definition: navigation.h:105
float get_dist2_to_waypoint(uint8_t wp_id)
Returns squared horizontal distance to given waypoint.
Definition: navigation.c:465
void nav_set_heading_towards_target(void)
Set heading in the direction of the target.
Definition: navigation.c:516
#define NAV_MODE_CIRCLE
Definition: navigation.h:69
void nav_set_heading_towards_waypoint(uint8_t wp)
Set heading in the direction of a waypoint.
Definition: navigation.c:510
uint8_t autopilot_get_mode(void)
get autopilot mode
Definition: autopilot.c:184
void(* nav_rover_oval)(struct EnuCoor_f *wp1, struct EnuCoor_f *wp2, float radius)
Definition: navigation.h:78