AP_ESC_Telem: support Extended DShot Telemetry v2

libraries/AP_ESC_Telem/AP_ESC_Telem.cpp

@@ -468,6 +468,15 @@ void AP_ESC_Telem::update_telem_data(const uint8_t esc_index, const AP_ESC_Telem
         telemdata.usage_s = new_data.usage_s;
     }
 
+#if AP_EXTENDED_DSHOT_TELEM_V2_ENABLED
+    if (data_mask & AP_ESC_Telem_Backend::TelemetryType::EDT2_STATUS) {
+        telemdata.edt2_status = merge_edt2_status(telemdata.edt2_status, new_data.edt2_status);
+    }
+    if (data_mask & AP_ESC_Telem_Backend::TelemetryType::EDT2_STRESS) {
+        telemdata.edt2_stress = merge_edt2_stress(telemdata.edt2_stress, new_data.edt2_stress);
+    }
+#endif
+
     telemdata.count++;
     telemdata.types |= data_mask;
     telemdata.last_update_ms = AP_HAL::millis();
@@ -499,6 +508,63 @@ void AP_ESC_Telem::update_rpm(const uint8_t esc_index, const float new_rpm, cons
 #endif
 }
 
+#if AP_EXTENDED_DSHOT_TELEM_V2_ENABLED
+
+// The following is based on https://github.com/bird-sanctuary/extended-dshot-telemetry.
+// For the following part we explain the bits of Extended DShot Telemetry v2 status telemetry:
+// - bits 0-3: the "stress level"
+// - bit 5: the "error" bit   (e.g. the stall event in Bluejay)
+// - bit 6: the "warning" bit (e.g. the desync event in Bluejay)
+// - bit 7: the "alert" bit   (e.g. the demag event in Bluejay)
+
+// Since logger can read out telemetry values less frequently than they are updated,
+// it makes sense to aggregate these status bits, and to collect the maximum observed stress level.
+// To reduce the logging rate of the EDT2 messages, we will try to log them only once a new frame comes.
+// To track this, we are going to (ab)use bit 15 of the field: 1 means there is something to write.
+
+// EDTv2 also features separate "stress" messages.
+// These come more frequently, and are scaled differently (the allowed range is from 0 to 255),
+// so we have to log them separately.
+
+constexpr uint16_t EDT2_TELEM_UPDATED = 0x8000U;
+constexpr uint16_t EDT2_STRESS_0F_MASK = 0xfU;
+constexpr uint16_t EDT2_STRESS_FF_MASK = 0xffU;
+constexpr uint16_t EDT2_ERROR_MASK = 0x20U;
+constexpr uint16_t EDT2_WARNING_MASK = 0x40U;
+constexpr uint16_t EDT2_ALERT_MASK = 0x80U;
+constexpr uint16_t EDT2_ALL_BITS = EDT2_ERROR_MASK | EDT2_WARNING_MASK | EDT2_ALERT_MASK;
+
+#define EDT2_HAS_NEW_DATA(status) bool((status) & EDT2_TELEM_UPDATED)
+#define EDT2_STRESS_FROM_STATUS(status) uint8_t((status) & EDT2_STRESS_0F_MASK)
+#define EDT2_ERROR_BIT_FROM_STATUS(status) bool((status) & EDT2_ERROR_MASK)
+#define EDT2_WARNING_BIT_FROM_STATUS(status) bool((status) & EDT2_WARNING_MASK)
+#define EDT2_ALERT_BIT_FROM_STATUS(status) bool((status) & EDT2_ALERT_MASK)
+#define EDT2_STRESS_FROM_STRESS(stress) uint8_t((stress) & EDT2_STRESS_FF_MASK)
+
+uint16_t AP_ESC_Telem::merge_edt2_status(uint16_t old_status, uint16_t new_status)
+{
+    if (EDT2_HAS_NEW_DATA(old_status)) {
+        new_status = uint16_t(
+            (old_status & ~EDT2_STRESS_0F_MASK) | // old status except for the stress is preserved
+            (new_status & EDT2_ALL_BITS) | // all new status bits are included
+            MAX(old_status & EDT2_STRESS_0F_MASK, new_status & EDT2_STRESS_0F_MASK) // the stress is maxed out
+        );
+    }
+    return EDT2_TELEM_UPDATED | new_status;
+}
+
+uint16_t AP_ESC_Telem::merge_edt2_stress(uint16_t old_stress, uint16_t new_stress)
+{
+    if (EDT2_HAS_NEW_DATA(old_stress)) {
+        new_stress = uint16_t(
+            MAX(old_stress & EDT2_STRESS_FF_MASK, new_stress & EDT2_STRESS_FF_MASK) // the stress is maxed out
+        );
+    }
+    return EDT2_TELEM_UPDATED | new_stress;
+}
+
+#endif // AP_EXTENDED_DSHOT_TELEM_V2_ENABLED
+
 void AP_ESC_Telem::update()
 {
 #if HAL_LOGGING_ENABLED
@@ -507,7 +573,7 @@ void AP_ESC_Telem::update()
 
     for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) {
         const volatile AP_ESC_Telem_Backend::RpmData &rpmdata = _rpm_data[i];
-        const volatile AP_ESC_Telem_Backend::TelemetryData &telemdata = _telem_data[i];
+        volatile AP_ESC_Telem_Backend::TelemetryData &telemdata = _telem_data[i];
         // Push received telemetry data into the logging system
         if (logger && logger->logging_enabled()) {
             if (telemdata.last_update_ms != _last_telem_log_ms[i]
@@ -544,6 +610,51 @@ void AP_ESC_Telem::update()
                 _last_telem_log_ms[i] = telemdata.last_update_ms;
                 _last_rpm_log_us[i] = rpmdata.last_update_us;
             }
+
+#if AP_EXTENDED_DSHOT_TELEM_V2_ENABLED
+            // Write an EDTv2 message, if there is any update
+            uint16_t edt2_status = telemdata.edt2_status;
+            uint16_t edt2_stress = telemdata.edt2_stress;
+            if (EDT2_HAS_NEW_DATA(edt2_status | edt2_stress)) {
+                // Could probably be faster/smaller with bitmasking, but not sure
+                uint8_t status = 0;
+                if (EDT2_HAS_NEW_DATA(edt2_stress)) {
+                    status |= uint8_t(log_Edt2_Status::HAS_STRESS_DATA);
+                }
+                if (EDT2_HAS_NEW_DATA(edt2_status)) {
+                    status |= uint8_t(log_Edt2_Status::HAS_STATUS_DATA);
+                }
+                if (EDT2_ALERT_BIT_FROM_STATUS(edt2_status)) {
+                    status |= uint8_t(log_Edt2_Status::ALERT_BIT);
+                }
+                if (EDT2_WARNING_BIT_FROM_STATUS(edt2_status)) {
+                    status |= uint8_t(log_Edt2_Status::WARNING_BIT);
+                }
+                if (EDT2_ERROR_BIT_FROM_STATUS(edt2_status)) {
+                    status |= uint8_t(log_Edt2_Status::ERROR_BIT);
+                }
+                // An EDT2 status message is:
+                //   id: starts from 0
+                //   stress: the current stress which comes from edt2_stress
+                //   max_stress: the maximum stress which comes from edt2_status
+                //   status: the status bits which come from both
+                const struct log_Edt2 pkt_edt2{
+                    LOG_PACKET_HEADER_INIT(uint8_t(LOG_EDT2_MSG)),
+                    time_us     : now_us64,
+                    instance    : i,
+                    stress      : EDT2_STRESS_FROM_STRESS(edt2_stress),
+                    max_stress  : EDT2_STRESS_FROM_STATUS(edt2_status),
+                    status      : status,
+                };
+                if (AP::logger().WriteBlock_first_succeed(&pkt_edt2, sizeof(pkt_edt2))) {
+                    // Only clean the telem_updated bits if the write succeeded.
+                    // This is important because, if rate limiting is enabled,
+                    // the log-on-change behavior may lose a lot of entries
+                    telemdata.edt2_status &= ~EDT2_TELEM_UPDATED;
+                    telemdata.edt2_stress &= ~EDT2_TELEM_UPDATED;
+                }
+            }
+#endif // AP_EXTENDED_DSHOT_TELEM_V2_ENABLED
         }
     }
 #endif  // HAL_LOGGING_ENABLED
