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) 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 
34 #include "modules/nav/waypoints.h"
36 #include "autopilot.h"
37 
39 #ifndef CARROT
40 #define CARROT 0
41 #endif
42 
44 #ifndef NAVIGATION_FREQUENCY
45 #if PERIODIC_FREQUENCY == 512
46 #define NAVIGATION_FREQUENCY 16
47 #else // if not 512, assume a multiple of 20 (e.g. 200, 500, 1000, ...)
48 #define NAVIGATION_FREQUENCY 20
49 #endif
50 #endif
51 
52 extern struct EnuCoor_i navigation_target;
53 extern struct EnuCoor_i navigation_carrot;
54 
55 extern uint8_t last_wp __attribute__((unused));
56 
58 
60 #define HORIZONTAL_MODE_WAYPOINT 0
61 #define HORIZONTAL_MODE_ROUTE 1
62 #define HORIZONTAL_MODE_CIRCLE 2
63 #define HORIZONTAL_MODE_ATTITUDE 3
64 #define HORIZONTAL_MODE_MANUAL 4
66 extern int32_t nav_heading;
68 extern float nav_radius;
70 
73 
74 extern bool nav_survey_active;
75 
76 extern uint8_t vertical_mode;
77 extern uint32_t nav_throttle;
79 extern float flight_altitude;
80 #define VERTICAL_MODE_MANUAL 0
81 #define VERTICAL_MODE_CLIMB 1
82 #define VERTICAL_MODE_ALT 2
83 
84 extern float dist2_to_home;
85 extern bool too_far_from_home;
86 extern float failsafe_mode_dist2;
87 
88 extern float dist2_to_wp;
89 
90 extern bool exception_flag[10];
91 
92 
93 /*****************************************************************
94  * macros to ensure compatibility between fixedwing and rotorcraft
95  *****************************************************************/
96 
98 #define GetPosX() (stateGetPositionEnu_f()->x)
99 #define GetPosY() (stateGetPositionEnu_f()->y)
101 #define GetPosAlt() (stateGetPositionEnu_f()->z+state.ned_origin_f.hmsl)
103 #define GetPosHeight() (stateGetPositionEnu_f()->z)
105 
111 #define GetAltRef() (state.ned_origin_f.hmsl)
112 
113 
115 #define NormCourse(x) { \
116  while (x < 0) x += 360; \
117  while (x >= 360) x -= 360; \
118  }
119 
120 extern void nav_init(void);
121 extern void nav_run(void);
122 extern void nav_parse_BLOCK(uint8_t *buf);
123 extern void nav_parse_MOVE_WP(uint8_t *buf);
124 
125 extern void set_exception_flag(uint8_t flag_num);
126 
127 extern float nav_max_speed;
128 extern bool force_forward;
129 extern struct FloatVect3 nav_get_speed_sp_from_go(struct EnuCoor_i target, float pos_gain);
130 extern struct FloatVect3 nav_get_speed_setpoint(float pos_gain);
131 extern struct FloatVect3 nav_get_speed_sp_from_line(struct FloatVect2 line_v, struct FloatVect2 to_end_v, struct EnuCoor_i target, float pos_gain);
132 
133 extern float get_dist2_to_waypoint(uint8_t wp_id);
134 extern float get_dist2_to_point(struct EnuCoor_i *p);
135 extern void compute_dist2_to_home(void);
136 extern void nav_home(void);
137 extern void nav_set_manual(int32_t roll, int32_t pitch, int32_t yaw);
138 
139 extern void nav_reset_reference(void) __attribute__((unused));
140 extern void nav_reset_alt(void) __attribute__((unused));
141 extern void nav_periodic_task(void);
142 
143 extern bool nav_is_in_flight(void);
144 
145 extern void nav_set_heading_rad(float rad);
146 extern void nav_set_heading_deg(float deg);
147 extern void nav_set_heading_towards(float x, float y);
149 extern void nav_set_heading_towards_target(void);
150 extern void nav_set_heading_current(void);
151 extern void nav_set_failsafe(void);
152 
153 /* ground detection */
154 extern bool nav_detect_ground(void);
155 #define NavStartDetectGround() ({ autopilot.detect_ground_once = true; false; })
156 #define NavDetectGround() nav_detect_ground()
157 
158 /* switching motors on/off */
159 static inline void NavKillThrottle(void)
160 {
162 }
163 static inline void NavResurrect(void)
164 {
166 }
167 
168 
169 #define NavSetManual nav_set_manual
170 #define NavSetFailsafe nav_set_failsafe
171 
172 
173 #define NavSetGroundReferenceHere nav_reset_reference
174 #define NavSetAltitudeReferenceHere nav_reset_alt
175 
176 #define NavSetWaypointHere waypoint_set_here_2d
177 #define NavCopyWaypoint waypoint_copy
178 #define NavCopyWaypointPositionOnly waypoint_position_copy
179 
180 
182 bool nav_approaching_from(struct EnuCoor_i *wp, struct EnuCoor_i *from, int16_t approaching_time);
183 #define NavApproaching(wp, time) nav_approaching_from(&waypoints[wp].enu_i, NULL, time)
184 #define NavApproachingFrom(wp, from, time) nav_approaching_from(&waypoints[wp].enu_i, &waypoints[from].enu_i, time)
185 
187 bool nav_check_wp_time(struct EnuCoor_i *wp, uint16_t stay_time);
188 #define NavCheckWaypointTime(wp, time) nav_check_wp_time(&waypoints[wp].enu_i, time)
189 
190 
191 /* should we really keep this one ??
192  * maybe better to use the `goto` flight plan primitive and
193  * add a `pre_call` or `call_once` to set the heading?
194  */
195 static inline void NavGotoWaypointHeading(uint8_t wp)
196 {
202 }
203 
204 
205 
206 /***********************************************************
207  * macros used by flight plan to set different modes
208  **********************************************************/
209 
212 #define NavVerticalAutoThrottleMode(_pitch) { \
213  nav_pitch = ANGLE_BFP_OF_REAL(_pitch); \
214  }
215 
218 #define NavVerticalAutoPitchMode(_throttle) {}
219 
222 #define NavVerticalAltitudeMode(_alt, _pre_climb) { \
223  vertical_mode = VERTICAL_MODE_ALT; \
224  nav_altitude = POS_BFP_OF_REAL(_alt); \
225  }
226 
228 #define NavVerticalClimbMode(_climb) { \
229  vertical_mode = VERTICAL_MODE_CLIMB; \
230  nav_climb = SPEED_BFP_OF_REAL(_climb); \
231  }
232 
234 #define NavVerticalThrottleMode(_throttle) { \
235  vertical_mode = VERTICAL_MODE_MANUAL; \
236  nav_throttle = _throttle; \
237  }
238 
240 #define NavHeading nav_set_heading_rad
241 
242 #define NavAttitude(_roll) { \
243  horizontal_mode = HORIZONTAL_MODE_ATTITUDE; \
244  nav_roll = ANGLE_BFP_OF_REAL(_roll); \
245  }
246 
247 
248 
249 /***********************************************************
250  * built in navigation routines
251  **********************************************************/
252 
253 /*********** Navigation to waypoint *************************************/
254 static inline void NavGotoWaypoint(uint8_t wp)
255 {
259 }
260 
261 /*********** Navigation on a circle **************************************/
262 extern void nav_circle(struct EnuCoor_i *wp_center, int32_t radius);
263 static inline void NavCircleWaypoint(uint8_t wp_center, float radius)
264 {
266  nav_circle(&waypoints[wp_center].enu_i, POS_BFP_OF_REAL(radius));
267 }
268 
269 #define NavCircleCount() ((float)abs(nav_circle_radians) / INT32_ANGLE_2_PI)
270 #define NavCircleQdr() ({ int32_t qdr = INT32_DEG_OF_RAD(INT32_ANGLE_PI_2 - nav_circle_qdr) >> INT32_ANGLE_FRAC; NormCourse(qdr); qdr; })
271 
272 #define CloseDegAngles(_c1, _c2) ({ int32_t _diff = _c1 - _c2; NormCourse(_diff); 350 < _diff || _diff < 10; })
273 #define CloseRadAngles(_c1, _c2) ({ float _diff = _c1 - _c2; NormRadAngle(_diff); fabsf(_diff) < 0.0177; })
274 
275 #define NavQdrCloseTo(x) CloseDegAngles(((x) >> INT32_ANGLE_FRAC), NavCircleQdr())
276 #define NavCourseCloseTo(x) {}
277 
278 /*********** Navigation along an oval *************************************/
279 extern void nav_oval_init(void);
280 extern void nav_oval(uint8_t, uint8_t, float);
281 extern uint8_t nav_oval_count;
282 #define Oval(a, b, c) nav_oval((b), (a), (c))
283 
284 /*********** Navigation along a line *************************************/
285 extern void nav_route(struct EnuCoor_i *wp_start, struct EnuCoor_i *wp_end);
286 extern struct FloatVect2 line_vect, to_end_vect;
287 #ifdef GUIDANCE_INDI_HYBRID
288 static inline void NavSegment(uint8_t wp_start, uint8_t wp_end)
289 {
290  VECT2_DIFF(line_vect, waypoints[wp_end].enu_f, waypoints[wp_start].enu_f);
292  VECT3_COPY(navigation_target, waypoints[wp_end].enu_i);
294 }
295 #else
296 static inline void NavSegment(uint8_t wp_start, uint8_t wp_end)
297 {
299  nav_route(&waypoints[wp_start].enu_i, &waypoints[wp_end].enu_i);
300 }
301 #endif
302 
304 static inline void NavGlide(uint8_t start_wp, uint8_t wp)
305 {
306  int32_t start_alt = waypoints[start_wp].enu_i.z;
307  int32_t diff_alt = waypoints[wp].enu_i.z - start_alt;
308  int32_t alt = start_alt + ((diff_alt * nav_leg_progress) / (int32_t)nav_leg_length);
310 }
311 
312 /* follow another aircraft */
313 #define NavFollow nav_follow
314 extern void nav_follow(uint8_t _ac_id, uint32_t distance, uint32_t height);
315 
316 
317 
318 /***********************************************************
319  * settings handlers
320  **********************************************************/
321 #define nav_IncreaseShift(x) {}
322 #define nav_SetNavRadius(x) {}
323 #define navigation_SetFlightAltitude(x) { \
324  flight_altitude = x; \
325  nav_flight_altitude = POS_BFP_OF_REAL(flight_altitude - state.ned_origin_f.hmsl); \
326  }
327 
328 #endif /* NAVIGATION_H */
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
NavSegment
static void NavSegment(uint8_t wp_start, uint8_t wp_end)
Definition: navigation.h:296
to_end_vect
struct FloatVect2 line_vect to_end_vect
Definition: navigation.c:86
nav_get_speed_setpoint
struct FloatVect3 nav_get_speed_setpoint(float pos_gain)
function that returns a speed setpoint based on flight plan.
Definition: guidance_indi_hybrid.c:567
nav_route
void nav_route(struct EnuCoor_i *wp_start, struct EnuCoor_i *wp_end)
Definition: navigation.c:594
common_flight_plan.h
uint32_t
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
NavGotoWaypointHeading
static void NavGotoWaypointHeading(uint8_t wp)
Definition: navigation.h:195
exception_flag
bool exception_flag[10]
Definition: navigation.c:103
HORIZONTAL_MODE_ROUTE
#define HORIZONTAL_MODE_ROUTE
Definition: navigation.h:61
HORIZONTAL_MODE_CIRCLE
#define HORIZONTAL_MODE_CIRCLE
Definition: navigation.h:62
nav_flight_altitude
int32_t nav_flight_altitude
Definition: navigation.h:78
nav_circle_radians
int32_t nav_circle_radians
Status on the current circle.
Definition: navigation.h:59
nav_descend_vspeed
float nav_descend_vspeed
Definition: navigation.h:69
vertical_mode
uint8_t vertical_mode
Definition: sim_ap.c:34
navigation_target
struct EnuCoor_i navigation_target
Definition: navigation.c:96
nav_climb
int32_t nav_climb
Definition: nav.c:59
nav_home
void nav_home(void)
Home mode navigation (circle around HOME)
Definition: nav.c:425
nav_detect_ground
bool nav_detect_ground(void)
Definition: navigation.c:418
nav_climb_vspeed
float nav_climb_vspeed
Definition: navigation.c:116
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
pprz_geodetic_int.h
Paparazzi fixed point math for geodetic calculations.
nav_altitude
int32_t nav_altitude
Definition: navigation.h:78
nav_roll
int32_t nav_roll
Definition: navigation.c:112
stateGetPositionEnu_f
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
nav_oval_count
uint8_t nav_oval_count
Navigation along a figure O.
Definition: nav.c:756
nav_radius
float nav_radius
Definition: nav.c:59
POS_FLOAT_OF_BFP
#define POS_FLOAT_OF_BFP(_ai)
Definition: pprz_algebra_int.h:218
nav_cmd_yaw
int32_t nav_cmd_yaw
Definition: navigation.h:67
nav_throttle
uint32_t nav_throttle
direct throttle from 0:MAX_PPRZ, used in VERTICAL_MODE_MANUAL
Definition: navigation.c:119
navigation_carrot
struct EnuCoor_i navigation_carrot
Definition: navigation.c:97
waypoints
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:39
nav_get_speed_sp_from_go
struct FloatVect3 nav_get_speed_sp_from_go(struct EnuCoor_i target, float pos_gain)
Go to a waypoint in the shortest way.
Definition: guidance_indi_hybrid.c:672
nav_init
void nav_init(void)
Navigation Initialisation.
Definition: nav.c:533
VECT2_DIFF
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:92
FloatVect2
Definition: pprz_algebra_float.h:49
FloatVect3
Definition: pprz_algebra_float.h:54
nav_reset_reference
void nav_reset_reference(void)
Reset the geographic reference to the current GPS fix.
Definition: navigation.c:382
int16_t
short int16_t
Typedef defining 16 bit short type.
Definition: vl53l1_types.h:93
std.h
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_leg_length
uint32_t nav_leg_length
Definition: navigation.c:108
get_dist2_to_point
float get_dist2_to_point(struct EnuCoor_i *p)
Returns squared horizontal distance to given point.
Definition: navigation.c:465
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_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_approaching_from
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:291
flight_altitude
float flight_altitude
Dynamically adjustable, reset to nav_altitude when it is changing.
Definition: nav.c:77
failsafe_mode_dist2
float failsafe_mode_dist2
maximum squared distance to home wp before going to failsafe mode
Definition: navigation.c:90
force_forward
bool force_forward
Definition: navigation.c:84
nav_max_speed
float nav_max_speed
Definition: guidance_indi_hybrid.c:81
NavCircleWaypoint
static void NavCircleWaypoint(uint8_t wp_center, float radius)
Definition: navigation.h:263
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
NavGlide
static void NavGlide(uint8_t start_wp, uint8_t wp)
Nav glide routine.
Definition: navigation.h:304
FloatVect3::y
float y
Definition: pprz_algebra_float.h:56
nav_get_speed_sp_from_line
struct FloatVect3 nav_get_speed_sp_from_line(struct FloatVect2 line_v, struct FloatVect2 to_end_v, struct EnuCoor_i target, float pos_gain)
follow a line.
Definition: guidance_indi_hybrid.c:586
nav_set_heading_towards_target
void nav_set_heading_towards_target(void)
Set heading in the direction of the target.
Definition: navigation.c:526
too_far_from_home
bool too_far_from_home
Definition: navigation.c:92
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
dist2_to_home
float dist2_to_home
squared distance to home waypoint
Definition: navigation.c:91
nav_reset_alt
void nav_reset_alt(void)
Reset the altitude reference to the current GPS alt.
Definition: navigation.c:389
int32_t
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
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_cmd_roll
int32_t nav_cmd_roll
Definition: navigation.c:114
FloatVect3::x
float x
Definition: pprz_algebra_float.h:55
nav_follow
void nav_follow(uint8_t _ac_id, uint32_t distance, uint32_t height)
Definition: navigation.c:748
nav_oval
void nav_oval(uint8_t, uint8_t, float)
Navigation along a figure O.
Definition: nav.c:764
VERTICAL_MODE_ALT
#define VERTICAL_MODE_ALT
Definition: navigation.h:82
nav_survey_active
bool nav_survey_active
Definition: nav.c:91
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
NavKillThrottle
static void NavKillThrottle(void)
Definition: navigation.h:159
nav_heading
int32_t nav_heading
with INT32_ANGLE_FRAC
Definition: navigation.c:113
NavGotoWaypoint
static void NavGotoWaypoint(uint8_t wp)
Definition: navigation.h:254
nav_leg_progress
int32_t nav_leg_progress
Definition: navigation.c:107
NavResurrect
static void NavResurrect(void)
Definition: navigation.h:163
nav_cmd_pitch
int32_t nav_cmd_pitch
Definition: navigation.h:67
nav_pitch
int32_t nav_pitch
with INT32_ANGLE_FRAC
Definition: navigation.h:65
HORIZONTAL_MODE_WAYPOINT
#define HORIZONTAL_MODE_WAYPOINT
Definition: navigation.h:60
POS_BFP_OF_REAL
#define POS_BFP_OF_REAL(_af)
Definition: pprz_algebra_int.h:217
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
NavVerticalAltitudeMode
#define NavVerticalAltitudeMode(_alt, _pre_climb)
Set the vertical mode to altitude control with the specified altitude setpoint and climb pre-command.
Definition: navigation.h:222
autopilot_get_mode
uint8_t autopilot_get_mode(void)
get autopilot mode
Definition: autopilot.c:216
FALSE
#define FALSE
Definition: std.h:5
waypoints.h
dist2_to_wp
float dist2_to_wp
squared distance to next waypoint
Definition: navigation.c:94
TRUE
#define TRUE
Definition: std.h:4
nav_circle_radius
int32_t nav_circle_radius
Definition: nav.c:71
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
uint16_t
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
nav_set_failsafe
void nav_set_failsafe(void)
Definition: navigation.c:538
target
struct FloatVect2 target
Definition: obstacle_avoidance.c:78
horizontal_mode
uint8_t horizontal_mode
Definition: nav.c:73
EnuCoor_i
vector in East North Up coordinates
Definition: pprz_geodetic_int.h:77
p
static float p[2][2]
Definition: ins_alt_float.c:257
VECT3_COPY
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
nav_circle_qdr
int32_t nav_circle_qdr
Definition: navigation.h:59