From 952baf860ce473fbb65c219a91fc84c142aeac4f Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 10 Aug 2023 12:09:49 +0100 Subject: [PATCH] AC_CustomControl: add documentation for PID notches and D feed-foward add advanced flag to PIDs and selectively compile advanced PID options --- .../AC_CustomControl/AC_CustomControl.cpp | 9 ++ libraries/AC_CustomControl/AC_CustomControl.h | 3 + .../AC_CustomControl_Backend.h | 3 + .../AC_CustomControl/AC_CustomControl_PID.cpp | 136 ++++++++++++++++++ .../AC_CustomControl/AC_CustomControl_PID.h | 3 + 5 files changed, 154 insertions(+) diff --git a/libraries/AC_CustomControl/AC_CustomControl.cpp b/libraries/AC_CustomControl/AC_CustomControl.cpp index b8c7a58eaa3..26755cbb47f 100644 --- a/libraries/AC_CustomControl/AC_CustomControl.cpp +++ b/libraries/AC_CustomControl/AC_CustomControl.cpp @@ -184,4 +184,13 @@ void AC_CustomControl::log_switch(void) { _custom_controller_active); } +void AC_CustomControl::set_notch_sample_rate(float sample_rate) +{ +#if AC_PID_ADVANCED_ENABLED + if (_backend != nullptr) { + _backend->set_notch_sample_rate(sample_rate); + } +#endif +} + #endif diff --git a/libraries/AC_CustomControl/AC_CustomControl.h b/libraries/AC_CustomControl/AC_CustomControl.h index bf0628302b7..9cda1f21db6 100644 --- a/libraries/AC_CustomControl/AC_CustomControl.h +++ b/libraries/AC_CustomControl/AC_CustomControl.h @@ -32,6 +32,9 @@ public: bool is_safe_to_run(void); void log_switch(void); + // set the PID notch sample rates + void set_notch_sample_rate(float sample_rate); + // zero index controller type param, only use it to access _backend or _backend_var_info array uint8_t get_type() { return _controller_type > 0 ? (_controller_type - 1) : 0; }; diff --git a/libraries/AC_CustomControl/AC_CustomControl_Backend.h b/libraries/AC_CustomControl/AC_CustomControl_Backend.h index d9c537d3454..c4068a7b1b8 100644 --- a/libraries/AC_CustomControl/AC_CustomControl_Backend.h +++ b/libraries/AC_CustomControl/AC_CustomControl_Backend.h @@ -23,6 +23,9 @@ public: // reset controller to avoid build up or abrupt response upon switch, ex: integrator, filter virtual void reset() = 0; + // set the PID notch sample rates + virtual void set_notch_sample_rate(float sample_rate) {}; + protected: // References to external libraries AP_AHRS_View*& _ahrs; diff --git a/libraries/AC_CustomControl/AC_CustomControl_PID.cpp b/libraries/AC_CustomControl/AC_CustomControl_PID.cpp index e468e8f0be0..61ae0fb2a76 100644 --- a/libraries/AC_CustomControl/AC_CustomControl_PID.cpp +++ b/libraries/AC_CustomControl/AC_CustomControl_PID.cpp @@ -101,6 +101,48 @@ const AP_Param::GroupInfo AC_CustomControl_PID::var_info[] = { // @Range: 0 1 // @Increment: 0.01 // @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_atti_rate_roll, "RAT_RLL_", 4, AC_CustomControl_PID, AC_PID), // @Param: RAT_PIT_P @@ -175,6 +217,48 @@ const AP_Param::GroupInfo AC_CustomControl_PID::var_info[] = { // @Range: 0 1 // @Increment: 0.01 // @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_atti_rate_pitch, "RAT_PIT_", 5, AC_CustomControl_PID, AC_PID), @@ -250,6 +334,48 @@ const AP_Param::GroupInfo AC_CustomControl_PID::var_info[] = { // @Range: 0 1 // @Increment: 0.01 // @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_atti_rate_yaw, "RAT_YAW_", 6, AC_CustomControl_PID, AC_PID), AP_GROUPEND @@ -332,4 +458,14 @@ void AC_CustomControl_PID::reset(void) _pid_atti_rate_yaw.reset_filter(); } + +void AC_CustomControl_PID::set_notch_sample_rate(float sample_rate) +{ +#if AC_PID_ADVANCED_ENABLED + _pid_atti_rate_roll.set_notch_sample_rate(sample_rate); + _pid_atti_rate_pitch.set_notch_sample_rate(sample_rate); + _pid_atti_rate_yaw.set_notch_sample_rate(sample_rate); +#endif +} + #endif diff --git a/libraries/AC_CustomControl/AC_CustomControl_PID.h b/libraries/AC_CustomControl/AC_CustomControl_PID.h index a48b5f97595..b3cdf688844 100644 --- a/libraries/AC_CustomControl/AC_CustomControl_PID.h +++ b/libraries/AC_CustomControl/AC_CustomControl_PID.h @@ -21,6 +21,9 @@ public: Vector3f update() override; void reset(void) 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[];