From 87ac0f42aa06748a1145c02ff1f4a58ebda22c60 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 30f422f83e..d7b2e68f0d 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) AP::internalerror().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) {