From 07ad4798dc105875a66c2c88fca402732f8935d4 Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Sat, 30 May 2020 14:07:46 +0100
Subject: [PATCH] AP_BLHeli: allow all motor frequencies to be returned

cater for ESCs not returning telemetry all the time
slew rpm updates for the harmonic notch
---
 libraries/AP_BLHeli/AP_BLHeli.cpp | 25 ++++++++++++++++++++++++-
 libraries/AP_BLHeli/AP_BLHeli.h   |  6 ++++++
 2 files changed, 30 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_BLHeli/AP_BLHeli.cpp b/libraries/AP_BLHeli/AP_BLHeli.cpp
index 3684fcd8ff..0aa5691736 100644
--- a/libraries/AP_BLHeli/AP_BLHeli.cpp
+++ b/libraries/AP_BLHeli/AP_BLHeli.cpp
@@ -121,6 +121,8 @@ const AP_Param::GroupInfo AP_BLHeli::var_info[] = {
     AP_GROUPEND
 };
 
+#define RPM_SLEW_RATE 50
+
 AP_BLHeli *AP_BLHeli::_singleton;
 
 // constructor
@@ -1337,7 +1339,9 @@ float AP_BLHeli::get_average_motor_frequency_hz() const
     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;
+            // slew the update
+            const float slew = MIN(1.0f, (now - last_telem[i].timestamp_ms) * telem_rate / 1000.0f);
+            motor_freq += (prev_motor_rpm[i] + (last_telem[i].rpm - prev_motor_rpm[i]) * slew) / 60.0f;
         }
     }
     if (valid_escs > 0) {
@@ -1347,6 +1351,23 @@ float AP_BLHeli::get_average_motor_frequency_hz() const
     return motor_freq;
 }
 
+// return all the motor frequencies in Hz for dynamic filtering
+uint8_t AP_BLHeli::get_motor_frequencies_hz(uint8_t nfreqs, float* freqs) const
+{
+    const 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 < nfreqs; i++) {
+        if (last_telem[i].timestamp_ms && (now - last_telem[i].timestamp_ms < 1000)) {
+            // slew the update
+            const float slew = MIN(1.0f, (now - last_telem[i].timestamp_ms) * telem_rate / 1000.0f);
+            freqs[valid_escs++] = (prev_motor_rpm[i] + (last_telem[i].rpm - prev_motor_rpm[i]) * slew) / 60.0f;
+        }
+    }
+
+    return MIN(valid_escs, nfreqs);
+}
+
 /*
   implement the 8 bit CRC used by the BLHeli ESC telemetry protocol
  */
@@ -1391,6 +1412,8 @@ void AP_BLHeli::read_telemetry_packet(void)
     td.consumption = (buf[5]<<8) | buf[6];
     td.rpm = ((buf[7]<<8) | buf[8]) * 200 / motor_poles;
     td.timestamp_ms = AP_HAL::millis();
+    // record the previous rpm so that we can slew to the new one
+    prev_motor_rpm[last_telem_esc] = last_telem[last_telem_esc].rpm;
 
     last_telem[last_telem_esc] = td;
     last_telem[last_telem_esc].count++;
diff --git a/libraries/AP_BLHeli/AP_BLHeli.h b/libraries/AP_BLHeli/AP_BLHeli.h
index f24ebb1bec..562f98c394 100644
--- a/libraries/AP_BLHeli/AP_BLHeli.h
+++ b/libraries/AP_BLHeli/AP_BLHeli.h
@@ -29,6 +29,7 @@
 #define HAVE_AP_BLHELI_SUPPORT
 
 #include <AP_Param/AP_Param.h>
+#include <Filter/LowPassFilter.h>
 #include "msp_protocol.h"
 #include "blheli_4way_protocol.h"
 
@@ -59,6 +60,8 @@ public:
     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;
+    // return all of the motor frequencies in Hz for dynamic filtering
+    uint8_t get_motor_frequencies_hz(uint8_t nfreqs, float* freqs) const;
 
     static AP_BLHeli *get_singleton(void) {
         return _singleton;
@@ -224,6 +227,9 @@ private:
 
     struct telem_data last_telem[max_motors];
 
+    // previous motor rpm so that changes can be slewed
+    float prev_motor_rpm[max_motors];
+
     // have we initialised the interface?
     bool initialised;