AP_ESC_Telem: add get_max_rpm_esc()

ArduPilot · Jun 26, 2024 · 0387356 · 0387356
1 parent a86f4fd
commit 0387356
Show file tree

Hide file tree

Showing 2 changed files with 27 additions and 0 deletions.
diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp
@@ -120,6 +120,30 @@ uint32_t AP_ESC_Telem::get_active_esc_mask() const {
     return ret;
 }
 
+// return an active ESC for the purposes of reporting (e.g. in the OSD)
+uint8_t AP_ESC_Telem::get_max_rpm_esc() const
+{
+    uint32_t ret = 0;
+    float max_rpm = 0;
+    const uint32_t now = AP_HAL::millis();
+    const uint32_t now_us = AP_HAL::micros();
+    for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) {
+        if (_telem_data[i].last_update_ms == 0 && !was_rpm_data_ever_reported(_rpm_data[i])) {
+            // have never seen telem from this ESC
+            continue;
+        }
+        if (_telem_data[i].stale(now)
+            && !rpm_data_within_timeout(_rpm_data[i], now_us, ESC_RPM_DATA_TIMEOUT_US)) {
+            continue;
+        }
+        if (_rpm_data[i].rpm > max_rpm) {
+            max_rpm = _rpm_data[i].rpm;
+            ret = i;
+        }
+    }
+    return ret;
+}
+
 // return number of active ESCs present
 uint8_t AP_ESC_Telem::get_num_active_escs() const {
     uint32_t active = get_active_esc_mask();

diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.h b/libraries/AP_ESC_Telem/AP_ESC_Telem.h
@@ -83,6 +83,9 @@ class AP_ESC_Telem {
     // ESC_TELEM_DATA_TIMEOUT_MS
     uint32_t get_active_esc_mask() const;
 
+    // return an active ESC with the highest RPM for the purposes of reporting (e.g. in the OSD)
+    uint8_t get_max_rpm_esc() const;
+
     // return the last time telemetry data was received in ms for the given ESC or 0 if never
     uint32_t get_last_telem_data_ms(uint8_t esc_index) const {
         if (esc_index >= ESC_TELEM_MAX_ESCS) {return 0;}