Paparazzi UAS  v5.15_devel-230-gc96ce27
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 */
162 static void compute_ground_speed(float airspeed,
163  float wind_east,
164  float wind_north)
165 {
166  uint8_t i;
167  float alpha = 0;
168  float c = wind_north * wind_north + wind_east * wind_east - airspeed * airspeed;
169  for (i = 0; i < NB_ANGLES; i++, alpha += ANGLE_STEP) {
170  /* g^2 -2 scal g + c = 0 */
171  float scal = wind_east * cos(alpha) + wind_north * sin(alpha);
172  float delta = 4 * (scal * scal - c);
173  ground_speeds[i] = scal + sqrt(delta) / 2.;
174  Bound(ground_speeds[i], NOMINAL_AIRSPEED / 4, 2 * NOMINAL_AIRSPEED);
175  }
176 }
177 
178 /* Adjusting a circle around CA, tangent in A, to end at snav_desired_tow */
179 bool snav_on_time(float nominal_radius)
180 {
181  nominal_radius = fabs(nominal_radius);
182 
183  float current_qdr = M_PI_2 - atan2(stateGetPositionEnu_f()->y - wp_ca.y, stateGetPositionEnu_f()->x - wp_ca.x);
184  float remaining_angle = Norm2Pi(Sign(a_radius) * (qdr_a - current_qdr));
185  float remaining_time = snav_desired_tow - gps.tow / 1000.;
186 
187  /* Use the nominal airspeed if the estimated one is not realistic */
188  float airspeed = stateGetAirspeed_f();
189  if (airspeed < NOMINAL_AIRSPEED / 2. ||
190  airspeed > 2.* NOMINAL_AIRSPEED) {
191  airspeed = NOMINAL_AIRSPEED;
192  }
193 
194  /* Recompute ground speeds every 10 s */
195  if (ground_speed_timer == 0) {
196  ground_speed_timer = 40; /* every 10s, called at 40Hz */
198  stateGetHorizontalWindspeed_f()->x); // Wind in NED frame
199  }
201 
202  /* Time to complete the circle at nominal_radius */
203  float nominal_time = 0.;
204 
205  float a;
206  float ground_speed = NOMINAL_AIRSPEED; /* Init to avoid a warning */
207  /* Going one step too far */
208  for (a = 0; a < remaining_angle + ANGLE_STEP; a += ANGLE_STEP) {
209  float qdr = current_qdr + Sign(a_radius) * a;
210  ground_speed = ground_speed_of_course(qdr + Sign(a_radius) * M_PI_2);
211  nominal_time += ANGLE_STEP * nominal_radius / ground_speed;
212  }
213  /* Removing what exceeds remaining_angle */
214  nominal_time -= (a - remaining_angle) * nominal_radius / ground_speed;
215 
216  /* Radius size to finish in one single circle */
217  float radius = remaining_time / nominal_time * nominal_radius;
218  if (radius > 2. * nominal_radius) {
219  radius = nominal_radius;
220  }
221 
222  NavVerticalAutoThrottleMode(0); /* No pitch */
223  NavVerticalAltitudeMode(wp_cd.a, 0.);
224 
225  radius *= Sign(a_radius);
226  wp_ca.x = WaypointX(wp_a) + radius * u_a_ca_x;
227  wp_ca.y = WaypointY(wp_a) + radius * u_a_ca_y;
228  nav_circle_XY(wp_ca.x, wp_ca.y, radius);
229 
230  /* Stay in this mode until the end of time */
231  return (remaining_time > 0);
232 }
#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