From aefc2a84927f1aeafa5fe94ffcc652e586b465a2 Mon Sep 17 00:00:00 2001
From: Tom Pittenger <tom.pittenger@krausaerospace.com>
Date: Wed, 2 Aug 2023 14:58:57 -0700
Subject: [PATCH] AP_RPM: enable AP_RPM to set ESC Telemetry

---
 libraries/AP_RPM/AP_RPM.cpp        | 14 ++++++++++++++
 libraries/AP_RPM/AP_RPM_Params.cpp |  4 ++--
 2 files changed, 16 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_RPM/AP_RPM.cpp b/libraries/AP_RPM/AP_RPM.cpp
index 11c1bd89e8..a0d9105458 100644
--- a/libraries/AP_RPM/AP_RPM.cpp
+++ b/libraries/AP_RPM/AP_RPM.cpp
@@ -23,6 +23,7 @@
 #include "RPM_Generator.h"
 #include "RPM_HarmonicNotch.h"
 #include "RPM_ESC_Telem.h"
+#include "AP_ESC_Telem/AP_ESC_Telem.h"
 
 #include <AP_Logger/AP_Logger.h>
 
@@ -199,6 +200,19 @@ void AP_RPM::update(void)
             }
 
             drivers[i]->update();
+
+#if HAL_WITH_ESC_TELEM
+            // Check if we need to write to esc_telem(). Note:
+            // RPM_TYPE_ESC_TELEM = read ESC_TELEM and store it in AP_RPM
+            // RPM_TYPE_EverythingElse = write to ESC_TELEM with data generated from AP_RPM
+            if (_params[i].type != RPM_TYPE_ESC_TELEM && _params[i].esc_mask > 0 && healthy(i)) {
+                for (uint32_t esc=0; esc<ESC_TELEM_MAX_ESCS; esc++) {
+                    if (_params[i].esc_mask & (1U<<esc)) {
+                        AP::esc_telem().update_rpm(esc, state[i].rate_rpm, 0);
+                    }
+                }
+            }
+#endif
         }
     }
 
diff --git a/libraries/AP_RPM/AP_RPM_Params.cpp b/libraries/AP_RPM/AP_RPM_Params.cpp
index 8a3419594e..391b99ea70 100644
--- a/libraries/AP_RPM/AP_RPM_Params.cpp
+++ b/libraries/AP_RPM/AP_RPM_Params.cpp
@@ -62,8 +62,8 @@ const AP_Param::GroupInfo AP_RPM_Params::var_info[] = {
     AP_GROUPINFO("PIN", 6, AP_RPM_Params, pin, -1),
 
     // @Param: ESC_MASK
-    // @DisplayName: Bitmask of ESC telemetry channels to average
-    // @Description: Mask of channels which support ESC rpm telemetry. RPM telemetry of the selected channels will be averaged
+    // @DisplayName: Bitmask of ESC telemetry channels
+    // @Description: Mask of channels to use with ESC rpm telemetry. When TYPE = 5, this will fetch and average the RPM telemetry from all the selected ESC telemetry channels. For all other TYPE values, the ESC telemetry will set the RPM on the selected channel(s).
     // @Bitmask: 0:Channel1,1:Channel2,2:Channel3,3:Channel4,4:Channel5,5:Channel6,6:Channel7,7:Channel8,8:Channel9,9:Channel10,10:Channel11,11:Channel12,12:Channel13,13:Channel14,14:Channel15,15:Channel16
     // @User: Advanced
     AP_GROUPINFO("ESC_MASK", 7, AP_RPM_Params, esc_mask, 0),