diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp index a64415a518..bff766f043 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp @@ -37,12 +37,14 @@ #include #include #include +#include #include #include #include #include #include +#include #include #include #include @@ -110,6 +112,7 @@ static uavcan::Publisher* esc_raw[HAL_MAX_CA static uavcan::Publisher* rgb_led[HAL_MAX_CAN_PROTOCOL_DRIVERS]; static uavcan::Publisher* buzzer[HAL_MAX_CAN_PROTOCOL_DRIVERS]; static uavcan::Publisher* safety_state[HAL_MAX_CAN_PROTOCOL_DRIVERS]; +static uavcan::Publisher* arming_status[HAL_MAX_CAN_PROTOCOL_DRIVERS]; static uavcan::Publisher* rtcm_stream[HAL_MAX_CAN_PROTOCOL_DRIVERS]; // subscribers @@ -286,6 +289,10 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters) safety_state[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); safety_state[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest); + arming_status[driver_index] = new uavcan::Publisher(*_node); + arming_status[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); + arming_status[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest); + rtcm_stream[driver_index] = new uavcan::Publisher(*_node); rtcm_stream[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); rtcm_stream[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest); @@ -702,25 +709,35 @@ void AP_UAVCAN::rtcm_stream_send() // SafetyState send void AP_UAVCAN::safety_state_send() { - ardupilot::indication::SafetyState msg; uint32_t now = AP_HAL::native_millis(); if (now - _last_safety_state_ms < 500) { // update at 2Hz return; } _last_safety_state_ms = now; - switch (hal.util->safety_switch_state()) { - case AP_HAL::Util::SAFETY_ARMED: - msg.status = ardupilot::indication::SafetyState::STATUS_SAFETY_OFF; - break; - case AP_HAL::Util::SAFETY_DISARMED: - msg.status = ardupilot::indication::SafetyState::STATUS_SAFETY_ON; - break; - default: - // nothing to send - return; + + { // handle SafetyState + ardupilot::indication::SafetyState safety_msg; + switch (hal.util->safety_switch_state()) { + case AP_HAL::Util::SAFETY_ARMED: + safety_msg.status = ardupilot::indication::SafetyState::STATUS_SAFETY_OFF; + break; + case AP_HAL::Util::SAFETY_DISARMED: + safety_msg.status = ardupilot::indication::SafetyState::STATUS_SAFETY_ON; + break; + default: + // nothing to send + break; + } + safety_state[_driver_index]->broadcast(safety_msg); + } + + { // handle ArmingStatus + uavcan::equipment::safety::ArmingStatus arming_msg; + arming_msg.status = AP::arming().is_armed() ? uavcan::equipment::safety::ArmingStatus::STATUS_FULLY_ARMED : + uavcan::equipment::safety::ArmingStatus::STATUS_DISARMED; + arming_status[_driver_index]->broadcast(arming_msg); } - safety_state[_driver_index]->broadcast(msg); } /*