From b9280630f57b921d18b65a66f6bb24368dee3d9d Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Wed, 25 May 2022 12:26:02 +0100
Subject: [PATCH] AP_ESC_Telem: add are_motors_running() based on rpm feedback

add spin armed rpm to SITL
use floats for rpm in API calls
implement is_telemetry_active()
simulate ESC telemetry in SITL
---
 libraries/AP_ESC_Telem/AP_ESC_Telem.cpp      | 36 ++++++++++++++++++++
 libraries/AP_ESC_Telem/AP_ESC_Telem.h        |  6 ++++
 libraries/AP_ESC_Telem/AP_ESC_Telem_SITL.cpp | 25 ++++++++++++--
 3 files changed, 65 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp
index 843de6ad14..5e7fb72a98 100644
--- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp
+++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp
@@ -24,6 +24,8 @@
 
 //#define ESC_TELEM_DEBUG
 
+#define ESC_RPM_CHECK_TIMEOUT_US 100000UL   // timeout for motor running validity
+
 extern const AP_HAL::HAL& hal;
 
 // table of user settable parameters
@@ -117,6 +119,40 @@ uint8_t AP_ESC_Telem::get_num_active_escs() const {
     return __builtin_popcount(active);
 }
 
+// return the whether all the motors in servo_channel_mask are running
+bool AP_ESC_Telem::are_motors_running(uint32_t servo_channel_mask, float min_rpm) const
+{
+    const uint32_t now = AP_HAL::micros();
+
+    for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) {
+        if (BIT_IS_SET(servo_channel_mask, i)) {
+            const volatile AP_ESC_Telem_Backend::RpmData& rpmdata = _rpm_data[i];
+            // we choose a relatively strict measure of health so that failsafe actions can rely on the results
+            if (now < rpmdata.last_update_us || now - rpmdata.last_update_us > ESC_RPM_CHECK_TIMEOUT_US) {
+                return false;
+            }
+            if (rpmdata.rpm < min_rpm) {
+                return false;
+            }
+        }
+    }
+    return true;
+}
+
+// is telemetry active for the provided channel mask
+bool AP_ESC_Telem::is_telemetry_active(uint32_t servo_channel_mask) const
+{
+    for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) {
+        if (BIT_IS_SET(servo_channel_mask, i)) {
+            // no data received
+            if (get_last_telem_data_ms(i) == 0 && _rpm_data[i].last_update_us == 0) {
+                return false;
+            }
+        }
+    }
+    return true;
+}
+
 // get an individual ESC's slewed rpm if available, returns true on success
 bool AP_ESC_Telem::get_rpm(uint8_t esc_index, float& rpm) const
 {
diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.h b/libraries/AP_ESC_Telem/AP_ESC_Telem.h
index edf5b98adc..29f1787e8c 100644
--- a/libraries/AP_ESC_Telem/AP_ESC_Telem.h
+++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.h
@@ -42,6 +42,9 @@ public:
     // return the average motor RPM
     float get_average_motor_rpm() const { return get_average_motor_rpm(0xFFFFFFFF); }
 
+    // determine whether all the motors in servo_channel_mask are running
+    bool are_motors_running(uint32_t servo_channel_mask, float min_rpm) const;
+
     // get an individual ESC's temperature in centi-degrees if available, returns true on success
     bool get_temperature(uint8_t esc_index, int16_t& temp) const;
 
@@ -91,6 +94,9 @@ public:
     // udpate at 10Hz to log telemetry
     void update();
 
+    // is rpm telemetry configured for the provided channel mask
+    bool is_telemetry_active(uint32_t servo_channel_mask) const;
+
     // 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 uint16_t new_rpm, const float error_rate);
diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem_SITL.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem_SITL.cpp
index f29fbad525..2889ef8987 100644
--- a/libraries/AP_ESC_Telem/AP_ESC_Telem_SITL.cpp
+++ b/libraries/AP_ESC_Telem/AP_ESC_Telem_SITL.cpp
@@ -36,13 +36,34 @@ void AP_ESC_Telem_SITL::update()
         return;
     }
 
+#if HAL_WITH_ESC_TELEM
+    for (uint8_t i = 0; i < sitl->state.num_motors; i++) {
+        // some fake values so that is_telemetry_active() returns true
+        TelemetryData t {
+            .temperature_cdeg = 32,
+            .voltage = 16.8f,
+            .current = 0.8f,
+            .consumption_mah = 1.0f,
+        };
+
+        update_telem_data(i, t,
+            AP_ESC_Telem_Backend::TelemetryType::CURRENT
+                | AP_ESC_Telem_Backend::TelemetryType::VOLTAGE
+                | AP_ESC_Telem_Backend::TelemetryType::CONSUMPTION
+                | AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE);
+    }
+
     if (is_zero(sitl->throttle)) {
+        if (!is_zero(sitl->esc_rpm_armed) && hal.util->get_soft_armed()) {
+            for (uint8_t i = 0; i < sitl->state.num_motors; i++) {
+                update_rpm(i, sitl->esc_rpm_armed);
+            }
+        }
         return;
     }
 
-#if HAL_WITH_ESC_TELEM
     for (uint8_t i = 0; i < sitl->state.num_motors; i++) {
-        update_rpm(i, sitl->state.rpm[sitl->state.vtol_motor_start+i]);
+        update_rpm(i, MAX(sitl->state.rpm[sitl->state.vtol_motor_start+i], sitl->esc_rpm_armed));
     }
 #endif