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
bomb.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 
27 #include "state.h"
28 #include "subsystems/nav.h"
30 #include "generated/flight_plan.h"
31 #include "generated/airframe.h"
32 #include "inter_mcu.h"
33 
34 
35 #if defined WP_RELEASE
36 
37 #ifndef TRIGGER_DELAY
38 #define TRIGGER_DELAY 1.
39 #endif
40 
41 #define ALPHA 6.26e-5
42 #define MASS 3.31e-3
43 #define ALPHA_M (ALPHA / MASS)
44 #define DT 0.1
45 #define MAX_STEPS 100
46 
47 float bomb_trigger_delay = TRIGGER_DELAY;
48 float airspeed = 14.;
49 float bomb_start_qdr;
50 
51 #define CLIMB_TIME 3 /* s */
52 #define SAFE_CLIMB 20 /* m */
53 
54 static float bomb_x, bomb_y, bomb_z;
55 static float bomb_vx, bomb_vy, bomb_vz;
56 
57 
58 static void integrate( uint8_t wp_target ) {
59  /* Inspired from Arnold Schroeter's code */
60  int i = 0;
61  while (bomb_z > 0. && i < MAX_STEPS) {
62  /* relative wind experienced by the ball (wind in NED frame) */
63  float airx = -bomb_vx + stateGetHorizontalWindspeed_f()->y;
64  float airy = -bomb_vy + stateGetHorizontalWindspeed_f()->x;
65  float airz = -bomb_vz;
66 
67  /* alpha / m * air */
68  float beta = ALPHA_M * sqrt(airx*airx+airy*airy+airz*airz);
69 
70  /* Euler integration */
71  bomb_vx += airx * beta * DT;
72  bomb_vy += airy * beta * DT;
73  bomb_vz += (airz * beta - G) * DT;
74 
75  bomb_x += bomb_vx * DT;
76  bomb_y += bomb_vy * DT;
77  bomb_z += bomb_vz * DT;
78 
79  i++;
80  }
81 
82  if (bomb_z > 0.) {
83  /* Integration not finished -> approximation of the time with the current
84  speed */
85  float t = - bomb_z / bomb_vz;
86  bomb_x += bomb_vx * t;
87  bomb_y += bomb_vy * t;
88  }
89 
90  waypoints[WP_RELEASE].x = waypoints[wp_target].x - bomb_x;
91  waypoints[WP_RELEASE].y = waypoints[wp_target].y - bomb_y;
92 }
93 
94 
96 unit_t bomb_update_release( uint8_t wp_target ) {
97 
98  bomb_z = stateGetPositionUtm_f()->alt - waypoints[wp_target].a;
99  bomb_x = 0.;
100  bomb_y = 0.;
101 
104  bomb_vz = 0.;
105 
106  integrate(wp_target);
107 
108  return 0;
109 }
110 
111 
114 unit_t bomb_compute_approach( uint8_t wp_target, uint8_t wp_start, float bomb_radius ) {
115  waypoints[WP_RELEASE].a = waypoints[wp_start].a;
116  bomb_z = waypoints[WP_RELEASE].a - waypoints[wp_target].a;
117  bomb_x = 0.;
118  bomb_y = 0.;
119 
120  /* We approximate vx and vy, taking into account that RELEASE is next to
121  TARGET */
122  float x_0 = waypoints[wp_target].x - waypoints[wp_start].x;
123  float y_0 = waypoints[wp_target].y - waypoints[wp_start].y;
124 
125  /* Unit vector from START to TARGET */
126  float d = sqrt(x_0*x_0+y_0*y_0);
127  float x1 = x_0 / d;
128  float y_1 = y_0 / d;
129 
130  waypoints[WP_BASELEG].x = waypoints[wp_start].x + y_1 * bomb_radius;
131  waypoints[WP_BASELEG].y = waypoints[wp_start].y - x1 * bomb_radius;
132  waypoints[WP_BASELEG].a = waypoints[wp_start].a;
133  bomb_start_qdr = M_PI - atan2(-y_1, -x1);
134  if (bomb_radius < 0)
135  bomb_start_qdr += M_PI;
136 
137  // wind in NED frame
138  bomb_vx = x1 * airspeed + stateGetHorizontalWindspeed_f()->y;
139  bomb_vy = y_1 * airspeed + stateGetHorizontalWindspeed_f()->x;
140  bomb_vz = 0.;
141 
142  float vx0 = bomb_vx;
143  float vy0 = bomb_vy;
144 
145  integrate(wp_target);
146 
147  waypoints[WP_CLIMB].x = waypoints[WP_RELEASE].x + (CLIMB_TIME + CARROT) * vx0;
148  waypoints[WP_CLIMB].y = waypoints[WP_RELEASE].y + (CLIMB_TIME + CARROT) * vy0;
149  waypoints[WP_CLIMB].a = waypoints[WP_RELEASE].a + SAFE_CLIMB;
150 
151  return 0;
152 }
153 
154 
155 
156 unit_t bomb_shoot( void ) {
157  ap_state->commands[COMMAND_HATCH] = MAX_PPRZ;
158  return 0;
159 }
160 
161 /* Compute start and end waypoints to be aligned on w1-w2 */
162 bool_t compute_alignment(uint8_t w1, uint8_t w2, uint8_t wp_before, uint8_t wp_after, float d_before, float d_after) {
163  float x_0 = waypoints[w2].x - waypoints[w1].x;
164  float y_0 = waypoints[w2].y - waypoints[w1].y;
165 
166  /* Unit vector from W1 to W2 */
167  float d = sqrt(x_0*x_0+y_0*y_0);
168  x_0 /= d;
169  y_0 /= d;
170 
171  waypoints[wp_before].x = waypoints[w1].x - d_before * x_0;
172  waypoints[wp_before].y = waypoints[w1].y - d_before * y_0;
173  waypoints[wp_after].x = waypoints[w2].x + d_after * x_0;
174  waypoints[wp_after].y = waypoints[w2].y + d_after * y_0;
175 
176  return FALSE;
177 }
178 
179 #endif /* WP_RELEASE */
static float * stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:861
bool_t compute_alignment(uint8_t w1, uint8_t w2, uint8_t start, uint8_t end, float d_before, float d_after)
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
unit_t bomb_update_release(uint8_t wp_target)
float bomb_start_qdr
unit_t bomb_shoot(void)
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
float bomb_trigger_delay
int32_t y
North.
#define MAX_PPRZ
Definition: paparazzi.h:8
unit_t bomb_compute_approach(uint8_t wp_target, uint8_t wp_start, float radius)
float alt
in meters above WGS84 reference ellipsoid
int32_t x
East.