AP_Vehicle: soft reboot iomcu on soft reboot

This commit is contained in:
Andy Piper 2023-06-08 08:09:10 +01:00 committed by Andrew Tridgell
parent 837c81af5e
commit 9611baf148
1 changed files with 8 additions and 0 deletions

View File

@ -18,6 +18,10 @@
#include <AP_HAL_ChibiOS/hwdef/common/stm32_util.h> #include <AP_HAL_ChibiOS/hwdef/common/stm32_util.h>
#endif #endif
#include <AP_DDS/AP_DDS_Client.h> #include <AP_DDS/AP_DDS_Client.h>
#if HAL_WITH_IO_MCU
#include <AP_IOMCU/AP_IOMCU.h>
extern AP_IOMCU iomcu;
#endif
#define SCHED_TASK(func, rate_hz, max_time_micros, prio) SCHED_TASK_CLASS(AP_Vehicle, &vehicle, func, rate_hz, max_time_micros, prio) #define SCHED_TASK(func, rate_hz, max_time_micros, prio) SCHED_TASK_CLASS(AP_Vehicle, &vehicle, func, rate_hz, max_time_micros, prio)
@ -785,6 +789,10 @@ void AP_Vehicle::reboot(bool hold_in_bootloader)
// the IO board safety to be forced on, the parameters to flush, ... // the IO board safety to be forced on, the parameters to flush, ...
hal.scheduler->delay(200); hal.scheduler->delay(200);
#if HAL_WITH_IO_MCU
iomcu.soft_reboot();
#endif
hal.scheduler->reboot(hold_in_bootloader); hal.scheduler->reboot(hold_in_bootloader);
} }