From c5c921d3211419afdbfa5042bae48ef7896c7d4b Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 4 Jan 2020 15:57:01 +1100
Subject: [PATCH] AP_UAVCAN: added logging of CAN ESC status

---
 libraries/AP_UAVCAN/AP_UAVCAN.cpp | 27 +++++++++++++++++++++++++++
 libraries/AP_UAVCAN/AP_UAVCAN.h   |  2 ++
 2 files changed, 29 insertions(+)

diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp
index 0f3272145d..71344918d0 100644
--- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp
+++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp
@@ -32,6 +32,7 @@
 #include <uavcan/equipment/actuator/Status.hpp>
 
 #include <uavcan/equipment/esc/RawCommand.hpp>
+#include <uavcan/equipment/esc/Status.hpp>
 #include <uavcan/equipment/indication/LightsCommand.hpp>
 #include <uavcan/equipment/indication/SingleLightCommand.hpp>
 #include <uavcan/equipment/indication/BeepCommand.hpp>
@@ -124,6 +125,10 @@ static uavcan::Subscriber<ardupilot::equipment::trafficmonitor::TrafficReport, T
 UC_REGISTRY_BINDER(ActuatorStatusCb, uavcan::equipment::actuator::Status);
 static uavcan::Subscriber<uavcan::equipment::actuator::Status, ActuatorStatusCb> *actuator_status_listener[MAX_NUMBER_OF_CAN_DRIVERS];
 
+// handler ESC status
+UC_REGISTRY_BINDER(ESCStatusCb, uavcan::equipment::esc::Status);
+static uavcan::Subscriber<uavcan::equipment::esc::Status, ESCStatusCb> *esc_status_listener[MAX_NUMBER_OF_CAN_DRIVERS];
+
 
 AP_UAVCAN::AP_UAVCAN() :
     _node_allocator()
@@ -282,6 +287,11 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
     if (actuator_status_listener[driver_index]) {
         actuator_status_listener[driver_index]->start(ActuatorStatusCb(this, &handle_actuator_status));
     }
+
+    esc_status_listener[driver_index] = new uavcan::Subscriber<uavcan::equipment::esc::Status, ESCStatusCb>(*_node);
+    if (esc_status_listener[driver_index]) {
+        esc_status_listener[driver_index]->start(ESCStatusCb(this, &handle_ESC_status));
+    }
     
     _led_conf.devices_count = 0;
     if (enable_filters) {
@@ -736,4 +746,21 @@ void AP_UAVCAN::handle_actuator_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, co
                                    cb.msg->power_rating_pct);
 }
 
+/*
+  handle ESC status message
+ */
+void AP_UAVCAN::handle_ESC_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ESCStatusCb &cb)
+{
+    // log as CESC message
+    AP::logger().Write_ESCStatus(AP_HAL::micros64(),
+                                 cb.msg->esc_index,
+                                 cb.msg->error_count,
+                                 cb.msg->voltage,
+                                 cb.msg->current,
+                                 cb.msg->temperature - C_TO_KELVIN,
+                                 cb.msg->rpm,
+                                 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 b283bf5ca7..99bba6a91c 100644
--- a/libraries/AP_UAVCAN/AP_UAVCAN.h
+++ b/libraries/AP_UAVCAN/AP_UAVCAN.h
@@ -50,6 +50,7 @@
 class ButtonCb;
 class TrafficReportCb;
 class ActuatorStatusCb;
+class ESCStatusCb;
 
 /*
     Frontend Backend-Registry Binder: Whenever a message of said DataType_ from new node is received,
@@ -228,6 +229,7 @@ private:
     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);
+    static void handle_ESC_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ESCStatusCb &cb);
 };
 
 #endif /* AP_UAVCAN_H_ */