Paparazzi UAS  v6.0_unstable-92-g17422e4-dirty
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 #define GetPosHeight() (stateGetPositionEnu_f()->z)
136 
142 #define GetAltRef() (state.ned_origin_f.hmsl)
143 
144 
146 #define NormCourse(x) { \
147  while (x < 0) x += 360; \
148  while (x >= 360) x -= 360; \
149  }
150 
151 #define NormCourseRad(x) { \
152  while (x < 0) x += 2*M_PI; \
153  while (x >= 2*M_PI) x -= 2*M_PI; \
154  }
155 
156 extern uint8_t last_wp __attribute__((unused));
157 
158 extern void nav_init(void);
159 extern void nav_run(void);
160 
161 extern void set_exception_flag(uint8_t flag_num);
162 
163 extern float get_dist2_to_waypoint(uint8_t wp_id);
164 extern float get_dist2_to_point(struct EnuCoor_f *p);
165 extern void compute_dist2_to_home(void);
166 extern void nav_home(void);
167 extern void nav_set_manual(float speed, float turn);
168 
169 extern void nav_reset_reference(void) __attribute__((unused));
170 extern void nav_periodic_task(void);
171 
172 extern bool nav_is_in_flight(void);
173 
175 extern void nav_set_heading_rad(float rad);
176 extern void nav_set_heading_deg(float deg);
177 extern void nav_set_heading_towards(float x, float y);
179 extern void nav_set_heading_towards_target(void);
180 extern void nav_set_heading_current(void);
181 
182 extern void nav_set_failsafe(void);
183 
184 /* switching motors on/off */
185 static inline void NavKillThrottle(void)
186 {
188 }
189 static inline void NavResurrect(void)
190 {
192 }
193 
194 
195 #define NavSetManual(_roll, _pitch, _yaw) _Pragma("GCC error \"Manual mode in flight plan for fixedwing is not available\"")
196 #define NavSetFailsafe nav_set_failsafe
197 
198 #define NavSetGroundReferenceHere nav_reset_reference
199 #define NavSetAltitudeReferenceHere nav_reset_alt
200 
201 #define NavSetWaypointHere waypoint_set_here_2d
202 #define NavCopyWaypoint waypoint_copy
203 #define NavCopyWaypointPositionOnly waypoint_position_copy
204 
205 
207 bool nav_check_wp_time(struct EnuCoor_f *wp, float stay_time);
208 #define NavCheckWaypointTime(wp, time) nav_check_wp_time(&waypoints[wp].enu_f, time)
209 
210 
211 /***********************************************************
212  * macros used by flight plan to set different modes
213  **********************************************************/
214 
215 // command direct turn rate from FP roll [-1.; 1.]
216 #define NavAttitude(_roll) { \
217  nav.mode = NAV_MODE_MANUAL; \
218  nav.turn = _roll; \
219  BoundAbs(nav.turn, 1.f); \
220 }
221 
222 // command direct motor speed from FP throttle [-1.; 1.]
223 #define NavVerticalThrottleMode(_speed) { \
224  nav.speed = _speed; \
225  BoundAbs(nav.speed, 1.f); \
226 }
227 
229 #define NavHeading nav_set_heading_rad
230 
231 
232 
233 /***********************************************************
234  * built in navigation routines
235  **********************************************************/
236 
237 /*********** Navigation to waypoint *************************************/
238 static inline void NavGotoWaypoint(uint8_t wp)
239 {
241  VECT3_COPY(nav.target, waypoints[wp].enu_f);
242  //nav.dist2_to_wp = get_dist2_to_waypoint(wp); FIXME
243 }
244 
245 /*********** Navigation along a line *************************************/
246 static inline void NavSegment(uint8_t wp_start, uint8_t wp_end)
247 {
249  if (nav.nav_route) {
250  nav.nav_route(&waypoints[wp_start].enu_f, &waypoints[wp_end].enu_f);
251  }
252 }
253 
254 static inline bool NavApproaching(uint8_t wp, float approaching_time)
255 {
256  if (nav.nav_approaching) {
257  return nav.nav_approaching(&waypoints[wp].enu_f, NULL, approaching_time);
258  }
259  else {
260  return true;
261  }
262 }
263 
264 static inline bool NavApproachingFrom(uint8_t to, uint8_t from, float approaching_time)
265 {
266  if (nav.nav_approaching) {
267  return nav.nav_approaching(&waypoints[to].enu_f, &waypoints[from].enu_f, approaching_time);
268  }
269  else {
270  return true;
271  }
272 }
273 
274 /*********** Navigation on a circle **************************************/
275 static inline void NavCircleWaypoint(uint8_t wp_center, float radius)
276 {
278  if (nav.nav_circle) {
279  nav.nav_circle(&waypoints[wp_center].enu_f, radius);
280  }
281 }
282 
283 /*********** Navigation along an oval *************************************/
284 static inline void nav_oval_init(void)
285 {
286  if (nav.nav_oval_init) {
287  nav.nav_oval_init();
288  }
289 }
290 
291 static inline void Oval(uint8_t wp1, uint8_t wp2, float radius)
292 {
293  if (nav.nav_oval) {
294  nav.nav_oval(&waypoints[wp1].enu_f, &waypoints[wp2].enu_f, radius);
295  }
296 }
297 
298 
301 #define navigation_IncreaseShift(x) { if (x==0) nav.shift = 0; else nav.shift += x; }
302 
303 #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; }
304 
305 
306 /* follow another aircraft TODO */
307 #define NavFollow nav_follow(_id, _dist, _height) {}
308 
309 
313 #define NavGlide(_start_wp, _wp) {}
314 #define NavVerticalAutoThrottleMode(_pitch) {}
315 #define NavVerticalAutoPitchMode(_throttle) {}
316 #define NavVerticalAltitudeMode(_alt, _pre_climb) {}
317 #define NavVerticalClimbMode(_climb) {}
318 
319 
320 #endif /* NAVIGATION_H */
void nav_set_heading_rad(float rad)
Set nav_heading in radians.
Definition: navigation.c:489
static void NavResurrect(void)
Definition: navigation.h:189
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:353
struct EnuCoor_f target
final target
Definition: navigation.h:88
static bool NavApproaching(uint8_t wp, float approaching_time)
Definition: navigation.h:254
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:543
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:478
#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:246
#define FALSE
Definition: std.h:5
void nav_route(struct EnuCoor_i *wp_start, struct EnuCoor_i *wp_end)
Definition: navigation.c:589
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:420
void nav_set_heading_current(void)
Set heading to the current yaw angle.
Definition: navigation.c:528
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:306
#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:264
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:460
static void NavKillThrottle(void)
Definition: navigation.h:185
static void NavGotoWaypoint(uint8_t wp)
Definition: navigation.h:238
#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:248
void nav_set_heading_towards(float x, float y)
Set heading to point towards x,y position in local coordinates.
Definition: navigation.c:502
float heading
heading setpoint (in radians)
Definition: navigation.h:90
void set_exception_flag(uint8_t flag_num)
Definition: navigation.c:130
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:451
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:275
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
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:496
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:533
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:291
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:470
void nav_set_heading_towards_target(void)
Set heading in the direction of the target.
Definition: navigation.c:521
#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:515
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
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98