Paparazzi UAS  v5.17_devel-24-g2ae834f
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
nav_smooth.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2007-2009 ENAC, Pascal Brisset, Antoine Drouin
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 
29 #include <math.h>
30 #include "generated/airframe.h"
31 #include "modules/nav/nav_smooth.h"
32 #include "state.h"
34 #include "subsystems/gps.h"
35 
36 #define Sign(_x) ((_x) > 0 ? 1 : (-1))
37 #define Norm2Pi(x) ({ uint8_t _i=1; float _x = x; while (_i && _x < 0.) { _i++;_x += 2*M_PI; } while (_i && _x > 2*M_PI) { _i++; _x -= 2*M_PI; } _x; })
38 
39 static struct point wp_cd, wp_td, wp_ca, wp_ta;
40 static float d_radius, a_radius;
41 static float qdr_td;
42 static float qdr_a;
43 static uint8_t wp_a;
44 float snav_desired_tow; /* time of week, s */
45 static float u_a_ca_x, u_a_ca_y;
47 
48 /* D is the current position */
49 bool snav_init(uint8_t a, float desired_course_rad, float radius)
50 {
51  wp_a = a;
52  radius = fabs(radius);
53 
54  float da_x = WaypointX(wp_a) - stateGetPositionEnu_f()->x;
55  float da_y = WaypointY(wp_a) - stateGetPositionEnu_f()->y;
56 
57  /* D_CD orthogonal to current course, CD on the side of A */
58  float u_x = cos(M_PI_2 - stateGetHorizontalSpeedDir_f());
59  float u_y = sin(M_PI_2 - stateGetHorizontalSpeedDir_f());
60  d_radius = - Sign(u_x * da_y - u_y * da_x) * radius;
61  wp_cd.x = stateGetPositionEnu_f()->x + d_radius * u_y;
62  wp_cd.y = stateGetPositionEnu_f()->y - d_radius * u_x;
63  wp_cd.a = WaypointAlt(wp_a);
64 
65  /* A_CA orthogonal to desired course, CA on the side of D */
66  float desired_u_x = cos(M_PI_2 - desired_course_rad);
67  float desired_u_y = sin(M_PI_2 - desired_course_rad);
68  a_radius = Sign(desired_u_x * da_y - desired_u_y * da_x) * radius;
69  u_a_ca_x = desired_u_y;
70  u_a_ca_y = - desired_u_x;
71  wp_ca.x = WaypointX(wp_a) + a_radius * u_a_ca_x;
72  wp_ca.y = WaypointY(wp_a) + a_radius * u_a_ca_y;
73  wp_ca.a = WaypointAlt(wp_a);
74 
75  /* Unit vector along CD-CA */
76  u_x = wp_ca.x - wp_cd.x;
77  u_y = wp_ca.y - wp_cd.y;
78  float cd_ca = sqrt(u_x * u_x + u_y * u_y);
79 
80  /* If it is too close in reverse direction, set CA on the other side */
81  if (a_radius * d_radius < 0 && cd_ca < 2 * radius) {
82  a_radius = -a_radius;
83  wp_ca.x = WaypointX(wp_a) + a_radius * u_a_ca_x;
84  wp_ca.y = WaypointY(wp_a) + a_radius * u_a_ca_y;
85  u_x = wp_ca.x - wp_cd.x;
86  u_y = wp_ca.y - wp_cd.y;
87  cd_ca = sqrt(u_x * u_x + u_y * u_y);
88  }
89 
90  u_x /= cd_ca;
91  u_y /= cd_ca;
92 
93  if (a_radius * d_radius > 0) {
94  /* Both arcs are in the same direction */
95  /* CD_TD orthogonal to CD_CA */
96  wp_td.x = wp_cd.x - d_radius * u_y;
97  wp_td.y = wp_cd.y + d_radius * u_x;
98 
99  /* CA_TA also orthogonal to CD_CA */
100  wp_ta.x = wp_ca.x - a_radius * u_y;
101  wp_ta.y = wp_ca.y + a_radius * u_x;
102  } else {
103  /* Arcs are in reverse directions: trigonemetric puzzle :-) */
104  float alpha = atan2(u_y, u_x) + acos(d_radius / (cd_ca / 2));
105  wp_td.x = wp_cd.x + d_radius * cos(alpha);
106  wp_td.y = wp_cd.y + d_radius * sin(alpha);
107 
108  wp_ta.x = wp_ca.x + a_radius * cos(alpha);
109  wp_ta.y = wp_ca.y + a_radius * sin(alpha);
110  }
111  qdr_td = M_PI_2 - atan2(wp_td.y - wp_cd.y, wp_td.x - wp_cd.x);
112  qdr_a = M_PI_2 - atan2(WaypointY(wp_a) - wp_ca.y, WaypointX(wp_a) - wp_ca.x);
113  wp_td.a = wp_cd.a;
114  wp_ta.a = wp_ca.a;
115  ground_speed_timer = 0;
116 
117  return false;
118 }
119 
120 
121 bool snav_circle1(void)
122 {
123  /* circle around CD until QDR_TD */
124  NavVerticalAutoThrottleMode(0); /* No pitch */
125  NavVerticalAltitudeMode(wp_cd.a, 0.);
126  nav_circle_XY(wp_cd.x, wp_cd.y, d_radius);
127  return (! NavQdrCloseTo(DegOfRad(qdr_td)));
128 }
129 
130 bool snav_route(void)
131 {
132  /* Straight route from TD to TA */
133  NavVerticalAutoThrottleMode(0); /* No pitch */
134  NavVerticalAltitudeMode(wp_cd.a, 0.);
135  nav_route_xy(wp_td.x, wp_td.y, wp_ta.x, wp_ta.y);
136 
137  return (! nav_approaching_xy(wp_ta.x, wp_ta.y, wp_td.x, wp_td.y, CARROT));
138 }
139 
140 bool snav_circle2(void)
141 {
142  /* circle around CA until QDR_A */
143  NavVerticalAutoThrottleMode(0); /* No pitch */
144  NavVerticalAltitudeMode(wp_cd.a, 0.);
145  nav_circle_XY(wp_ca.x, wp_ca.y, a_radius);
146 
147  return (! NavQdrCloseTo(DegOfRad(qdr_a)));
148 }
149 
150 #define NB_ANGLES 24
151 #define ANGLE_STEP (2.*M_PI/NB_ANGLES)
152 static float ground_speeds[NB_ANGLES]; /* Indexed by trigo angles */
153 
154 static inline float ground_speed_of_course(float x)
155 {
156  uint8_t i = Norm2Pi(M_PI_2 - x) / ANGLE_STEP;
157  return ground_speeds[i];
158 }
159 
160 /* Compute the ground speed for courses 0, 360/NB_ANGLES, ...
161  (NB_ANGLES-1)360/NB_ANGLES. Return false if wind speed
162  is greater than airspeed */
163 static bool compute_ground_speed(float airspeed,
164  float wind_east,
165  float wind_north)
166 {
167  uint8_t i;
168  float alpha = 0;
169  float c = wind_north * wind_north + wind_east * wind_east - airspeed * airspeed;
170  for (i = 0; i < NB_ANGLES; i++, alpha += ANGLE_STEP) {
171  /* g^2 -2 scal g + c = 0 */
172  float scal = wind_east * cos(alpha) + wind_north * sin(alpha);
173  float delta = 4 * (scal * scal - c);
174  if (delta < 0)
175  return false;
176  ground_speeds[i] = scal + sqrt(delta) / 2.;
177  Bound(ground_speeds[i], NOMINAL_AIRSPEED / 4, 2 * NOMINAL_AIRSPEED);
178  }
179  return true;
180 }
181 
182 /* Adjusting a circle around CA, tangent in A, to end at snav_desired_tow */
183 bool snav_on_time(float nominal_radius)
184 {
185  nominal_radius = fabs(nominal_radius);
186 
187  float current_qdr = M_PI_2 - atan2(stateGetPositionEnu_f()->y - wp_ca.y, stateGetPositionEnu_f()->x - wp_ca.x);
188  float remaining_angle = Norm2Pi(Sign(a_radius) * (qdr_a - current_qdr));
189  float remaining_time = snav_desired_tow - gps.tow / 1000.;
190 
191  /* Use the nominal airspeed if the estimated one is not realistic */
192  float airspeed = stateGetAirspeed_f();
193  if (airspeed < NOMINAL_AIRSPEED / 2. ||
194  airspeed > 2.* NOMINAL_AIRSPEED) {
195  airspeed = NOMINAL_AIRSPEED;
196  }
197 
198  /* Recompute ground speeds every 10 s */
199  if (ground_speed_timer == 0) {
200  ground_speed_timer = 40; /* every 10s, called at 40Hz */
202  stateGetHorizontalWindspeed_f()->x)) { // Wind in NED frame
203  return false; /* return false if the computation of ground speeds fails */
204  }
205  }
207 
208  /* Time to complete the circle at nominal_radius */
209  float nominal_time = 0.;
210 
211  float a;
212  float ground_speed = NOMINAL_AIRSPEED; /* Init to avoid a warning */
213  /* Going one step too far */
214  for (a = 0; a < remaining_angle + ANGLE_STEP; a += ANGLE_STEP) {
215  float qdr = current_qdr + Sign(a_radius) * a;
216  ground_speed = ground_speed_of_course(qdr + Sign(a_radius) * M_PI_2);
217  nominal_time += ANGLE_STEP * nominal_radius / ground_speed;
218  }
219  /* Removing what exceeds remaining_angle */
220  nominal_time -= (a - remaining_angle) * nominal_radius / ground_speed;
221 
222  /* Radius size to finish in one single circle */
223  float radius = remaining_time / nominal_time * nominal_radius;
224  if (radius > 2. * nominal_radius) {
225  radius = nominal_radius;
226  }
227 
228  NavVerticalAutoThrottleMode(0); /* No pitch */
229  NavVerticalAltitudeMode(wp_cd.a, 0.);
230 
231  radius *= Sign(a_radius);
232  wp_ca.x = WaypointX(wp_a) + radius * u_a_ca_x;
233  wp_ca.y = WaypointY(wp_a) + radius * u_a_ca_y;
234  nav_circle_XY(wp_ca.x, wp_ca.y, radius);
235 
236  /* Stay in this mode until the end of time */
237  return (remaining_time > 0);
238 }
#define WaypointAlt(_wp)
waypoint altitude in m above MSL
Definition: common_nav.h:48
float x
Definition: common_nav.h:40
float alpha
Definition: textons.c:107
static float stateGetAirspeed_f(void)
Get airspeed (float).
Definition: state.h:1407
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
float y
Definition: common_nav.h:41
#define WaypointX(_wp)
Definition: common_nav.h:45
float x
in meters
uint32_t tow
GPS time of week in ms.
Definition: gps.h:109
Device independent GPS code (interface)
#define WaypointY(_wp)
Definition: common_nav.h:46
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:944
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
static struct FloatVect2 * stateGetHorizontalWindspeed_f(void)
Get horizontal windspeed (float).
Definition: state.h:1377
float a
Definition: common_nav.h:42
float y
in meters
struct GpsState gps
global GPS state
Definition: gps.c:69
#define CARROT
default approaching_time for a wp
Definition: navigation.h:40