Servo: CAN servo telemetry data for MKS/Zeus servos

1. Update uavcan.equipment.power.CircuitStatus
   - Voltage, Current and Error_flags
2. update uavcan.equipment.device.Temperature
   - Temperature and Error_flags

libraries/AP_Logger/AP_Logger.h

@@ -309,6 +309,8 @@ class AP_Logger
     void Write_Message(const char *message);
     void Write_MessageF(const char *fmt, ...);
     void Write_ServoStatus(uint64_t time_us, uint8_t id, float position, float force, float speed, uint8_t power_pct);
+    void Write_PowerCktStatus(uint64_t time_us, uint8_t id, float voltage, float current, uint8_t error_flags);
+    void Write_DeviceTemperature(uint64_t time_us, uint8_t id, float temperature, uint8_t error_flags);
     void Write_Compass();
     void Write_Mode(uint8_t mode, const ModeReason reason);
 

libraries/AP_Logger/LogFile.cpp

@@ -453,6 +453,36 @@ void AP_Logger::Write_ServoStatus(uint64_t time_us, uint8_t id, float position,
     WriteBlock(&pkt, sizeof(pkt));
 }
 
+/*
+  write servo voltage status from CAN servo
+ */
+void AP_Logger::Write_PowerCktStatus(uint64_t time_us, uint8_t id, float voltage, float current, uint8_t error_flags)
+{
+    const struct log_CSVI pkt {
+        LOG_PACKET_HEADER_INIT(LOG_CSVI_MSG),
+        time_us     : time_us,
+        id          : id,
+        voltage     : voltage,
+        current     : current,
+        error_flags : error_flags
+    };
+    WriteBlock(&pkt, sizeof(pkt));
+}
+
+/*
+  write servo Temperature datafrom CAN servo
+ */
+void AP_Logger::Write_DeviceTemperature(uint64_t time_us, uint8_t id, float temperature, uint8_t error_flags)
+{
+    const struct log_TEMP pkt {
+        LOG_PACKET_HEADER_INIT(LOG_TEMP_MSG),
+        time_us     : time_us,
+        device_id   : id,
+        temperature : temperature,
+        error_flags : error_flags
+    };
+    WriteBlock(&pkt, sizeof(pkt));
+}
 
 // Write a Yaw PID packet
 void AP_Logger::Write_PID(uint8_t msg_type, const AP_PIDInfo &info)

libraries/AP_Logger/LogStructure.h

@@ -487,6 +487,23 @@ struct PACKED log_CSRV {
     uint8_t power_pct;
 };
 
+struct PACKED log_CSVI {
+  LOG_PACKET_HEADER;
+  uint64_t time_us;
+  uint8_t id;
+  float voltage;
+  float current;
+  uint8_t error_flags;
+};
+
+struct PACKED log_TEMP {
+  LOG_PACKET_HEADER;
+  uint64_t time_us;
+  uint8_t device_id;
+  float temperature;
+  uint8_t error_flags;
+};
+
 struct PACKED log_ARSP {
     LOG_PACKET_HEADER;
     uint64_t time_us;
@@ -1271,6 +1288,10 @@ LOG_STRUCTURE_FROM_AVOIDANCE \
 LOG_STRUCTURE_FROM_ESC_TELEM \
     { LOG_CSRV_MSG, sizeof(log_CSRV), \
       "CSRV","QBfffB","TimeUS,Id,Pos,Force,Speed,Pow", "s#---%", "F-0000", true }, \
+    { LOG_CSVI_MSG, sizeof(log_CSVI), \
+      "CSVI","QBffB","TimeUS,CId,vol,cur,err", "s#--%", "F-000", true }, \
+    { LOG_TEMP_MSG, sizeof(log_TEMP), \
+      "TEMP","QBfB","TimeUS,CId,tmp,err", "s#-%", "F-00", true }, \
     { LOG_PIDR_MSG, sizeof(log_PID), \
       "PIDR", PID_FMT,  PID_LABELS, PID_UNITS, PID_MULTS, true },  \
     { LOG_PIDP_MSG, sizeof(log_PID), \
@@ -1358,6 +1379,8 @@ enum LogMessages : uint8_t {
     LOG_IDS_FROM_CAMERA,
     LOG_TERRAIN_MSG,
     LOG_CSRV_MSG,
+    LOG_CSVI_MSG,
+    LOG_TEMP_MSG,
     LOG_IDS_FROM_ESC_TELEM,
     LOG_IDS_FROM_BATTMONITOR,
     LOG_IDS_FROM_HAL_CHIBIOS,

libraries/AP_UAVCAN/AP_UAVCAN.cpp

@@ -30,6 +30,8 @@
 #include <uavcan/equipment/actuator/ArrayCommand.hpp>
 #include <uavcan/equipment/actuator/Command.hpp>
 #include <uavcan/equipment/actuator/Status.hpp>
+#include <uavcan/equipment/power/CircuitStatus.hpp>
+#include <uavcan/equipment/device/Temperature.hpp>
 
 #include <uavcan/equipment/esc/RawCommand.hpp>
 #include <uavcan/equipment/esc/Status.hpp>
@@ -190,6 +192,14 @@ 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[HAL_MAX_CAN_PROTOCOL_DRIVERS];
 
+// handler power circuit status
+UC_REGISTRY_BINDER(PowerCktStatusCb, uavcan::equipment::power::CircuitStatus);
+static uavcan::Subscriber<uavcan::equipment::power::CircuitStatus, PowerCktStatusCb> *power_cktstatus_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS];
+
+// handler device temperature and error flag status
+UC_REGISTRY_BINDER(DeviceTemperatureCb, uavcan::equipment::device::Temperature);
+static uavcan::Subscriber<uavcan::equipment::device::Temperature, DeviceTemperatureCb> *device_temperature_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS];
+
 // handler ESC status
 UC_REGISTRY_BINDER(ESCStatusCb, uavcan::equipment::esc::Status);
 static uavcan::Subscriber<uavcan::equipment::esc::Status, ESCStatusCb> *esc_status_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS];
