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