From d8e9379289afebdaec08d55589257de628264685 Mon Sep 17 00:00:00 2001
From: Tom Pittenger <tom.pittenger@krausaerospace.com>
Date: Mon, 14 Aug 2023 19:16:48 -0700
Subject: [PATCH] AP_RPM: add writing to outbound ESC_Telem

---
 libraries/AP_RPM/AP_RPM.cpp        |  4 ++++
 libraries/AP_RPM/AP_RPM_Params.cpp | 10 ++++++++++
 libraries/AP_RPM/AP_RPM_Params.h   |  4 ++++
 libraries/AP_RPM/AP_RPM_config.h   |  5 +++++
 libraries/AP_RPM/RPM_Backend.cpp   | 22 ++++++++++++++++++++++
 libraries/AP_RPM/RPM_Backend.h     |  2 ++
 6 files changed, 47 insertions(+)

diff --git a/libraries/AP_RPM/AP_RPM.cpp b/libraries/AP_RPM/AP_RPM.cpp
index 11c1bd89e8..de1dc27c96 100644
--- a/libraries/AP_RPM/AP_RPM.cpp
+++ b/libraries/AP_RPM/AP_RPM.cpp
@@ -199,6 +199,10 @@ void AP_RPM::update(void)
             }
 
             drivers[i]->update();
+
+#if AP_RPM_ESC_TELEM_OUTBOUND_ENABLED
+            drivers[i]->update_esc_telem_outbound();
+#endif
         }
     }
 
diff --git a/libraries/AP_RPM/AP_RPM_Params.cpp b/libraries/AP_RPM/AP_RPM_Params.cpp
index 8a3419594e..1d58d41eb6 100644
--- a/libraries/AP_RPM/AP_RPM_Params.cpp
+++ b/libraries/AP_RPM/AP_RPM_Params.cpp
@@ -68,6 +68,16 @@ const AP_Param::GroupInfo AP_RPM_Params::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("ESC_MASK", 7, AP_RPM_Params, esc_mask, 0),
 
+#if AP_RPM_ESC_TELEM_OUTBOUND_ENABLED
+    // @Param: ESC_INDEX
+    // @DisplayName: ESC Telemetry Index to write RPM to
+    // @Description: ESC Telemetry Index to write RPM to. Use 0 to disable.
+    // @Range: 0 10
+    // @Increment: 1
+    // @User: Advanced
+    AP_GROUPINFO("ESC_INDEX", 8, AP_RPM_Params, esc_telem_outbound_index, 0),
+#endif
+
     AP_GROUPEND
 };
 
diff --git a/libraries/AP_RPM/AP_RPM_Params.h b/libraries/AP_RPM/AP_RPM_Params.h
index c6f5d77adb..591bd76e7f 100644
--- a/libraries/AP_RPM/AP_RPM_Params.h
+++ b/libraries/AP_RPM/AP_RPM_Params.h
@@ -14,6 +14,7 @@
  */
 #pragma once
 #include <AP_Param/AP_Param.h>
+#include "AP_RPM_config.h"
 
 class AP_RPM_Params {
 
@@ -29,6 +30,9 @@ public:
     AP_Float minimum;
     AP_Float quality_min;
     AP_Int32 esc_mask;
+#if AP_RPM_ESC_TELEM_OUTBOUND_ENABLED
+    AP_Int8  esc_telem_outbound_index;
+#endif
 
     static const struct AP_Param::GroupInfo var_info[];
 
diff --git a/libraries/AP_RPM/AP_RPM_config.h b/libraries/AP_RPM/AP_RPM_config.h
index e38968c274..fd035381df 100644
--- a/libraries/AP_RPM/AP_RPM_config.h
+++ b/libraries/AP_RPM/AP_RPM_config.h
@@ -5,6 +5,7 @@
 #include <AP_EFI/AP_EFI_config.h>
 #include <AP_Generator/AP_Generator_config.h>
 #include <AP_InertialSensor/AP_InertialSensor_config.h>
+#include <AP_ESC_Telem/AP_ESC_Telem_config.h>
 
 #ifndef AP_RPM_ENABLED
 #define AP_RPM_ENABLED 1
@@ -41,3 +42,7 @@
 #ifndef AP_RPM_GENERATOR_ENABLED
 #define AP_RPM_GENERATOR_ENABLED AP_RPM_BACKEND_DEFAULT_ENABLED && HAL_GENERATOR_ENABLED
 #endif
+
+#ifndef AP_RPM_ESC_TELEM_OUTBOUND_ENABLED
+#define AP_RPM_ESC_TELEM_OUTBOUND_ENABLED AP_RPM_BACKEND_DEFAULT_ENABLED && HAL_WITH_ESC_TELEM
+#endif
diff --git a/libraries/AP_RPM/RPM_Backend.cpp b/libraries/AP_RPM/RPM_Backend.cpp
index 9c90fb56a5..a04778fd37 100644
--- a/libraries/AP_RPM/RPM_Backend.cpp
+++ b/libraries/AP_RPM/RPM_Backend.cpp
@@ -19,6 +19,10 @@
 
 #include "AP_RPM.h"
 
+#if HAL_WITH_ESC_TELEM
+#include "AP_ESC_Telem/AP_ESC_Telem.h"
+#endif
+
 /*
   base class constructor. 
 */
@@ -29,4 +33,22 @@ AP_RPM_Backend::AP_RPM_Backend(AP_RPM &_ap_rpm, uint8_t instance, AP_RPM::RPM_St
     state.instance = instance;
 }
 
+#if AP_RPM_ESC_TELEM_OUTBOUND_ENABLED
+void AP_RPM_Backend::update_esc_telem_outbound()
+{
+    const uint8_t esc_index = ap_rpm._params[state.instance].esc_telem_outbound_index;
+    if (esc_index == 0) {
+        // Disabled if there's no ESC identified to route the data to
+        return;
+    }
+    if (!ap_rpm.healthy(state.instance)) {
+        // If we're unhealthy don't update the telemetry. Let it
+        // timeout on it's own instead of showing potentially wrong data
+        return;
+    }
+
+    AP::esc_telem().update_rpm(esc_index-1, state.rate_rpm, 0);
+}
+#endif
+
 #endif  // AP_RPM_ENABLED
diff --git a/libraries/AP_RPM/RPM_Backend.h b/libraries/AP_RPM/RPM_Backend.h
index 7c2726c7ed..1ba4625da5 100644
--- a/libraries/AP_RPM/RPM_Backend.h
+++ b/libraries/AP_RPM/RPM_Backend.h
@@ -38,6 +38,8 @@ public:
         return ap_rpm._params[state.instance].pin.get();
     }
 
+    void update_esc_telem_outbound();
+
 protected:
 
     AP_RPM &ap_rpm;