AP_RPM: add writing to outbound ESC_Telem

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
         }
     }
 

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
 };
 

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[];
 

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

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

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;