Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros 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 
22 #ifndef NAVIGATION_H
23 #define NAVIGATION_H
24 
25 #include "std.h"
26 #include "math/pprz_geodetic_int.h"
28 
30 
31 #define NAV_FREQ 16
32 // FIXME use periodic FREQ
33 #define NAV_PRESCALER (512/NAV_FREQ)
34 
35 extern struct EnuCoor_i navigation_target;
36 extern struct EnuCoor_i navigation_carrot;
37 
38 extern struct EnuCoor_f waypoints_float[];
39 extern struct EnuCoor_i waypoints[];
40 extern const uint8_t nb_waypoint;
41 
42 extern void nav_init(void);
43 extern void nav_run(void);
44 
45 extern uint8_t last_wp __attribute__ ((unused));
46 
47 extern int32_t ground_alt;
48 
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 extern int32_t nav_roll, nav_pitch;
59 extern float nav_radius;
60 
62 extern int32_t nav_leg_length;
63 
64 extern uint8_t vertical_mode;
65 extern uint32_t nav_throttle;
67 extern float flight_altitude;
68 #define VERTICAL_MODE_MANUAL 0
69 #define VERTICAL_MODE_CLIMB 1
70 #define VERTICAL_MODE_ALT 2
71 
72 
73 void compute_dist2_to_home(void);
74 unit_t nav_reset_reference( void ) __attribute__ ((unused));
75 unit_t nav_reset_alt( void ) __attribute__ ((unused));
76 void nav_periodic_task(void);
77 void nav_move_waypoint(uint8_t wp_id, struct EnuCoor_i * new_pos);
78 bool_t nav_detect_ground(void);
79 
80 void nav_home(void);
81 
82 #define NavKillThrottle() ({ if (autopilot_mode == AP_MODE_NAV) { autopilot_set_motors_on(FALSE); } FALSE; })
83 #define NavResurrect() ({ if (autopilot_mode == AP_MODE_NAV) { autopilot_set_motors_on(TRUE); } FALSE; })
84 
85 
86 #define NavSetGroundReferenceHere() ({ nav_reset_reference(); FALSE; })
87 #define NavSetAltitudeReferenceHere() ({ nav_reset_alt(); FALSE; })
88 
89 #define NavSetWaypointHere(_wp) ({ VECT2_COPY(waypoints[_wp], ins_enu_pos); FALSE; })
90 #define NavCopyWaypoint(_wp1, _wp2) ({ VECT2_COPY(waypoints[_wp1], waypoints[_wp2]); FALSE; })
91 
92 #define WaypointX(_wp) POS_FLOAT_OF_BFP(waypoints[_wp].x)
93 #define WaypointY(_wp) POS_FLOAT_OF_BFP(waypoints[_wp].y)
94 #define WaypointAlt(_wp) POS_FLOAT_OF_BFP(waypoints[_wp].z)
95 #define Height(_h) (_h)
96 
98 #define NormCourse(x) { \
99  while (x < 0) x += 360; \
100  while (x >= 360) x -= 360; \
101 }
102 
103 /*********** Navigation to waypoint *************************************/
104 #define NavGotoWaypoint(_wp) { \
105  horizontal_mode = HORIZONTAL_MODE_WAYPOINT; \
106  INT32_VECT3_COPY( navigation_target, waypoints[_wp]); \
107 }
108 
109 /*********** Navigation on a circle **************************************/
110 extern void nav_circle(uint8_t wp_center, int32_t radius);
111 #define NavCircleWaypoint(_center, _radius) { \
112  horizontal_mode = HORIZONTAL_MODE_CIRCLE; \
113  nav_circle(_center, POS_BFP_OF_REAL(_radius)); \
114 }
115 
116 #define NavCircleCount() (abs(nav_circle_radians) / INT32_ANGLE_2_PI)
117 #define NavCircleQdr() ({ int32_t qdr = INT32_DEG_OF_RAD(INT32_ANGLE_2_PI_2 - nav_circle_qdr) >> INT32_ANGLE_FRAC; NormCourse(qdr); qdr; })
118 
120 #define NavQdrCloseTo(x) {}
121 #define NavCourseCloseTo(x) {}
122 
123 /*********** Navigation along a line *************************************/
124 extern void nav_route(uint8_t wp_start, uint8_t wp_end);
125 #define NavSegment(_start, _end) { \
126  horizontal_mode = HORIZONTAL_MODE_ROUTE; \
127  nav_route(_start, _end); \
128 }
129 
131 #define NavGlide(_last_wp, _wp) { \
132  int32_t start_alt = waypoints[_last_wp].z; \
133  int32_t diff_alt = waypoints[_wp].z - start_alt; \
134  int32_t alt = start_alt + ((diff_alt * nav_leg_progress) / nav_leg_length); \
135  NavVerticalAltitudeMode(POS_FLOAT_OF_BFP(alt),0); \
136 }
137 
138 bool_t nav_approaching_from(uint8_t wp_idx, uint8_t from_idx);
139 #define NavApproaching(wp, time) nav_approaching_from(wp, 0)
140 #define NavApproachingFrom(wp, from, time) nav_approaching_from(wp, from)
141 
144 #define NavVerticalAutoThrottleMode(_pitch) { \
145  nav_pitch = ANGLE_BFP_OF_REAL(_pitch); \
146 }
147 
150 #define NavVerticalAutoPitchMode(_throttle) {}
151 
154 #define NavVerticalAltitudeMode(_alt, _pre_climb) { \
155  vertical_mode = VERTICAL_MODE_ALT; \
156  nav_altitude = POS_BFP_OF_REAL(_alt); \
157 }
158 
160 #define NavVerticalClimbMode(_climb) { \
161  vertical_mode = VERTICAL_MODE_CLIMB; \
162  nav_climb = SPEED_BFP_OF_REAL(_climb); \
163 }
164 
166 #define NavVerticalThrottleMode(_throttle) { \
167  vertical_mode = VERTICAL_MODE_MANUAL; \
168  nav_throttle = _throttle; \
169 }
170 
171 #define NavHeading(_course) {}
172 
173 #define NavAttitude(_roll) { \
174  horizontal_mode = HORIZONTAL_MODE_ATTITUDE; \
175  nav_roll = ANGLE_BFP_OF_REAL(_roll); \
176 }
177 
178 #define NavStartDetectGround() ({ autopilot_detect_ground_once = TRUE; FALSE; })
179 #define NavDetectGround() nav_detect_ground()
180 
181 #define nav_IncreaseShift(x) {}
182 
183 #define nav_SetNavRadius(x) {}
184 
185 #define navigation_SetNavHeading(x) { \
186  nav_heading = ANGLE_BFP_OF_REAL(x); \
187 }
188 
189 #define navigation_SetFlightAltitude(x) { \
190  flight_altitude = x; \
191  nav_flight_altitude = POS_BFP_OF_REAL(flight_altitude) - ground_alt; \
192 }
193 
194 
195 #define GetPosX() POS_FLOAT_OF_BFP(ins_enu_pos.x)
196 #define GetPosY() POS_FLOAT_OF_BFP(ins_enu_pos.y)
197 #define GetPosAlt() (POS_FLOAT_OF_BFP(ins_enu_pos.z+ground_alt))
198 
199 
200 extern void navigation_update_wp_from_speed(uint8_t wp, struct Int16Vect3 speed_sp, int16_t heading_rate_sp );
201 
202 #endif /* NAVIGATION_H */
static float radius
Definition: chemotaxis.c:15
Paparazzi floating point math for geodetic calculations.
unsigned long uint32_t
Definition: types.h:18
signed short int16_t
Definition: types.h:17
Paparazzi fixed point math for geodetic calculations.
signed long int32_t
Definition: types.h:19
unsigned char uint8_t
Definition: types.h:14
vector in East North Up coordinates Units: meters
vector in East North Up coordinates