Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 "inter_mcu.h"
34 
35 
36 #if defined WP_RELEASE
37 
38 #ifndef TRIGGER_DELAY
39 #define TRIGGER_DELAY 1.
40 #endif
41 
42 #ifndef ALPHA
43 #define ALPHA 6.26e-5
44 #endif
45 #ifndef MASS
46 #define MASS 3.31e-3
47 #endif
48 #define ALPHA_M (ALPHA / MASS)
49 #define DT 0.1
50 #define MAX_STEPS 100
51 
53 #ifndef CLIMB_TIME
54 #define CLIMB_TIME 3
55 #endif
56 
58 #ifndef SAFE_CLIMB
59 #define SAFE_CLIMB 20
60 #endif
61 
65 #ifndef AIRSPEED_AT_RELEASE
66 #define AIRSPEED_AT_RELEASE 14.
67 #endif
68 
69 float nav_drop_trigger_delay = TRIGGER_DELAY;
70 float airspeed = AIRSPEED_AT_RELEASE;
71 float nav_drop_start_qdr;
72 
73 static float nav_drop_x, nav_drop_y, nav_drop_z;
74 static float nav_drop_vx, nav_drop_vy, nav_drop_vz;
75 
76 
77 static void integrate( uint8_t wp_target ) {
78  /* Inspired from Arnold Schroeter's code */
79  int i = 0;
80  while (nav_drop_z > 0. && i < MAX_STEPS) {
81  /* relative wind experienced by the ball (wind in NED frame) */
82  float airx = -nav_drop_vx + stateGetHorizontalWindspeed_f()->y;
83  float airy = -nav_drop_vy + stateGetHorizontalWindspeed_f()->x;
84  float airz = -nav_drop_vz;
85 
86  /* alpha / m * air */
87  float beta = ALPHA_M * sqrt(airx*airx+airy*airy+airz*airz);
88 
89  /* Euler integration */
90  nav_drop_vx += airx * beta * DT;
91  nav_drop_vy += airy * beta * DT;
92  nav_drop_vz += (airz * beta - G) * DT;
93 
94  nav_drop_x += nav_drop_vx * DT;
95  nav_drop_y += nav_drop_vy * DT;
96  nav_drop_z += nav_drop_vz * DT;
97 
98  i++;
99  }
100 
101  if (nav_drop_z > 0.) {
102  /* Integration not finished -> approximation of the time with the current
103  speed */
104  float t = - nav_drop_z / nav_drop_vz;
105  nav_drop_x += nav_drop_vx * t;
106  nav_drop_y += nav_drop_vy * t;
107  }
108 
109  waypoints[WP_RELEASE].x = waypoints[wp_target].x - nav_drop_x;
110  waypoints[WP_RELEASE].y = waypoints[wp_target].y - nav_drop_y;
111 }
112 
113 
115 unit_t nav_drop_update_release( uint8_t wp_target ) {
116 
117  nav_drop_z = stateGetPositionUtm_f()->alt - waypoints[wp_target].a;
118  nav_drop_x = 0.;
119  nav_drop_y = 0.;
120 
121  nav_drop_vx = (*stateGetHorizontalSpeedNorm_f()) * sin((*stateGetHorizontalSpeedDir_f()));
122  nav_drop_vy = (*stateGetHorizontalSpeedNorm_f()) * cos((*stateGetHorizontalSpeedDir_f()));
123  nav_drop_vz = 0.;
124 
125  integrate(wp_target);
126 
127  return 0;
128 }
129 
130 
133 unit_t nav_drop_compute_approach( uint8_t wp_target, uint8_t wp_start, float nav_drop_radius ) {
134  waypoints[WP_RELEASE].a = waypoints[wp_start].a;
135  nav_drop_z = waypoints[WP_RELEASE].a - waypoints[wp_target].a;
136  nav_drop_x = 0.;
137  nav_drop_y = 0.;
138 
139  /* We approximate vx and vy, taking into account that RELEASE is next to
140  TARGET */
141  float x_0 = waypoints[wp_target].x - waypoints[wp_start].x;
142  float y_0 = waypoints[wp_target].y - waypoints[wp_start].y;
143 
144  /* Unit vector from START to TARGET */
145  float d = sqrt(x_0*x_0+y_0*y_0);
146  float x1 = x_0 / d;
147  float y_1 = y_0 / d;
148 
149  waypoints[WP_BASELEG].x = waypoints[wp_start].x + y_1 * nav_drop_radius;
150  waypoints[WP_BASELEG].y = waypoints[wp_start].y - x1 * nav_drop_radius;
151  waypoints[WP_BASELEG].a = waypoints[wp_start].a;
152  nav_drop_start_qdr = M_PI - atan2(-y_1, -x1);
153  if (nav_drop_radius < 0)
154  nav_drop_start_qdr += M_PI;
155 
156  // wind in NED frame
157  if (stateIsAirspeedValid()) {
158  nav_drop_vx = x1 * *stateGetAirspeed_f() + stateGetHorizontalWindspeed_f()->y;
159  nav_drop_vy = y_1 * *stateGetAirspeed_f() + stateGetHorizontalWindspeed_f()->x;
160  }
161  else {
162  // use approximate airspeed, initially set to AIRSPEED_AT_RELEASE
163  nav_drop_vx = x1 * airspeed + stateGetHorizontalWindspeed_f()->y;
164  nav_drop_vy = y_1 * airspeed + stateGetHorizontalWindspeed_f()->x;
165  }
166  nav_drop_vz = 0.;
167 
168  float vx0 = nav_drop_vx;
169  float vy0 = nav_drop_vy;
170 
171  integrate(wp_target);
172 
173  waypoints[WP_CLIMB].x = waypoints[WP_RELEASE].x + (CLIMB_TIME + CARROT) * vx0;
174  waypoints[WP_CLIMB].y = waypoints[WP_RELEASE].y + (CLIMB_TIME + CARROT) * vy0;
175  waypoints[WP_CLIMB].a = waypoints[WP_RELEASE].a + SAFE_CLIMB;
176 
177  return 0;
178 }
179 
180 
181 
182 unit_t nav_drop_shoot( void ) {
183  ap_state->commands[COMMAND_HATCH] = MAX_PPRZ;
184  return 0;
185 }
186 
187 /* Compute start and end waypoints to be aligned on w1-w2 */
188 bool_t compute_alignment(uint8_t w1, uint8_t w2, uint8_t wp_before, uint8_t wp_after, float d_before, float d_after) {
189  float x_0 = waypoints[w2].x - waypoints[w1].x;
190  float y_0 = waypoints[w2].y - waypoints[w1].y;
191 
192  /* Unit vector from W1 to W2 */
193  float d = sqrt(x_0*x_0+y_0*y_0);
194  x_0 /= d;
195  y_0 /= d;
196 
197  waypoints[wp_before].x = waypoints[w1].x - d_before * x_0;
198  waypoints[wp_before].y = waypoints[w1].y - d_before * y_0;
199  waypoints[wp_after].x = waypoints[w2].x + d_after * x_0;
200  waypoints[wp_after].y = waypoints[w2].y + d_after * y_0;
201 
202  return FALSE;
203 }
204 
205 #endif /* WP_RELEASE */
static float * stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:861
static float * stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:854
#define FALSE
Definition: imu_chimu.h:141
struct ap_state * ap_state
Definition: inter_mcu.c:33
static float * stateGetAirspeed_f(void)
Get airspeed (float).
Definition: state.h:1199
static bool_t stateIsAirspeedValid(void)
test if air speed is available.
Definition: state.h:1111
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:651
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:1192
int32_t y
North.
#define MAX_PPRZ
Definition: paparazzi.h:8
float alt
in meters above WGS84 reference ellipsoid
int32_t x
East.