AP_UAVCAN: Broadcast ArmingStatus regularly

libraries/AP_UAVCAN/AP_UAVCAN.cpp

@@ -37,12 +37,14 @@
 #include <uavcan/equipment/indication/SingleLightCommand.hpp>
 #include <uavcan/equipment/indication/BeepCommand.hpp>
 #include <uavcan/equipment/indication/RGB565.hpp>
+#include <uavcan/equipment/safety/ArmingStatus.hpp>
 #include <ardupilot/indication/SafetyState.hpp>
 #include <ardupilot/indication/Button.hpp>
 #include <ardupilot/equipment/trafficmonitor/TrafficReport.hpp>
 #include <uavcan/equipment/gnss/RTCMStream.hpp>
 #include <uavcan/protocol/debug/LogMessage.hpp>
 
+#include <AP_Arming/AP_Arming.h>
 #include <AP_Baro/AP_Baro_UAVCAN.h>
 #include <AP_RangeFinder/AP_RangeFinder_UAVCAN.h>
 #include <AP_GPS/AP_GPS_UAVCAN.h>
@@ -110,6 +112,7 @@ static uavcan::Publisher<uavcan::equipment::esc::RawCommand>* esc_raw[HAL_MAX_CA
 static uavcan::Publisher<uavcan::equipment::indication::LightsCommand>* rgb_led[HAL_MAX_CAN_PROTOCOL_DRIVERS];
 static uavcan::Publisher<uavcan::equipment::indication::BeepCommand>* buzzer[HAL_MAX_CAN_PROTOCOL_DRIVERS];
 static uavcan::Publisher<ardupilot::indication::SafetyState>* safety_state[HAL_MAX_CAN_PROTOCOL_DRIVERS];
+static uavcan::Publisher<uavcan::equipment::safety::ArmingStatus>* arming_status[HAL_MAX_CAN_PROTOCOL_DRIVERS];
 static uavcan::Publisher<uavcan::equipment::gnss::RTCMStream>* 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<uavcan::equipment::safety::ArmingStatus>(*_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<uavcan::equipment::gnss::RTCMStream>(*_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:
+    { // handle SafetyState
-        msg.status = ardupilot::indication::SafetyState::STATUS_SAFETY_OFF;
+        ardupilot::indication::SafetyState safety_msg;
-        break;
+        switch (hal.util->safety_switch_state()) {
-    case AP_HAL::Util::SAFETY_DISARMED:
+        case AP_HAL::Util::SAFETY_ARMED:
-        msg.status = ardupilot::indication::SafetyState::STATUS_SAFETY_ON;
+            safety_msg.status = ardupilot::indication::SafetyState::STATUS_SAFETY_OFF;
-        break;
+            break;
-    default:
+        case AP_HAL::Util::SAFETY_DISARMED:
-        // nothing to send
+            safety_msg.status = ardupilot::indication::SafetyState::STATUS_SAFETY_ON;
-        return;
+            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);
 }
 
 /*