From 6447bd1cdd9070d0cb1c706277fe433e921002c8 Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Mon, 1 Mar 2021 20:40:39 +0000
Subject: [PATCH] AP_HAL_ChibiOS: use AP_ESC_Telem to record erpm data and
 error rate

don't output RPM data if bi-dir dshot is not enabled
---
 libraries/AP_HAL_ChibiOS/RCOutput.cpp | 10 +++++++++-
 libraries/AP_HAL_ChibiOS/RCOutput.h   | 11 ++++++++++-
 2 files changed, 19 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp
index b580f04354..a3fcf3a9aa 100644
--- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp
+++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp
@@ -1353,9 +1353,17 @@ void RCOutput::dshot_send(pwm_group &group, uint32_t time_out_us)
     for (uint8_t i=0; i<4; i++) {
         uint8_t chan = group.chan[i];
         if (group.is_chan_enabled(i)) {
+#ifdef HAL_WITH_BIDIR_DSHOT
             // retrieve the last erpm values
-            _bdshot.erpm[chan] = group.bdshot.erpm[i];
+            const uint16_t erpm = group.bdshot.erpm[i];
 
+            // update the ESC telemetry data
+            if (erpm < 0xFFFF && group.bdshot.enabled) {
+                update_rpm(chan, erpm * 200 / _bdshot.motor_poles, get_erpm_error_rate(chan));
+            }
+
+            _bdshot.erpm[chan] = erpm;
+#endif
             uint16_t pwm = period[chan];
 
             if (safety_on && !(safety_mask & (1U<<(chan+chan_offset)))) {
diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.h b/libraries/AP_HAL_ChibiOS/RCOutput.h
index 203c75c475..6cb9112e46 100644
--- a/libraries/AP_HAL_ChibiOS/RCOutput.h
+++ b/libraries/AP_HAL_ChibiOS/RCOutput.h
@@ -18,6 +18,8 @@
 
 #include "AP_HAL_ChibiOS.h"
 #include <AP_HAL/Semaphores.h>
+#include <AP_ESC_Telem/AP_ESC_Telem.h>
+
 #include "shared_dma.h"
 #include "ch.h"
 #include "hal.h"
@@ -30,7 +32,11 @@
 
 #define RCOU_DSHOT_TIMING_DEBUG 0
 
-class ChibiOS::RCOutput : public AP_HAL::RCOutput {
+class ChibiOS::RCOutput : public AP_HAL::RCOutput
+#ifdef HAL_WITH_BIDIR_DSHOT
+  , AP_ESC_Telem_Backend
+#endif
+{
 public:
     // disabled channel marker
     const static uint8_t CHAN_DISABLED = 255;
@@ -151,6 +157,8 @@ public:
       with Dshot to get telemetry feedback
      */
     void set_bidir_dshot_mask(uint16_t mask) override;
+
+    void set_motor_poles(uint8_t poles) override { _bdshot.motor_poles = poles; }
 #endif
 
     /*
@@ -458,6 +466,7 @@ private:
         uint16_t erpm_errors[max_channels];
         uint16_t erpm_clean_frames[max_channels];
         uint32_t erpm_last_stats_ms[max_channels];
+        uint8_t motor_poles;
 #endif
     } _bdshot;