32#include "generated/flight_plan.h"
33#include "generated/airframe.h"
39#if defined(USE_PARACHUTE) && USE_PARACHUTE == 1
41#ifndef GLUE_DEFINITIONS_STEP2
42#define GLUE_DEFINITIONS_STEP2(a, b) (a##b)
44#ifndef GLUE_DEFINITIONS
45#define GLUE_DEFINITIONS(a, b) GLUE_DEFINITIONS_STEP2(a, b)
48#if defined(PARACHUTE_SERVO_CHANNEL) && PARACHUTE_SERVO_CHANNEL != -1
49#define PARACHUTE_OUTPUT_COMMAND GLUE_DEFINITIONS(COMMAND_, PARACHUTE_SERVO_CHANNEL)
51#warning YOU HAVE NOT DEFINED A PARACHUTE RELEASE AUTOPILOT SERVO
57#ifndef PARACHUTE_TRIGGER_DELAY
58#define PARACHUTE_TRIGGER_DELAY 2.
61#ifndef PARACHUTE_DESCENT_RATE
62#define PARACHUTE_DESCENT_RATE 3.0
64#ifndef PARACHUTE_WIND_CORRECTION
65#define PARACHUTE_WIND_CORRECTION 1.0
67#ifndef PARACHUTE_LINE_LENGTH
68#define PARACHUTE_LINE_LENGTH 3.0
87 float foo1 = 0, foo2 = 0, foo3 = 0;
102#if defined(PARACHUTE_OUTPUT_COMMAND)
103 command_set(PARACHUTE_OUTPUT_COMMAND,
MIN_PPRZ);
106#if PERIODIC_TELEMETRY
117#if defined(PARACHUTE_OUTPUT_COMMAND)
120 command_set(PARACHUTE_OUTPUT_COMMAND,
MAX_PPRZ);
122 }
else { command_set(PARACHUTE_OUTPUT_COMMAND,
MIN_PPRZ); }
125#warning PARACHUTE COMMAND NOT FOUND
154 static bool init =
false;
155 static float circle_count = 0;
156 static bool wind_measurement_started =
false;
159 float estimator_hspeed_dir = 0;
160 float estimator_hspeed_mod = 0;
164 float speed_max_buf = 0;
165 float speed_min_buf = 1000.0;
166 float heading_angle_buf = 0;
173 speed_min_buf = 1000.;
174 heading_angle_buf = 0;
175 wind_measurement_started =
false;
187 if (wind_measurement_started ==
false) {
189 wind_measurement_started =
true;
191 if (estimator_hspeed_mod > speed_max_buf) {
192 speed_max_buf = estimator_hspeed_mod;
195 if (estimator_hspeed_mod < speed_min_buf) {
196 speed_min_buf = estimator_hspeed_mod;
197 heading_angle_buf = estimator_hspeed_dir;
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."
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.);
214 heading_angle_buf = RadOfDeg(FIXED_WIND_DIRECTION_FOR_TESTING);
239 float approach_dir = 0;
240 float calculated_wind_east = 0;
241 float calculated_wind_north = 0;
252 if (approach_dir > M_PI) { approach_dir -= 2 * M_PI; }
253 approach_dir -= M_PI;
260 float x_0 =
waypoints[wp_target].x - start_wp.x;
261 float y_0 =
waypoints[wp_target].y - start_wp.y;
264 float d = sqrt(x_0 * x_0 + y_0 * y_0);
struct pprz_autopilot autopilot
Global autopilot structure.
Core autopilot interface common to all firmwares.
Hardware independent code for commands handling.
float ground_alt
size == nb_waypoint, waypoint 0 is a dummy waypoint
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Handling of messages coming from ground and other A/Cs.
static struct uart_periph * dev
#define AP_MODE_MANUAL
AP modes.
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Fixedwing Navigation library.
#define GetPosAlt()
Get current altitude above MSL.
#define NavCircleWaypoint(wp, radius)
#define NavVerticalThrottleMode(_throttle)
Set the vertical mode to fixed throttle with the specified setpoint.
#define NavVerticalAltitudeMode(_alt, _pre_climb)
Set the vertical mode to altitude control with the specified altitude setpoint and climb pre-command.
#define NavVerticalAutoThrottleMode(_pitch)
Set the climb control to auto-throttle with the specified pitch pre-command.
API to get/set the generic vehicle states.
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
bool wind_measurements_valid
#define PARACHUTE_LINE_LENGTH
uint8_t DeployParachute(void)
void uav_recovery_periodic(void)
#define PARACHUTE_DESCENT_RATE
static void send_wind_info(struct transport_tx *trans, struct link_device *dev)
unit_t parachute_compute_approach(uint8_t baseleg, uint8_t release, uint8_t wp_target)
#define PARACHUTE_WIND_CORRECTION
uint8_t LockParachute(void)
float calculated_wind_dir
bool deploy_parachute_var
uint8_t calculate_wind_no_airspeed(uint8_t wp, float radius, float height)
float parachute_start_qdr
void uav_recovery_init(void)
float airborne_wind_speed
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.