From fd32dff45ab24e1c46acb7051798587f8df7d4e1 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 12 May 2020 20:26:37 +1000 Subject: [PATCH] AP_IOMCU: force safety off on IOMCU reset if safety was forced off previously and we get an IOMCU reset then force it off when the reset happens so vehicle can keep flying --- libraries/AP_IOMCU/AP_IOMCU.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/libraries/AP_IOMCU/AP_IOMCU.cpp b/libraries/AP_IOMCU/AP_IOMCU.cpp index 13ddbb3f21..cd519d982f 100644 --- a/libraries/AP_IOMCU/AP_IOMCU.cpp +++ b/libraries/AP_IOMCU/AP_IOMCU.cpp @@ -1012,6 +1012,13 @@ void AP_IOMCU::check_iomcu_reset(void) INTERNAL_ERROR(AP_InternalError::error_t::iomcu_reset); hal.console->printf("IOMCU reset t=%u %u %u dt=%u\n", unsigned(AP_HAL::millis()), unsigned(ts1), unsigned(reg_status.timestamp_ms), unsigned(dt_ms)); + + if (safety_forced_off && !reg_status.flag_safety_off && hal.util->get_soft_armed()) { + // IOMCU has reset while armed with safety off - force it off + // again so we can keep flying + force_safety_off(); + } + // we need to ensure the mixer data and the rates are sent over to // the IOMCU if (mixing.enabled) {