31#include "generated/airframe.h"
51#ifndef V_CTL_AUTO_THROTTLE_DGAIN
52#define V_CTL_AUTO_THROTTLE_DGAIN 0.
65#define V_CTL_AUTO_THROTTLE_MAX_SUM_ERR 150
69#ifndef V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_DGAIN
70#define V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_DGAIN 0.
74#ifdef TUNE_AGRESSIVE_CLIMB
75float agr_climb_throttle;
77float agr_climb_nav_ratio;
78float agr_descent_throttle;
79float agr_descent_pitch;
80float agr_descent_nav_ratio;
87#define V_CTL_AUTO_PITCH_MAX_SUM_ERR 100
92#ifndef V_CTL_PITCH_TRIM
93#define V_CTL_PITCH_TRIM 0.
98#ifdef V_CTL_AUTO_PITCH_PGAIN
99inline static void v_ctl_climb_auto_pitch_loop(
void);
105float v_ctl_auto_airspeed_pgain;
106float v_ctl_auto_airspeed_igain;
107float v_ctl_auto_airspeed_sum_err;
112#define V_CTL_AUTO_AIRSPEED_MAX_SUM_ERR 200
113#define V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR 100
114#define V_CTL_AUTO_CLIMB_LIMIT 0.5/4.0
115#define V_CTL_AUTO_AGR_CLIMB_GAIN 2.0
118#ifndef V_CTL_ALTITUDE_PRE_CLIMB_CORRECTION
119#define V_CTL_ALTITUDE_PRE_CLIMB_CORRECTION 1.0f
122#if CTRL_VERTICAL_LANDING
123#ifndef V_CTL_LANDING_THROTTLE_PGAIN
124#error "V_CTL_LANDING_THROTTLE_PGAIN undefined, please define it in your airfame config file"
126#ifndef V_CTL_LANDING_THROTTLE_IGAIN
127#error "V_CTL_LANDING_THROTTLE_IGAIN undefined, please define it in your airfame config file"
129#ifndef V_CTL_LANDING_THROTTLE_MAX
130INFO(
"V_CTL_LANDING_THROTTLE_MAX undefined, using V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE instead")
131#define V_CTL_LANDING_THROTTLE_MAX V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE
133#ifndef V_CTL_LANDING_DESIRED_SPEED
134#error "V_CTL_LANDING_DESIRED_SPEED undefined, please define it in your airfame config file"
138#ifndef V_CTL_LANDING_PITCH_PGAIN
139INFO(
"V_CTL_LANDING_PITCH_PGAIN undefined, using V_CTL_AUTO_PITCH_PGAIN instead")
140#define V_CTL_LANDING_PITCH_PGAIN V_CTL_AUTO_PITCH_PGAIN
142#ifndef V_CTL_LANDING_PITCH_IGAIN
143INFO(
"V_CTL_LANDING_PITCH_IGAIN undefined, using V_CTL_AUTO_PITCH_IGAIN instead")
144#define V_CTL_LANDING_PITCH_IGAIN V_CTL_AUTO_PITCH_IGAIN
146#ifndef V_CTL_LANDING_PITCH_LIMITS
147INFO(
"V_CTL_LANDING_PITCH_LIMITS undefined, using V_CTL_AUTO_PITCH_MAX_PITCH instead")
148#define V_CTL_LANDING_PITCH_LIMITS V_CTL_AUTO_PITCH_MAX_PITCH
150#ifndef V_CTL_LANDING_PITCH_FLARE
151#error "V_CTL_LANDING_PITCH_FLARE undefined, please define it in your airfame config file"
153#ifndef V_CTL_LANDING_ALT_THROTTLE_KILL
154#error "V_CTL_LANDING_ALT_THROTTLE_KILL undefined, please define it in your airfame config file"
156#ifndef V_CTL_LANDING_ALT_FLARE
157#error "V_CTL_LANDING_ALT_FLARE undefined, please define it in your airfame config file"
160float v_ctl_landing_throttle_pgain;
161float v_ctl_landing_throttle_igain;
162float v_ctl_landing_throttle_max;
163float v_ctl_landing_desired_speed;
164float v_ctl_landing_pitch_pgain;
165float v_ctl_landing_pitch_igain;
166float v_ctl_landing_pitch_limits;
167float v_ctl_landing_pitch_flare;
168float v_ctl_landing_alt_throttle_kill;
169float v_ctl_landing_alt_flare;
177#if CTRL_VERTICAL_LANDING
179 v_ctl_landing_throttle_pgain = V_CTL_LANDING_THROTTLE_PGAIN;
180 v_ctl_landing_throttle_igain = V_CTL_LANDING_THROTTLE_IGAIN;
181 v_ctl_landing_throttle_max = V_CTL_LANDING_THROTTLE_MAX;
182 v_ctl_landing_desired_speed = V_CTL_LANDING_DESIRED_SPEED;
183 v_ctl_landing_pitch_pgain = V_CTL_LANDING_PITCH_PGAIN;
184 v_ctl_landing_pitch_igain = V_CTL_LANDING_PITCH_IGAIN;
185 v_ctl_landing_pitch_limits = V_CTL_LANDING_PITCH_LIMITS;
186 v_ctl_landing_pitch_flare = V_CTL_LANDING_PITCH_FLARE;
187 v_ctl_landing_alt_throttle_kill = V_CTL_LANDING_ALT_THROTTLE_KILL;
188 v_ctl_landing_alt_flare = V_CTL_LANDING_ALT_FLARE;
220#ifdef V_CTL_AUTO_PITCH_PGAIN
230 v_ctl_auto_airspeed_pgain = V_CTL_AUTO_AIRSPEED_PGAIN;
231 v_ctl_auto_airspeed_igain = V_CTL_AUTO_AIRSPEED_IGAIN;
232 v_ctl_auto_airspeed_sum_err = 0.;
243#ifdef TUNE_AGRESSIVE_CLIMB
244 agr_climb_throttle = AGR_CLIMB_THROTTLE;
245#undef AGR_CLIMB_THROTTLE
246#define AGR_CLIMB_THROTTLE agr_climb_throttle
247 agr_climb_pitch = AGR_CLIMB_PITCH;
248#undef AGR_CLIMB_PITCH
249#define AGR_CLIMB_PITCH agr_climb_pitch
250 agr_climb_nav_ratio = AGR_CLIMB_NAV_RATIO;
251#undef AGR_CLIMB_NAV_RATIO
252#define AGR_CLIMB_NAV_RATIO agr_climb_nav_ratio
253 agr_descent_throttle = AGR_DESCENT_THROTTLE;
254#undef AGR_DESCENT_THROTTLE
255#define AGR_DESCENT_THROTTLE agr_descent_throttle
256 agr_descent_pitch = AGR_DESCENT_PITCH;
257#undef AGR_DESCENT_PITCH
258#define AGR_DESCENT_PITCH agr_descent_pitch
259 agr_descent_nav_ratio = AGR_DESCENT_NAV_RATIO;
260#undef AGR_DESCENT_NAV_RATIO
261#define AGR_DESCENT_NAV_RATIO agr_descent_nav_ratio
270#if CTRL_VERTICAL_LANDING
283#if CTRL_VERTICAL_LANDING
287#if defined V_CTL_THROTTLE_IDLE
291#ifdef V_CTL_POWER_CTL_BAT_NOMINAL
310 float altitude_pgain_boost = 1.0;
312#if USE_AIRSPEED && defined(AGR_CLIMB)
316 altitude_pgain_boost = 1.0 + (V_CTL_AUTO_AGR_CLIMB_GAIN - 1.0) * (dist - AGR_BLEND_END) /
317 (AGR_BLEND_START - AGR_BLEND_END);
318 Bound(altitude_pgain_boost, 1.0, V_CTL_AUTO_AGR_CLIMB_GAIN);
330 if (dist < AGR_BLEND_END) {
332 }
else if (dist > AGR_BLEND_START) {
348#ifdef V_CTL_AUTO_PITCH_PGAIN
349#pragma message "AUTO PITCH Enabled!"
351 v_ctl_climb_auto_pitch_loop();
359#if CTRL_VERTICAL_LANDING
360 static float land_speed_i_err;
361 static float land_alt_i_err;
362 static float kill_alt;
368 > (v_ctl_landing_alt_throttle_kill - v_ctl_landing_alt_flare)) {
376 + land_alt_i_err * v_ctl_landing_throttle_igain;
380 BoundAbs(land_alt_i_err, 50);
383 nav_pitch -= land_speed_err * v_ctl_landing_pitch_pgain / 1000
384 + land_speed_i_err * v_ctl_landing_pitch_igain / 1000;
385 Bound(
nav_pitch, -v_ctl_landing_pitch_limits, v_ctl_landing_pitch_limits);
388 BoundAbs(land_speed_i_err, .2);
394 if (land_alt_err > 0.0) {
411 static float last_err;
413 float f_throttle = 0;
415 float d_err = err - last_err;
431 f_throttle = AGR_CLIMB_THROTTLE;
434 f_throttle = AGR_DESCENT_THROTTLE;
441 / (AGR_BLEND_START - AGR_BLEND_END);
448 f_throttle += ratio * AGR_CLIMB_THROTTLE;
451 f_throttle += ratio * AGR_DESCENT_THROTTLE;
476 float f_throttle = 0;
478 float v_ctl_pitch_of_vz;
481 static float v_ctl_climb_setpoint_last = 0;
510 v_ctl_auto_airspeed_sum_err += err_airspeed;
511 BoundAbs(v_ctl_auto_airspeed_sum_err, V_CTL_AUTO_AIRSPEED_MAX_SUM_ERR);
512 controlled_throttle = (err_airspeed + v_ctl_auto_airspeed_sum_err * v_ctl_auto_airspeed_igain) *
513 v_ctl_auto_airspeed_pgain;
530#ifdef V_CTL_AUTO_PITCH_PGAIN
531inline static void v_ctl_climb_auto_pitch_loop(
void)
543#ifdef V_CTL_THROTTLE_SLEW_LIMITER
544#define V_CTL_THROTTLE_SLEW (1./CONTROL_FREQUENCY/(V_CTL_THROTTLE_SLEW_LIMITER))
547#ifndef V_CTL_THROTTLE_SLEW
548#define V_CTL_THROTTLE_SLEW 1.
struct pprz_autopilot autopilot
Global autopilot structure.
bool autopilot_throttle_killed(void)
get kill status
Core autopilot interface common to all firmwares.
struct Electrical electrical
pprz_t v_ctl_throttle_setpoint
float v_ctl_auto_groundspeed_sum_err
float v_ctl_auto_throttle_climb_throttle_increment
float v_ctl_auto_airspeed_controlled
uint8_t v_ctl_auto_throttle_submode
float v_ctl_auto_groundspeed_igain
float v_ctl_altitude_pgain
float v_ctl_pitch_setpoint
float v_ctl_auto_throttle_sum_err
float v_ctl_altitude_setpoint
in meters above MSL
pprz_t v_ctl_throttle_slewed
#define V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR
float v_ctl_auto_throttle_nominal_cruise_throttle
#define V_CTL_ALTITUDE_MAX_CLIMB
float v_ctl_auto_throttle_pitch_of_vz_pgain
#define V_CTL_THROTTLE_SLEW
float v_ctl_auto_throttle_cruise_throttle
float v_ctl_altitude_pre_climb
Path Angle.
float v_ctl_auto_airspeed_setpoint
in meters per second
float v_ctl_altitude_error
in meters, (setpoint - alt) -> positive = too low
float v_ctl_auto_groundspeed_pgain
float v_ctl_auto_groundspeed_setpoint
in meters per second
float v_ctl_climb_setpoint
#define LATERAL_MODE_ROLL
#define CONTROL_FREQUENCY
float v_ctl_altitude_pre_climb_correction
void v_ctl_landing_loop(void)
float v_ctl_auto_throttle_pgain
float v_ctl_auto_pitch_igain
#define V_CTL_AUTO_THROTTLE_MAX_SUM_ERR
#define V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_DGAIN
void v_ctl_climb_loop(void)
Auto-throttle inner loop.
float v_ctl_auto_pitch_pgain
float v_ctl_auto_throttle_min_cruise_throttle
float v_ctl_auto_pitch_sum_err
#define V_CTL_AUTO_PITCH_MAX_SUM_ERR
float v_ctl_auto_throttle_max_cruise_throttle
float v_ctl_auto_throttle_igain
void v_ctl_guidance_loop(void)
General guidance logic This will call the proper control loops according to the sub-modes.
void v_ctl_throttle_slew(void)
Computes slewed throttle from throttle setpoint called at 20Hz.
float v_ctl_altitude_max_climb
float v_ctl_auto_throttle_pitch_of_vz_dgain
void v_ctl_altitude_loop(void)
outer loop
#define V_CTL_ALTITUDE_PRE_CLIMB_CORRECTION
static void v_ctl_climb_auto_throttle_loop(void)
auto throttle inner loop
float v_ctl_auto_throttle_dgain
#define V_CTL_AUTO_THROTTLE_DGAIN
Vertical control for fixed wing vehicles.
Fixed wing horizontal control.
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 struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
static float stateGetAirspeed_f(void)
Get airspeed (float).
#define V_CTL_MODE_AUTO_THROTTLE
#define V_CTL_MODE_MANUAL
#define V_CTL_AUTO_THROTTLE_AGRESSIVE
#define V_CTL_CLIMB_MODE_AUTO_PITCH
#define V_CTL_AUTO_THROTTLE_STANDARD
#define V_CTL_MODE_LANDING
#define V_CTL_MODE_AUTO_ALT
#define V_CTL_CLIMB_MODE_AUTO_THROTTLE
#define V_CTL_MODE_AUTO_CLIMB
#define V_CTL_AUTO_THROTTLE_BLENDED
#define V_CTL_AUTO_CLIMB_LIMIT
float controlled_throttle
#define V_CTL_AUTO_PITCH_IGAIN
pprz_t nav_throttle_setpoint
Fixedwing Navigation library.
float alt
in meters (above WGS84 reference ellipsoid or above MSL)
float h_ctl_roll_setpoint
API to get/set the generic vehicle states.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.