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
snav.c
Go to the documentation of this file.
1 /* Smooth navigation to wp_a along an arc (around wp_cd), a
2  segment (from wp_rd to wp_ta) and a second arc (around wp_ca) */
3 
4 #include <math.h>
5 #include "generated/airframe.h"
7 #include "estimator.h"
8 #include "subsystems/nav.h"
9 #include "subsystems/gps.h"
10 
11 #define Sign(_x) ((_x) > 0 ? 1 : (-1))
12 #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; })
13 
14 static struct point wp_cd, wp_td, wp_ca, wp_ta;
15 static float d_radius, a_radius;
16 static float qdr_td;
17 static float qdr_a;
18 static uint8_t wp_a;
19 float snav_desired_tow; /* time of week, s */
20 static float u_a_ca_x, u_a_ca_y;
22 
23 /* D is the current position */
24 bool_t snav_init(uint8_t a, float desired_course_rad, float radius) {
25  wp_a = a;
26  radius = fabs(radius);
27 
28  float da_x = WaypointX(wp_a) - estimator_x;
29  float da_y = WaypointY(wp_a) - estimator_y;
30 
31  /* D_CD orthogonal to current course, CD on the side of A */
32  float u_x = cos(M_PI_2 - estimator_hspeed_dir);
33  float u_y = sin(M_PI_2 - estimator_hspeed_dir);
34  d_radius = - Sign(u_x*da_y - u_y*da_x) * radius;
35  wp_cd.x = estimator_x + d_radius * u_y;
36  wp_cd.y = estimator_y - d_radius * u_x;
37  wp_cd.a = WaypointAlt(wp_a);
38 
39  /* A_CA orthogonal to desired course, CA on the side of D */
40  float desired_u_x = cos(M_PI_2 - desired_course_rad);
41  float desired_u_y = sin(M_PI_2 - desired_course_rad);
42  a_radius = Sign(desired_u_x*da_y - desired_u_y*da_x) * radius;
43  u_a_ca_x = desired_u_y;
44  u_a_ca_y = - desired_u_x;
45  wp_ca.x = WaypointX(wp_a) + a_radius * u_a_ca_x;
46  wp_ca.y = WaypointY(wp_a) + a_radius * u_a_ca_y;
47  wp_ca.a = WaypointAlt(wp_a);
48 
49  /* Unit vector along CD-CA */
50  u_x = wp_ca.x - wp_cd.x;
51  u_y = wp_ca.y - wp_cd.y;
52  float cd_ca = sqrt(u_x*u_x+u_y*u_y);
53 
54  /* If it is too close in reverse direction, set CA on the other side */
55  if (a_radius * d_radius < 0 && cd_ca < 2 * radius) {
56  a_radius = -a_radius;
57  wp_ca.x = WaypointX(wp_a) + a_radius * u_a_ca_x;
58  wp_ca.y = WaypointY(wp_a) + a_radius * u_a_ca_y;
59  u_x = wp_ca.x - wp_cd.x;
60  u_y = wp_ca.y - wp_cd.y;
61  cd_ca = sqrt(u_x*u_x+u_y*u_y);
62  }
63 
64  u_x /= cd_ca;
65  u_y /= cd_ca;
66 
67  if (a_radius * d_radius > 0) {
68  /* Both arcs are in the same direction */
69  /* CD_TD orthogonal to CD_CA */
70  wp_td.x = wp_cd.x - d_radius * u_y;
71  wp_td.y = wp_cd.y + d_radius * u_x;
72 
73  /* CA_TA also orthogonal to CD_CA */
74  wp_ta.x = wp_ca.x - a_radius * u_y;
75  wp_ta.y = wp_ca.y + a_radius * u_x;
76  } else {
77  /* Arcs are in reverse directions: trigonemetric puzzle :-) */
78  float alpha = atan2(u_y, u_x) + acos(d_radius/(cd_ca/2));
79  wp_td.x = wp_cd.x + d_radius * cos(alpha);
80  wp_td.y = wp_cd.y + d_radius * sin(alpha);
81 
82  wp_ta.x = wp_ca.x + a_radius * cos(alpha);
83  wp_ta.y = wp_ca.y + a_radius * sin(alpha);
84  }
85  qdr_td = M_PI_2 - atan2(wp_td.y-wp_cd.y, wp_td.x-wp_cd.x);
86  qdr_a = M_PI_2 - atan2(WaypointY(wp_a)-wp_ca.y, WaypointX(wp_a)-wp_ca.x);
87  wp_td.a = wp_cd.a;
88  wp_ta.a = wp_ca.a;
90 
91  return FALSE;
92 }
93 
94 
95 bool_t snav_circle1(void) {
96  /* circle around CD until QDR_TD */
97  NavVerticalAutoThrottleMode(0); /* No pitch */
98  NavVerticalAltitudeMode(wp_cd.a, 0.);
99  nav_circle_XY(wp_cd.x, wp_cd.y, d_radius);
100  return(! NavQdrCloseTo(DegOfRad(qdr_td)));
101 }
102 
103 bool_t snav_route(void) {
104  /* Straight route from TD to TA */
105  NavVerticalAutoThrottleMode(0); /* No pitch */
106  NavVerticalAltitudeMode(wp_cd.a, 0.);
107  nav_route_xy(wp_td.x, wp_td.y, wp_ta.x, wp_ta.y);
108 
109  return (! nav_approaching_xy(wp_ta.x, wp_ta.y, wp_td.x, wp_td.y, CARROT));
110 }
111 
112 bool_t snav_circle2(void) {
113  /* circle around CA until QDR_A */
114  NavVerticalAutoThrottleMode(0); /* No pitch */
115  NavVerticalAltitudeMode(wp_cd.a, 0.);
116  nav_circle_XY(wp_ca.x, wp_ca.y, a_radius);
117 
118  return(! NavQdrCloseTo(DegOfRad(qdr_a)));
119 }
120 
121 #define NB_ANGLES 24
122 #define ANGLE_STEP (2.*M_PI/NB_ANGLES)
123 static float ground_speeds[NB_ANGLES]; /* Indexed by trigo angles */
124 
125 static inline float ground_speed_of_course(float x) {
126  uint8_t i = Norm2Pi(M_PI_2-x)/ANGLE_STEP;
127  return ground_speeds[i];
128 }
129 
130 /* Compute the ground speed for courses 0, 360/NB_ANGLES, ...
131  (NB_ANGLES-1)360/NB_ANGLES */
132 static void compute_ground_speed(float airspeed,
133  float wind_x,
134  float wind_y) {
135  uint8_t i;
136  float alpha = 0;
137  float c = wind_x*wind_x+wind_y*wind_y-airspeed*airspeed;
138  for( i=0; i < NB_ANGLES; i++, alpha+=ANGLE_STEP) {
139  /* g^2 -2 scal g + c = 0 */
140  float scal = wind_x*cos(alpha) + wind_y*sin(alpha);
141  float delta = 4 * (scal*scal - c);
142  ground_speeds[i] = scal + sqrt(delta)/2.;
143  Bound(ground_speeds[i], NOMINAL_AIRSPEED/4, 2*NOMINAL_AIRSPEED);
144  }
145 }
146 
147 /* Adjusting a circle around CA, tangent in A, to end at snav_desired_tow */
148 bool_t snav_on_time(float nominal_radius) {
149  nominal_radius = fabs(nominal_radius);
150 
151  float current_qdr = M_PI_2 - atan2(estimator_y-wp_ca.y, estimator_x-wp_ca.x);
152  float remaining_angle = Norm2Pi(Sign(a_radius)*(qdr_a - current_qdr));
153  float remaining_time = snav_desired_tow - gps.tow / 1000.;
154 
155  /* Use the nominal airspeed if the estimated one is not realistic */
156  float airspeed = estimator_airspeed;
157  if (airspeed < NOMINAL_AIRSPEED / 2. ||
158  airspeed > 2.* NOMINAL_AIRSPEED)
159  airspeed = NOMINAL_AIRSPEED;
160 
161  /* Recompute ground speeds every 10 s */
162  if (ground_speed_timer == 0) {
163  ground_speed_timer = 40; /* every 10s, called at 40Hz */
165  }
167 
168  /* Time to complete the circle at nominal_radius */
169  float nominal_time = 0.;
170 
171  float a;
172  float ground_speed = NOMINAL_AIRSPEED; /* Init to avoid a warning */
173  /* Going one step too far */
174  for(a = 0; a < remaining_angle + ANGLE_STEP; a += ANGLE_STEP) {
175  float qdr = current_qdr + Sign(a_radius)*a;
176  ground_speed = ground_speed_of_course(qdr+Sign(a_radius)*M_PI_2);
177  nominal_time += ANGLE_STEP*nominal_radius/ground_speed;
178  }
179  /* Removing what exceeds remaining_angle */
180  nominal_time -= (a-remaining_angle)*nominal_radius/ground_speed;
181 
182  /* Radius size to finish in one single circle */
183  float radius = remaining_time / nominal_time * nominal_radius;
184  if (radius > 2. * nominal_radius)
185  radius = nominal_radius;
186 
187  NavVerticalAutoThrottleMode(0); /* No pitch */
188  NavVerticalAltitudeMode(wp_cd.a, 0.);
189 
190  radius *= Sign(a_radius);
191  wp_ca.x = WaypointX(wp_a) + radius * u_a_ca_x;
192  wp_ca.y = WaypointY(wp_a) + radius * u_a_ca_y;
193  nav_circle_XY(wp_ca.x, wp_ca.y, radius);
194 
195  /* Stay in this mode until the end of time */
196  return(remaining_time > 0);
197 }
static void compute_ground_speed(float airspeed, float wind_x, float wind_y)
Definition: snav.c:132
static float u_a_ca_y
Definition: snav.c:20
#define ANGLE_STEP
Definition: snav.c:122
static float u_a_ca_x
Definition: snav.c:20
float a
Definition: common_nav.h:39
#define Norm2Pi(x)
Definition: snav.c:12
static uint8_t ground_speed_timer
Definition: snav.c:21
static float qdr_a
Definition: snav.c:17
static float radius
Definition: chemotaxis.c:15
static float qdr_td
Definition: snav.c:16
static float ground_speed_of_course(float x)
Definition: snav.c:125
float estimator_y
north position in meters
Definition: estimator.c:43
float snav_desired_tow
Definition: snav.c:19
#define Sign(_x)
Definition: snav.c:11
float estimator_hspeed_dir
direction of horizontal ground speed in rad (CW/North)
Definition: estimator.c:65
bool_t snav_route(void)
Definition: snav.c:103
#define FALSE
Definition: imu_chimu.h:141
uint32_t tow
GPS time of week in ms.
Definition: gps.h:79
static float d_radius
Definition: snav.c:15
float wind_east
Definition: estimator.c:68
Device independent GPS code (interface)
float estimator_airspeed
m/s
Definition: estimator.c:69
bool_t snav_on_time(float nominal_radius)
Definition: snav.c:148
static float ground_speeds[NB_ANGLES]
Definition: snav.c:123
static struct point wp_cd wp_td wp_ca wp_ta
Definition: snav.c:14
float estimator_x
east position in meters
Definition: estimator.c:42
bool_t snav_circle1(void)
Definition: snav.c:95
unsigned char uint8_t
Definition: types.h:14
bool_t snav_init(uint8_t a, float desired_course_rad, float radius)
Definition: snav.c:24
float y
Definition: common_nav.h:38
static float a_radius
Definition: snav.c:15
State estimation, fusioning sensors.
float wind_north
Definition: estimator.c:68
static struct point c
Definition: discsurvey.c:13
bool_t snav_circle2(void)
Definition: snav.c:112
static uint8_t wp_a
Definition: snav.c:18
struct GpsState gps
global GPS state
Definition: gps.c:31
float x
Definition: common_nav.h:37
#define NB_ANGLES
Definition: snav.c:121