@@ -407,6 +417,16 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
         actuator_status_listener[driver_index]->start(ActuatorStatusCb(this, &handle_actuator_status));
     }
 
+    power_cktstatus_listener[driver_index] = new uavcan::Subscriber<uavcan::equipment::power::CircuitStatus, PowerCktStatusCb>(*_node);
+    if (power_cktstatus_listener[driver_index]) {
+        power_cktstatus_listener[driver_index]->start(PowerCktStatusCb(this, &handle_power_cktstatus));
+    }
+
+    device_temperature_listener[driver_index] = new uavcan::Subscriber<uavcan::equipment::device::Temperature, DeviceTemperatureCb>(*_node);
+    if (device_temperature_listener[driver_index]) {
+        device_temperature_listener[driver_index]->start(DeviceTemperatureCb(this, &handle_device_temperature));
+    }
+
     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));
@@ -1001,6 +1021,31 @@ void AP_UAVCAN::handle_actuator_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, co
                                    cb.msg->power_rating_pct);
 }
 
+/*
+  handle actuator voltage and current message
+ */
+void AP_UAVCAN::handle_power_cktstatus(AP_UAVCAN* ap_uavcan, uint8_t node_id, const PowerCktStatusCb &cb)
+{
+    // log as CSVI message
+    AP::logger().Write_PowerCktStatus(AP_HAL::native_micros64(),
+                                   cb.msg->circuit_id,
+                                   cb.msg->voltage,
+                                   cb.msg->current,
+                                   cb.msg->error_flags);
+}
+
+/*
+  handle actuator temperature message
+ */
+void AP_UAVCAN::handle_device_temperature(AP_UAVCAN* ap_uavcan, uint8_t node_id, const DeviceTemperatureCb &cb)
+{
+    // log as CSTE message
+    AP::logger().Write_DeviceTemperature(AP_HAL::native_micros64(),
+                                   cb.msg->device_id,
+                                   cb.msg->temperature,
+                                   cb.msg->error_flags);
+}
+
 /*
   handle ESC status message
  */

libraries/AP_UAVCAN/AP_UAVCAN.h

@@ -48,6 +48,8 @@
 class ButtonCb;
 class TrafficReportCb;
 class ActuatorStatusCb;
+class PowerCktStatusCb;
+class DeviceTemperatureCb;
 class ESCStatusCb;
 class DebugCb;
 class ParamGetSetCb;
@@ -341,6 +343,8 @@ class AP_UAVCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
     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_power_cktstatus(AP_UAVCAN* ap_uavcan, uint8_t node_id, const PowerCktStatusCb &cb);
+    static void handle_device_temperature(AP_UAVCAN* ap_uavcan, uint8_t node_id, const DeviceTemperatureCb &cb);
     static void handle_ESC_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ESCStatusCb &cb);
     static bool is_esc_data_index_valid(const uint8_t index);
     static void handle_debug(AP_UAVCAN* ap_uavcan, uint8_t node_id, const DebugCb &cb);