Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
uav_recovery.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2013-2020 Chris Efstathiou hendrixgr@gmail.com
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 
28 #include "autopilot.h"
29 #include "state.h"
31 #include "modules/nav/common_nav.h"
32 #include "generated/flight_plan.h"
33 #include "generated/airframe.h"
36 #include "modules/core/commands.h"
37 #include "uav_recovery.h"
38 
39 #if defined(USE_PARACHUTE) && USE_PARACHUTE == 1
40 
41 #ifndef GLUE_DEFINITIONS_STEP2
42 #define GLUE_DEFINITIONS_STEP2(a, b) (a##b)
43 #endif
44 #ifndef GLUE_DEFINITIONS
45 #define GLUE_DEFINITIONS(a, b) GLUE_DEFINITIONS_STEP2(a, b)
46 #endif
47 
48 #if defined(PARACHUTE_SERVO_CHANNEL) && PARACHUTE_SERVO_CHANNEL != -1
49 #define PARACHUTE_OUTPUT_COMMAND GLUE_DEFINITIONS(COMMAND_, PARACHUTE_SERVO_CHANNEL)
50 #else
51 #warning YOU HAVE NOT DEFINED A PARACHUTE RELEASE AUTOPILOT SERVO
52 #endif
53 
54 #endif
55 
57 #ifndef PARACHUTE_TRIGGER_DELAY
58 #define PARACHUTE_TRIGGER_DELAY 2.
59 #endif
60 
61 #ifndef PARACHUTE_DESCENT_RATE
62 #define PARACHUTE_DESCENT_RATE 3.0
63 #endif
64 #ifndef PARACHUTE_WIND_CORRECTION
65 #define PARACHUTE_WIND_CORRECTION 1.0
66 #endif
67 #ifndef PARACHUTE_LINE_LENGTH
68 #define PARACHUTE_LINE_LENGTH 3.0
69 #endif
70 
77 bool wind_info_valid = false;
79 bool land_direction = 0;
80 
81 #if PERIODIC_TELEMETRY
83 
84 static void send_wind_info(struct transport_tx *trans, struct link_device *dev)
85 {
86 
87  float foo1 = 0, foo2 = 0, foo3 = 0;
88  pprz_msg_send_WEATHER(trans, dev, AC_ID, &foo1, &foo2, &airborne_wind_speed, &airborne_wind_dir, &foo3);
89 
90  return;
91 }
92 #endif
93 
95 {
96 
101  wind_info_valid = true;
102 #if defined(PARACHUTE_OUTPUT_COMMAND)
103  command_set(PARACHUTE_OUTPUT_COMMAND, MIN_PPRZ);
104 #endif
105 
106 #if PERIODIC_TELEMETRY
108 #endif
109 
110 
111  return;
112 }
113 
114 
116 {
117 #if defined(PARACHUTE_OUTPUT_COMMAND)
118  if (autopilot.mode != AP_MODE_MANUAL) {
119  if (deploy_parachute_var == 1) {
120  command_set(PARACHUTE_OUTPUT_COMMAND, MAX_PPRZ);
121 
122  } else { command_set(PARACHUTE_OUTPUT_COMMAND, MIN_PPRZ); }
123  }
124 #else
125 #warning PARACHUTE COMMAND NOT FOUND
126 #endif
127 
128  return;
129 }
130 
132 {
133 
135 
136  return (0);
137 }
138 
140 {
141 
143 
144  return (0);
145 }
146 
147 
148 uint8_t calculate_wind_no_airspeed(uint8_t wp, float radius, float height)
149 {
150 
151 //struct EnuCoor_f* pos = stateGetPositionEnu_f();
152 //struct UtmCoor_f * pos = stateGetPositionUtm_f();
153 
154  static bool init = false;
155  static float circle_count = 0;
156  static bool wind_measurement_started = false;
157 
158 //Legacy estimator values
159  float estimator_hspeed_dir = 0;
160  float estimator_hspeed_mod = 0;
161  estimator_hspeed_dir = stateGetHorizontalSpeedDir_f();
162  estimator_hspeed_mod = stateGetHorizontalSpeedNorm_f();
163 
164  float speed_max_buf = 0;
165  float speed_min_buf = 1000.0;
166  float heading_angle_buf = 0;
167 
168  if (init == false) {
169  airborne_wind_dir = 0;
171  wind_measurements_valid = false;
172  speed_max_buf = 0;
173  speed_min_buf = 1000.;
174  heading_angle_buf = 0;
175  wind_measurement_started = false;
176  init = true;
177  }
178 
179  NavVerticalAutoThrottleMode(RadOfDeg(0.000000));
180  NavVerticalAltitudeMode(Height(height), 0.);
181  NavCircleWaypoint(wp, radius);
182  if (nav_in_circle == false || GetPosAlt() < Height(height - 10)) {
183  return (true);
184  }
185  NavVerticalThrottleMode(MAX_PPRZ * (float)V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE);
186  // Wait until we are in altitude and in circle.
187  if (wind_measurement_started == false) {
188  circle_count = NavCircleCount();
189  wind_measurement_started = true;
190  }
191  if (estimator_hspeed_mod > speed_max_buf) {
192  speed_max_buf = estimator_hspeed_mod;
193  }
194  // WHEN THE GROUND SPEED IS LOWEST THIS IS WHERE THE WIND IS BLOWING FROM.
195  if (estimator_hspeed_mod < speed_min_buf) {
196  speed_min_buf = estimator_hspeed_mod;
197  heading_angle_buf = estimator_hspeed_dir; //180 to -180 in radians, 0 is to the NORTH!
198  }
199 
200  if ((NavCircleCount() - circle_count) >= 1.1) {
201  //Update the wind direction and speed in the WEATHER message.
202  airborne_wind_speed = ((speed_max_buf - speed_min_buf) / 2) * AIRBORNE_WIND_CORRECTION;
203 #if defined(FIXED_WIND_SPEED_FOR_TESTING) && defined(SITL)
204 #pragma message "Wind SPEED for UAV recovery is fixed for the simulation"
205 #pragma message "You can change the value by editing 'uav_recovery.xml' file."
206  airborne_wind_speed = FIXED_WIND_SPEED_FOR_TESTING; // FOR TESTING IN SIMULATION ONLY!
207 #endif
208 #if defined(FIXED_WIND_DIRECTION_FOR_TESTING) && defined(SITL)
209 #pragma message "Wind direction for UAV recovery is fixed for the simulation"
210 #pragma message "You can change the value by editing 'uav_recovery.xml' file."
211  if (FIXED_WIND_DIRECTION_FOR_TESTING > 180.) {
212  heading_angle_buf = RadOfDeg(FIXED_WIND_DIRECTION_FOR_TESTING - 360.);
213  } else {
214  heading_angle_buf = RadOfDeg(FIXED_WIND_DIRECTION_FOR_TESTING);
215  }
216 #endif
217  airborne_wind_dir = DegOfRad(heading_angle_buf); //For use with the WEATHER message
218  //Convert from 180, -180 to 0, 360 in Degrees. 0=NORTH, 90=EAST
219  if (airborne_wind_dir < 0) { airborne_wind_dir += 360; }
220  calculated_wind_dir = heading_angle_buf;
222  wind_info_valid = true;
223 
224  //NavVerticalAutoThrottleMode(RadOfDeg(0.000000));
225  // EXIT and continue the flight plan.
226  init = false;
227  return (false);
228  }
229 
230  return (true);
231 }
232 
233 
234 // Compute an approximation for the RELEASE waypoint from expected descent and altitude
235 unit_t parachute_compute_approach(uint8_t baseleg, uint8_t release, uint8_t wp_target)
236 {
237 
238  float x = 0, y = 0;
239  float approach_dir = 0;
240  float calculated_wind_east = 0;
241  float calculated_wind_north = 0;
242 
243  struct {
244  float x;
245  float y;
246  float a;
247  } start_wp;
248 
249  // Calculate a starting waypoint opposite to the wind.
250  //180 TO -180 in Radians. Convert from 0 = NORTH to 0 = EAST
251  approach_dir = RadOfDeg(90.) - calculated_wind_dir;
252  if (approach_dir > M_PI) { approach_dir -= 2 * M_PI; }
253  approach_dir -= M_PI; // reverse the direction as we must release the parachute against the wind!
254 
255  start_wp.x = (cos(approach_dir) * nav_radius) + waypoints[wp_target].x;
256  start_wp.y = (sin(approach_dir) * nav_radius) + waypoints[wp_target].y;
257  start_wp.a = waypoints[release].a;
258 
259  // We approximate vx and vy, taking into account that RELEASE is close to the TARGET
260  float x_0 = waypoints[wp_target].x - start_wp.x;
261  float y_0 = waypoints[wp_target].y - start_wp.y;
262 
263  // Unit vector from the calculated starting waypoint to TARGET
264  float d = sqrt(x_0 * x_0 + y_0 * y_0);
265  float x1 = x_0 / d;
266  float y_1 = y_0 / d;
267 
268  waypoints[baseleg].x = start_wp.x + y_1 * nav_radius;
269  waypoints[baseleg].y = start_wp.y - x1 * nav_radius;
270  waypoints[baseleg].a = start_wp.a;
271  parachute_start_qdr = M_PI - atan2(-y_1, -x1);
272  if (nav_radius < 0) {
273  parachute_start_qdr += M_PI;
274  }
275 
277 
278  calculated_wind_north = cosf(calculated_wind_dir) * airborne_wind_speed;
279  calculated_wind_east = sinf(calculated_wind_dir) * airborne_wind_speed;
280  x = (parachute_z / PARACHUTE_DESCENT_RATE) * calculated_wind_east * PARACHUTE_WIND_CORRECTION;
281  y = (parachute_z / PARACHUTE_DESCENT_RATE) * calculated_wind_north * PARACHUTE_WIND_CORRECTION;
282 
283  waypoints[release].x = (waypoints[wp_target].x + x);
284  waypoints[release].y = waypoints[wp_target].y + y;
285 
286 
287  return 0;
288 }
289 
struct pprz_autopilot autopilot
Global autopilot structure.
Definition: autopilot.c:49
Core autopilot interface common to all firmwares.
uint8_t mode
current autopilot mode
Definition: autopilot.h:63
Hardware independent code for commands handling.
float ground_alt
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:41
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
#define Height(_h)
Definition: common_nav.h:49
#define AP_MODE_MANUAL
AP modes.
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
float nav_radius
Definition: nav.c:56
bool nav_in_circle
Definition: nav.c:66
Fixedwing Navigation library.
#define GetPosAlt()
Get current altitude above MSL.
Definition: nav.h:232
#define NavCircleWaypoint(wp, radius)
Definition: nav.h:151
#define NavVerticalThrottleMode(_throttle)
Set the vertical mode to fixed throttle with the specified setpoint.
Definition: nav.h:204
#define NavCircleCount()
Definition: nav.h:155
#define NavVerticalAltitudeMode(_alt, _pre_climb)
Set the vertical mode to altitude control with the specified altitude setpoint and climb pre-command.
Definition: nav.h:191
#define NavVerticalAutoThrottleMode(_pitch)
Set the climb control to auto-throttle with the specified pitch pre-command.
Definition: nav.h:177
bool init
Definition: nav_gls.c:57
#define MAX_PPRZ
Definition: paparazzi.h:8
#define MIN_PPRZ
Definition: paparazzi.h:9
API to get/set the generic vehicle states.
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
bool wind_measurements_valid
Definition: uav_recovery.c:76
#define PARACHUTE_LINE_LENGTH
Definition: uav_recovery.c:68
uint8_t DeployParachute(void)
Definition: uav_recovery.c:139
void uav_recovery_periodic(void)
Definition: uav_recovery.c:115
#define PARACHUTE_DESCENT_RATE
Definition: uav_recovery.c:62
float airborne_wind_dir
Definition: uav_recovery.c:73
static void send_wind_info(struct transport_tx *trans, struct link_device *dev)
Definition: uav_recovery.c:84
bool land_direction
Definition: uav_recovery.c:79
bool wind_info_valid
Definition: uav_recovery.c:77
unit_t parachute_compute_approach(uint8_t baseleg, uint8_t release, uint8_t wp_target)
Definition: uav_recovery.c:235
#define PARACHUTE_WIND_CORRECTION
Definition: uav_recovery.c:65
uint8_t LockParachute(void)
Definition: uav_recovery.c:131
float calculated_wind_dir
Definition: uav_recovery.c:75
bool deploy_parachute_var
Definition: uav_recovery.c:78
float parachute_z
Definition: uav_recovery.c:72
uint8_t calculate_wind_no_airspeed(uint8_t wp, float radius, float height)
Definition: uav_recovery.c:148
float parachute_start_qdr
Definition: uav_recovery.c:71
void uav_recovery_init(void)
Definition: uav_recovery.c:94
float airborne_wind_speed
Definition: uav_recovery.c:74
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98