Paparazzi UAS  v5.15_devel-81-gd13dafb
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) 2008-2011 The Paparazzi Team
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, write to
18 * the Free Software Foundation, 59 Temple Place - Suite 330,
19 * Boston, MA 02111-1307, USA.
20 */
21 
28 #ifndef NAVIGATION_H
29 #define NAVIGATION_H
30 
31 #include "std.h"
32 #include "math/pprz_geodetic_int.h"
33 
36 #include "autopilot.h"
37 
39 #ifndef CARROT
40 #define CARROT 0
41 #endif
42 
43 #define NAV_FREQ 16
44 
45 extern struct EnuCoor_i navigation_target;
46 extern struct EnuCoor_i navigation_carrot;
47 
48 extern uint8_t last_wp __attribute__((unused));
49 
51 
53 #define HORIZONTAL_MODE_WAYPOINT 0
54 #define HORIZONTAL_MODE_ROUTE 1
55 #define HORIZONTAL_MODE_CIRCLE 2
56 #define HORIZONTAL_MODE_ATTITUDE 3
57 #define HORIZONTAL_MODE_MANUAL 4
58 extern int32_t nav_roll, nav_pitch;
59 extern int32_t nav_heading;
61 extern float nav_radius;
63 
66 
67 extern bool nav_survey_active;
68 
69 extern uint8_t vertical_mode;
70 extern uint32_t nav_throttle;
72 extern float flight_altitude;
73 #define VERTICAL_MODE_MANUAL 0
74 #define VERTICAL_MODE_CLIMB 1
75 #define VERTICAL_MODE_ALT 2
76 
77 extern float dist2_to_home;
78 extern bool too_far_from_home;
79 extern float failsafe_mode_dist2;
80 
81 extern float dist2_to_wp;
82 
83 extern bool exception_flag[10];
84 
85 
86 /*****************************************************************
87  * macros to ensure compatibility between fixedwing and rotorcraft
88  *****************************************************************/
89 
91 #define GetPosX() (stateGetPositionEnu_f()->x)
92 #define GetPosY() (stateGetPositionEnu_f()->y)
94 #define GetPosAlt() (stateGetPositionEnu_f()->z+state.ned_origin_f.hmsl)
96 
102 #define GetAltRef() (state.ned_origin_f.hmsl)
103 
104 
106 #define NormCourse(x) { \
107  while (x < 0) x += 360; \
108  while (x >= 360) x -= 360; \
109  }
110 
111 extern void nav_init(void);
112 extern void nav_run(void);
113 
114 extern void set_exception_flag(uint8_t flag_num);
115 
116 extern float get_dist2_to_waypoint(uint8_t wp_id);
117 extern float get_dist2_to_point(struct EnuCoor_i *p);
118 extern void compute_dist2_to_home(void);
119 extern void nav_home(void);
120 extern void nav_set_manual(int32_t roll, int32_t pitch, int32_t yaw);
121 
122 extern void nav_reset_reference(void) __attribute__((unused));
123 extern void nav_reset_alt(void) __attribute__((unused));
124 extern void nav_periodic_task(void);
125 
126 extern bool nav_is_in_flight(void);
127 
128 extern void nav_set_heading_rad(float rad);
129 extern void nav_set_heading_deg(float deg);
130 extern void nav_set_heading_towards(float x, float y);
132 extern void nav_set_heading_towards_target(void);
133 extern void nav_set_heading_current(void);
134 extern void nav_set_failsafe(void);
135 
136 /* ground detection */
137 extern bool nav_detect_ground(void);
138 #define NavStartDetectGround() ({ autopilot.detect_ground_once = true; false; })
139 #define NavDetectGround() nav_detect_ground()
140 
141 /* switching motors on/off */
142 static inline void NavKillThrottle(void)
143 {
145 }
146 static inline void NavResurrect(void)
147 {
149 }
150 
151 
152 #define NavSetManual nav_set_manual
153 #define NavSetFailsafe nav_set_failsafe
154 
155 
156 #define NavSetGroundReferenceHere nav_reset_reference
157 #define NavSetAltitudeReferenceHere nav_reset_alt
158 
159 #define NavSetWaypointHere waypoint_set_here_2d
160 #define NavCopyWaypoint waypoint_copy
161 #define NavCopyWaypointPositionOnly waypoint_position_copy
162 
163 
165 bool nav_approaching_from(struct EnuCoor_i *wp, struct EnuCoor_i *from, int16_t approaching_time);
166 #define NavApproaching(wp, time) nav_approaching_from(&waypoints[wp].enu_i, NULL, time)
167 #define NavApproachingFrom(wp, from, time) nav_approaching_from(&waypoints[wp].enu_i, &waypoints[from].enu_i, time)
168 
170 bool nav_check_wp_time(struct EnuCoor_i *wp, uint16_t stay_time);
171 #define NavCheckWaypointTime(wp, time) nav_check_wp_time(&waypoints[wp].enu_i, time)
172 
173 
174 extern void navigation_update_wp_from_speed(uint8_t wp, struct Int16Vect3 speed_sp, int16_t heading_rate_sp);
175 
176 /* should we really keep this one ??
177  * maybe better to use the `goto` flight plan primitive and
178  * add a `pre_call` or `call_once` to set the heading?
179  */
180 static inline void NavGotoWaypointHeading(uint8_t wp)
181 {
182  vertical_mode = VERTICAL_MODE_ALT;
183  horizontal_mode = HORIZONTAL_MODE_WAYPOINT;
185  dist2_to_wp = get_dist2_to_waypoint(wp);
187 }
188 
189 
190 
191 /***********************************************************
192  * macros used by flight plan to set different modes
193  **********************************************************/
194 
197 #define NavVerticalAutoThrottleMode(_pitch) { \
198  nav_pitch = ANGLE_BFP_OF_REAL(_pitch); \
199  }
200 
203 #define NavVerticalAutoPitchMode(_throttle) {}
204 
207 #define NavVerticalAltitudeMode(_alt, _pre_climb) { \
208  vertical_mode = VERTICAL_MODE_ALT; \
209  nav_altitude = POS_BFP_OF_REAL(_alt); \
210  }
211 
213 #define NavVerticalClimbMode(_climb) { \
214  vertical_mode = VERTICAL_MODE_CLIMB; \
215  nav_climb = SPEED_BFP_OF_REAL(_climb); \
216  }
217 
219 #define NavVerticalThrottleMode(_throttle) { \
220  vertical_mode = VERTICAL_MODE_MANUAL; \
221  nav_throttle = _throttle; \
222  }
223 
225 #define NavHeading nav_set_heading_rad
226 
227 #define NavAttitude(_roll) { \
228  horizontal_mode = HORIZONTAL_MODE_ATTITUDE; \
229  nav_roll = ANGLE_BFP_OF_REAL(_roll); \
230  }
231 
232 
233 
234 /***********************************************************
235  * built in navigation routines
236  **********************************************************/
237 
238 /*********** Navigation to waypoint *************************************/
239 static inline void NavGotoWaypoint(uint8_t wp)
240 {
241  horizontal_mode = HORIZONTAL_MODE_WAYPOINT;
243  dist2_to_wp = get_dist2_to_waypoint(wp);
244 }
245 
246 /*********** Navigation on a circle **************************************/
247 extern void nav_circle(struct EnuCoor_i *wp_center, int32_t radius);
248 static inline void NavCircleWaypoint(uint8_t wp_center, float radius)
249 {
250  horizontal_mode = HORIZONTAL_MODE_CIRCLE;
251  nav_circle(&waypoints[wp_center].enu_i, POS_BFP_OF_REAL(radius));
252 }
253 
254 #define NavCircleCount() ((float)abs(nav_circle_radians) / INT32_ANGLE_2_PI)
255 #define NavCircleQdr() ({ int32_t qdr = INT32_DEG_OF_RAD(INT32_ANGLE_PI_2 - nav_circle_qdr) >> INT32_ANGLE_FRAC; NormCourse(qdr); qdr; })
256 
257 #define CloseDegAngles(_c1, _c2) ({ int32_t _diff = _c1 - _c2; NormCourse(_diff); 350 < _diff || _diff < 10; })
258 #define CloseRadAngles(_c1, _c2) ({ float _diff = _c1 - _c2; NormRadAngle(_diff); fabsf(_diff) < 0.0177; })
259 
260 #define NavQdrCloseTo(x) CloseDegAngles(((x) >> INT32_ANGLE_FRAC), NavCircleQdr())
261 #define NavCourseCloseTo(x) {}
262 
263 /*********** Navigation along an oval *************************************/
264 extern void nav_oval_init(void);
265 extern void nav_oval(uint8_t, uint8_t, float);
266 extern uint8_t nav_oval_count;
267 #define Oval(a, b, c) nav_oval((b), (a), (c))
268 
269 /*********** Navigation along a line *************************************/
270 extern void nav_route(struct EnuCoor_i *wp_start, struct EnuCoor_i *wp_end);
271 static inline void NavSegment(uint8_t wp_start, uint8_t wp_end)
272 {
273  horizontal_mode = HORIZONTAL_MODE_ROUTE;
274  nav_route(&waypoints[wp_start].enu_i, &waypoints[wp_end].enu_i);
275 }
276 
278 static inline void NavGlide(uint8_t start_wp, uint8_t wp)
279 {
280  int32_t start_alt = waypoints[start_wp].enu_i.z;
281  int32_t diff_alt = waypoints[wp].enu_i.z - start_alt;
282  int32_t alt = start_alt + ((diff_alt * nav_leg_progress) / nav_leg_length);
284 }
285 
286 /* follow another aircraft */
287 #define NavFollow nav_follow
288 extern void nav_follow(uint8_t _ac_id, uint32_t distance, uint32_t height);
289 
290 
291 
292 /***********************************************************
293  * settings handlers
294  **********************************************************/
295 #define nav_IncreaseShift(x) {}
296 #define nav_SetNavRadius(x) {}
297 #define navigation_SetFlightAltitude(x) { \
298  flight_altitude = x; \
299  nav_flight_altitude = POS_BFP_OF_REAL(flight_altitude - state.ned_origin_f.hmsl); \
300  }
301 
302 #endif /* NAVIGATION_H */
unsigned short uint16_t
Definition: types.h:16
void nav_set_heading_rad(float rad)
Set nav_heading in radians.
Definition: navigation.c:484
uint32_t nav_leg_length
Definition: navigation.c:99
bool nav_detect_ground(void)
Definition: navigation.c:408
float nav_climb_vspeed
Definition: navigation.c:107
int32_t y
North.
int32_t x
East.
int32_t nav_leg_progress
Definition: navigation.c:98
uint8_t last_wp
Index of last waypoint.
Definition: nav.c:48
uint32_t nav_throttle
direct throttle from 0:MAX_PPRZ, used in VERTICAL_MODE_MANUAL
Definition: navigation.c:110
#define POS_BFP_OF_REAL(_af)
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
float nav_radius
Definition: nav.c:57
static void NavSegment(uint8_t wp_start, uint8_t wp_end)
Definition: navigation.h:271
int32_t nav_circle_radius
Definition: nav.c:69
void nav_reset_reference(void)
Reset the geographic reference to the current GPS fix.
Definition: navigation.c:348
#define HORIZONTAL_MODE_CIRCLE
Definition: navigation.h:55
void nav_reset_alt(void)
Reset the altitude reference to the current GPS alt.
Definition: navigation.c:355
float dist2_to_wp
squared distance to next waypoint
Definition: navigation.c:85
void nav_circle(struct EnuCoor_i *wp_center, int32_t radius)
Definition: navigation.c:538
int32_t nav_flight_altitude
Definition: navigation.c:111
bool nav_approaching_from(struct EnuCoor_i *wp, struct EnuCoor_i *from, int16_t approaching_time)
Proximity tests on approaching a wp.
Definition: navigation.c:257
float flight_altitude
Dynamically adjustable, reset to nav_altitude when it is changing.
Definition: nav.c:75
static void NavGlide(uint8_t start_wp, uint8_t wp)
Nav glide routine.
Definition: navigation.h:278
uint8_t horizontal_mode
Definition: nav.c:71
int32_t nav_cmd_roll
Definition: navigation.c:105
bool nav_survey_active
Definition: nav.c:89
uint8_t vertical_mode
Definition: sim_ap.c:35
int32_t nav_climb
Definition: nav.c:57
void compute_dist2_to_home(void)
Computes squared distance to the HOME waypoint potentially sets too_far_from_home.
Definition: navigation.c:473
int32_t nav_altitude
Definition: nav.c:305
#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
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
#define HORIZONTAL_MODE_ROUTE
Definition: navigation.h:54
static void NavGotoWaypointHeading(uint8_t wp)
Definition: navigation.h:180
void nav_follow(uint8_t _ac_id, uint32_t distance, uint32_t height)
Definition: navigation.c:738
float get_dist2_to_point(struct EnuCoor_i *p)
Returns squared horizontal distance to given point.
Definition: navigation.c:455
bool too_far_from_home
Definition: navigation.c:83
static void NavKillThrottle(void)
Definition: navigation.h:142
unsigned long uint32_t
Definition: types.h:18
signed short int16_t
Definition: types.h:17
static void NavResurrect(void)
Definition: navigation.h:146
int32_t nav_cmd_pitch
Definition: navigation.c:105
void nav_oval(uint8_t, uint8_t, float)
Navigation along a figure O.
Definition: nav.c:733
Paparazzi fixed point math for geodetic calculations.
uint8_t nav_oval_count
Navigation along a figure O.
Definition: nav.c:725
void nav_run(void)
Definition: navigation.c:243
bool exception_flag[10]
Definition: navigation.c:94
void nav_set_heading_towards(float x, float y)
Set heading to point towards x,y position in local coordinates.
Definition: navigation.c:497
#define NavVerticalAltitudeMode(_alt, _pre_climb)
Set the vertical mode to altitude control with the specified altitude setpoint and climb pre-command...
Definition: navigation.h:207
signed long int32_t
Definition: types.h:19
void set_exception_flag(uint8_t flag_num)
Definition: navigation.c:128
Core autopilot interface common to all firmwares.
int32_t nav_circle_qdr
Definition: navigation.c:116
struct EnuCoor_i navigation_carrot
Definition: navigation.c:88
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
int32_t nav_circle_radians
Status on the current circle.
Definition: nav.c:54
vector in East North Up coordinates
#define AP_MODE_NAV
unsigned char uint8_t
Definition: types.h:14
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
int32_t nav_roll
Definition: navigation.c:103
static void NavGotoWaypoint(uint8_t wp)
Definition: navigation.h:239
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
#define VERTICAL_MODE_ALT
Definition: navigation.h:75
Common flight_plan functions shared between fixedwing and rotorcraft.
void nav_oval_init(void)
Definition: nav.c:727
float nav_descend_vspeed
Definition: navigation.c:107
void navigation_update_wp_from_speed(uint8_t wp, struct Int16Vect3 speed_sp, int16_t heading_rate_sp)
Definition: navigation.c:384
static void NavCircleWaypoint(uint8_t wp_center, float radius)
Definition: navigation.h:248
void nav_set_heading_deg(float deg)
Set nav_heading in degrees.
Definition: navigation.c:491
static float p[2][2]
float dist2_to_home
squared distance to home waypoint
Definition: navigation.c:82
float failsafe_mode_dist2
maximum squared distance to home wp before going to failsafe mode
Definition: navigation.c:81
#define POS_FLOAT_OF_BFP(_ai)
#define HORIZONTAL_MODE_WAYPOINT
Definition: navigation.h:53
void nav_init(void)
Navigation Initialisation.
Definition: nav.c:530
void nav_set_failsafe(void)
Definition: navigation.c:528
struct EnuCoor_i navigation_target
Definition: navigation.c:87
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
int32_t nav_cmd_yaw
Definition: navigation.c:105
void nav_set_heading_towards_waypoint(uint8_t wp)
Set heading in the direction of a waypoint.
Definition: navigation.c:510
int32_t nav_heading
with INT32_ANGLE_FRAC
Definition: navigation.c:104
uint8_t autopilot_get_mode(void)
get autopilot mode
Definition: autopilot.c:184
int32_t nav_pitch
with INT32_ANGLE_FRAC
Definition: nav.c:308