Servo: CAN servo telemetry data for MKS/Zeus servos

1. Update uavcan.equipment.power.CircuitStatus
2. update uavcan.equipment.device.Temperature

libraries/AP_Logger/AP_Logger.h

@@ -310,6 +310,7 @@ class AP_Logger
     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_ServoCktStatus(uint64_t time_us, uint8_t id, float voltage, float current, uint8_t error_flags);
+    void Write_ServoTemperature(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

@@ -454,7 +454,7 @@ void AP_Logger::Write_ServoStatus(uint64_t time_us, uint8_t id, float position,
 }
 
 /*
-  write servo volz status from CAN servo
+  write servo voltage status from CAN servo
  */
 void AP_Logger::Write_ServoCktStatus(uint64_t time_us, uint8_t id, float voltage, float current, uint8_t error_flags)
 {
@@ -469,6 +469,20 @@ void AP_Logger::Write_ServoCktStatus(uint64_t time_us, uint8_t id, float voltage
     WriteBlock(&pkt, sizeof(pkt));
 }
 
+/*
+  write servo Temperature datafrom CAN servo
+ */
+void AP_Logger::Write_ServoTemperature(uint64_t time_us, uint8_t id, float temperature, uint8_t error_flags)
+{
+    const struct log_CSTE pkt {
+        LOG_PACKET_HEADER_INIT(LOG_CSTE_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

@@ -496,6 +496,14 @@ struct PACKED log_CSVI {
   uint8_t error_flags;
 };
 
+struct PACKED log_CSTE {
+  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;
@@ -1282,6 +1290,8 @@ LOG_STRUCTURE_FROM_ESC_TELEM \
       "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_CSTE_MSG, sizeof(log_CSTE), \
+      "CSTE","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), \
@@ -1370,6 +1380,7 @@ enum LogMessages : uint8_t {
     LOG_TERRAIN_MSG,
     LOG_CSRV_MSG,
     LOG_CSVI_MSG,
+    LOG_CSTE_MSG,
     LOG_IDS_FROM_ESC_TELEM,
     LOG_IDS_FROM_BATTMONITOR,
     LOG_IDS_FROM_HAL_CHIBIOS,

libraries/AP_UAVCAN/AP_UAVCAN.cpp

@@ -31,6 +31,7 @@
 #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>
@@ -191,10 +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 actuator status
+// handler actuator circuit status
 UC_REGISTRY_BINDER(ActuatorCktStatusCb, uavcan::equipment::power::CircuitStatus);
 static uavcan::Subscriber<uavcan::equipment::power::CircuitStatus, ActuatorCktStatusCb> *actuator_cktstatus_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS];
 
+// handler actuator temperature and error flag status
+UC_REGISTRY_BINDER(ActuatorTemperatureCb, uavcan::equipment::device::Temperature);
+static uavcan::Subscriber<uavcan::equipment::device::Temperature, ActuatorTemperatureCb> *actuator_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];
@@ -417,6 +422,11 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
         actuator_cktstatus_listener[driver_index]->start(ActuatorCktStatusCb(this, &handle_actuator_cktstatus));
     }
 
+    actuator_temperature_listener[driver_index] = new uavcan::Subscriber<uavcan::equipment::device::Temperature, ActuatorTemperatureCb>(*_node);
+    if (actuator_temperature_listener[driver_index]) {
+        actuator_temperature_listener[driver_index]->start(ActuatorTemperatureCb(this, &handle_actuator_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));
@@ -1012,18 +1022,29 @@ void AP_UAVCAN::handle_actuator_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, co
 }
 
 /*
-  handle actuator volz status message
+  handle actuator voltage and current message
  */
 void AP_UAVCAN::handle_actuator_cktstatus(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ActuatorCktStatusCb &cb)
 {
-    // log as VOLZ message
+    // log as CSVI message
     AP::logger().Write_ServoCktStatus(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_actuator_temperature(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ActuatorTemperatureCb &cb)
+{
+    // log as CSTE message
+    AP::logger().Write_ServoTemperature(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

@@ -49,6 +49,7 @@ class ButtonCb;
 class TrafficReportCb;
 class ActuatorStatusCb;
 class ActuatorCktStatusCb;
+class ActuatorTemperatureCb;
 class ESCStatusCb;
 class DebugCb;
 class ParamGetSetCb;
@@ -343,6 +344,7 @@ class AP_UAVCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
     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_actuator_cktstatus(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ActuatorCktStatusCb &cb);
+    static void handle_actuator_temperature(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ActuatorTemperatureCb &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);