Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
bomb.c
Go to the documentation of this file.
1 /*
2  * $Id$
3  *
4  * Copyright (C) 2007-2009 ENAC, Pascal Brisset, Antoine Drouin
5  *
6  * This file is part of paparazzi.
7  *
8  * paparazzi is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2, or (at your option)
11  * any later version.
12  *
13  * paparazzi is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with paparazzi; see the file COPYING. If not, write to
20  * the Free Software Foundation, 59 Temple Place - Suite 330,
21  * Boston, MA 02111-1307, USA.
22  *
23  */
24 
25 #include "estimator.h"
26 #include "subsystems/nav.h"
28 #include "generated/flight_plan.h"
29 #include "generated/airframe.h"
30 #include "inter_mcu.h"
31 
32 
33 #if defined WP_RELEASE
34 
35 #ifndef TRIGGER_DELAY
36 #define TRIGGER_DELAY 1.
37 #endif
38 
39 #define ALPHA 6.26e-5
40 #define MASS 3.31e-3
41 #define ALPHA_M (ALPHA / MASS)
42 #define DT 0.1
43 #define MAX_STEPS 100
44 
45 float bomb_trigger_delay = TRIGGER_DELAY;
46 float airspeed = 14.;
47 float bomb_start_qdr;
48 
49 #define CLIMB_TIME 3 /* s */
50 #define SAFE_CLIMB 20 /* m */
51 
52 static float bomb_x, bomb_y, bomb_z;
53 static float bomb_vx, bomb_vy, bomb_vz;
54 
55 
56 static void integrate( uint8_t wp_target ) {
57  /* Inspired from Arnold Schroeter's code */
58  int i = 0;
59  while (bomb_z > 0. && i < MAX_STEPS) {
60  /* relative wind experienced by the ball */
61  float airx = -bomb_vx + wind_east;
62  float airy = -bomb_vy + wind_north;
63  float airz = -bomb_vz;
64 
65  /* alpha / m * air */
66  float beta = ALPHA_M * sqrt(airx*airx+airy*airy+airz*airz);
67 
68  /* Euler integration */
69  bomb_vx += airx * beta * DT;
70  bomb_vy += airy * beta * DT;
71  bomb_vz += (airz * beta - G) * DT;
72 
73  bomb_x += bomb_vx * DT;
74  bomb_y += bomb_vy * DT;
75  bomb_z += bomb_vz * DT;
76 
77  i++;
78  }
79 
80  if (bomb_z > 0.) {
81  /* Integration not finished -> approximation of the time with the current
82  speed */
83  float t = - bomb_z / bomb_vz;
84  bomb_x += bomb_vx * t;
85  bomb_y += bomb_vy * t;
86  }
87 
88  waypoints[WP_RELEASE].x = waypoints[wp_target].x - bomb_x;
89  waypoints[WP_RELEASE].y = waypoints[wp_target].y - bomb_y;
90 }
91 
92 
94 unit_t bomb_update_release( uint8_t wp_target ) {
95 
96  bomb_z = estimator_z - waypoints[wp_target].a;
97  bomb_x = 0.;
98  bomb_y = 0.;
99 
102  bomb_vz = 0.;
103 
104  integrate(wp_target);
105 
106  return 0;
107 }
108 
109 
112 unit_t bomb_compute_approach( uint8_t wp_target, uint8_t wp_start, float bomb_radius ) {
113  waypoints[WP_RELEASE].a = waypoints[wp_start].a;
114  bomb_z = waypoints[WP_RELEASE].a - waypoints[wp_target].a;
115  bomb_x = 0.;
116  bomb_y = 0.;
117 
118  /* We approximate vx and vy, taking into account that RELEASE is next to
119  TARGET */
120  float x_0 = waypoints[wp_target].x - waypoints[wp_start].x;
121  float y_0 = waypoints[wp_target].y - waypoints[wp_start].y;
122 
123  /* Unit vector from START to TARGET */
124  float d = sqrt(x_0*x_0+y_0*y_0);
125  float x1 = x_0 / d;
126  float y_1 = y_0 / d;
127 
128  waypoints[WP_BASELEG].x = waypoints[wp_start].x + y_1 * bomb_radius;
129  waypoints[WP_BASELEG].y = waypoints[wp_start].y - x1 * bomb_radius;
130  waypoints[WP_BASELEG].a = waypoints[wp_start].a;
131  bomb_start_qdr = M_PI - atan2(-y_1, -x1);
132  if (bomb_radius < 0)
133  bomb_start_qdr += M_PI;
134 
135  bomb_vx = x1 * airspeed + wind_east;
136  bomb_vy = y_1 * airspeed + wind_north;
137  bomb_vz = 0.;
138 
139  float vx0 = bomb_vx;
140  float vy0 = bomb_vy;
141 
142  integrate(wp_target);
143 
144  waypoints[WP_CLIMB].x = waypoints[WP_RELEASE].x + (CLIMB_TIME + CARROT) * vx0;
145  waypoints[WP_CLIMB].y = waypoints[WP_RELEASE].y + (CLIMB_TIME + CARROT) * vy0;
146  waypoints[WP_CLIMB].a = waypoints[WP_RELEASE].a + SAFE_CLIMB;
147 
148  return 0;
149 }
150 
151 
152 
153 unit_t bomb_shoot( void ) {
154  ap_state->commands[COMMAND_HATCH] = MAX_PPRZ;
155  return 0;
156 }
157 
158 /* Compute start and end waypoints to be aligned on w1-w2 */
159 bool_t compute_alignment(uint8_t w1, uint8_t w2, uint8_t wp_before, uint8_t wp_after, float d_before, float d_after) {
160  float x_0 = waypoints[w2].x - waypoints[w1].x;
161  float y_0 = waypoints[w2].y - waypoints[w1].y;
162 
163  /* Unit vector from W1 to W2 */
164  float d = sqrt(x_0*x_0+y_0*y_0);
165  x_0 /= d;
166  y_0 /= d;
167 
168  waypoints[wp_before].x = waypoints[w1].x - d_before * x_0;
169  waypoints[wp_before].y = waypoints[w1].y - d_before * y_0;
170  waypoints[wp_after].x = waypoints[w2].x + d_after * x_0;
171  waypoints[wp_after].y = waypoints[w2].y + d_after * y_0;
172 
173  return FALSE;
174 }
175 
176 #endif /* WP_RELEASE */
float estimator_hspeed_mod
module of horizontal ground speed in m/s
Definition: estimator.c:64
bool_t compute_alignment(uint8_t w1, uint8_t w2, uint8_t start, uint8_t end, float d_before, float d_after)
float estimator_hspeed_dir
direction of horizontal ground speed in rad (CW/North)
Definition: estimator.c:65
#define FALSE
Definition: imu_chimu.h:141
struct ap_state * ap_state
Definition: inter_mcu.c:35
unit_t bomb_update_release(uint8_t wp_target)
float bomb_start_qdr
float wind_east
Definition: estimator.c:68
unit_t bomb_shoot(void)
unsigned char uint8_t
Definition: types.h:14
float estimator_z
altitude above MSL in meters
Definition: estimator.c:44
State estimation, fusioning sensors.
float wind_north
Definition: estimator.c:68
float bomb_trigger_delay
#define MAX_PPRZ
Definition: paparazzi.h:8
unit_t bomb_compute_approach(uint8_t wp_target, uint8_t wp_start, float radius)