From b4966f21d8229ebd524ee9fc7bb094f14defa216 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 4 Jan 2020 14:52:59 +1100
Subject: [PATCH] AP_UAVCAN: added logging of UAVCAN actuator::Status messages

this allows for logging of CAN servo status
---
 libraries/AP_UAVCAN/AP_UAVCAN.cpp | 24 ++++++++++++++++++++++++
 libraries/AP_UAVCAN/AP_UAVCAN.h   |  2 ++
 2 files changed, 26 insertions(+)

diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp
index 9f1186fc62..f3db790550 100644
--- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp
+++ b/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
diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.h b/libraries/AP_UAVCAN/AP_UAVCAN.h
index ab2f322fcf..b9efd121b2 100644
--- a/libraries/AP_UAVCAN/AP_UAVCAN.h
+++ b/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_ */