AP_BoardConfig: expose BRD_SAFETYENABLE on all boards

default BRD_SAFETYENABLE to 0 on boards with no safety switch, which
gives us the same behaviour as before, but users can choose to enable
the safety

this fixes two problems:

  - CAN servos and ESCs work on boards with no safety switch
    (eg. MatekH743 with CAN)

  - during startup we could get spurious outputs before out aircraft
    type is setup
This commit is contained in:
Andrew Tridgell 2022-05-21 18:49:24 +10:00
parent c8b328c3a1
commit cf4768454f
2 changed files with 13 additions and 12 deletions

View File

@ -32,9 +32,6 @@
#endif #endif
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
#ifndef BOARD_SAFETY_ENABLE_DEFAULT
# define BOARD_SAFETY_ENABLE_DEFAULT 1
#endif
#ifndef BOARD_SER1_RTSCTS_DEFAULT #ifndef BOARD_SER1_RTSCTS_DEFAULT
# define BOARD_SER1_RTSCTS_DEFAULT 2 # define BOARD_SER1_RTSCTS_DEFAULT 2
#endif #endif
@ -43,6 +40,18 @@
#endif #endif
#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 #ifndef HAL_IMU_TEMP_DEFAULT
#define HAL_IMU_TEMP_DEFAULT -1 // disabled #define HAL_IMU_TEMP_DEFAULT -1 // disabled
#endif #endif
@ -146,7 +155,6 @@ const AP_Param::GroupInfo AP_BoardConfig::var_info[] = {
#endif #endif
#endif #endif
#if HAL_HAVE_SAFETY_SWITCH
// @Param: SAFETYENABLE // @Param: SAFETYENABLE
// @DisplayName: Enable use of safety arming switch // @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. // @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 // @RebootRequired: True
// @User: Standard // @User: Standard
AP_GROUPINFO("SAFETYENABLE", 3, AP_BoardConfig, state.safety_enable, BOARD_SAFETY_ENABLE_DEFAULT), AP_GROUPINFO("SAFETYENABLE", 3, AP_BoardConfig, state.safety_enable, BOARD_SAFETY_ENABLE_DEFAULT),
#endif
#if AP_FEATURE_SBUS_OUT #if AP_FEATURE_SBUS_OUT
// @Param: SBUS_OUT // @Param: SBUS_OUT
@ -173,7 +180,6 @@ const AP_Param::GroupInfo AP_BoardConfig::var_info[] = {
// @User: Standard // @User: Standard
AP_GROUPINFO("SERIAL_NUM", 5, AP_BoardConfig, vehicleSerialNumber, 0), AP_GROUPINFO("SERIAL_NUM", 5, AP_BoardConfig, vehicleSerialNumber, 0),
#if HAL_HAVE_SAFETY_SWITCH
// @Param: SAFETY_MASK // @Param: SAFETY_MASK
// @DisplayName: Outputs which ignore the safety switch state // @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 // @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 // @RebootRequired: True
// @User: Advanced // @User: Advanced
AP_GROUPINFO("SAFETY_MASK", 7, AP_BoardConfig, state.ignore_safety_channels, 0), AP_GROUPINFO("SAFETY_MASK", 7, AP_BoardConfig, state.ignore_safety_channels, 0),
#endif
#if HAL_HAVE_IMU_HEATER #if HAL_HAVE_IMU_HEATER
// @Param: HEAT_TARG // @Param: HEAT_TARG
@ -375,9 +380,7 @@ void AP_BoardConfig::init()
// set default value for BRD_SAFETY_MASK // set default value for BRD_SAFETY_MASK
void AP_BoardConfig::set_default_safety_ignore_mask(uint32_t mask) void AP_BoardConfig::set_default_safety_ignore_mask(uint32_t mask)
{ {
#if HAL_HAVE_SAFETY_SWITCH
state.ignore_safety_channels.set_default(mask); state.ignore_safety_channels.set_default(mask);
#endif
} }
void AP_BoardConfig::init_safety() void AP_BoardConfig::init_safety()

View File

@ -33,10 +33,9 @@ extern const AP_HAL::HAL& hal;
*/ */
void AP_BoardConfig::board_init_safety() void AP_BoardConfig::board_init_safety()
{ {
#if HAL_HAVE_SAFETY_SWITCH
bool force_safety_off = (state.safety_enable.get() == 0); bool force_safety_off = (state.safety_enable.get() == 0);
if (!force_safety_off && hal.util->was_watchdog_safety_off()) { 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; force_safety_off = true;
} }
if (force_safety_off) { if (force_safety_off) {
@ -47,7 +46,6 @@ void AP_BoardConfig::board_init_safety()
hal.scheduler->delay(20); hal.scheduler->delay(20);
} }
} }
#endif
} }
/* /*