32#include "generated/airframe.h"
49#ifndef V_CTL_AUTO_THROTTLE_DGAIN
50#define V_CTL_AUTO_THROTTLE_DGAIN 0.
63#define V_CTL_AUTO_THROTTLE_MAX_SUM_ERR 0.4
67#ifndef V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_DGAIN
68#define V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_DGAIN 0.
76#define V_CTL_AUTO_PITCH_MAX_SUM_ERR (RadOfDeg(10.))
78#ifndef V_CTL_AUTO_PITCH_DGAIN
79#define V_CTL_AUTO_PITCH_DGAIN 0.
81#ifndef V_CTL_AUTO_PITCH_IGAIN
82#define V_CTL_AUTO_PITCH_IGAIN 0.
89#ifndef V_CTL_PITCH_TRIM
90#define V_CTL_PITCH_TRIM 0.
95#ifndef V_CTL_AUTO_CLIMB_LIMIT
96#define V_CTL_AUTO_CLIMB_LIMIT (0.5/4.0)
116#define V_CTL_AUTO_AIRSPEED_PITCH_MAX_SUM_ERR (RadOfDeg(15.))
117#define V_CTL_AUTO_AIRSPEED_THROTTLE_MAX_SUM_ERR 0.2
118#define V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR 100
187#if CTRL_VERTICAL_LANDING
200#if CTRL_VERTICAL_LANDING
204#if defined V_CTL_THROTTLE_IDLE
208#ifdef V_CTL_POWER_CTL_BAT_NOMINAL
226#define LEAD_CTRL_TAU 0.8
227#define LEAD_CTRL_A 1.
228#define LEAD_CTRL_Te (1./60.)
232 static float v_ctl_climb_setpoint_last = 0.;
255 static float last_err = 0.;
263 float d_err = err - last_err;
283 static float last_err = 0.;
291 float d_err = err - last_err;
309#define AIRSPEED_LOOP_PERIOD (1./60.)
312static inline void v_ctl_set_airspeed(
void)
314 static float last_err_vz = 0.;
315 static float last_err_as = 0.;
322 float d_err_vz = (err_vz - last_err_vz) * AIRSPEED_LOOP_PERIOD;
323 last_err_vz = err_vz;
334 float d_err_airspeed = (err_airspeed - last_err_as) * AIRSPEED_LOOP_PERIOD;
335 last_err_as = err_airspeed;
378static inline void v_ctl_set_groundspeed(
void)
402 v_ctl_set_airspeed();
405 v_ctl_set_groundspeed();
406 v_ctl_set_airspeed();
421#ifdef V_CTL_THROTTLE_SLEW_LIMITER
422#define V_CTL_THROTTLE_SLEW (1./CONTROL_FREQUENCY/(V_CTL_THROTTLE_SLEW_LIMITER))
425#ifndef V_CTL_THROTTLE_SLEW
426#define V_CTL_THROTTLE_SLEW (1.)
uint8_t autopilot_get_mode(void)
get autopilot mode
struct pprz_autopilot autopilot
Global autopilot structure.
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 V_CTL_SPEED_THROTTLE
#define V_CTL_SPEED_GROUNDSPEED
#define V_CTL_SPEED_AIRSPEED
#define AP_MODE_MANUAL
AP modes.
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
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
float v_ctl_auto_throttle_pitch_of_vz_dgain
float v_ctl_auto_throttle_dgain
#define V_CTL_AUTO_THROTTLE_DGAIN
Vertical control for fixed wing vehicles.
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).
void v_ctl_landing_loop(void)
#define V_CTL_MODE_AUTO_THROTTLE
#define V_CTL_MODE_MANUAL
#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_CLIMB_LIMIT
static void v_ctl_set_pitch(void)
void v_ctl_climb_loop(void)
Auto-throttle inner loop.
static void v_ctl_set_throttle(void)
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 controlled_throttle
#define V_CTL_AUTO_PITCH_IGAIN
float v_ctl_auto_pitch_dgain
#define V_CTL_AUTO_PITCH_DGAIN
void v_ctl_altitude_loop(void)
outer loop
"New" vertical control for fixed wing vehicles.
float v_ctl_auto_airspeed_pitch_sum_err
float v_ctl_auto_airspeed_pitch_pgain
float v_ctl_auto_airspeed_pitch_igain
float v_ctl_auto_airspeed_throttle_dgain
float v_ctl_auto_airspeed_pitch_dgain
float v_ctl_auto_airspeed_throttle_sum_err
float v_ctl_auto_airspeed_throttle_igain
float v_ctl_auto_airspeed_throttle_pgain
pprz_t nav_throttle_setpoint
Fixedwing Navigation library.
float alt
in meters (above WGS84 reference ellipsoid or above MSL)
API to get/set the generic vehicle states.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.