diff --git a/libraries/AP_HAL_PX4/RCOutput.cpp b/libraries/AP_HAL_PX4/RCOutput.cpp index 9d4f78bfdc..6aad126f18 100644 --- a/libraries/AP_HAL_PX4/RCOutput.cpp +++ b/libraries/AP_HAL_PX4/RCOutput.cpp @@ -309,6 +309,29 @@ void PX4RCOutput::force_safety_pending_requests(void) _safety_state_request_last_ms = now; } } + // also update safety button options if needed + if (now - _last_safety_options_check_ms > 1000) { + _last_safety_options_check_ms = now; + AP_BoardConfig *boardconfig = AP_BoardConfig::get_instance(); + if (boardconfig) { + uint16_t desired_options = 0; + uint16_t options = boardconfig->get_safety_button_options(); + if (!(options & AP_BoardConfig::BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_OFF)) { + desired_options |= PWM_SERVO_SET_SAFETY_OPTION_DISABLE_BUTTON_OFF; + } + if (!(options & AP_BoardConfig::BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_ON)) { + desired_options |= PWM_SERVO_SET_SAFETY_OPTION_DISABLE_BUTTON_ON; + } + if (!(options & AP_BoardConfig::BOARD_SAFETY_OPTION_BUTTON_ACTIVE_ARMED) && hal.util->get_soft_armed()) { + desired_options |= PWM_SERVO_SET_SAFETY_OPTION_DISABLE_BUTTON_OFF | PWM_SERVO_SET_SAFETY_OPTION_DISABLE_BUTTON_ON; + } + if (_last_safety_options != desired_options) { + if (ioctl(_pwm_fd, PWM_SERVO_SET_SAFETY_OPTIONS, desired_options) == OK) { + _last_safety_options = desired_options; + } + } + } + } } void PX4RCOutput::force_safety_no_wait(void) diff --git a/libraries/AP_HAL_PX4/RCOutput.h b/libraries/AP_HAL_PX4/RCOutput.h index 217bbb54c2..d093adace0 100644 --- a/libraries/AP_HAL_PX4/RCOutput.h +++ b/libraries/AP_HAL_PX4/RCOutput.h @@ -68,6 +68,8 @@ private: uint32_t _rate_mask_alt; uint16_t _enabled_channels; uint32_t _period_max; + uint32_t _last_safety_options_check_ms; + uint16_t _last_safety_options; struct { int pwm_sub; actuator_outputs_s outputs;