diff --git a/libraries/AP_BoardConfig/AP_BoardConfig.cpp b/libraries/AP_BoardConfig/AP_BoardConfig.cpp index 084c1964fa..4116d4d717 100644 --- a/libraries/AP_BoardConfig/AP_BoardConfig.cpp +++ b/libraries/AP_BoardConfig/AP_BoardConfig.cpp @@ -32,9 +32,6 @@ #endif #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS -#ifndef BOARD_SAFETY_ENABLE_DEFAULT -# define BOARD_SAFETY_ENABLE_DEFAULT 1 -#endif #ifndef BOARD_SER1_RTSCTS_DEFAULT # define BOARD_SER1_RTSCTS_DEFAULT 2 #endif @@ -43,6 +40,18 @@ #endif #endif +#ifndef BOARD_SAFETY_ENABLE_DEFAULT +#if defined(HAL_GPIO_PIN_SAFETY_IN) + // have safety startup enabled if we have a safety pin + # define BOARD_SAFETY_ENABLE_DEFAULT 1 +#elif defined(HAL_WITH_IO_MCU) + // if we have an IOMCU then enable by default + # define BOARD_SAFETY_ENABLE_DEFAULT HAL_WITH_IO_MCU +#else + # define BOARD_SAFETY_ENABLE_DEFAULT 0 +#endif +#endif + #ifndef HAL_IMU_TEMP_DEFAULT #define HAL_IMU_TEMP_DEFAULT -1 // disabled #endif @@ -146,7 +155,6 @@ const AP_Param::GroupInfo AP_BoardConfig::var_info[] = { #endif #endif -#if HAL_HAVE_SAFETY_SWITCH // @Param: SAFETYENABLE // @DisplayName: Enable use of safety arming switch // @Description: This controls the default state of the safety switch at startup. When set to 1 the safety switch will start in the safe state (flashing) at boot. When set to zero the safety switch will start in the unsafe state (solid) at startup. Note that if a safety switch is fitted the user can still control the safety state after startup using the switch. The safety state can also be controlled in software using a MAVLink message. @@ -154,7 +162,6 @@ const AP_Param::GroupInfo AP_BoardConfig::var_info[] = { // @RebootRequired: True // @User: Standard AP_GROUPINFO("SAFETYENABLE", 3, AP_BoardConfig, state.safety_enable, BOARD_SAFETY_ENABLE_DEFAULT), -#endif #if AP_FEATURE_SBUS_OUT // @Param: SBUS_OUT @@ -173,7 +180,6 @@ const AP_Param::GroupInfo AP_BoardConfig::var_info[] = { // @User: Standard AP_GROUPINFO("SERIAL_NUM", 5, AP_BoardConfig, vehicleSerialNumber, 0), -#if HAL_HAVE_SAFETY_SWITCH // @Param: SAFETY_MASK // @DisplayName: Outputs which ignore the safety switch state // @Description: A bitmask which controls what outputs can move while the safety switch has not been pressed @@ -181,7 +187,6 @@ const AP_Param::GroupInfo AP_BoardConfig::var_info[] = { // @RebootRequired: True // @User: Advanced AP_GROUPINFO("SAFETY_MASK", 7, AP_BoardConfig, state.ignore_safety_channels, 0), -#endif #if HAL_HAVE_IMU_HEATER // @Param: HEAT_TARG @@ -372,9 +377,7 @@ void AP_BoardConfig::init() // set default value for BRD_SAFETY_MASK void AP_BoardConfig::set_default_safety_ignore_mask(uint16_t mask) { -#if HAL_HAVE_SAFETY_SWITCH state.ignore_safety_channels.set_default(mask); -#endif } void AP_BoardConfig::init_safety() diff --git a/libraries/AP_BoardConfig/board_drivers.cpp b/libraries/AP_BoardConfig/board_drivers.cpp index bd58df7b10..84e088bf07 100644 --- a/libraries/AP_BoardConfig/board_drivers.cpp +++ b/libraries/AP_BoardConfig/board_drivers.cpp @@ -33,10 +33,9 @@ extern const AP_HAL::HAL& hal; */ void AP_BoardConfig::board_init_safety() { -#if HAL_HAVE_SAFETY_SWITCH 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"); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Forcing safety off for watchdog\n"); force_safety_off = true; } if (force_safety_off) { @@ -47,7 +46,6 @@ void AP_BoardConfig::board_init_safety() hal.scheduler->delay(20); } } -#endif } /*