Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures 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"
34 
36 
37 #define NAV_FREQ 16
38 
39 extern struct EnuCoor_i navigation_target;
40 extern struct EnuCoor_i navigation_carrot;
41 
42 extern struct EnuCoor_i waypoints[];
43 extern const uint8_t nb_waypoint;
44 
45 extern void nav_init(void);
46 extern void nav_run(void);
47 
48 extern uint8_t last_wp __attribute__ ((unused));
49 
51 extern struct EnuCoor_i nav_segment_start, nav_segment_end;
52 extern struct EnuCoor_i nav_circle_center;
54 #define HORIZONTAL_MODE_WAYPOINT 0
55 #define HORIZONTAL_MODE_ROUTE 1
56 #define HORIZONTAL_MODE_CIRCLE 2
57 #define HORIZONTAL_MODE_ATTITUDE 3
58 extern int32_t nav_roll, nav_pitch;
59 extern int32_t nav_heading;
60 extern float nav_radius;
61 
63 extern int32_t nav_leg_length;
64 
65 extern uint8_t vertical_mode;
66 extern uint32_t nav_throttle;
68 extern float flight_altitude;
69 #define VERTICAL_MODE_MANUAL 0
70 #define VERTICAL_MODE_CLIMB 1
71 #define VERTICAL_MODE_ALT 2
72 
73 extern float dist2_to_home;
74 extern bool_t too_far_from_home;
75 extern float failsafe_mode_dist2;
76 
77 extern float dist2_to_wp;
78 
79 extern float get_dist2_to_waypoint(uint8_t wp_id);
80 extern float get_dist2_to_point(struct EnuCoor_i *p);
81 extern void compute_dist2_to_home(void);
82 extern void nav_home(void);
83 
84 unit_t nav_reset_reference( void ) __attribute__ ((unused));
85 unit_t nav_reset_alt( void ) __attribute__ ((unused));
86 void nav_periodic_task(void);
87 void nav_move_waypoint_lla(uint8_t wp_id, struct LlaCoor_i* new_lla_pos);
88 void nav_move_waypoint(uint8_t wp_id, struct EnuCoor_i * new_pos);
89 bool_t nav_detect_ground(void);
90 bool_t nav_is_in_flight(void);
91 
92 extern bool_t nav_set_heading_rad(float rad);
93 extern bool_t nav_set_heading_deg(float deg);
94 extern bool_t nav_set_heading_towards(float x, float y);
96 extern bool_t nav_set_heading_current(void);
97 
99 #ifndef CARROT
100 #define CARROT 0
101 #endif
102 
103 #define NavKillThrottle() ({ if (autopilot_mode == AP_MODE_NAV) { autopilot_set_motors_on(FALSE); } FALSE; })
104 #define NavResurrect() ({ if (autopilot_mode == AP_MODE_NAV) { autopilot_set_motors_on(TRUE); } FALSE; })
105 
106 
107 #define NavSetGroundReferenceHere() ({ nav_reset_reference(); FALSE; })
108 #define NavSetAltitudeReferenceHere() ({ nav_reset_alt(); FALSE; })
109 
110 #define NavSetWaypointHere(_wp) ({ VECT2_COPY(waypoints[_wp], *stateGetPositionEnu_i()); FALSE; })
111 #define NavCopyWaypoint(_wp1, _wp2) ({ VECT2_COPY(waypoints[_wp1], waypoints[_wp2]); FALSE; })
112 
113 #define WaypointX(_wp) POS_FLOAT_OF_BFP(waypoints[_wp].x)
114 #define WaypointY(_wp) POS_FLOAT_OF_BFP(waypoints[_wp].y)
115 #define WaypointAlt(_wp) POS_FLOAT_OF_BFP(waypoints[_wp].z)
116 #define Height(_h) (_h)
117 
119 #define NormCourse(x) { \
120  while (x < 0) x += 360; \
121  while (x >= 360) x -= 360; \
122 }
123 
124 /*********** Navigation to waypoint *************************************/
125 #define NavGotoWaypoint(_wp) { \
126  horizontal_mode = HORIZONTAL_MODE_WAYPOINT; \
127  INT32_VECT3_COPY( navigation_target, waypoints[_wp]); \
128  dist2_to_wp = get_dist2_to_waypoint(_wp); \
129 }
130 
131 /*********** Navigation on a circle **************************************/
132 extern void nav_circle(struct EnuCoor_i * wp_center, int32_t radius);
133 #define NavCircleWaypoint(_center, _radius) { \
134  horizontal_mode = HORIZONTAL_MODE_CIRCLE; \
135  nav_circle(&waypoints[_center], POS_BFP_OF_REAL(_radius)); \
136 }
137 
138 #define NavCircleCount() (abs(nav_circle_radians) / INT32_ANGLE_2_PI)
139 #define NavCircleQdr() ({ int32_t qdr = INT32_DEG_OF_RAD(INT32_ANGLE_2_PI_2 - nav_circle_qdr) >> INT32_ANGLE_FRAC; NormCourse(qdr); qdr; })
140 
142 #define NavQdrCloseTo(x) {}
143 #define NavCourseCloseTo(x) {}
144 
145 /*********** Navigation along a line *************************************/
146 extern void nav_route(struct EnuCoor_i * wp_start, struct EnuCoor_i * wp_end);
147 #define NavSegment(_start, _end) { \
148  horizontal_mode = HORIZONTAL_MODE_ROUTE; \
149  nav_route(&waypoints[_start], &waypoints[_end]); \
150 }
151 
153 #define NavGlide(_last_wp, _wp) { \
154  int32_t start_alt = waypoints[_last_wp].z; \
155  int32_t diff_alt = waypoints[_wp].z - start_alt; \
156  int32_t alt = start_alt + ((diff_alt * nav_leg_progress) / nav_leg_length); \
157  NavVerticalAltitudeMode(POS_FLOAT_OF_BFP(alt),0); \
158 }
159 
161 bool_t nav_approaching_from(struct EnuCoor_i * wp, struct EnuCoor_i * from, int16_t approaching_time);
162 #define NavApproaching(wp, time) nav_approaching_from(&waypoints[wp], NULL, time)
163 #define NavApproachingFrom(wp, from, time) nav_approaching_from(&waypoints[wp], &waypoints[from], time)
164 
166 bool_t nav_check_wp_time(struct EnuCoor_i * wp, uint16_t stay_time);
167 #define NavCheckWaypointTime(wp, time) nav_check_wp_time(&waypoints[wp], time)
168 
171 #define NavVerticalAutoThrottleMode(_pitch) { \
172  nav_pitch = ANGLE_BFP_OF_REAL(_pitch); \
173 }
174 
177 #define NavVerticalAutoPitchMode(_throttle) {}
178 
181 #define NavVerticalAltitudeMode(_alt, _pre_climb) { \
182  vertical_mode = VERTICAL_MODE_ALT; \
183  nav_altitude = POS_BFP_OF_REAL(_alt); \
184 }
185 
187 #define NavVerticalClimbMode(_climb) { \
188  vertical_mode = VERTICAL_MODE_CLIMB; \
189  nav_climb = SPEED_BFP_OF_REAL(_climb); \
190 }
191 
193 #define NavVerticalThrottleMode(_throttle) { \
194  vertical_mode = VERTICAL_MODE_MANUAL; \
195  nav_throttle = _throttle; \
196 }
197 
198 #define NavHeading(_course) {}
199 
200 #define NavAttitude(_roll) { \
201  horizontal_mode = HORIZONTAL_MODE_ATTITUDE; \
202  nav_roll = ANGLE_BFP_OF_REAL(_roll); \
203 }
204 
205 #define NavStartDetectGround() ({ autopilot_detect_ground_once = TRUE; FALSE; })
206 #define NavDetectGround() nav_detect_ground()
207 
208 #define nav_IncreaseShift(x) {}
209 
210 #define nav_SetNavRadius(x) {}
211 
212 
213 #define navigation_SetFlightAltitude(x) { \
214  flight_altitude = x; \
215  nav_flight_altitude = POS_BFP_OF_REAL(flight_altitude - state.ned_origin_f.hmsl); \
216 }
217 
218 
219 #define GetPosX() (stateGetPositionEnu_f()->x)
220 #define GetPosY() (stateGetPositionEnu_f()->y)
221 #define GetPosAlt() (stateGetPositionEnu_f()->z+state.ned_origin_f.hmsl)
222 #define GetAltRef() (state.ned_origin_f.hmsl)
223 
224 
225 extern void navigation_update_wp_from_speed(uint8_t wp, struct Int16Vect3 speed_sp, int16_t heading_rate_sp );
226 
227 #endif /* NAVIGATION_H */
unsigned short uint16_t
Definition: types.h:16
static float radius
Definition: chemotaxis.c:15
vector in Latitude, Longitude and Altitude
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
static float p[2][2]
int32_t y
North.
vector in East North Up coordinates
struct sockaddr_in pc_addr drone_at drone_nav from
Definition: at_com.c:43
int32_t x
East.