From 4663382b01dce9e1f0c538616e898d8fd6fc2a05 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 20 Sep 2024 17:17:02 +1000 Subject: [PATCH] AC_AttitudeControl: use AC_PID defaults to tidy heli rate PID initialisation makes it look like the Copter initialisation - ie. legible --- .../AC_AttitudeControl_Heli.cpp | 5 +-- .../AC_AttitudeControl_Heli.h | 36 +++++++++++++++++-- 2 files changed, 34 insertions(+), 7 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp index 4c67892fe0..3b5028ad4b 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp @@ -307,10 +307,7 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] = { }; AC_AttitudeControl_Heli::AC_AttitudeControl_Heli(AP_AHRS_View &ahrs, const AP_MultiCopter &aparm, AP_MotorsHeli& motors) : - AC_AttitudeControl(ahrs, aparm, motors), - _pid_rate_roll(AC_ATC_HELI_RATE_RP_P, AC_ATC_HELI_RATE_RP_I, AC_ATC_HELI_RATE_RP_D, AC_ATC_HELI_RATE_RP_FF, AC_ATC_HELI_RATE_RP_IMAX, AC_ATTITUDE_HELI_RATE_RP_FF_FILTER, AC_ATC_HELI_RATE_RP_FILT_HZ, 0.0f), - _pid_rate_pitch(AC_ATC_HELI_RATE_RP_P, AC_ATC_HELI_RATE_RP_I, AC_ATC_HELI_RATE_RP_D, AC_ATC_HELI_RATE_RP_FF, AC_ATC_HELI_RATE_RP_IMAX, AC_ATTITUDE_HELI_RATE_RP_FF_FILTER, AC_ATC_HELI_RATE_RP_FILT_HZ, 0.0f), - _pid_rate_yaw(AC_ATC_HELI_RATE_YAW_P, AC_ATC_HELI_RATE_YAW_I, AC_ATC_HELI_RATE_YAW_D, AC_ATC_HELI_RATE_YAW_FF, AC_ATC_HELI_RATE_YAW_IMAX, AC_ATTITUDE_HELI_RATE_Y_FF_FILTER, AC_ATC_HELI_RATE_YAW_FILT_HZ, 0.0f) + AC_AttitudeControl(ahrs, aparm, motors) { AP_Param::setup_object_defaults(this, var_info); diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h index b2695b59cf..7e25eef899 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h @@ -150,8 +150,38 @@ private: // parameters AP_Int8 _piro_comp_enabled; // Flybar present or not. Affects attitude controller used during ACRO flight mode AP_Int16 _hover_roll_trim; // Angle in centi-degrees used to counter tail rotor thrust in hover - AC_HELI_PID _pid_rate_roll; - AC_HELI_PID _pid_rate_pitch; - AC_HELI_PID _pid_rate_yaw; + + // Roll and Pitch rate PIDs share the same defaults: + const AC_PID::Defaults rp_defaults { + AC_PID::Defaults{ + .p = AC_ATC_HELI_RATE_RP_P, + .i = AC_ATC_HELI_RATE_RP_I, + .d = AC_ATC_HELI_RATE_RP_D, + .ff = AC_ATC_HELI_RATE_RP_FF, + .imax = AC_ATC_HELI_RATE_RP_IMAX, + .filt_T_hz = AC_ATTITUDE_HELI_RATE_RP_FF_FILTER, + .filt_E_hz = AC_ATC_HELI_RATE_RP_FILT_HZ, + .filt_D_hz = 0.0, + .srmax = 0, + .srtau = 1.0 + } + }; + AC_HELI_PID _pid_rate_roll { rp_defaults }; + AC_HELI_PID _pid_rate_pitch { rp_defaults }; + + AC_HELI_PID _pid_rate_yaw { + AC_PID::Defaults{ + .p = AC_ATC_HELI_RATE_YAW_P, + .i = AC_ATC_HELI_RATE_YAW_I, + .d = AC_ATC_HELI_RATE_YAW_D, + .ff = AC_ATC_HELI_RATE_YAW_FF, + .imax = AC_ATC_HELI_RATE_YAW_IMAX, + .filt_T_hz = AC_ATTITUDE_HELI_RATE_Y_FF_FILTER, + .filt_E_hz = AC_ATC_HELI_RATE_YAW_FILT_HZ, + .filt_D_hz = 0.0, + .srmax = 0, + .srtau = 1.0 + } + }; };