AP_UAVCAN: added logging of UAVCAN actuator::Status messages

this allows for logging of CAN servo status

libraries/AP_UAVCAN/AP_UAVCAN.cpp

@@ -51,6 +51,7 @@
 #include <AP_OpticalFlow/AP_OpticalFlow_HereFlow.h>
 #include <AP_ADSB/AP_ADSB.h>
 #include "AP_UAVCAN_DNA_Server.h"
+#include <AP_Logger/AP_Logger.h>
 
 #define LED_DELAY_US 50000
 
@@ -119,6 +120,10 @@ static uavcan::Subscriber<ardupilot::indication::Button, ButtonCb> *safety_butto
 UC_REGISTRY_BINDER(TrafficReportCb, ardupilot::equipment::trafficmonitor::TrafficReport);
 static uavcan::Subscriber<ardupilot::equipment::trafficmonitor::TrafficReport, TrafficReportCb> *traffic_report_listener[MAX_NUMBER_OF_CAN_DRIVERS];
 
+// handler actuator status
+UC_REGISTRY_BINDER(ActuatorStatusCb, uavcan::equipment::actuator::Status);
+static uavcan::Subscriber<uavcan::equipment::actuator::Status, ActuatorStatusCb> *actuator_status_listener[MAX_NUMBER_OF_CAN_DRIVERS];
+
 
 AP_UAVCAN::AP_UAVCAN() :
     _node_allocator()
@@ -272,6 +277,11 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
     if (traffic_report_listener[driver_index]) {
         traffic_report_listener[driver_index]->start(TrafficReportCb(this, &handle_traffic_report));
     }
+
+    actuator_status_listener[driver_index] = new uavcan::Subscriber<uavcan::equipment::actuator::Status, ActuatorStatusCb>(*_node);
+    if (actuator_status_listener[driver_index]) {
+        actuator_status_listener[driver_index]->start(ActuatorStatusCb(this, &handle_actuator_status));
+    }
     
     _led_conf.devices_count = 0;
     if (enable_filters) {
@@ -712,4 +722,18 @@ void AP_UAVCAN::handle_traffic_report(AP_UAVCAN* ap_uavcan, uint8_t node_id, con
     adsb->handle_adsb_vehicle(vehicle);
 }
 
+/*
+  handle actuator status message
+ */
+void AP_UAVCAN::handle_actuator_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ActuatorStatusCb &cb)
+{
+    // log as CSRV message
+    AP::logger().Write_ServoStatus(AP_HAL::micros64(),
+                                   cb.msg->actuator_id,
+                                   cb.msg->position,
+                                   cb.msg->force,
+                                   cb.msg->speed,
+                                   cb.msg->power_rating_pct);
+}
+
 #endif // HAL_WITH_UAVCAN

libraries/AP_UAVCAN/AP_UAVCAN.h

@@ -49,6 +49,7 @@
 // fwd-declare callback classes
 class ButtonCb;
 class TrafficReportCb;
+class ActuatorStatusCb;
 
 /*
     Frontend Backend-Registry Binder: Whenever a message of said DataType_ from new node is received,
@@ -226,6 +227,7 @@ private:
     // safety button handling
     static void handle_button(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ButtonCb &cb);
     static void handle_traffic_report(AP_UAVCAN* ap_uavcan, uint8_t node_id, const TrafficReportCb &cb);
+    static void handle_actuator_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ActuatorStatusCb &cb);
 };
 
 #endif /* AP_UAVCAN_H_ */