Paparazzi UAS  v4.0.4_stable-3-gf39211a
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
nav.h
Go to the documentation of this file.
1 /*
2  * $Id$
3  *
4  * Copyright (C) 2003-2009 ENAC, Pascal Brisset, Antoine Drouin
5  *
6  * This file is part of paparazzi.
7  *
8  * paparazzi is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2, or (at your option)
11  * any later version.
12  *
13  * paparazzi is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with paparazzi; see the file COPYING. If not, write to
20  * the Free Software Foundation, 59 Temple Place - Suite 330,
21  * Boston, MA 02111-1307, USA.
22  *
23  */
24 
32 #ifndef NAV_H
33 #define NAV_H
34 
35 #include "std.h"
36 #include "paparazzi.h"
41 
42 #define G 9.806
43 #define Square(_x) ((_x)*(_x))
44 #define DistanceSquare(p1_x, p1_y, p2_x, p2_y) (Square(p1_x-p2_x)+Square(p1_y-p2_y))
45 
46 enum oval_status { OR12, OC2, OR21, OC1 };
47 
48 extern float cur_pos_x;
49 extern float cur_pos_y;
50 extern float last_x, last_y;
51 
53 
55 extern float nav_pitch; /* Rad */
56 extern float rc_pitch;
57 extern float fp_pitch; /* Degrees */
58 
59 extern float carrot_x, carrot_y;
60 
61 extern float nav_circle_radians; /* Cumulated */
62 extern bool_t nav_in_circle;
63 extern bool_t nav_in_segment;
64 extern float nav_circle_x, nav_circle_y, nav_circle_radius; /* m */
66 
67 extern int nav_mode;
68 #define NAV_MODE_ROLL 1
69 #define NAV_MODE_COURSE 2
70 
72 
73 #define HORIZONTAL_MODE_WAYPOINT 0
74 #define HORIZONTAL_MODE_ROUTE 1
75 #define HORIZONTAL_MODE_CIRCLE 2
76 
77 extern void fly_to_xy(float x, float y);
78 
79 #define NavGotoWaypoint(_wp) { \
80  horizontal_mode = HORIZONTAL_MODE_WAYPOINT; \
81  fly_to_xy(waypoints[_wp].x, waypoints[_wp].y); \
82 }
83 
84 
85 extern void nav_eight_init( void );
86 extern void nav_eight(uint8_t, uint8_t, float);
87 #define Eight(a, b, c) nav_eight((a), (b), (c))
88 
89 extern void nav_oval_init( void );
90 extern void nav_oval(uint8_t, uint8_t, float);
91 extern uint8_t nav_oval_count;
92 #define Oval(a, b, c) nav_oval((b), (a), (c))
93 
94 extern float nav_radius; /* m */
95 extern float nav_course; /* degrees, clockwise, 0.0 = N */
96 extern float nav_climb; /* m/s */
97 extern float nav_shift; /* Lateral shift along a route. In meters */
98 
100 
101 
102 extern float nav_survey_shift;
104 extern bool_t nav_survey_active;
105 
106 void nav_periodic_task(void);
107 void nav_home(void);
108 void nav_init(void);
109 void nav_without_gps(void);
110 
111 extern float nav_circle_trigo_qdr;
112 extern void nav_circle_XY(float x, float y, float radius);
113 
114 #define NavCircleWaypoint(wp, radius) \
115  nav_circle_XY(waypoints[wp].x, waypoints[wp].y, radius)
116 
118 #define NormCourse(x) { \
119  uint8_t dont_loop_forever = 0; \
120  while (x < 0 && ++dont_loop_forever) x += 360; \
121  while (x >= 360 && ++dont_loop_forever) x -= 360; \
122 }
123 
124 #define NavCircleCount() (fabs(nav_circle_radians) / (2*M_PI))
125 #define NavCircleQdr() ({ float qdr = DegOfRad(M_PI_2 - nav_circle_trigo_qdr); NormCourse(qdr); qdr; })
126 
127 #define CloseDegAngles(_c1, _c2) ({ float _diff = _c1 - _c2; NormCourse(_diff); 350 < _diff || _diff < 10; })
128 
130 #define NavQdrCloseTo(x) CloseDegAngles(x, NavCircleQdr())
131 
132 #define NavCourseCloseTo(x) CloseDegAngles(x, DegOfRad(estimator_hspeed_dir))
133 
134 /*********** Navigation along a line *************************************/
135 extern void nav_route_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_y);
136 #define NavSegment(_start, _end) \
137  nav_route_xy(waypoints[_start].x, waypoints[_start].y, waypoints[_end].x, waypoints[_end].y)
138 
139 bool_t nav_approaching_xy(float x, float y, float from_x, float from_y, float approaching_time);
140 #define NavApproaching(wp, time) nav_approaching_xy(waypoints[wp].x, waypoints[wp].y, last_x, last_y, time)
141 #define NavApproachingFrom(wp, from, time) nav_approaching_xy(waypoints[wp].x, waypoints[wp].y, waypoints[from].x, waypoints[from].y, time)
142 
143 
146 #define NavVerticalAutoThrottleMode(_pitch) { \
147  v_ctl_climb_mode = V_CTL_CLIMB_MODE_AUTO_THROTTLE; \
148  nav_pitch = _pitch; \
149 }
150 
153 #define NavVerticalAutoPitchMode(_throttle) { \
154  v_ctl_climb_mode = V_CTL_CLIMB_MODE_AUTO_PITCH; \
155  nav_throttle_setpoint = _throttle; \
156 }
157 
160 #define NavVerticalAltitudeMode(_alt, _pre_climb) { \
161  v_ctl_mode = V_CTL_MODE_AUTO_ALT; \
162  nav_altitude = _alt; \
163  v_ctl_altitude_pre_climb = _pre_climb; \
164 }
165 
167 #define NavVerticalClimbMode(_climb) { \
168  v_ctl_mode = V_CTL_MODE_AUTO_CLIMB; \
169  v_ctl_climb_setpoint = _climb; \
170 }
171 
173 #define NavVerticalThrottleMode(_throttle) { \
174  v_ctl_mode = V_CTL_MODE_AUTO_THROTTLE; \
175  nav_throttle_setpoint = _throttle; \
176 }
177 
178 #define NavHeading(_course) { \
179  lateral_mode = LATERAL_MODE_COURSE; \
180  h_ctl_course_setpoint = _course; \
181 }
182 
183 #define NavAttitude(_roll) { \
184  lateral_mode = LATERAL_MODE_ROLL; \
185  if(pprz_mode != PPRZ_MODE_AUTO1) \
186  {h_ctl_roll_setpoint = _roll;} \
187 }
188 
189 #define nav_IncreaseShift(x) { if (x==0) nav_shift = 0; else nav_shift += x; }
190 
191 #define nav_SetNavRadius(x) { if (x==1) nav_radius = DEFAULT_CIRCLE_RADIUS; else if (x==-1) nav_radius = -DEFAULT_CIRCLE_RADIUS; else nav_radius = x; }
192 
193 #define NavKillThrottle() { kill_throttle = 1; }
194 #endif /* NAV_H */
int16_t pprz_t
Definition: paparazzi.h:6
static float radius
Definition: chemotaxis.c:15
Vertical control for fixed wing vehicles.
unsigned char uint8_t
Definition: types.h:14
float y
Definition: common_nav.h:38
float x
Definition: common_nav.h:37