diff --git a/libraries/AP_BoardConfig/AP_BoardConfig.cpp b/libraries/AP_BoardConfig/AP_BoardConfig.cpp index b42c8d7c22..c8c09e55a4 100644 --- a/libraries/AP_BoardConfig/AP_BoardConfig.cpp +++ b/libraries/AP_BoardConfig/AP_BoardConfig.cpp @@ -168,3 +168,12 @@ void AP_BoardConfig::init() hal.util->set_imu_target_temp((int8_t *)&_imu_target_temperature); #endif } + +// set default value for BRD_SAFETY_MASK +void AP_BoardConfig::set_default_safety_ignore_mask(uint16_t mask) +{ +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN + px4.ignore_safety_channels.set_default(mask); + px4_setup_safety_mask(); +#endif +} diff --git a/libraries/AP_BoardConfig/AP_BoardConfig.h b/libraries/AP_BoardConfig/AP_BoardConfig.h index 668c873fcd..114183fb17 100644 --- a/libraries/AP_BoardConfig/AP_BoardConfig.h +++ b/libraries/AP_BoardConfig/AP_BoardConfig.h @@ -58,7 +58,10 @@ public: #endif }; #endif - + + // set default value for BRD_SAFETY_MASK + void set_default_safety_ignore_mask(uint16_t mask); + private: AP_Int16 vehicleSerialNumber; @@ -81,6 +84,7 @@ private: void px4_setup(void); void px4_setup_pwm(void); void px4_setup_safety(void); + void px4_setup_safety_mask(void); void px4_setup_uart(void); void px4_setup_sbus(void); void px4_setup_canbus(void); diff --git a/libraries/AP_BoardConfig/px4_drivers.cpp b/libraries/AP_BoardConfig/px4_drivers.cpp index 9a65a0ecb0..5c48c45126 100644 --- a/libraries/AP_BoardConfig/px4_drivers.cpp +++ b/libraries/AP_BoardConfig/px4_drivers.cpp @@ -132,7 +132,7 @@ void AP_BoardConfig::px4_setup_uart() /* setup safety switch */ -void AP_BoardConfig::px4_setup_safety() +void AP_BoardConfig::px4_setup_safety_mask() { #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 // setup channels to ignore the armed state @@ -157,6 +157,14 @@ void AP_BoardConfig::px4_setup_safety() close(px4io_fd); } #endif +} + +/* + setup safety switch + */ +void AP_BoardConfig::px4_setup_safety() +{ + px4_setup_safety_mask(); if (px4.safety_enable.get() == 0) { hal.rcout->force_safety_off();