diff --git a/libraries/AP_HAL_QURT/RCOutput.cpp b/libraries/AP_HAL_QURT/RCOutput.cpp index 6b9124fcfa..eb509d0709 100644 --- a/libraries/AP_HAL_QURT/RCOutput.cpp +++ b/libraries/AP_HAL_QURT/RCOutput.cpp @@ -2,6 +2,7 @@ #include "RCOutput.h" #include +#include extern const AP_HAL::HAL& hal; @@ -114,10 +115,24 @@ void RCOutput::send_receive(void) return; } + AP_BoardConfig *boardconfig = AP_BoardConfig::get_singleton(); + uint32_t safety_mask = 0; + + if (boardconfig != nullptr) { + // mask of channels to allow with safety on + safety_mask = boardconfig->get_safety_mask(); + } + int16_t data[5] {}; for (uint8_t i=0; i<4; i++) { - data[i] = pwm_to_esc(period[i]); + uint16_t v = period[i]; + if (safety_on && (safety_mask & (1U<safety_on) { + return SAFETY_DISARMED; + } + return SAFETY_ARMED; +} + #if ENABLE_HEAP void *Util::allocate_heap_memory(size_t size) { diff --git a/libraries/AP_HAL_QURT/Util.h b/libraries/AP_HAL_QURT/Util.h index 7c389703da..f1eea1493c 100644 --- a/libraries/AP_HAL_QURT/Util.h +++ b/libraries/AP_HAL_QURT/Util.h @@ -21,6 +21,11 @@ public: uint32_t available_memory(void) override; + /* + return state of safety switch, if applicable + */ + enum safety_state safety_switch_state(void) override; + #if ENABLE_HEAP // heap functions, note that a heap once alloc'd cannot be dealloc'd virtual void *allocate_heap_memory(size_t size) override;