Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 "modules/gps/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;
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 WaypointX(_wp)
Definition: common_nav.h:45
#define WaypointAlt(_wp)
waypoint altitude in m above MSL
Definition: common_nav.h:48
float y
Definition: common_nav.h:41
float a
Definition: common_nav.h:42
float x
Definition: common_nav.h:40
#define WaypointY(_wp)
Definition: common_nav.h:46
static float ground_speed
Definition: follow_me.c:76
struct GpsState gps
global GPS state
Definition: gps.c:69
Device independent GPS code (interface)
uint32_t tow
GPS time of week in ms.
Definition: gps.h:107
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:944
static float stateGetAirspeed_f(void)
Get airspeed (float).
Definition: state.h:1407
static struct FloatVect2 * stateGetHorizontalWindspeed_f(void)
Get horizontal windspeed (float).
Definition: state.h:1377
bool nav_approaching_xy(float x, float y, float from_x, float from_y, float approaching_time)
Decide if the UAV is approaching the current waypoint.
Definition: nav.c:325
void nav_route_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_y)
Computes the carrot position along the desired segment.
Definition: nav.c:382
void nav_circle_XY(float x, float y, float radius)
Navigates around (x, y).
Definition: nav.c:108
Fixedwing Navigation library.
#define NavQdrCloseTo(x)
True if x (in degrees) is close to the current QDR (less than 10 degrees)
Definition: nav.h:161
#define NavVerticalAltitudeMode(_alt, _pre_climb)
Set the vertical mode to altitude control with the specified altitude setpoint and climb pre-command.
Definition: nav.h:191
#define NavVerticalAutoThrottleMode(_pitch)
Set the climb control to auto-throttle with the specified pitch pre-command.
Definition: nav.h:177
bool snav_circle2(void)
Definition: nav_smooth.c:140
#define Sign(_x)
Definition: nav_smooth.c:36
static uint8_t wp_a
Definition: nav_smooth.c:43
bool snav_init(uint8_t a, float desired_course_rad, float radius)
Definition: nav_smooth.c:49
static float qdr_td
Definition: nav_smooth.c:41
static float ground_speeds[NB_ANGLES]
Definition: nav_smooth.c:152
bool snav_on_time(float nominal_radius)
Definition: nav_smooth.c:183
static struct point wp_cd wp_td wp_ca wp_ta
Definition: nav_smooth.c:39
static float u_a_ca_x
Definition: nav_smooth.c:45
bool snav_circle1(void)
Definition: nav_smooth.c:121
#define Norm2Pi(x)
Definition: nav_smooth.c:37
static uint8_t ground_speed_timer
Definition: nav_smooth.c:46
#define NB_ANGLES
Definition: nav_smooth.c:150
static float d_radius
Definition: nav_smooth.c:40
static float a_radius
Definition: nav_smooth.c:40
static float ground_speed_of_course(float x)
Definition: nav_smooth.c:154
bool snav_route(void)
Definition: nav_smooth.c:130
static bool compute_ground_speed(float airspeed, float wind_east, float wind_north)
Definition: nav_smooth.c:163
#define ANGLE_STEP
Definition: nav_smooth.c:151
static float qdr_a
Definition: nav_smooth.c:42
static float u_a_ca_y
Definition: nav_smooth.c:45
float snav_desired_tow
Definition: nav_smooth.c:44
Smooth navigation to wp_a along an arc (around wp_cd), a segment (from wp_rd to wp_ta) and a second a...
float y
in meters
float x
in meters
#define CARROT
default approaching_time for a wp
Definition: navigation.h:70
API to get/set the generic vehicle states.
float alpha
Definition: textons.c:133
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98