31 #include "generated/flight_plan.h"
32 #include "generated/airframe.h"
36 #if defined WP_RELEASE
39 #define TRIGGER_DELAY 1.
49 #define ALPHA_M (ALPHA / MASS)
66 #ifndef AIRSPEED_AT_RELEASE
67 #define AIRSPEED_AT_RELEASE 14.
71 float airspeed = AIRSPEED_AT_RELEASE;
74 static float nav_drop_x, nav_drop_y, nav_drop_z;
75 static float nav_drop_vx, nav_drop_vy, nav_drop_vz;
78 static void integrate(
uint8_t wp_target)
82 while (nav_drop_z > 0. && i < MAX_STEPS) {
86 float airz = -nav_drop_vz;
89 float beta = ALPHA_M * sqrt(airx * airx + airy * airy + airz * airz);
92 nav_drop_vx += airx * beta * DT;
93 nav_drop_vy += airy * beta * DT;
96 nav_drop_x += nav_drop_vx * DT;
97 nav_drop_y += nav_drop_vy * DT;
98 nav_drop_z += nav_drop_vz * DT;
103 if (nav_drop_z > 0.) {
106 float t = - nav_drop_z / nav_drop_vz;
107 nav_drop_x += nav_drop_vx * t;
108 nav_drop_y += nav_drop_vy * t;
128 integrate(wp_target);
137 float nav_drop_radius)
150 float d = sqrt(x_0 * x_0 + y_0 * y_0);
158 if (nav_drop_radius < 0) {
173 float vx0 = nav_drop_vx;
174 float vy0 = nav_drop_vy;
176 integrate(wp_target);
189 command_set(COMMAND_HATCH,
MAX_PPRZ);
200 float d = sqrt(x_0 * x_0 + y_0 * y_0);
Hardware independent code for commands handling.
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
static bool stateIsAirspeedValid(void)
test if air speed is available.
static float stateGetAirspeed_f(void)
Get airspeed (float).
static struct FloatVect2 * stateGetHorizontalWindspeed_f(void)
Get horizontal windspeed (float).
Navigation module to drop a ball at a given point taking into account the wind and ground speed.
unit_t nav_drop_shoot(void)
unit_t nav_drop_compute_approach(uint8_t wp_target, uint8_t wp_start, uint8_t wp_baseturn, uint8_t wp_climbout, float radius)
float nav_drop_trigger_delay
bool compute_alignment(uint8_t w1, uint8_t w2, uint8_t start, uint8_t end, float d_before, float d_after)
unit_t nav_drop_update_release(uint8_t wp_target)
float alt
in meters (above WGS84 reference ellipsoid or above MSL)
#define CARROT
default approaching_time for a wp
API to get/set the generic vehicle states.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.