AP_BLHeli: add support for 13-byte ext. telemetry

SW-187
CarbonixUAV · May 27, 2024 · 986d24a · 986d24a
1 parent f8c1a98
commit 986d24a
Show file tree

Hide file tree

Showing 2 changed files with 35 additions and 10 deletions.
diff --git a/libraries/AP_BLHeli/AP_BLHeli.cpp b/libraries/AP_BLHeli/AP_BLHeli.cpp
@@ -151,12 +151,23 @@ const AP_Param::GroupInfo AP_BLHeli::var_info[] = {
     // @RebootRequired: True
     AP_GROUPINFO("RVMASK",  12, AP_BLHeli, channel_reversed_mask, 0),
 
+#if HAL_WITH_ESC_TELEM
+    // @Param: EXT_TLM
+    // @DisplayName: Enable extended BLHeli telemetry
+    // @Description: When set to 1, this listens for 13-byte extended telemetry packets from BLHeli ESCs
+    // @Values: 0:Disabled,1:Enabled
+    // @User: Advanced
+    // @RebootRequired: True
+    AP_GROUPINFO("EXTLM",  13, AP_BLHeli, extended_telemetry, 0),
+#endif
+
     AP_GROUPEND
 };
 
 #define RPM_SLEW_RATE 50
 
 AP_BLHeli *AP_BLHeli::_singleton;
+uint8_t AP_BLHeli::telem_packet_size;
 
 // constructor
 AP_BLHeli::AP_BLHeli(void)
@@ -1415,6 +1426,8 @@ void AP_BLHeli::init(void)
             telem_uart = serial_manager->find_serial(AP_SerialManager::SerialProtocol_ESCTelemetry,0);
         }
     }
+
+    telem_packet_size = extended_telemetry ? 13 : 10;
 }
 
 /*
@@ -1423,7 +1436,7 @@ void AP_BLHeli::init(void)
 void AP_BLHeli::read_telemetry_packet(void)
 {
 #if HAL_WITH_ESC_TELEM
-    uint8_t buf[telem_packet_size];
+    uint8_t buf[telem_packet_array_size];
     if (telem_uart->read(buf, telem_packet_size) < telem_packet_size) {
         // short read, we should have 10 bytes ready when this function is called
         return;
@@ -1447,18 +1460,28 @@ void AP_BLHeli::read_telemetry_packet(void)
     hal.rcout->set_active_escs_mask(1<<motor_idx);
     update_rpm(motor_idx - chan_offset, new_rpm);
 
-    TelemetryData t {
+    TelemetryData telemetryData {
         .temperature_cdeg = int16_t(buf[0] * 100),
         .voltage = float(uint16_t((buf[1]<<8) | buf[2])) * 0.01,
         .current = float(uint16_t((buf[3]<<8) | buf[4])) * 0.01,
         .consumption_mah = float(uint16_t((buf[5]<<8) | buf[6])),
     };
 
-    update_telem_data(motor_idx - chan_offset, t,
-        AP_ESC_Telem_Backend::TelemetryType::CURRENT
+    uint16_t types = AP_ESC_Telem_Backend::TelemetryType::CURRENT
             | AP_ESC_Telem_Backend::TelemetryType::VOLTAGE
             | AP_ESC_Telem_Backend::TelemetryType::CONSUMPTION
-            | AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE);
+            | AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE;
+
+    if (extended_telemetry) {
+        telemetryData.input_duty = buf[9];
+        telemetryData.output_duty = buf[10];
+        telemetryData.flags = buf[11];
+        types |= AP_ESC_Telem_Backend::TelemetryType::INPUT_DUTY
+                | AP_ESC_Telem_Backend::TelemetryType::OUTPUT_DUTY
+                | AP_ESC_Telem_Backend::TelemetryType::FLAGS;
+    }
+
+    update_telem_data(motor_idx - chan_offset, telemetryData, types);
 
     if (debug_level >= 2) {
         uint16_t trpm = new_rpm;
@@ -1470,10 +1493,10 @@ void AP_BLHeli::read_telemetry_packet(void)
         }
         DEV_PRINTF("ESC[%u] T=%u V=%f C=%f con=%f RPM=%u e=%.1f t=%u\n",
                             last_telem_esc,
-                            t.temperature_cdeg,
-                            t.voltage,
-                            t.current,
-                            t.consumption_mah,
+                            telemetryData.temperature_cdeg,
+                            telemetryData.voltage,
+                            telemetryData.current,
+                            telemetryData.consumption_mah,
                             trpm, hal.rcout->get_erpm_error_rate(motor_idx), (unsigned)AP_HAL::millis());
     }
 #endif // HAL_WITH_ESC_TELEM

diff --git a/libraries/AP_BLHeli/AP_BLHeli.h b/libraries/AP_BLHeli/AP_BLHeli.h
@@ -76,6 +76,7 @@ class AP_BLHeli : public AP_ESC_Telem_Backend {
     AP_Int8 motor_poles;
     // mask of channels with bi-directional dshot enabled
     AP_Int32 channel_bidir_dshot_mask;
+    AP_Int8 extended_telemetry;
 
     enum mspState {
         MSP_IDLE=0,
@@ -250,7 +251,8 @@ class AP_BLHeli : public AP_ESC_Telem_Backend {
     // when did we last request telemetry?
     uint32_t last_telem_request_us;
     uint8_t last_telem_esc;
-    static const uint8_t telem_packet_size = 10;
+    static const uint8_t telem_packet_array_size = 13;
+    static uint8_t telem_packet_size;
     bool telem_uart_started;
     uint32_t last_telem_byte_read_us;
     int8_t last_control_port;