From ada091f1fca7ce51f92e60eb55ce5268db49101f Mon Sep 17 00:00:00 2001
From: Maxim Buzdalov <mbuzdalov@gmail.com>
Date: Tue, 14 May 2024 09:16:23 +1000
Subject: [PATCH] AP_IOMCU: add support for Extended DShot Telemetry v2

---
 libraries/AP_IOMCU/AP_IOMCU.cpp              |  6 ++++-
 libraries/AP_IOMCU/iofirmware/iofirmware.cpp | 25 +++++++++++++-------
 libraries/AP_IOMCU/iofirmware/ioprotocol.h   |  5 ++++
 3 files changed, 27 insertions(+), 9 deletions(-)

diff --git a/libraries/AP_IOMCU/AP_IOMCU.cpp b/libraries/AP_IOMCU/AP_IOMCU.cpp
index 83ebd6e01a..47b75e8d5e 100644
--- a/libraries/AP_IOMCU/AP_IOMCU.cpp
+++ b/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]);
     }
diff --git a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp
index bb24b0dcad..37d991bd1c 100644
--- a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp
+++ b/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
         }
     }
diff --git a/libraries/AP_IOMCU/iofirmware/ioprotocol.h b/libraries/AP_IOMCU/iofirmware/ioprotocol.h
index be00d92759..c6a804ad45 100644
--- a/libraries/AP_IOMCU/iofirmware/ioprotocol.h
+++ b/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
 };