74 #include "generated/airframe.h"
115 #ifndef H_CTL_REF_W_P
116 #define H_CTL_REF_W_P 5.
118 #ifndef H_CTL_REF_XI_P
119 #define H_CTL_REF_XI_P 0.85
121 #ifndef H_CTL_REF_W_Q
122 #define H_CTL_REF_W_Q 5.
124 #ifndef H_CTL_REF_XI_Q
125 #define H_CTL_REF_XI_Q 0.85
127 #ifndef H_CTL_REF_W_R
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.)
182 float h_ctl_yaw_rate_setpoint;
183 float h_ctl_yaw_dgain;
184 float h_ctl_yaw_ny_igain;
185 float h_ctl_yaw_ny_sum_err;
186 pprz_t h_ctl_rudder_setpoint;
191 pprz_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.
214 inline static void h_ctl_yaw_loop(
void);
217 inline 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,
286 &h_ctl_roll_setpoint,
289 &h_ctl_aileron_setpoint,
290 &h_ctl_pitch_sum_err,
291 &h_ctl_pitch_loop_setpoint,
294 &h_ctl_elevator_setpoint);
314 h_ctl_roll_setpoint = 0.;
315 h_ctl_roll_attitude_gain = H_CTL_ROLL_ATTITUDE_GAIN;
318 h_ctl_roll_sum_err = 0;
321 h_ctl_aileron_setpoint = 0;
322 #ifdef H_CTL_AILERON_OF_THROTTLE
323 h_ctl_aileron_of_throttle = H_CTL_AILERON_OF_THROTTLE;
326 h_ctl_pitch_setpoint = 0.;
327 h_ctl_pitch_loop_setpoint = 0.;
328 h_ctl_pitch_pgain = H_CTL_PITCH_PGAIN;
331 h_ctl_pitch_sum_err = 0.;
334 h_ctl_elevator_setpoint = 0;
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;
347 h_ctl_elevator_of_roll = 0;
348 #ifdef H_CTL_PITCH_OF_ROLL
349 h_ctl_pitch_of_roll = H_CTL_PITCH_OF_ROLL;
352 use_airspeed_ratio =
false;
353 airspeed_ratio2 = 1.;
356 v_ctl_pitch_loiter_trim = V_CTL_PITCH_LOITER_TRIM;
357 v_ctl_pitch_dash_trim = V_CTL_PITCH_DASH_TRIM;
359 v_ctl_pitch_loiter_trim = 0.;
360 v_ctl_pitch_dash_trim = 0.;
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);
397 static inline void compute_airspeed_ratio(
void)
399 if (use_airspeed_ratio) {
401 static float airspeed = 0.;
404 float airspeed_ratio = airspeed / NOMINAL_AIRSPEED;
406 airspeed_ratio2 = airspeed_ratio * airspeed_ratio;
408 airspeed_ratio2 = 1.;
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
479 printf(
"%f %f %f\n", h_ctl_roll_Kffa, h_ctl_roll_Kffd, cmd_fb);
481 h_ctl_roll_Kffa =
Max(h_ctl_roll_Kffa, 0);
482 h_ctl_roll_Kffd =
Max(h_ctl_roll_Kffd, 0);
491 h_ctl_roll_sum_err = 0.;
493 if (h_ctl_roll_igain > 0.) {
497 h_ctl_roll_sum_err = 0.;
501 cmd_fb = h_ctl_roll_attitude_gain * err;
505 - h_ctl_roll_rate_gain * d_err
506 - h_ctl_roll_igain * h_ctl_roll_sum_err
517 inline 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;
544 h_ctl_pitch_loop_setpoint += pitch_trim;
551 static float cmd_fb = 0.;
552 #if !USE_GYRO_PITCH_RATE
553 static float last_err;
557 if (h_ctl_pitch_of_roll < 0.) {
558 h_ctl_pitch_of_roll = 0.;
587 #ifdef USE_KFF_UPDATE_PITCH
592 printf(
"%f %f %f\n", h_ctl_pitch_Kffa, h_ctl_pitch_Kffd, cmd_fb);
594 h_ctl_pitch_Kffa =
Max(h_ctl_pitch_Kffa, 0);
595 h_ctl_pitch_Kffd =
Max(h_ctl_pitch_Kffd, 0);
600 #if USE_GYRO_PITCH_RATE
602 #else // soft derivation
608 h_ctl_pitch_sum_err = 0.;
610 if (h_ctl_pitch_igain > 0.) {
614 h_ctl_pitch_sum_err = 0.;
618 cmd_fb = h_ctl_pitch_pgain * err;
622 + h_ctl_pitch_dgain * d_err
627 h_ctl_elevator_setpoint =
TRIM_PPRZ(cmd);
632 inline static void h_ctl_yaw_loop(
void)
635 #if H_CTL_YAW_TRIM_NY
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;
673 + 9.81f / Vo * sinf(h_ctl_roll_setpoint);
676 float cmd = + h_ctl_yaw_dgain * d_err
677 #if H_CTL_YAW_TRIM_NY
678 + h_ctl_yaw_ny_igain * h_ctl_yaw_ny_sum_err
688 inline 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);
bool launch
request launch
float v_ctl_auto_throttle_nominal_cruise_throttle
#define H_CTL_PITCH_DGAIN
void h_ctl_course_loop(void)
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
void h_ctl_attitude_loop(void)
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
float v_ctl_pitch_loiter_trim
pprz_t v_ctl_throttle_setpoint
Periodic telemetry system header (includes downlink utility and generated code).
static struct Int32RMat * stateGetNedToBodyRMat_i(void)
Get vehicle body attitude rotation matrix (int).
static void send_ctl_a(struct transport_tx *trans, struct link_device *dev)
float h_ctl_course_setpoint
#define H_CTL_REF_DT
Define reference generator time step default to control frequency and ahrs propagation freq if contro...
float v_ctl_pitch_dash_trim
#define PITCH_TRIM_RATE_LIMITER
static void send_tune_roll(struct transport_tx *trans, struct link_device *dev)
#define AIRSPEED_RATIO_MAX
#define VECT3_COPY(_a, _b)
static float stateGetAirspeed_f(void)
Get airspeed (float).
#define H_CTL_REF_MAX_P_DOT
pprz_t h_ctl_elevator_setpoint
pprz_t h_ctl_aileron_setpoint
float v_ctl_auto_throttle_sum_err
float v_ctl_auto_throttle_max_cruise_throttle
struct pprz_autopilot autopilot
Global autopilot structure.
float v_ctl_auto_airspeed_setpoint
in meters per second
#define H_CTL_PITCH_SUM_ERR_MAX
Fixed wing horizontal control.
#define AP_MODE_MANUAL
AP modes.
void int32_rmat_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_a2b, struct Int32Vect3 *va)
rotate 3D vector by rotation matrix.
float h_ctl_aileron_of_throttle
#define H_CTL_REF_MAX_Q_DOT
#define H_CTL_ROLL_RATE_GAIN
#define ACCEL_FLOAT_OF_BFP(_ai)
float h_ctl_pitch_loop_setpoint
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
#define H_CTL_COURSE_DGAIN
#define DefaultPeriodic
Set default periodic telemetry.
uint8_t v_ctl_auto_throttle_submode
float v_ctl_auto_throttle_min_cruise_throttle
static void h_ctl_pitch_loop(void)
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
float h_ctl_roll_max_setpoint
#define H_CTL_ROLL_SUM_ERR_MAX
static const struct usb_device_descriptor dev
Core autopilot interface common to all firmwares.
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
float h_ctl_course_pre_bank
#define KFFA_UPDATE
Adaptive control tuning parameters activate with USE_KFF_UPDATE.
float v_ctl_auto_throttle_cruise_throttle
float h_ctl_pitch_setpoint
#define H_CTL_PITCH_IGAIN
API to get/set the generic vehicle states.
vector in North East Down coordinates
float h_ctl_pitch_of_roll
float h_ctl_roll_setpoint
float h_ctl_elevator_of_roll
float h_ctl_pitch_sum_err
float h_ctl_course_pre_bank_correction
#define H_CTL_COURSE_PRE_BANK_CORRECTION
arch independent LED (Light Emitting Diodes) API
float h_ctl_roll_attitude_gain
static void h_ctl_roll_loop(void)
static void send_calibration(struct transport_tx *trans, struct link_device *dev)
#define ACCEL_BFP_OF_REAL(_af)
Fixedwing Navigation library.
struct HCtlAdaptRef h_ctl_ref
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
static struct NedCoor_i * stateGetAccelNed_i(void)
Get acceleration in NED coordinates (int).
float h_ctl_roll_rate_gain
Fixed wing horizontal adaptive control.
uint8_t autopilot_get_mode(void)
get autopilot mode
#define AIRSPEED_RATIO_MIN