AP_ESC_Telem: added ESC_TLM_MAV_OFS parameter

this allows for the ESC telemetry to be remapped to a lower range for
GCS displays. Users often want their quadplane ESCs to show up as ESCs
1 to 8 instead of the high numbers used internally

libraries/AP_ESC_Telem/AP_ESC_Telem.cpp

@@ -26,12 +26,27 @@
 
 extern const AP_HAL::HAL& hal;
 
+// table of user settable parameters
+const AP_Param::GroupInfo AP_ESC_Telem::var_info[] = {
+
+    // @Param: _MAV_OFS
+    // @DisplayName: ESC Telemetry mavlink offset
+    // @Description: Offset to apply to ESC numbers when reporting as ESC_TELEMETRY packets over MAVLink. This allows high numbered motors to be displayed as low numbered ESCs for convenience on GCS displays. A value of 4 would send ESC on output 5 as ESC number 1 in ESC_TELEMETRY packets
+    // @Increment: 1
+    // @Range: 0 31
+    // @User: Standard
+    AP_GROUPINFO("_MAV_OFS", 1, AP_ESC_Telem, mavlink_offset, 0),
+    
+    AP_GROUPEND
+};
+
 AP_ESC_Telem::AP_ESC_Telem()
 {
     if (_singleton) {
         AP_HAL::panic("Too many AP_ESC_Telem instances");
     }
     _singleton = this;
+    AP_Param::setup_object_defaults(this, var_info);
 }
 
 // return the average motor RPM
@@ -241,6 +256,7 @@ void AP_ESC_Telem::send_esc_telemetry_mavlink(uint8_t mav_chan)
     uint32_t now_us = AP_HAL::micros();
 
     // loop through groups of 4 ESCs
+    const uint8_t esc_offset = constrain_int16(mavlink_offset, 0, ESC_TELEM_MAX_ESCS-1);
     const uint8_t num_idx = ESC_TELEM_MAX_ESCS/4;
     for (uint8_t idx = 0; idx < num_idx; idx++) {
         const uint8_t i = (next_idx + idx) % num_idx;
@@ -251,12 +267,19 @@ void AP_ESC_Telem::send_esc_telemetry_mavlink(uint8_t mav_chan)
             next_idx = i;
             return;
         }
-#define ESC_DATA_STALE(idx) \
-        (now - _telem_data[idx].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS \
-            && now_us - _rpm_data[idx].last_update_us > ESC_RPM_DATA_TIMEOUT_US)
 
-        // skip this group of ESCs if no data to send
+        bool all_stale = true;
-        if (ESC_DATA_STALE(i * 4) && ESC_DATA_STALE(i * 4 + 1) && ESC_DATA_STALE(i * 4 + 2) && ESC_DATA_STALE(i * 4 + 3)) {
+        for (uint8_t j=0; j<4; j++) {
+            const uint8_t esc_id = (i * 4 + j) + esc_offset;
+            if (esc_id < ESC_TELEM_MAX_ESCS &&
+                (now - _telem_data[esc_id].last_update_ms <= ESC_TELEM_DATA_TIMEOUT_MS ||
+                 now_us - _rpm_data[esc_id].last_update_us <= ESC_RPM_DATA_TIMEOUT_US)) {
+                all_stale = false;
+                break;
+            }
+        }
+        if (all_stale) {
+            // skip this group of ESCs if no data to send
             continue;
         }
 
@@ -271,7 +294,11 @@ void AP_ESC_Telem::send_esc_telemetry_mavlink(uint8_t mav_chan)
 
         // fill in output arrays
         for (uint8_t j = 0; j < 4; j++) {
-            const uint8_t esc_id = i * 4 + j;
+            const uint8_t esc_id = (i * 4 + j) + esc_offset;
+            if (esc_id >= ESC_TELEM_MAX_ESCS) {
+                continue;
+            }
+
             temperature[j] = _telem_data[esc_id].temperature_cdeg / 100;
             voltage[j] = constrain_float(_telem_data[esc_id].voltage * 100.0f, 0, UINT16_MAX);
             current[j] = constrain_float(_telem_data[esc_id].current * 100.0f, 0, UINT16_MAX);

libraries/AP_ESC_Telem/AP_ESC_Telem.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <AP_HAL/AP_HAL.h>
+#include <AP_Param/AP_Param.h>
 #include "AP_ESC_Telem_Backend.h"
 
 #if HAL_WITH_ESC_TELEM
@@ -23,8 +24,9 @@ public:
     AP_ESC_Telem();
 
     /* Do not allow copies */
-    AP_ESC_Telem(const AP_ESC_Telem &other) = delete;
+    CLASS_NO_COPY(AP_ESC_Telem);
-    AP_ESC_Telem &operator=(const AP_ESC_Telem&) = delete;
+
+    static const struct AP_Param::GroupInfo var_info[];
 
     static AP_ESC_Telem *get_singleton();
 
@@ -106,6 +108,8 @@ private:
 
     bool _have_data;
 
+    AP_Int8 mavlink_offset;
+
     static AP_ESC_Telem *_singleton;
 };