AP_Vehicle: support per-motor throttle based notches

mirkix · Aug 7, 2024 · fd19c3f · fd19c3f
1 parent e23e58f
commit fd19c3f
Showing 1 changed file with 27 additions and 5 deletions.
diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp
@@ -776,21 +776,43 @@ bool AP_Vehicle::is_crashed() const
 // update the harmonic notch filter for throttle based notch
 void AP_Vehicle::update_throttle_notch(AP_InertialSensor::HarmonicNotch &notch)
 {
-#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI||APM_BUILD_TYPE(APM_BUILD_Rover)
+#if APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_Rover)
     const float ref_freq = notch.params.center_freq_hz();
     const float ref = notch.params.reference();
 
-#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI
+#if APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_COPTER_OR_HELI
     const AP_Motors* motors = AP::motors();
-    const float motors_throttle = motors != nullptr ? MAX(0,motors->get_throttle_out()) : 0;
+    if (motors == nullptr) {
+         notch.update_freq_hz(0);
+         return;
+    }
+    const float motors_throttle = MAX(0,motors->get_throttle_out());
+    // set the harmonic notch filter frequency scaled on measured frequency
+    if (notch.params.hasOption(HarmonicNotchFilterParams::Options::DynamicHarmonic)) {
+        float notches[INS_MAX_NOTCHES];
+        uint8_t motor_num = 0;
+        for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
+            float motor_throttle = 0;
+            if (motors->get_thrust(i, motor_throttle)) {
+                notches[motor_num] = ref_freq * sqrtf(MAX(0, motor_throttle) / ref);
+                motor_num++;
+            }
+            if (motor_num >= INS_MAX_NOTCHES) {
+                break;
+            }
+        }
+        notch.update_frequencies_hz(motor_num, notches);
+    } else
 #else  // APM_BUILD_Rover
     const AP_MotorsUGV *motors = AP::motors_ugv();
     const float motors_throttle = motors != nullptr ? abs(motors->get_throttle() / 100.0f) : 0;
 #endif
+    {
+        float throttle_freq = ref_freq * sqrtf(MAX(0,motors_throttle) / ref);
 
-    float throttle_freq = ref_freq * sqrtf(MAX(0,motors_throttle) / ref);
+        notch.update_freq_hz(throttle_freq);
+    }
 
-    notch.update_freq_hz(throttle_freq);
 #endif
 }