AP_BLHeli: add get_average_motor_frequency_hz() for dynamic filtering

correctly calculate rpm from erpm

libraries/AP_BLHeli/AP_BLHeli.cpp

@@ -1296,6 +1296,26 @@ bool AP_BLHeli::get_telem_data(uint8_t esc_index, struct telem_data &td)
     return true;
 }
 
+// return the average motor frequency in Hz for dynamic filtering
+float AP_BLHeli::get_average_motor_frequency_hz() const
+{
+    float motor_freq = 0.0f;
+    uint32_t now = AP_HAL::millis();
+    uint8_t valid_escs = 0;
+    // average the rpm of each motor as reported by BLHeli and convert to Hz
+    for (uint8_t i = 0; i < num_motors; i++) {
+        if (last_telem[i].timestamp_ms && (now - last_telem[i].timestamp_ms < 1000)) {
+            valid_escs++;
+            motor_freq += last_telem[i].rpm / 60.0f;
+        }
+    }
+    if (valid_escs) {
+        motor_freq /= valid_escs;
+    }
+
+    return motor_freq;
+}
+
 /*
   implement the 8 bit CRC used by the BLHeli ESC telemetry protocol
  */
@@ -1342,7 +1362,7 @@ void AP_BLHeli::read_telemetry_packet(void)
     td.voltage = (buf[1]<<8) | buf[2];
     td.current = (buf[3]<<8) | buf[4];
     td.consumption = (buf[5]<<8) | buf[6];
-    td.rpm = ((buf[7]<<8) | buf[8]) * 100 * 2 / motor_poles;
+    td.rpm = ((buf[7]<<8) | buf[8]) * 200 / motor_poles;
     td.timestamp_ms = AP_HAL::millis();
 
     last_telem[last_telem_esc] = td;

libraries/AP_BLHeli/AP_BLHeli.h

@@ -57,6 +57,8 @@ public:
 
     // get the most recent telemetry data packet for a motor
     bool get_telem_data(uint8_t esc_index, struct telem_data &td);
+    // return the average motor frequency in Hz for dynamic filtering
+    float get_average_motor_frequency_hz() const;
 
     static AP_BLHeli *get_singleton(void) {
         return _singleton;