From 153c5181cb46f0e884396c65183f8477247ca105 Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Mon, 23 Oct 2023 14:36:02 +0100
Subject: [PATCH] AP_IOMCU: reset erpm to zero on timeout

remove unneeded packed attribute
reset the PWM status after channels have been enabled or disabled
---
 libraries/AP_IOMCU/iofirmware/iofirmware.cpp | 11 ++++++++++-
 libraries/AP_IOMCU/iofirmware/iofirmware.h   |  1 +
 libraries/AP_IOMCU/iofirmware/ioprotocol.h   |  8 ++++----
 3 files changed, 15 insertions(+), 5 deletions(-)

diff --git a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp
index 2ff7db713b..87e2054e10 100644
--- a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp
+++ b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp
@@ -622,8 +622,13 @@ void AP_IOMCU_FW::rcin_update()
 #ifdef HAL_WITH_BIDIR_DSHOT
 void AP_IOMCU_FW::erpm_update()
 {
+    uint32_t now_us = AP_HAL::micros();
+
     if (hal.rcout->new_erpm()) {
         dshot_erpm.update_mask |= hal.rcout->read_erpm(dshot_erpm.erpm, IOMCU_MAX_CHANNELS);
+        last_erpm_us = now_us;
+    } else if (now_us - last_erpm_us > ESC_RPM_DATA_TIMEOUT_US) {
+        dshot_erpm.update_mask = 0;
     }
 }
 
@@ -640,7 +645,7 @@ void AP_IOMCU_FW::telem_update()
             dshot_telem[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 the set to zero to avoidn phantom data appearing in mavlink
+            // 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;
@@ -1146,6 +1151,10 @@ void AP_IOMCU_FW::rcout_config_update(void)
             }
         }
         last_channel_mask = reg_setup.channel_mask;
+        // channel enablement will affect the reported output mode
+        uint32_t output_mask = 0;
+        reg_status.rcout_mode = hal.rcout->get_output_mode(output_mask);
+        reg_status.rcout_mask = uint8_t(0xFF & output_mask);
     }
 
     // see if there is anything to do, we only support setting the mode for a particular channel once
diff --git a/libraries/AP_IOMCU/iofirmware/iofirmware.h b/libraries/AP_IOMCU/iofirmware/iofirmware.h
index 16227f39f7..ebc4f17138 100644
--- a/libraries/AP_IOMCU/iofirmware/iofirmware.h
+++ b/libraries/AP_IOMCU/iofirmware/iofirmware.h
@@ -140,6 +140,7 @@ public:
 #endif
 #ifdef HAL_WITH_BIDIR_DSHOT
     struct page_dshot_erpm dshot_erpm;
+    uint32_t last_erpm_us;
     struct page_dshot_telem dshot_telem[IOMCU_MAX_CHANNELS/4];
     uint32_t last_telem_ms;
 #if HAL_WITH_ESC_TELEM
diff --git a/libraries/AP_IOMCU/iofirmware/ioprotocol.h b/libraries/AP_IOMCU/iofirmware/ioprotocol.h
index 44c10d5054..b79f562fb1 100644
--- a/libraries/AP_IOMCU/iofirmware/ioprotocol.h
+++ b/libraries/AP_IOMCU/iofirmware/ioprotocol.h
@@ -189,14 +189,14 @@ struct __attribute__((packed, aligned(2))) page_GPIO {
     uint8_t output_mask;
 };
 
-struct __attribute__((packed, aligned(2))) page_mode_out {
+struct page_mode_out {
     uint16_t mask;
     uint16_t mode;
     uint16_t bdmask;
     uint16_t esc_type;
 };
 
-struct __attribute__((packed, aligned(2))) page_dshot {
+struct page_dshot {
     uint16_t telem_mask;
     uint8_t command;
     uint8_t chan;
@@ -205,13 +205,13 @@ struct __attribute__((packed, aligned(2))) page_dshot {
     uint8_t priority;
 };
 
-struct __attribute__((packed, aligned(2))) page_dshot_erpm {
+struct page_dshot_erpm {
     uint16_t erpm[IOMCU_MAX_CHANNELS];
     uint32_t update_mask;
 };
 
 // separate telemetry packet because (a) it's too big otherwise and (b) slower update rate
-struct __attribute__((packed, aligned(2))) page_dshot_telem {
+struct page_dshot_telem {
     uint16_t  error_rate[4]; // as a centi-percentage
     uint16_t  voltage_cvolts[4];
     uint16_t  current_camps[4];