mirror of https://github.com/ArduPilot/ardupilot
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
This commit is contained in:
parent
782c91474e
commit
87ac0f42aa
|
@ -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) {
|
||||
|
|
Loading…
Reference in New Issue