@@ -608,4 +719,4 @@ AP_ESC_Telem &esc_telem()
 
 };
 
-#endif
+#endif // HAL_WITH_ESC_TELEM

libraries/AP_ESC_Telem/AP_ESC_Telem.h

@@ -118,6 +118,12 @@ class AP_ESC_Telem {
     static bool rpm_data_within_timeout (const volatile AP_ESC_Telem_Backend::RpmData &instance, const uint32_t now_us, const uint32_t timeout_us);
     static bool was_rpm_data_ever_reported (const volatile AP_ESC_Telem_Backend::RpmData &instance);
 
+#if AP_EXTENDED_DSHOT_TELEM_V2_ENABLED
+    // helpers that aggregate data in EDTv2 messages
+    static uint16_t merge_edt2_status(uint16_t old_status, uint16_t new_status);
+    static uint16_t merge_edt2_stress(uint16_t old_stress, uint16_t new_stress);
+#endif
+
     // rpm data
     volatile AP_ESC_Telem_Backend::RpmData _rpm_data[ESC_TELEM_MAX_ESCS];
     // telemetry data

libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h

@@ -19,6 +19,10 @@ class AP_ESC_Telem_Backend {
         uint32_t last_update_ms;    // last update time in milliseconds, determines whether active
         uint16_t types;             // telemetry types present
         uint16_t count;             // number of times updated
+#if AP_EXTENDED_DSHOT_TELEM_V2_ENABLED
+        uint16_t edt2_status;       // status reported by Extended DShot Telemetry v2
+        uint16_t edt2_stress;       // stress reported in dedicated messages by Extended DShot Telemetry v2
+#endif
 
         // return true if the data is stale
         bool stale(uint32_t now_ms=0) const volatile;
@@ -42,6 +46,10 @@ class AP_ESC_Telem_Backend {
         USAGE       = 1 << 5,
         TEMPERATURE_EXTERNAL = 1 << 6,
         MOTOR_TEMPERATURE_EXTERNAL  = 1 << 7,
+#if AP_EXTENDED_DSHOT_TELEM_V2_ENABLED
+        EDT2_STATUS = 1 << 8,
+        EDT2_STRESS = 1 << 9,
+#endif
     };
 
 

libraries/AP_ESC_Telem/LogStructure.h

@@ -3,7 +3,8 @@
 #include <AP_Logger/LogStructure.h>
 
 #define LOG_IDS_FROM_ESC_TELEM                  \
-    LOG_ESC_MSG
+    LOG_ESC_MSG,                                \
+    LOG_EDT2_MSG
 
 // @LoggerMessage: ESC
 // @Description: Feedback received from ESCs
@@ -31,6 +32,35 @@ struct PACKED log_Esc {
     float error_rate;
 };
 
-#define LOG_STRUCTURE_FROM_ESC_TELEM \
+enum class log_Edt2_Status : uint8_t {
+    HAS_STRESS_DATA = 1U<<0, // true if the message contains up-to-date stress data
+    HAS_STATUS_DATA = 1U<<1, // true if the message contains up-to-date status data
+    ALERT_BIT   = 1U<<2, // true if the last status had the "alert" bit (e.g. demag)
+    WARNING_BIT = 1U<<3, // true if the last status had the "warning" bit (e.g. desync)
+    ERROR_BIT   = 1U<<4, // true if the last status had the "error" bit (e.g. stall)
+};
+
+
+// @LoggerMessage: EDT2
+// @Description: Status received from ESCs via Extended DShot telemetry v2
+// @Field: TimeUS: microseconds since system startup
+// @Field: Instance: ESC instance number
+// @Field: Stress: current stress level (commutation effort), scaled to [0..255]
+// @Field: MaxStress: maximum stress level (commutation effort) since arming, scaled to [0..15]
+// @Field: Status: status bits
+// @FieldBitmaskEnum: Status: log_Edt2_Status
+struct PACKED log_Edt2 {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    uint8_t instance;
+    uint8_t stress;
+    uint8_t max_stress;
+    uint8_t status;
+};
+
+#define LOG_STRUCTURE_FROM_ESC_TELEM  \
     { LOG_ESC_MSG, sizeof(log_Esc), \
-      "ESC",  "QBffffcfcf", "TimeUS,Instance,RPM,RawRPM,Volt,Curr,Temp,CTot,MotTemp,Err", "s#qqvAOaO%", "F-00--BCB-" , true },
+      "ESC",  "QBffffcfcf", "TimeUS,Instance,RPM,RawRPM,Volt,Curr,Temp,CTot,MotTemp,Err", "s#qqvAOaO%", "F-00--BCB-" , true }, \
+    { LOG_EDT2_MSG, sizeof(log_Edt2), \
+      "EDT2",  "QBBBB", "TimeUS,Instance,Stress,MaxStress,Status", "s#---", "F----" , true },
+

libraries/AP_HAL/AP_HAL_Boards.h

@@ -175,6 +175,19 @@
 #define HAL_WITH_IO_MCU_DSHOT HAL_WITH_IO_MCU_BIDIR_DSHOT
 #endif
 
+#ifndef HAL_REQUIRES_BDSHOT_SUPPORT
+#define HAL_REQUIRES_BDSHOT_SUPPORT (defined(HAL_WITH_BIDIR_DSHOT) || HAL_WITH_IO_MCU_BIDIR_DSHOT)
+#endif
+
+// support for Extended DShot Telemetry v2 is enabled only if any kind of such telemetry
+// can in principle arrive, either from servo outputs or from IOMCU
+
+// if not desired, set to 0 - and if IOMCU has bidirectional DShot enabled, recompile it too,
+// otherwise the communication to IOMCU breaks!
+#ifndef AP_EXTENDED_DSHOT_TELEM_V2_ENABLED
+#define AP_EXTENDED_DSHOT_TELEM_V2_ENABLED HAL_REQUIRES_BDSHOT_SUPPORT
+#endif
+
 // this is used as a general mechanism to make a 'small' build by
 // dropping little used features. We use this to allow us to keep
 // FMUv2 going for as long as possible

libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp

@@ -755,12 +755,12 @@ bool RCOutput::bdshot_decode_telemetry_from_erpm(uint16_t encodederpm, uint8_t c
     uint16_t value = (encodederpm & 0x000001ffU);               // 9bits
 #if HAL_WITH_ESC_TELEM
     uint8_t normalized_chan = chan;
-#endif
 #if HAL_WITH_IO_MCU
     if (iomcu_enabled) {
         normalized_chan = chan + chan_offset;
     }
 #endif
+#endif // HAL_WITH_ESC_TELEM: one can possibly imagine a FC with IOMCU but with ESC_TELEM compiled out...
 
     if (!(value & 0x100U) && (_dshot_esc_type == DSHOT_ESC_BLHELI_EDT || _dshot_esc_type == DSHOT_ESC_BLHELI_EDT_S)) {
         switch (expo) {
@@ -796,10 +796,28 @@ bool RCOutput::bdshot_decode_telemetry_from_erpm(uint16_t encodederpm, uint8_t c
             break;
         case 0b100:  // Debug 1
         case 0b101:  // Debug 2
-        case 0b110:  // Stress level
-        case 0b111:  // Status
             return false;
             break;
+        case 0b110: { // Stress level
+    #if HAL_WITH_ESC_TELEM && AP_EXTENDED_DSHOT_TELEM_V2_ENABLED
+            TelemetryData t {
+                .edt2_stress = value
+            };
+            update_telem_data(normalized_chan, t, AP_ESC_Telem_Backend::TelemetryType::EDT2_STRESS);
+    #endif
+            return false;
+            }
+            break;
+        case 0b111: { // Status
+    #if HAL_WITH_ESC_TELEM && AP_EXTENDED_DSHOT_TELEM_V2_ENABLED
+            TelemetryData t {
+                .edt2_status = value
+            };
+            update_telem_data(normalized_chan, t, AP_ESC_Telem_Backend::TelemetryType::EDT2_STATUS);
+    #endif
+            return false;
+            }
+            break;
         default:     // eRPM
             break;
         }

libraries/AP_IOMCU/AP_IOMCU.cpp

@@ -453,7 +453,11 @@ void AP_IOMCU::read_telem()
         TelemetryData t {
             .temperature_cdeg = int16_t(telem->temperature_cdeg[i]),
             .voltage = float(telem->voltage_cvolts[i]) * 0.01,
-            .current = float(telem->current_camps[i]) * 0.01
+            .current = float(telem->current_camps[i]) * 0.01,
+#if AP_EXTENDED_DSHOT_TELEM_V2_ENABLED
+            .edt2_status = telem->edt2_status[i],
+            .edt2_stress = telem->edt2_stress[i],
+#endif
         };
         update_telem_data(esc_group * 4 + i, t, telem->types[i]);
     }

libraries/AP_IOMCU/iofirmware/iofirmware.cpp

@@ -635,25 +635,34 @@ void AP_IOMCU_FW::telem_update()
     uint32_t now_ms = AP_HAL::millis();
 
     for (uint8_t i = 0; i < IOMCU_MAX_TELEM_CHANNELS/4; i++) {
+        struct page_dshot_telem &dshot_i = dshot_telem[i];
         for (uint8_t j = 0; j < 4; j++) {
             const uint8_t esc_id = (i * 4 + j);
             if (esc_id >= IOMCU_MAX_TELEM_CHANNELS) {
                 continue;
             }
-            dshot_telem[i].error_rate[j] = uint16_t(roundf(hal.rcout->get_erpm_error_rate(esc_id) * 100.0));
+            dshot_i.error_rate[j] = uint16_t(roundf(hal.rcout->get_erpm_error_rate(esc_id) * 100.0));
 #if HAL_WITH_ESC_TELEM
             const volatile AP_ESC_Telem_Backend::TelemetryData& telem = esc_telem.get_telem_data(esc_id);
             // if data is stale then set to zero to avoid phantom data appearing in mavlink
             if (now_ms - telem.last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS) {
-                dshot_telem[i].voltage_cvolts[j] = 0;
-                dshot_telem[i].current_camps[j] = 0;
-                dshot_telem[i].temperature_cdeg[j] = 0;
+                dshot_i.voltage_cvolts[j] = 0;
+                dshot_i.current_camps[j] = 0;
+                dshot_i.temperature_cdeg[j] = 0;
+#if AP_EXTENDED_DSHOT_TELEM_V2_ENABLED
+                dshot_i.edt2_status[j] = 0;
+                dshot_i.edt2_stress[j] = 0;
+#endif
                 continue;
             }
-            dshot_telem[i].voltage_cvolts[j] = uint16_t(roundf(telem.voltage * 100));
-            dshot_telem[i].current_camps[j] = uint16_t(roundf(telem.current * 100));
-            dshot_telem[i].temperature_cdeg[j] = telem.temperature_cdeg;
-            dshot_telem[i].types[j] = telem.types;
+            dshot_i.voltage_cvolts[j] = uint16_t(roundf(telem.voltage * 100));
+            dshot_i.current_camps[j] = uint16_t(roundf(telem.current * 100));
+            dshot_i.temperature_cdeg[j] = telem.temperature_cdeg;
+#if AP_EXTENDED_DSHOT_TELEM_V2_ENABLED
+            dshot_i.edt2_status[j] = uint8_t(telem.edt2_status);
+            dshot_i.edt2_stress[j] = uint8_t(telem.edt2_stress);
+#endif
+            dshot_i.types[j] = telem.types;
 #endif
         }
     }

libraries/AP_IOMCU/iofirmware/ioprotocol.h

@@ -223,4 +223,9 @@ struct page_dshot_telem {
     uint16_t  current_camps[4];
     uint16_t  temperature_cdeg[4];
     uint16_t  types[4];
+// if EDTv2 needs to be disabled, IOMCU firmware should be recompiled too, this is the reason
+#if AP_EXTENDED_DSHOT_TELEM_V2_ENABLED
+    uint8_t   edt2_status[4];
+    uint8_t   edt2_stress[4];
+#endif
 };