Support per-motor throttle-based harmonic notch

libraries/AP_Motors/AP_MotorsMulticopter.cpp

@@ -457,6 +457,21 @@ float AP_MotorsMulticopter::actuator_spin_up_to_ground_idle() const
     return constrain_float(_spin_up_ratio, 0.0f, 1.0f) * thr_lin.get_spin_min();
 }
 
+// return throttle out for motor motor_num, returns true if value is valid false otherwise
+bool AP_MotorsMulticopter::get_thrust(uint8_t motor_num, float& thr_out) const
+{
+    if (motor_num >= AP_MOTORS_MAX_NUM_MOTORS || !motor_enabled[motor_num]) {
+        return false;
+    }
+
+    // Constrain to linearization range.
+    const float actuator = constrain_float(_actuator[motor_num], thr_lin.get_spin_min(), thr_lin.get_spin_max());
+
+    // Remove linearization and compensation gain
+    thr_out = thr_lin.actuator_to_thrust(actuator) / thr_lin.get_compensation_gain();
+    return true;
+}
+
 // parameter checks for MOT_PWM_MIN/MAX, returns true if parameters are valid
 bool AP_MotorsMulticopter::check_mot_pwm_params() const
 {

libraries/AP_Motors/AP_MotorsMulticopter.h

@@ -89,6 +89,9 @@ class AP_MotorsMulticopter : public AP_Motors {
     // convert values to PWM min and max if not configured
     void                convert_pwm_min_max_param(int16_t radio_min, int16_t radio_max);
 
+    // return thrust for motor motor_num, returns true if value is valid false otherwise
+    bool                get_thrust(uint8_t motor_num, float& thr_out) const override;
+
 #if HAL_LOGGING_ENABLED
     // 10hz logging of voltage scaling and max trust
     void                Log_Write() override;

libraries/AP_Motors/AP_Motors_Class.h

@@ -155,6 +155,7 @@ class AP_Motors {
     float               get_yaw() const { return _yaw_in; }
     float               get_yaw_ff() const { return _yaw_in_ff; }
     float               get_throttle_out() const { return _throttle_out; }
+    virtual bool        get_thrust(uint8_t motor_num, float& thr_out) const { return false; }
     float               get_throttle() const { return constrain_float(_throttle_filter.get(), 0.0f, 1.0f); }
     float               get_throttle_bidirectional() const { return constrain_float(2 * (_throttle_filter.get() - 0.5f), -1.0f, 1.0f); }
     float               get_throttle_slew_rate() const { return _throttle_slew_rate; }

libraries/AP_Vehicle/AP_Vehicle.cpp

@@ -764,21 +764,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
 }