diff --git a/libraries/AP_HAL_PX4/RCOutput.cpp b/libraries/AP_HAL_PX4/RCOutput.cpp index f5530cfbd5..64f9ee3857 100644 --- a/libraries/AP_HAL_PX4/RCOutput.cpp +++ b/libraries/AP_HAL_PX4/RCOutput.cpp @@ -14,6 +14,12 @@ #include #include +#include + +#if HAL_WITH_UAVCAN +#include +#endif + extern const AP_HAL::HAL& hal; using namespace PX4; @@ -529,8 +535,37 @@ void PX4RCOutput::_send_outputs(void) } } - // also publish to actuator_direct - _publish_actuators(); + if(AP_BoardConfig::get_can_enable() >= 1) + { + // also publish to actuator_direct (UAVCAN is published via AP_UAVCAN) + _publish_actuators(); +#if HAL_WITH_UAVCAN + + if(hal.can_mgr != nullptr) + { + AP_UAVCAN *ap_uc = hal.can_mgr->get_UAVCAN(); + if(ap_uc != nullptr) + { + if(ap_uc->rc_out_sem_take()) + { + for(uint8_t i = 0; i < _max_channel; i++) + { + ap_uc->rco_write(_period[i], i); + } + + bool armed = hal.util->get_soft_armed(); + if (armed) { + ap_uc->rco_arm_actuators(true); + } else { + ap_uc->rco_arm_actuators(false); + } + + ap_uc->rc_out_sem_give(); + } + } + } +#endif // HAL_WITH_UAVCAN + } perf_end(_perf_rcout); _last_output = now;