From 3512ea792ffa81c219221ffddac30df3fc979e31 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 1 Sep 2022 18:02:33 +1000
Subject: [PATCH] AP_ESC_Telem: support set_rpm_scale() call for scripting

---
 libraries/AP_ESC_Telem/AP_ESC_Telem.cpp | 20 ++++++++++++++++++++
 libraries/AP_ESC_Telem/AP_ESC_Telem.h   | 14 ++++++++++++++
 2 files changed, 34 insertions(+)

diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp
index cb85e15332..b0ee54c5d9 100644
--- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp
+++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp
@@ -171,6 +171,13 @@ bool AP_ESC_Telem::get_rpm(uint8_t esc_index, float& rpm) const
         && (now - rpmdata.last_update_us < ESC_RPM_DATA_TIMEOUT_US)) {
         const float slew = MIN(1.0f, (now - rpmdata.last_update_us) * rpmdata.update_rate_hz * (1.0f / 1e6f));
         rpm = (rpmdata.prev_rpm + (rpmdata.rpm - rpmdata.prev_rpm) * slew);
+
+#if AP_SCRIPTING_ENABLED
+        if ((1U<<esc_index) & rpm_scale_mask) {
+            rpm *= rpm_scale_factor[esc_index];
+        }
+#endif
+
         return true;
     }
     return false;
@@ -507,6 +514,19 @@ void AP_ESC_Telem::update()
     }
 }
 
+#if AP_SCRIPTING_ENABLED
+/*
+  set RPM scale factor from script
+*/
+void AP_ESC_Telem::set_rpm_scale(const uint8_t esc_index, const float scale_factor)
+{
+    if (esc_index < ESC_TELEM_MAX_ESCS) {
+        rpm_scale_factor[esc_index] = scale_factor;
+        rpm_scale_mask |= (1U<<esc_index);
+    }
+}
+#endif
+
 AP_ESC_Telem *AP_ESC_Telem::_singleton = nullptr;
 
 /*
diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.h b/libraries/AP_ESC_Telem/AP_ESC_Telem.h
index 3537f83187..e0a8276426 100644
--- a/libraries/AP_ESC_Telem/AP_ESC_Telem.h
+++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.h
@@ -100,6 +100,14 @@ public:
     // callback to update the rpm in the frontend, should be called by the driver when new data is available
     // can also be called from scripting
     void update_rpm(const uint8_t esc_index, const float new_rpm, const float error_rate);
+
+#if AP_SCRIPTING_ENABLED
+    /*
+      set RPM scale factor from script
+     */
+    void set_rpm_scale(const uint8_t esc_index, const float scale_factor);
+#endif
+
 private:
 
     // callback to update the data in the frontend, should be called by the driver when new data is available
@@ -114,6 +122,12 @@ private:
     uint32_t _last_rpm_log_us[ESC_TELEM_MAX_ESCS];
     uint8_t next_idx;
 
+#if AP_SCRIPTING_ENABLED
+    // allow for scaling of RPMs via lua scripts
+    float rpm_scale_factor[ESC_TELEM_MAX_ESCS];
+    uint32_t rpm_scale_mask;
+#endif
+    
     bool _have_data;
 
     AP_Int8 mavlink_offset;