Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
nav_drop.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 "modules/nav/nav_drop.h"
30 #include "state.h"
31 #include "generated/flight_plan.h"
32 #include "generated/airframe.h"
33 #include "modules/core/commands.h"
34 
35 
36 #if defined WP_RELEASE
38 #ifndef TRIGGER_DELAY
39 #define TRIGGER_DELAY 1.
40 #endif
41 
42 
43 #ifndef ALPHA
44 #define ALPHA 6.26e-5
45 #endif
46 #ifndef MASS
47 #define MASS 3.31e-3
48 #endif
49 #define ALPHA_M (ALPHA / MASS)
50 #define DT 0.1
51 #define MAX_STEPS 100
52 
54 #ifndef CLIMB_TIME
55 #define CLIMB_TIME 3
56 #endif
57 
59 #ifndef SAFE_CLIMB
60 #define SAFE_CLIMB 20
61 #endif
62 
66 #ifndef AIRSPEED_AT_RELEASE
67 #define AIRSPEED_AT_RELEASE 14.
68 #endif
69 
70 float nav_drop_trigger_delay = TRIGGER_DELAY;
71 float airspeed = AIRSPEED_AT_RELEASE;
72 float nav_drop_start_qdr;
73 
74 static float nav_drop_x, nav_drop_y, nav_drop_z;
75 static float nav_drop_vx, nav_drop_vy, nav_drop_vz;
76 
77 
78 static void integrate(uint8_t wp_target)
79 {
80  /* Inspired from Arnold Schroeter's code */
81  int i = 0;
82  while (nav_drop_z > 0. && i < MAX_STEPS) {
83  /* relative wind experienced by the ball (wind in NED frame) */
84  float airx = -nav_drop_vx + stateGetHorizontalWindspeed_f()->y;
85  float airy = -nav_drop_vy + stateGetHorizontalWindspeed_f()->x;
86  float airz = -nav_drop_vz;
87 
88  /* alpha / m * air */
89  float beta = ALPHA_M * sqrt(airx * airx + airy * airy + airz * airz);
90 
91  /* Euler integration */
92  nav_drop_vx += airx * beta * DT;
93  nav_drop_vy += airy * beta * DT;
94  nav_drop_vz += (airz * beta - NAV_GRAVITY) * DT;
95 
96  nav_drop_x += nav_drop_vx * DT;
97  nav_drop_y += nav_drop_vy * DT;
98  nav_drop_z += nav_drop_vz * DT;
99 
100  i++;
101  }
102 
103  if (nav_drop_z > 0.) {
104  /* Integration not finished -> approximation of the time with the current
105  speed */
106  float t = - nav_drop_z / nav_drop_vz;
107  nav_drop_x += nav_drop_vx * t;
108  nav_drop_y += nav_drop_vy * t;
109  }
110 
111  waypoints[WP_RELEASE].x = waypoints[wp_target].x - nav_drop_x;
112  waypoints[WP_RELEASE].y = waypoints[wp_target].y - nav_drop_y;
113 }
114 
115 
117 unit_t nav_drop_update_release(uint8_t wp_target)
118 {
119 
120  nav_drop_z = stateGetPositionUtm_f()->alt - waypoints[wp_target].a;
121  nav_drop_x = 0.;
122  nav_drop_y = 0.;
123 
126  nav_drop_vz = 0.;
127 
128  integrate(wp_target);
129 
130  return 0;
131 }
132 
133 
136 unit_t nav_drop_compute_approach(uint8_t wp_target, uint8_t wp_start, uint8_t wp_baseturn, uint8_t wp_climbout,
137  float nav_drop_radius)
138 {
139  waypoints[WP_RELEASE].a = waypoints[wp_start].a;
140  nav_drop_z = waypoints[WP_RELEASE].a - waypoints[wp_target].a;
141  nav_drop_x = 0.;
142  nav_drop_y = 0.;
143 
144  /* We approximate vx and vy, taking into account that RELEASE is next to
145  TARGET */
146  float x_0 = waypoints[wp_target].x - waypoints[wp_start].x;
147  float y_0 = waypoints[wp_target].y - waypoints[wp_start].y;
148 
149  /* Unit vector from START to TARGET */
150  float d = sqrt(x_0 * x_0 + y_0 * y_0);
151  float x1 = x_0 / d;
152  float y_1 = y_0 / d;
153 
154  waypoints[wp_baseturn].x = waypoints[wp_start].x + y_1 * nav_drop_radius;
155  waypoints[wp_baseturn].y = waypoints[wp_start].y - x1 * nav_drop_radius;
156  waypoints[wp_baseturn].a = waypoints[wp_start].a;
157  nav_drop_start_qdr = M_PI - atan2(-y_1, -x1);
158  if (nav_drop_radius < 0) {
159  nav_drop_start_qdr += M_PI;
160  }
161 
162  // wind in NED frame
163  if (stateIsAirspeedValid()) {
164  nav_drop_vx = x1 * Max(0, stateGetAirspeed_f()) + stateGetHorizontalWindspeed_f()->y;
165  nav_drop_vy = y_1 * Max(0, stateGetAirspeed_f()) + stateGetHorizontalWindspeed_f()->x;
166  } else {
167  // use approximate airspeed, initially set to AIRSPEED_AT_RELEASE
168  nav_drop_vx = x1 * airspeed + stateGetHorizontalWindspeed_f()->y;
169  nav_drop_vy = y_1 * airspeed + stateGetHorizontalWindspeed_f()->x;
170  }
171  nav_drop_vz = 0.;
172 
173  float vx0 = nav_drop_vx;
174  float vy0 = nav_drop_vy;
175 
176  integrate(wp_target);
177 
178  waypoints[wp_climbout].x = waypoints[WP_RELEASE].x + (CLIMB_TIME + CARROT) * vx0;
179  waypoints[wp_climbout].y = waypoints[WP_RELEASE].y + (CLIMB_TIME + CARROT) * vy0;
180  waypoints[wp_climbout].a = waypoints[WP_RELEASE].a + SAFE_CLIMB;
181 
182  return 0;
183 }
184 
185 
186 
187 unit_t nav_drop_shoot(void)
188 {
189  command_set(COMMAND_HATCH, MAX_PPRZ);
190  return 0;
191 }
192 
193 /* Compute start and end waypoints to be aligned on w1-w2 */
194 bool compute_alignment(uint8_t w1, uint8_t w2, uint8_t wp_before, uint8_t wp_after, float d_before, float d_after)
195 {
196  float x_0 = waypoints[w2].x - waypoints[w1].x;
197  float y_0 = waypoints[w2].y - waypoints[w1].y;
198 
199  /* Unit vector from W1 to W2 */
200  float d = sqrt(x_0 * x_0 + y_0 * y_0);
201  x_0 /= d;
202  y_0 /= d;
203 
204  waypoints[wp_before].x = waypoints[w1].x - d_before * x_0;
205  waypoints[wp_before].y = waypoints[w1].y - d_before * y_0;
206  waypoints[wp_after].x = waypoints[w2].x + d_after * x_0;
207  waypoints[wp_after].y = waypoints[w2].y + d_after * y_0;
208 
209  return false;
210 }
211 
212 #endif /* WP_RELEASE */
Hardware independent code for commands handling.
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:39
float y
Definition: common_nav.h:41
float a
Definition: common_nav.h:42
float x
Definition: common_nav.h:40
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:692
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:935
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:944
static bool stateIsAirspeedValid(void)
test if air speed is available.
Definition: state.h:1244
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
#define NAV_GRAVITY
Definition: nav.h:52
Navigation module to drop a ball at a given point taking into account the wind and ground speed.
unit_t nav_drop_shoot(void)
unit_t nav_drop_compute_approach(uint8_t wp_target, uint8_t wp_start, uint8_t wp_baseturn, uint8_t wp_climbout, float radius)
float nav_drop_trigger_delay
float nav_drop_start_qdr
Definition: nav_drop.h:39
bool compute_alignment(uint8_t w1, uint8_t w2, uint8_t start, uint8_t end, float d_before, float d_after)
unit_t nav_drop_update_release(uint8_t wp_target)
#define MAX_PPRZ
Definition: paparazzi.h:8
float alt
in meters (above WGS84 reference ellipsoid or above MSL)
#define CARROT
default approaching_time for a wp
Definition: navigation.h:70
API to get/set the generic vehicle states.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98