From 370ee7d4d3e192d85e8966ab743dfa0ffcf9ea47 Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Thu, 10 Aug 2023 12:10:17 +0100
Subject: [PATCH] AP_WheelEncoder: add documentation for PID notches and D
 feed-foward

add advanced flag to PIDs and selectively compile advanced PID options
---
 .../AP_WheelEncoder/AP_WheelRateControl.cpp   | 88 +++++++++++++++++++
 .../AP_WheelEncoder/AP_WheelRateControl.h     |  3 +
 2 files changed, 91 insertions(+)

diff --git a/libraries/AP_WheelEncoder/AP_WheelRateControl.cpp b/libraries/AP_WheelEncoder/AP_WheelRateControl.cpp
index e23a13b93f..0edf03fe9d 100644
--- a/libraries/AP_WheelEncoder/AP_WheelRateControl.cpp
+++ b/libraries/AP_WheelEncoder/AP_WheelRateControl.cpp
@@ -90,6 +90,46 @@ const AP_Param::GroupInfo AP_WheelRateControl::var_info[] = {
     // @DisplayName: Wheel rate control PD sum maximum
     // @Description: Wheel rate control PD sum maximum.  The maximum/minimum value that the sum of the P and D term can output
     // @Range: 0.000 1.000
+
+    // @Param: _RATE_ADV
+    // @DisplayName: Wheel rate Advanced parameters enable
+    // @Description: Wheel rate Advanced parameters enable
+    // @Values: 0:Disabled,1:Enabled
+    // @User: Advanced
+
+    // @Param: _RATE_D_FF
+    // @DisplayName: Wheel rate Derivative FeedForward Gain
+    // @Description: FF D Gain which produces an output that is proportional to the rate of change of the error
+    // @Range: 0.000 0.400
+    // @Increment: 0.001
+    // @User: Advanced
+
+    // @Param: _RATE_NTF
+    // @DisplayName: Wheel rate Target notch Filter center frequency
+    // @Description: Wheel rate Target notch Filter center frequency in Hz.
+    // @Range: 10 495
+    // @Units: Hz
+    // @User: Advanced
+
+    // @Param: _RATE_NEF
+    // @DisplayName: Wheel rate Error notch Filter center frequency
+    // @Description: Wheel rate Error notch Filter center frequency in Hz.
+    // @Range: 10 495
+    // @Units: Hz
+    // @User: Advanced
+
+    // @Param: _RATE_NBW
+    // @DisplayName: Wheel rate notch Filter bandwidth
+    // @Description: Wheel rate notch Filter bandwidth in Hz.
+    // @Range: 5 250
+    // @Units: Hz
+    // @User: Advanced
+
+    // @Param: _RATE_NATT
+    // @DisplayName: Wheel rate notch Filter attenuation
+    // @Description: Wheel rate notch Filter attenuation in dB.
+    // @Range: 5 50
+    // @Units: dB
     // @User: Advanced
 
     AP_SUBGROUPINFO(_rate_pid0, "_RATE_", 3, AP_WheelRateControl, AC_PID),
@@ -166,6 +206,46 @@ const AP_Param::GroupInfo AP_WheelRateControl::var_info[] = {
     // @DisplayName: Wheel rate control PD sum maximum
     // @Description: Wheel rate control PD sum maximum.  The maximum/minimum value that the sum of the P and D term can output
     // @Range: 0.000 1.000
+
+    // @Param: 2_RATE_ADV
+    // @DisplayName: Wheel rate Advanced parameters enable
+    // @Description: Wheel rate Advanced parameters enable
+    // @Values: 0:Disabled,1:Enabled
+    // @User: Advanced
+
+    // @Param: 2_RATE_D_FF
+    // @DisplayName: Wheel rate Derivative FeedForward Gain
+    // @Description: FF D Gain which produces an output that is proportional to the rate of change of the target
+    // @Range: 0.000 0.400
+    // @Increment: 0.001
+    // @User: Advanced
+
+    // @Param: 2_RATE_NTF
+    // @DisplayName: Wheel rate Target notch Filter center frequency
+    // @Description: Wheel rate Target notch Filter center frequency in Hz.
+    // @Range: 10 495
+    // @Units: Hz
+    // @User: Advanced
+
+    // @Param: 2_RATE_NEF
+    // @DisplayName: Wheel rate Error notch Filter center frequency
+    // @Description: Wheel rate Error notch Filter center frequency in Hz.
+    // @Range: 10 495
+    // @Units: Hz
+    // @User: Advanced
+
+    // @Param: 2_RATE_NBW
+    // @DisplayName: Wheel rate notch Filter bandwidth
+    // @Description: Wheel rate notch Filter bandwidth in Hz.
+    // @Range: 5 250
+    // @Units: Hz
+    // @User: Advanced
+
+    // @Param: 2_RATE_NATT
+    // @DisplayName: Wheel rate notch Filter attenuation
+    // @Description: Wheel rate notch Filter attenuation in dB.
+    // @Range: 5 50
+    // @Units: dB
     // @User: Advanced
 
     AP_SUBGROUPINFO(_rate_pid1, "2_RATE_", 4, AP_WheelRateControl, AC_PID),
@@ -237,3 +317,11 @@ AC_PID& AP_WheelRateControl::get_pid(uint8_t instance)
         return _rate_pid1;
     }
 }
+
+void AP_WheelRateControl::set_notch_sample_rate(float sample_rate)
+{
+#if AC_PID_ADVANCED_ENABLED
+    _rate_pid0.set_notch_sample_rate(sample_rate);
+    _rate_pid1.set_notch_sample_rate(sample_rate);
+#endif
+}
diff --git a/libraries/AP_WheelEncoder/AP_WheelRateControl.h b/libraries/AP_WheelEncoder/AP_WheelRateControl.h
index 1f72817845..f62d981bfb 100644
--- a/libraries/AP_WheelEncoder/AP_WheelRateControl.h
+++ b/libraries/AP_WheelEncoder/AP_WheelRateControl.h
@@ -51,6 +51,9 @@ public:
     // get pid objects for reporting
     AC_PID& get_pid(uint8_t instance);
 
+    // set the PID notch sample rates
+    void set_notch_sample_rate(float sample_rate);
+
     static const struct AP_Param::GroupInfo        var_info[];
 
 private: