74#include "generated/airframe.h"
116#define H_CTL_REF_W_P 5.
118#ifndef H_CTL_REF_XI_P
119#define H_CTL_REF_XI_P 0.85
122#define H_CTL_REF_W_Q 5.
124#ifndef H_CTL_REF_XI_Q
125#define H_CTL_REF_XI_Q 0.85
128#define H_CTL_REF_W_R 5.
130#ifndef H_CTL_REF_XI_R
131#define H_CTL_REF_XI_R 0.85
133#ifndef H_CTL_REF_MAX_P
134#define H_CTL_REF_MAX_P RadOfDeg(150.)
136#ifndef H_CTL_REF_MAX_P_DOT
137#define H_CTL_REF_MAX_P_DOT RadOfDeg(500.)
139#ifndef H_CTL_REF_MAX_Q
140#define H_CTL_REF_MAX_Q RadOfDeg(150.)
142#ifndef H_CTL_REF_MAX_Q_DOT
143#define H_CTL_REF_MAX_Q_DOT RadOfDeg(500.)
182float h_ctl_yaw_rate_setpoint;
183float h_ctl_yaw_dgain;
184float h_ctl_yaw_ny_igain;
185float h_ctl_yaw_ny_sum_err;
186pprz_t h_ctl_rudder_setpoint;
191pprz_t h_ctl_flaps_setpoint;
201#define AIRSPEED_RATIO_MIN 0.5
202#define AIRSPEED_RATIO_MAX 2.
208#ifndef PITCH_TRIM_RATE_LIMITER
209#define PITCH_TRIM_RATE_LIMITER 3.
214inline static void h_ctl_yaw_loop(
void);
217inline static void h_ctl_cl_loop(
void);
222#ifndef H_CTL_COURSE_PRE_BANK_CORRECTION
223#define H_CTL_COURSE_PRE_BANK_CORRECTION 1.
225#ifndef H_CTL_COURSE_DGAIN
226#define H_CTL_COURSE_DGAIN 0.
231#ifndef H_CTL_ROLL_RATE_GAIN
232#define H_CTL_ROLL_RATE_GAIN 0.
234#ifndef H_CTL_ROLL_IGAIN
235#define H_CTL_ROLL_IGAIN 0.
237#ifndef H_CTL_ROLL_KFFA
238#define H_CTL_ROLL_KFFA 0.
240#ifndef H_CTL_ROLL_KFFD
241#define H_CTL_ROLL_KFFD 0.
246#ifndef H_CTL_PITCH_DGAIN
247#define H_CTL_PITCH_DGAIN 0.
249#ifndef H_CTL_PITCH_IGAIN
250#define H_CTL_PITCH_IGAIN 0.
252#ifndef H_CTL_PITCH_KFFA
253#define H_CTL_PITCH_KFFA 0.
255#ifndef H_CTL_PITCH_KFFD
256#define H_CTL_PITCH_KFFD 0.
260#ifndef H_CTL_YAW_KFFD
261#define H_CTL_YAW_KFFD 0.
264#ifndef USE_GYRO_PITCH_RATE
265#define USE_GYRO_PITCH_RATE TRUE
268#if PERIODIC_TELEMETRY
278 pprz_msg_send_TUNE_ROLL(trans,
dev, AC_ID,
284 pprz_msg_send_H_CTL_A(trans,
dev, AC_ID,
322#ifdef H_CTL_AILERON_OF_THROTTLE
337 h_ctl_yaw_dgain = H_CTL_YAW_DGAIN;
338 h_ctl_yaw_ny_igain = H_CTL_YAW_NY_IGAIN;
339 h_ctl_yaw_ny_sum_err = 0.;
340 h_ctl_rudder_setpoint = 0;
344 h_ctl_flaps_setpoint = 0;
348#ifdef H_CTL_PITCH_OF_ROLL
363#if PERIODIC_TELEMETRY
376 static float last_err;
382 float d_err = err - last_err;
387 Bound(speed_depend_nav, 0.66, 1.5);
397static inline void compute_airspeed_ratio(
void)
401 static float airspeed = 0.;
404 float airspeed_ratio = airspeed / NOMINAL_AIRSPEED;
417 compute_airspeed_ratio();
434#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
435#define H_CTL_REF_DT (1./AHRS_PROPAGATE_FREQUENCY)
437#define H_CTL_REF_DT (1./CONTROL_FREQUENCY)
445#define KFFA_UPDATE 0.1
446#define KFFD_UPDATE 0.05
451 static float cmd_fb = 0.;
474#ifdef USE_KFF_UPDATE_ROLL
488 float d_err =
h_ctl_ref.roll_rate - body_rate->
p;
517inline static void loiter(
void)
520 static float last_pitch_trim;
530 if (throttle_diff > 0) {
541 Bound(pitch_trim, last_pitch_trim - max_change, last_pitch_trim + max_change);
543 last_pitch_trim = pitch_trim;
551 static float cmd_fb = 0.;
552#if !USE_GYRO_PITCH_RATE
553 static float last_err;
587#ifdef USE_KFF_UPDATE_PITCH
600#if USE_GYRO_PITCH_RATE
632inline static void h_ctl_yaw_loop(
void)
637#if (!defined SITL || defined USE_NPS)
650 h_ctl_yaw_ny_sum_err = 0.;
652 if (h_ctl_yaw_ny_igain > 0.) {
657 BoundAbs(h_ctl_yaw_ny_sum_err,
MAX_PPRZ / (2. * h_ctl_yaw_ny_igain));
660 h_ctl_yaw_ny_sum_err = 0.;
669 float Vo = NOMINAL_AIRSPEED;
672 h_ctl_ref.yaw_rate = h_ctl_yaw_rate_setpoint
676 float cmd = + h_ctl_yaw_dgain * d_err
678 + h_ctl_yaw_ny_igain * h_ctl_yaw_ny_sum_err
688inline static void h_ctl_cl_loop(
void)
691#if H_CTL_CL_LOOP_INCREASE_FLAPS_WITH_LOADFACTOR
692#if (!defined SITL || defined USE_NPS)
712#if H_CTL_CL_LOOP_USE_AIRSPEED_SETPOINT
717#if H_CTL_CL_LOOP_INCREASE_FLAPS_WITH_LOADFACTOR
718 corrected_airspeed /= sqrtf(nz);
724 if (corrected_airspeed > NOMINAL_AIRSPEED + H_CTL_CL_DEADBAND) {
725 cmd = (corrected_airspeed - NOMINAL_AIRSPEED) * (H_CTL_CL_FLAPS_RACE - H_CTL_CL_FLAPS_NOMINAL) / (RACE_AIRSPEED - NOMINAL_AIRSPEED);
727 else if (corrected_airspeed < NOMINAL_AIRSPEED - H_CTL_CL_DEADBAND) {
728 cmd = (corrected_airspeed - NOMINAL_AIRSPEED) * (H_CTL_CL_FLAPS_STALL - H_CTL_CL_FLAPS_NOMINAL) / (
STALL_AIRSPEED - NOMINAL_AIRSPEED);
736 Bound(cmd, H_CTL_CL_FLAPS_RACE, H_CTL_CL_FLAPS_STALL);
uint8_t autopilot_get_mode(void)
get autopilot mode
struct pprz_autopilot autopilot
Global autopilot structure.
Core autopilot interface common to all firmwares.
static struct uart_periph * dev
pprz_t v_ctl_throttle_setpoint
uint8_t v_ctl_auto_throttle_submode
float v_ctl_auto_throttle_sum_err
float v_ctl_auto_throttle_nominal_cruise_throttle
float v_ctl_auto_throttle_cruise_throttle
float v_ctl_auto_airspeed_setpoint
in meters per second
#define AP_MODE_MANUAL
AP modes.
float v_ctl_auto_throttle_min_cruise_throttle
float v_ctl_auto_throttle_max_cruise_throttle
Fixed wing horizontal control.
#define VECT3_COPY(_a, _b)
#define ACCEL_BFP_OF_REAL(_af)
void int32_rmat_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_a2b, struct Int32Vect3 *va)
rotate 3D vector by rotation matrix.
#define ACCEL_FLOAT_OF_BFP(_ai)
vector in North East Down coordinates
static struct NedCoor_i * stateGetAccelNed_i(void)
Get acceleration in NED coordinates (int).
static struct Int32RMat * stateGetNedToBodyRMat_i(void)
Get vehicle body attitude rotation matrix (int).
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (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 float stateGetAirspeed_f(void)
Get airspeed (float).
float v_ctl_pitch_loiter_trim
float v_ctl_pitch_dash_trim
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
arch independent LED (Light Emitting Diodes) API
Fixedwing Navigation library.
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
#define AIRSPEED_RATIO_MIN
void h_ctl_course_loop(void)
#define H_CTL_PITCH_IGAIN
static void send_tune_roll(struct transport_tx *trans, struct link_device *dev)
static void h_ctl_pitch_loop(void)
float h_ctl_pitch_setpoint
static void send_calibration(struct transport_tx *trans, struct link_device *dev)
pprz_t h_ctl_elevator_setpoint
float h_ctl_pitch_sum_err
#define AIRSPEED_RATIO_MAX
#define H_CTL_COURSE_PRE_BANK_CORRECTION
float h_ctl_roll_max_setpoint
float h_ctl_elevator_of_roll
float h_ctl_roll_setpoint
static void send_ctl_a(struct transport_tx *trans, struct link_device *dev)
#define H_CTL_PITCH_DGAIN
#define PITCH_TRIM_RATE_LIMITER
struct HCtlAdaptRef h_ctl_ref
static void h_ctl_roll_loop(void)
pprz_t h_ctl_aileron_setpoint
float h_ctl_roll_rate_gain
#define H_CTL_REF_MAX_P_DOT
void h_ctl_attitude_loop(void)
float h_ctl_roll_attitude_gain
#define H_CTL_REF_MAX_Q_DOT
#define KFFA_UPDATE
Adaptive control tuning parameters activate with USE_KFF_UPDATE.
float h_ctl_aileron_of_throttle
float h_ctl_pitch_loop_setpoint
#define H_CTL_COURSE_DGAIN
float h_ctl_pitch_of_roll
float h_ctl_course_pre_bank_correction
#define H_CTL_REF_DT
Define reference generator time step default to control frequency and ahrs propagation freq if contro...
float h_ctl_course_pre_bank
float h_ctl_course_setpoint
#define H_CTL_ROLL_RATE_GAIN
Fixed wing horizontal adaptive control.
#define H_CTL_ROLL_SUM_ERR_MAX
#define H_CTL_PITCH_SUM_ERR_MAX
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.