AP_BoardConfig: moved logic for safety button press to AP_BoardConfig

this allows for common code between CAN and pin safety switches, as
well as making the code neater as most of the logic is related to
AP_BoardConfig options

libraries/AP_BoardConfig/AP_BoardConfig.cpp

@@ -51,13 +51,11 @@
 #define HAL_IMU_TEMP_DEFAULT       -1 // disabled
 #endif
 
-#if HAL_HAVE_SAFETY_SWITCH
-#  ifndef BOARD_SAFETY_OPTION_DEFAULT
-#    define BOARD_SAFETY_OPTION_DEFAULT (BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_OFF|BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_ON)
-#  endif
-#  ifndef BOARD_SAFETY_ENABLE
-#    define BOARD_SAFETY_ENABLE 1
-#  endif
+#ifndef BOARD_SAFETY_OPTION_DEFAULT
+#  define BOARD_SAFETY_OPTION_DEFAULT (BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_OFF|BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_ON)
+#endif
+#ifndef BOARD_SAFETY_ENABLE
+#  define BOARD_SAFETY_ENABLE 1
 #endif
 
 #ifndef BOARD_PWM_COUNT_DEFAULT
@@ -187,14 +185,12 @@ const AP_Param::GroupInfo AP_BoardConfig::var_info[] = {
     AP_SUBGROUPINFO(_radio, "RADIO", 11, AP_BoardConfig, AP_Radio),
 #endif
 
-#if HAL_HAVE_SAFETY_SWITCH
     // @Param: SAFETYOPTION
     // @DisplayName: Options for safety button behavior
     // @Description: This controls the activation of the safety button. It allows you to control if the safety button can be used for safety enable and/or disable, and whether the button is only active when disarmed
     // @Bitmask: 0:ActiveForSafetyEnable,1:ActiveForSafetyDisable,2:ActiveWhenArmed,3:Force safety on when the aircraft disarms
     // @User: Standard
     AP_GROUPINFO("SAFETYOPTION",   13, AP_BoardConfig, state.safety_option, BOARD_SAFETY_OPTION_DEFAULT),
-#endif
 
     // @Group: RTC
     // @Path: ../AP_RTC/AP_RTC.cpp
@@ -345,3 +341,32 @@ void AP_BoardConfig::sensor_config_error(const char *reason)
         hal.scheduler->delay(5);
     }
 }
+
+/*
+  handle logic for safety state button press. This should be called at
+  10Hz when the button is pressed. The button can either be directly
+  on a pin or on a UAVCAN device
+  This function returns true if the safety state should be toggled
+ */
+bool AP_BoardConfig::safety_button_handle_pressed(uint8_t press_count)
+{
+    if (press_count != 10) {
+        return false;
+    }
+    // get button options
+    uint16_t safety_options = get_safety_button_options();
+    if (!(safety_options & BOARD_SAFETY_OPTION_BUTTON_ACTIVE_ARMED) &&
+        hal.util->get_soft_armed()) {
+        return false;
+    }
+    AP_HAL::Util::safety_state safety_state = hal.util->safety_switch_state();
+    if (safety_state == AP_HAL::Util::SAFETY_DISARMED &&
+        !(safety_options & BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_OFF)) {
+        return false;
+    }
+    if (safety_state == AP_HAL::Util::SAFETY_ARMED &&
+        !(safety_options & BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_ON)) {
+        return false;
+    }
+    return true;
+}

libraries/AP_BoardConfig/AP_BoardConfig.h

@@ -115,7 +115,6 @@ public:
         return _singleton?_singleton->pwm_count.get():8;
     }
 
-#if HAL_HAVE_SAFETY_SWITCH
     enum board_safety_button_option {
         BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_OFF= (1 << 0),
         BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_ON=  (1 << 1),
@@ -127,7 +126,6 @@ public:
     uint16_t get_safety_button_options(void) {
         return uint16_t(state.safety_option.get());
     }
-#endif
 
     // return the value of BRD_SAFETY_MASK
     uint16_t get_safety_mask(void) const {
@@ -167,13 +165,16 @@ public:
         return _singleton?(_singleton->_options & BOARD_OPTION_WATCHDOG)!=0:false;
     }
 
+    // handle press of safety button. Return true if safety state
+    // should be toggled
+    bool safety_button_handle_pressed(uint8_t press_count);
+
 private:
     static AP_BoardConfig *_singleton;
     
     AP_Int16 vehicleSerialNumber;
     AP_Int8 pwm_count;
 
-#if AP_FEATURE_BOARD_DETECT || defined(AP_FEATURE_BRD_PWM_COUNT_PARAM) || HAL_HAVE_SAFETY_SWITCH
     struct {
         AP_Int8 safety_enable;
         AP_Int16 safety_option;
@@ -186,7 +187,6 @@ private:
         AP_Int8 board_type;
         AP_Int8 io_enable;
     } state;
-#endif
 
 #if AP_FEATURE_BOARD_DETECT
     static enum px4_board_type px4_configured_board;