From b7a969d46211de506edf1985f949d5430a15c73f Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sun, 1 Oct 2023 18:21:00 +0100 Subject: [PATCH] AC_AttitudeControl: update for new AC_PID settings enable filters with AP_FILTER_ENABLED --- .../AC_AttitudeControl_Heli.cpp | 105 ++++-------------- .../AC_AttitudeControl_Multi.cpp | 105 ++++-------------- .../AC_AttitudeControl_Sub.cpp | 104 ++++------------- .../AC_AttitudeControl/AC_PosControl.cpp | 34 +----- 4 files changed, 65 insertions(+), 283 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp index 26004c3a13..982ff2048a 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp @@ -88,12 +88,6 @@ 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 @@ -102,31 +96,15 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] = { // @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 + // @DisplayName: Roll Target notch filter index + // @Description: Roll Target notch filter index + // @Range: 1 8 // @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 + // @DisplayName: Roll Error notch filter index + // @Description: Roll Error notch filter index + // @Range: 1 8 // @User: Advanced AP_SUBGROUPINFO(_pid_rate_roll, "RAT_RLL_", 2, AC_AttitudeControl_Heli, AC_HELI_PID), @@ -203,12 +181,6 @@ 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 @@ -217,31 +189,15 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] = { // @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 + // @DisplayName: Pitch Target notch filter index + // @Description: Pitch Target notch filter index + // @Range: 1 8 // @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 + // @DisplayName: Pitch Error notch filter index + // @Description: Pitch Error notch filter index + // @Range: 1 8 // @User: Advanced AP_SUBGROUPINFO(_pid_rate_pitch, "RAT_PIT_", 3, AC_AttitudeControl_Heli, AC_HELI_PID), @@ -318,12 +274,6 @@ 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 @@ -332,31 +282,16 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] = { // @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 + // @DisplayName: Yaw Target notch filter index + // @Description: Yaw Target notch filter index + // @Range: 1 8 // @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 + // @DisplayName: Yaw Error notch filter index + // @Description: Yaw Error notch filter index + // @Range: 1 8 // @User: Advanced AP_SUBGROUPINFO(_pid_rate_yaw, "RAT_YAW_", 4, AC_AttitudeControl_Heli, AC_HELI_PID), @@ -383,7 +318,7 @@ AC_AttitudeControl_Heli::AC_AttitudeControl_Heli(AP_AHRS_View &ahrs, const AP_Mu _flags_heli.leaky_i = true; _flags_heli.flybar_passthrough = false; _flags_heli.tail_passthrough = false; -#if AC_PID_ADVANCED_ENABLED +#if AP_FILTER_ENABLED set_notch_sample_rate(AP::scheduler().get_loop_rate_hz()); #endif } @@ -629,7 +564,7 @@ void AC_AttitudeControl_Heli::input_euler_angle_roll_pitch_yaw(float euler_roll_ void AC_AttitudeControl_Heli::set_notch_sample_rate(float sample_rate) { -#if AC_PID_ADVANCED_ENABLED +#if AP_FILTER_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); diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp index 99ef248091..947975a0a1 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp @@ -81,12 +81,6 @@ const AP_Param::GroupInfo AC_AttitudeControl_Multi::var_info[] = { // @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 @@ -95,31 +89,15 @@ const AP_Param::GroupInfo AC_AttitudeControl_Multi::var_info[] = { // @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 + // @DisplayName: Roll Target notch filter index + // @Description: Roll Target notch filter index + // @Range: 1 8 // @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 + // @DisplayName: Roll Error notch filter index + // @Description: Roll Error notch filter index + // @Range: 1 8 // @User: Advanced AP_SUBGROUPINFO(_pid_rate_roll, "RAT_RLL_", 1, AC_AttitudeControl_Multi, AC_PID), @@ -196,12 +174,6 @@ const AP_Param::GroupInfo AC_AttitudeControl_Multi::var_info[] = { // @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 @@ -210,31 +182,15 @@ const AP_Param::GroupInfo AC_AttitudeControl_Multi::var_info[] = { // @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 + // @DisplayName: Pitch Target notch filter index + // @Description: Pitch Target notch filter index + // @Range: 1 8 // @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 + // @DisplayName: Pitch Error notch filter index + // @Description: Pitch Error notch filter index + // @Range: 1 8 // @User: Advanced AP_SUBGROUPINFO(_pid_rate_pitch, "RAT_PIT_", 2, AC_AttitudeControl_Multi, AC_PID), @@ -311,12 +267,6 @@ const AP_Param::GroupInfo AC_AttitudeControl_Multi::var_info[] = { // @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 @@ -325,31 +275,16 @@ const AP_Param::GroupInfo AC_AttitudeControl_Multi::var_info[] = { // @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 + // @DisplayName: Yaw Target notch filter index + // @Description: Yaw Target notch filter index + // @Range: 1 8 // @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 + // @DisplayName: Yaw Error notch filter index + // @Description: Yaw Error notch filter index + // @Range: 1 8 // @User: Advanced AP_SUBGROUPINFO(_pid_rate_yaw, "RAT_YAW_", 3, AC_AttitudeControl_Multi, AC_PID), @@ -391,7 +326,7 @@ AC_AttitudeControl_Multi::AC_AttitudeControl_Multi(AP_AHRS_View &ahrs, const AP_ { AP_Param::setup_object_defaults(this, var_info); -#if AC_PID_ADVANCED_ENABLED +#if AP_FILTER_ENABLED set_notch_sample_rate(AP::scheduler().get_loop_rate_hz()); #endif } @@ -559,7 +494,7 @@ void AC_AttitudeControl_Multi::parameter_sanity_check() void AC_AttitudeControl_Multi::set_notch_sample_rate(float sample_rate) { -#if AC_PID_ADVANCED_ENABLED +#if AP_FILTER_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); diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.cpp index 71b28013de..7f2791c241 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.cpp @@ -79,12 +79,6 @@ const AP_Param::GroupInfo AC_AttitudeControl_Sub::var_info[] = { // @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 @@ -93,31 +87,15 @@ const AP_Param::GroupInfo AC_AttitudeControl_Sub::var_info[] = { // @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 + // @DisplayName: Roll Target notch filter index + // @Description: Roll Target notch filter index + // @Range: 1 8 // @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 + // @DisplayName: Roll Error notch filter index + // @Description: Roll Error notch filter index + // @Range: 1 8 // @User: Advanced AP_SUBGROUPINFO(_pid_rate_roll, "RAT_RLL_", 1, AC_AttitudeControl_Sub, AC_PID), @@ -194,12 +172,6 @@ const AP_Param::GroupInfo AC_AttitudeControl_Sub::var_info[] = { // @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 @@ -208,31 +180,15 @@ const AP_Param::GroupInfo AC_AttitudeControl_Sub::var_info[] = { // @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 + // @DisplayName: Pitch Target notch filter index + // @Description: Pitch Target notch filter index + // @Range: 1 8 // @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 + // @DisplayName: Pitch Error notch filter index + // @Description: Pitch Error notch filter index + // @Range: 1 8 // @User: Advanced AP_SUBGROUPINFO(_pid_rate_pitch, "RAT_PIT_", 2, AC_AttitudeControl_Sub, AC_PID), @@ -309,12 +265,6 @@ const AP_Param::GroupInfo AC_AttitudeControl_Sub::var_info[] = { // @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 @@ -323,31 +273,15 @@ const AP_Param::GroupInfo AC_AttitudeControl_Sub::var_info[] = { // @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 + // @DisplayName: Yaw Target notch filter index + // @Description: Yaw Target notch filter index + // @Range: 1 8 // @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 + // @DisplayName: Yaw Error notch filter index + // @Description: Yaw Error notch filter index + // @Range: 1 8 // @User: Advanced AP_SUBGROUPINFO(_pid_rate_yaw, "RAT_YAW_", 3, AC_AttitudeControl_Sub, AC_PID), @@ -552,7 +486,7 @@ void AC_AttitudeControl_Sub::input_euler_angle_roll_pitch_slew_yaw(float euler_r void AC_AttitudeControl_Sub::set_notch_sample_rate(float sample_rate) { -#if AC_PID_ADVANCED_ENABLED +#if AP_FILTER_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); diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index c952cbd65a..282237fb3a 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -210,12 +210,6 @@ const AP_Param::GroupInfo AC_PosControl::var_info[] = { // @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 @@ -224,31 +218,15 @@ const AP_Param::GroupInfo AC_PosControl::var_info[] = { // @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 + // @DisplayName: Accel (vertical) Target notch filter index + // @Description: Accel (vertical) Target notch filter index + // @Range: 1 8 // @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 + // @DisplayName: Accel (vertical) Error notch filter index + // @Description: Accel (vertical) Error notch filter index + // @Range: 1 8 // @User: Advanced AP_SUBGROUPINFO(_pid_accel_z, "_ACCZ_", 4, AC_PosControl, AC_PID),