diff --git a/libraries/AP_BoardConfig/AP_BoardConfig.h b/libraries/AP_BoardConfig/AP_BoardConfig.h index 21b6d82c6e..f83a656881 100644 --- a/libraries/AP_BoardConfig/AP_BoardConfig.h +++ b/libraries/AP_BoardConfig/AP_BoardConfig.h @@ -161,7 +161,7 @@ public: // return true if watchdog enabled static bool watchdog_enabled(void) { - return _singleton?(_singleton->_options & BOARD_OPTION_WATCHDOG)!=0:false; + return instance?(instance->_options & BOARD_OPTION_WATCHDOG)!=0:false; } private: diff --git a/libraries/AP_BoardConfig/board_drivers.cpp b/libraries/AP_BoardConfig/board_drivers.cpp index da5a46652f..0a4e87168b 100644 --- a/libraries/AP_BoardConfig/board_drivers.cpp +++ b/libraries/AP_BoardConfig/board_drivers.cpp @@ -19,6 +19,7 @@ #include #include "AP_BoardConfig.h" #include +#include #include extern const AP_HAL::HAL& hal; @@ -29,7 +30,12 @@ extern const AP_HAL::HAL& hal; */ void AP_BoardConfig::board_init_safety() { - if (state.safety_enable.get() == 0) { + bool force_safety_off = (state.safety_enable.get() == 0); + if (!force_safety_off && hal.util->was_watchdog_safety_off()) { + gcs().send_text(MAV_SEVERITY_INFO, "Forcing safety off for watchdog\n"); + force_safety_off = true; + } + if (force_safety_off) { hal.rcout->force_safety_off(); hal.rcout->force_safety_no_wait(); // wait until safety has been turned off