From d6287e90f12e9e98a1f4d3f2527051dc52eb4a4e Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 27 Jul 2023 15:59:16 +0200 Subject: [PATCH] AC_AttitudeControl: update PID notch centres add documentation for PID notches and D feed-foward add notches and D feedforward to heli PIDs add advanced flag to PIDs and selectively compile advanced PID options --- .../AC_AttitudeControl/AC_AttitudeControl.h | 3 + .../AC_AttitudeControl_Heli.cpp | 150 ++++++++++++++++++ .../AC_AttitudeControl_Heli.h | 19 +-- .../AC_AttitudeControl_Multi.cpp | 134 ++++++++++++++++ .../AC_AttitudeControl_Multi.h | 3 + .../AC_AttitudeControl_Sub.cpp | 129 +++++++++++++++ .../AC_AttitudeControl_Sub.h | 3 + .../AC_AttitudeControl/AC_PosControl.cpp | 40 +++++ 8 files changed, 467 insertions(+), 14 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index a08dedda08..f752309804 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -342,6 +342,9 @@ public: // sanity check parameters. should be called once before take-off virtual void parameter_sanity_check() {} + // set the PID notch sample rates + virtual void set_notch_sample_rate(float sample_rate) {} + // return true if the rpy mix is at lowest value virtual bool is_throttle_mix_min() const { return true; } diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp index e05d6e4af7..26004c3a13 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp @@ -1,5 +1,6 @@ #include "AC_AttitudeControl_Heli.h" #include +#include // table of user settable parameters const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] = { @@ -87,6 +88,47 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] = { // @Increment: 0.5 // @User: Advanced + // @Param: RAT_RLL_ADV + // @DisplayName: Roll Advanced parameters enable + // @Description: Roll Advanced parameters enable + // @Values: 0:Disabled,1:Enabled + // @User: Advanced + + // @Param: RAT_RLL_D_FF + // @DisplayName: Roll Derivative FeedForward Gain + // @Description: FF D Gain which produces an output that is proportional to the rate of change of the target + // @Range: 0 0.02 + // @Increment: 0.0001 + // @User: Advanced + + // @Param: RAT_RLL_NTF + // @DisplayName: Roll Target notch Filter center frequency + // @Description: Roll Target notch Filter center frequency in Hz. + // @Range: 10 495 + // @Units: Hz + // @User: Advanced + + // @Param: RAT_RLL_NEF + // @DisplayName: Roll Error notch Filter center frequency + // @Description: Roll Error notch Filter center frequency in Hz. + // @Range: 10 495 + // @Units: Hz + // @User: Advanced + + // @Param: RAT_RLL_NBW + // @DisplayName: Roll notch Filter bandwidth + // @Description: Roll notch Filter bandwidth in Hz. + // @Range: 5 250 + // @Units: Hz + // @User: Advanced + + // @Param: RAT_RLL_NATT + // @DisplayName: Roll notch Filter attenuation + // @Description: Roll notch Filter attenuation in dB. + // @Range: 5 50 + // @Units: dB + // @User: Advanced + AP_SUBGROUPINFO(_pid_rate_roll, "RAT_RLL_", 2, AC_AttitudeControl_Heli, AC_HELI_PID), // @Param: RAT_PIT_P @@ -161,6 +203,47 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] = { // @Increment: 0.5 // @User: Advanced + // @Param: RAT_PIT_ADV + // @DisplayName: Pitch Advanced parameters enable + // @Description: Pitch Advanced parameters enable + // @Values: 0:Disabled,1:Enabled + // @User: Advanced + + // @Param: RAT_PIT_D_FF + // @DisplayName: Pitch Derivative FeedForward Gain + // @Description: FF D Gain which produces an output that is proportional to the rate of change of the target + // @Range: 0 0.02 + // @Increment: 0.0001 + // @User: Advanced + + // @Param: RAT_PIT_NTF + // @DisplayName: Pitch Target notch Filter center frequency + // @Description: Pitch Target notch Filter center frequency in Hz. + // @Range: 10 495 + // @Units: Hz + // @User: Advanced + + // @Param: RAT_PIT_NEF + // @DisplayName: Pitch Error notch Filter center frequency + // @Description: Pitch Error notch Filter center frequency in Hz. + // @Range: 10 495 + // @Units: Hz + // @User: Advanced + + // @Param: RAT_PIT_NBW + // @DisplayName: Pitch notch Filter bandwidth + // @Description: Pitch notch Filter bandwidth in Hz. + // @Range: 5 250 + // @Units: Hz + // @User: Advanced + + // @Param: RAT_PIT_NATT + // @DisplayName: Pitch notch Filter attenuation + // @Description: Pitch notch Filter attenuation in dB. + // @Range: 5 50 + // @Units: dB + // @User: Advanced + AP_SUBGROUPINFO(_pid_rate_pitch, "RAT_PIT_", 3, AC_AttitudeControl_Heli, AC_HELI_PID), // @Param: RAT_YAW_P @@ -235,6 +318,47 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] = { // @Increment: 0.5 // @User: Advanced + // @Param: RAT_YAW_ADV + // @DisplayName: Yaw Advanced parameters enable + // @Description: Yaw Advanced parameters enable + // @Values: 0:Disabled,1:Enabled + // @User: Advanced + + // @Param: RAT_YAW_D_FF + // @DisplayName: Yaw Derivative FeedForward Gain + // @Description: FF D Gain which produces an output that is proportional to the rate of change of the target + // @Range: 0 0.02 + // @Increment: 0.0001 + // @User: Advanced + + // @Param: RAT_YAW_NTF + // @DisplayName: Yaw Target notch Filter center frequency + // @Description: Yaw Target notch Filter center frequency in Hz. + // @Range: 10 495 + // @Units: Hz + // @User: Advanced + + // @Param: RAT_YAW_NEF + // @DisplayName: Yaw Error notch Filter center frequency + // @Description: Yaw Error notch Filter center frequency in Hz. + // @Range: 10 495 + // @Units: Hz + // @User: Advanced + + // @Param: RAT_YAW_NBW + // @DisplayName: Yaw notch Filter bandwidth + // @Description: Yaw notch Filter bandwidth in Hz. + // @Range: 5 250 + // @Units: Hz + // @User: Advanced + + // @Param: RAT_YAW_NATT + // @DisplayName: Yaw notch Filter attenuation + // @Description: Yaw notch Filter attenuation in dB. + // @Range: 5 50 + // @Units: dB + // @User: Advanced + AP_SUBGROUPINFO(_pid_rate_yaw, "RAT_YAW_", 4, AC_AttitudeControl_Heli, AC_HELI_PID), // @Param: PIRO_COMP @@ -247,6 +371,23 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] = { AP_GROUPEND }; +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) +{ + AP_Param::setup_object_defaults(this, var_info); + + // initialise flags + _flags_heli.leaky_i = true; + _flags_heli.flybar_passthrough = false; + _flags_heli.tail_passthrough = false; +#if AC_PID_ADVANCED_ENABLED + set_notch_sample_rate(AP::scheduler().get_loop_rate_hz()); +#endif +} + // passthrough_bf_roll_pitch_rate_yaw - passthrough the pilots roll and pitch inputs directly to swashplate for flybar acro mode void AC_AttitudeControl_Heli::passthrough_bf_roll_pitch_rate_yaw(float roll_passthrough, float pitch_passthrough, float yaw_rate_bf_cds) { @@ -485,3 +626,12 @@ void AC_AttitudeControl_Heli::input_euler_angle_roll_pitch_yaw(float euler_roll_ } AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(euler_roll_angle_cd, euler_pitch_angle_cd, euler_yaw_angle_cd, slew_yaw); } + +void AC_AttitudeControl_Heli::set_notch_sample_rate(float sample_rate) +{ +#if AC_PID_ADVANCED_ENABLED + _pid_rate_roll.set_notch_sample_rate(sample_rate); + _pid_rate_pitch.set_notch_sample_rate(sample_rate); + _pid_rate_yaw.set_notch_sample_rate(sample_rate); +#endif +} \ No newline at end of file diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h index c20b2fe5a2..0d519bf87d 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h @@ -33,19 +33,7 @@ class AC_AttitudeControl_Heli : public AC_AttitudeControl { public: 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) - { - AP_Param::setup_object_defaults(this, var_info); - - // initialise flags - _flags_heli.leaky_i = true; - _flags_heli.flybar_passthrough = false; - _flags_heli.tail_passthrough = false; - } + AP_MotorsHeli& motors); // pid accessors AC_PID& get_rate_roll_pid() override { return _pid_rate_roll; } @@ -97,7 +85,10 @@ public: void set_inverted_flight(bool inverted) override { _inverted_flight = inverted; } - + + // set the PID notch sample rates + void set_notch_sample_rate(float sample_rate) override; + // user settable parameters static const struct AP_Param::GroupInfo var_info[]; diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp index 2254ebd7b6..99ef248091 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp @@ -2,6 +2,7 @@ #include #include #include +#include // table of user settable parameters const AP_Param::GroupInfo AC_AttitudeControl_Multi::var_info[] = { @@ -79,6 +80,46 @@ const AP_Param::GroupInfo AC_AttitudeControl_Multi::var_info[] = { // @Description: Roll axis rate controller PD sum maximum. The maximum/minimum value that the sum of the P and D term can output // @Range: 0 1 // @Increment: 0.01 + + // @Param: RAT_RLL_ADV + // @DisplayName: Roll Advanced parameters enable + // @Description: Roll Advanced parameters enable + // @Values: 0:Disabled,1:Enabled + // @User: Advanced + + // @Param: RAT_RLL_D_FF + // @DisplayName: Roll Derivative FeedForward Gain + // @Description: FF D Gain which produces an output that is proportional to the rate of change of the target + // @Range: 0 0.02 + // @Increment: 0.0001 + // @User: Advanced + + // @Param: RAT_RLL_NTF + // @DisplayName: Roll Target notch Filter center frequency + // @Description: Roll Target notch Filter center frequency in Hz. + // @Range: 10 495 + // @Units: Hz + // @User: Advanced + + // @Param: RAT_RLL_NEF + // @DisplayName: Roll Error notch Filter center frequency + // @Description: Roll Error notch Filter center frequency in Hz. + // @Range: 10 495 + // @Units: Hz + // @User: Advanced + + // @Param: RAT_RLL_NBW + // @DisplayName: Roll notch Filter bandwidth + // @Description: Roll notch Filter bandwidth in Hz. + // @Range: 5 250 + // @Units: Hz + // @User: Advanced + + // @Param: RAT_RLL_NATT + // @DisplayName: Roll notch Filter attenuation + // @Description: Roll notch Filter attenuation in dB. + // @Range: 5 50 + // @Units: dB // @User: Advanced AP_SUBGROUPINFO(_pid_rate_roll, "RAT_RLL_", 1, AC_AttitudeControl_Multi, AC_PID), @@ -154,6 +195,46 @@ const AP_Param::GroupInfo AC_AttitudeControl_Multi::var_info[] = { // @Description: Pitch axis rate controller PD sum maximum. The maximum/minimum value that the sum of the P and D term can output // @Range: 0 1 // @Increment: 0.01 + + // @Param: RAT_PIT_ADV + // @DisplayName: Pitch Advanced parameters enable + // @Description: Pitch Advanced parameters enable + // @Values: 0:Disabled,1:Enabled + // @User: Advanced + + // @Param: RAT_PIT_D_FF + // @DisplayName: Pitch Derivative FeedForward Gain + // @Description: FF D Gain which produces an output that is proportional to the rate of change of the target + // @Range: 0 0.02 + // @Increment: 0.0001 + // @User: Advanced + + // @Param: RAT_PIT_NTF + // @DisplayName: Pitch Target notch Filter center frequency + // @Description: Pitch Target notch Filter center frequency in Hz. + // @Range: 10 495 + // @Units: Hz + // @User: Advanced + + // @Param: RAT_PIT_NEF + // @DisplayName: Pitch Error notch Filter center frequency + // @Description: Pitch Error notch Filter center frequency in Hz. + // @Range: 10 495 + // @Units: Hz + // @User: Advanced + + // @Param: RAT_PIT_NBW + // @DisplayName: Pitch notch Filter bandwidth + // @Description: Pitch notch Filter bandwidth in Hz. + // @Range: 5 250 + // @Units: Hz + // @User: Advanced + + // @Param: RAT_PIT_NATT + // @DisplayName: Pitch notch Filter attenuation + // @Description: Pitch notch Filter attenuation in dB. + // @Range: 5 50 + // @Units: dB // @User: Advanced AP_SUBGROUPINFO(_pid_rate_pitch, "RAT_PIT_", 2, AC_AttitudeControl_Multi, AC_PID), @@ -229,6 +310,46 @@ const AP_Param::GroupInfo AC_AttitudeControl_Multi::var_info[] = { // @Description: Yaw axis rate controller PD sum maximum. The maximum/minimum value that the sum of the P and D term can output // @Range: 0 1 // @Increment: 0.01 + + // @Param: RAT_YAW_ADV + // @DisplayName: Yaw Advanced parameters enable + // @Description: Yaw Advanced parameters enable + // @Values: 0:Disabled,1:Enabled + // @User: Advanced + + // @Param: RAT_YAW_D_FF + // @DisplayName: Yaw Derivative FeedForward Gain + // @Description: FF D Gain which produces an output that is proportional to the rate of change of the target + // @Range: 0 0.02 + // @Increment: 0.0001 + // @User: Advanced + + // @Param: RAT_YAW_NTF + // @DisplayName: Yaw Target notch Filter center frequency + // @Description: Yaw Target notch Filter center frequency in Hz. + // @Range: 10 495 + // @Units: Hz + // @User: Advanced + + // @Param: RAT_YAW_NEF + // @DisplayName: Yaw Error notch Filter center frequency + // @Description: Yaw Error notch Filter center frequency in Hz. + // @Range: 10 495 + // @Units: Hz + // @User: Advanced + + // @Param: RAT_YAW_NBW + // @DisplayName: Yaw notch Filter bandwidth + // @Description: Yaw notch Filter bandwidth in Hz. + // @Range: 5 250 + // @Units: Hz + // @User: Advanced + + // @Param: RAT_YAW_NATT + // @DisplayName: Yaw notch Filter attenuation + // @Description: Yaw notch Filter attenuation in dB. + // @Range: 5 50 + // @Units: dB // @User: Advanced AP_SUBGROUPINFO(_pid_rate_yaw, "RAT_YAW_", 3, AC_AttitudeControl_Multi, AC_PID), @@ -269,6 +390,10 @@ AC_AttitudeControl_Multi::AC_AttitudeControl_Multi(AP_AHRS_View &ahrs, const AP_ _motors_multi(motors) { AP_Param::setup_object_defaults(this, var_info); + +#if AC_PID_ADVANCED_ENABLED + set_notch_sample_rate(AP::scheduler().get_loop_rate_hz()); +#endif } // Update Alt_Hold angle maximum @@ -431,3 +556,12 @@ void AC_AttitudeControl_Multi::parameter_sanity_check() _thr_mix_max.set_and_save(AC_ATTITUDE_CONTROL_MAX_DEFAULT); } } + +void AC_AttitudeControl_Multi::set_notch_sample_rate(float sample_rate) +{ +#if AC_PID_ADVANCED_ENABLED + _pid_rate_roll.set_notch_sample_rate(sample_rate); + _pid_rate_pitch.set_notch_sample_rate(sample_rate); + _pid_rate_yaw.set_notch_sample_rate(sample_rate); +#endif +} diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h index 8c85c666c2..c9966cbefd 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h @@ -81,6 +81,9 @@ public: // sanity check parameters. should be called once before take-off void parameter_sanity_check() override; + // set the PID notch sample rates + void set_notch_sample_rate(float sample_rate) override; + // user settable parameters static const struct AP_Param::GroupInfo var_info[]; diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.cpp index cc7f7c566e..71b28013de 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.cpp @@ -78,6 +78,46 @@ const AP_Param::GroupInfo AC_AttitudeControl_Sub::var_info[] = { // @Description: Roll axis rate controller PD sum maximum. The maximum/minimum value that the sum of the P and D term can output // @Range: 0 1 // @Increment: 0.01 + + // @Param: RAT_RLL_ADV + // @DisplayName: Roll Advanced parameters enable + // @Description: Roll Advanced parameters enable + // @Values: 0:Disabled,1:Enabled + // @User: Advanced + + // @Param: RAT_RLL_D_FF + // @DisplayName: Roll Derivative FeedForward Gain + // @Description: FF D Gain which produces an output that is proportional to the rate of change of the target + // @Range: 0 0.02 + // @Increment: 0.0001 + // @User: Advanced + + // @Param: RAT_RLL_NTF + // @DisplayName: Roll Target notch Filter center frequency + // @Description: Roll Target notch Filter center frequency in Hz. + // @Range: 10 495 + // @Units: Hz + // @User: Advanced + + // @Param: RAT_RLL_NEF + // @DisplayName: Roll Error notch Filter center frequency + // @Description: Roll Error notch Filter center frequency in Hz. + // @Range: 10 495 + // @Units: Hz + // @User: Advanced + + // @Param: RAT_RLL_NBW + // @DisplayName: Roll notch Filter bandwidth + // @Description: Roll notch Filter bandwidth in Hz. + // @Range: 5 250 + // @Units: Hz + // @User: Advanced + + // @Param: RAT_RLL_NATT + // @DisplayName: Roll notch Filter attenuation + // @Description: Roll notch Filter attenuation in dB. + // @Range: 5 50 + // @Units: dB // @User: Advanced AP_SUBGROUPINFO(_pid_rate_roll, "RAT_RLL_", 1, AC_AttitudeControl_Sub, AC_PID), @@ -153,6 +193,46 @@ const AP_Param::GroupInfo AC_AttitudeControl_Sub::var_info[] = { // @Description: Pitch axis rate controller PD sum maximum. The maximum/minimum value that the sum of the P and D term can output // @Range: 0 1 // @Increment: 0.01 + + // @Param: RAT_PIT_ADV + // @DisplayName: Pitch Advanced parameters enable + // @Description: Pitch Advanced parameters enable + // @Values: 0:Disabled,1:Enabled + // @User: Advanced + + // @Param: RAT_PIT_D_FF + // @DisplayName: Pitch Derivative FeedForward Gain + // @Description: FF D Gain which produces an output that is proportional to the rate of change of the target + // @Range: 0 0.02 + // @Increment: 0.0001 + // @User: Advanced + + // @Param: RAT_PIT_NTF + // @DisplayName: Pitch Target notch Filter center frequency + // @Description: Pitch Target notch Filter center frequency in Hz. + // @Range: 10 495 + // @Units: Hz + // @User: Advanced + + // @Param: RAT_PIT_NEF + // @DisplayName: Pitch Error notch Filter center frequency + // @Description: Pitch Error notch Filter center frequency in Hz. + // @Range: 10 495 + // @Units: Hz + // @User: Advanced + + // @Param: RAT_PIT_NBW + // @DisplayName: Pitch notch Filter bandwidth + // @Description: Pitch notch Filter bandwidth in Hz. + // @Range: 5 250 + // @Units: Hz + // @User: Advanced + + // @Param: RAT_PIT_NATT + // @DisplayName: Pitch notch Filter attenuation + // @Description: Pitch notch Filter attenuation in dB. + // @Range: 5 50 + // @Units: dB // @User: Advanced AP_SUBGROUPINFO(_pid_rate_pitch, "RAT_PIT_", 2, AC_AttitudeControl_Sub, AC_PID), @@ -228,6 +308,46 @@ const AP_Param::GroupInfo AC_AttitudeControl_Sub::var_info[] = { // @Description: Yaw axis rate controller PD sum maximum. The maximum/minimum value that the sum of the P and D term can output // @Range: 0 1 // @Increment: 0.01 + + // @Param: RAT_YAW_ADV + // @DisplayName: Yaw Advanced parameters enable + // @Description: Yaw Advanced parameters enable + // @Values: 0:Disabled,1:Enabled + // @User: Advanced + + // @Param: RAT_YAW_D_FF + // @DisplayName: Yaw Derivative FeedForward Gain + // @Description: FF D Gain which produces an output that is proportional to the rate of change of the target + // @Range: 0 0.02 + // @Increment: 0.0001 + // @User: Advanced + + // @Param: RAT_YAW_NTF + // @DisplayName: Yaw Target notch Filter center frequency + // @Description: Yaw Target notch Filter center frequency in Hz. + // @Range: 10 495 + // @Units: Hz + // @User: Advanced + + // @Param: RAT_YAW_NEF + // @DisplayName: Yaw Error notch Filter center frequency + // @Description: Yaw Error notch Filter center frequency in Hz. + // @Range: 10 495 + // @Units: Hz + // @User: Advanced + + // @Param: RAT_YAW_NBW + // @DisplayName: Yaw notch Filter bandwidth + // @Description: Yaw notch Filter bandwidth in Hz. + // @Range: 5 250 + // @Units: Hz + // @User: Advanced + + // @Param: RAT_YAW_NATT + // @DisplayName: Yaw notch Filter attenuation + // @Description: Yaw notch Filter attenuation in dB. + // @Range: 5 50 + // @Units: dB // @User: Advanced AP_SUBGROUPINFO(_pid_rate_yaw, "RAT_YAW_", 3, AC_AttitudeControl_Sub, AC_PID), @@ -429,3 +549,12 @@ void AC_AttitudeControl_Sub::input_euler_angle_roll_pitch_slew_yaw(float euler_r input_euler_angle_roll_pitch_yaw(euler_roll_angle_cd, euler_pitch_angle_cd, euler_yaw_angle_cd, true); } } + +void AC_AttitudeControl_Sub::set_notch_sample_rate(float sample_rate) +{ +#if AC_PID_ADVANCED_ENABLED + _pid_rate_roll.set_notch_sample_rate(sample_rate); + _pid_rate_pitch.set_notch_sample_rate(sample_rate); + _pid_rate_yaw.set_notch_sample_rate(sample_rate); +#endif +} \ No newline at end of file diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.h index 18a627a3d7..fb69f7094f 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.h @@ -65,6 +65,9 @@ public: // sanity check parameters. should be called once before take-off void parameter_sanity_check() override; + // set the PID notch sample rates + void set_notch_sample_rate(float sample_rate) override; + // This function ensures that the ROV reaches the target orientation with the desired yaw rate void input_euler_angle_roll_pitch_slew_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, float slew_yaw); diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 06eed5ddba..c952cbd65a 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -209,6 +209,46 @@ const AP_Param::GroupInfo AC_PosControl::var_info[] = { // @Description: Acceleration (vertical) controller PD sum maximum. The maximum/minimum value that the sum of the P and D term can output // @Range: 0 1000 // @Units: d% + + // @Param: _ACCZ_ADV + // @DisplayName: Accel (vertical) Advanced parameters enable + // @Description: Accel (vertical) Advanced parameters enable + // @Values: 0:Disabled,1:Enabled + // @User: Advanced + + // @Param: _ACCZ_D_FF + // @DisplayName: Accel (vertical) Derivative FeedForward Gain + // @Description: FF D Gain which produces an output that is proportional to the rate of change of the target + // @Range: 0 0.02 + // @Increment: 0.0001 + // @User: Advanced + + // @Param: _ACCZ_NTF + // @DisplayName: Accel (vertical) Target notch Filter center frequency + // @Description: Accel (vertical) Target notch Filter center frequency in Hz. + // @Range: 10 495 + // @Units: Hz + // @User: Advanced + + // @Param: _ACCZ_NEF + // @DisplayName: Accel (vertical) Error notch Filter center frequency + // @Description: Accel (vertical) Error notch Filter center frequency in Hz. + // @Range: 10 495 + // @Units: Hz + // @User: Advanced + + // @Param: _ACCZ_NBW + // @DisplayName: Accel (vertical) notch Filter bandwidth + // @Description: Accel (vertical) notch Filter bandwidth in Hz. + // @Range: 5 250 + // @Units: Hz + // @User: Advanced + + // @Param: _ACCZ_NATT + // @DisplayName: Accel (vertical) notch Filter attenuation + // @Description: Accel (vertical) notch Filter attenuation in dB. + // @Range: 5 50 + // @Units: dB // @User: Advanced AP_SUBGROUPINFO(_pid_accel_z, "_ACCZ_", 4, AC_PosControl, AC_PID),