From 1897411b9d93c036ac1eea39921f8a4e82438b84 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 2 Dec 2022 14:17:14 +1100
Subject: [PATCH] AP_Logger: expand servo status logging

---
 libraries/AP_Logger/AP_Logger.h    |  3 ++-
 libraries/AP_Logger/LogFile.cpp    | 11 +++++++++--
 libraries/AP_Logger/LogStructure.h | 14 +++++++++++++-
 3 files changed, 24 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h
index 717ee5f604..75c68e5414 100644
--- a/libraries/AP_Logger/AP_Logger.h
+++ b/libraries/AP_Logger/AP_Logger.h
@@ -255,7 +255,8 @@ public:
     void Write_Radio(const mavlink_radio_t &packet);
     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_ServoStatus(uint64_t time_us, uint8_t id, float position, float force, float speed, uint8_t power_pct,
+                           float pos_cmd, float voltage, float current, float mot_temp, float pcb_temp, uint8_t error);
     void Write_Compass();
     void Write_Mode(uint8_t mode, const ModeReason reason);
 
diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp
index 22bedfe546..ac1d59e53d 100644
--- a/libraries/AP_Logger/LogFile.cpp
+++ b/libraries/AP_Logger/LogFile.cpp
@@ -447,7 +447,8 @@ bool AP_Logger_Backend::Write_Mode(uint8_t mode, const ModeReason reason)
 /*
   write servo status from CAN servo
  */
-void AP_Logger::Write_ServoStatus(uint64_t time_us, uint8_t id, float position, float force, float speed, uint8_t power_pct)
+void AP_Logger::Write_ServoStatus(uint64_t time_us, uint8_t id, float position, float force, float speed, uint8_t power_pct,
+                                  float pos_cmd, float voltage, float current, float mot_temp, float pcb_temp, uint8_t error)
 {
     const struct log_CSRV pkt {
         LOG_PACKET_HEADER_INIT(LOG_CSRV_MSG),
@@ -456,7 +457,13 @@ void AP_Logger::Write_ServoStatus(uint64_t time_us, uint8_t id, float position,
         position    : position,
         force       : force,
         speed       : speed,
-        power_pct   : power_pct
+        power_pct   : power_pct,
+        pos_cmd     : pos_cmd,
+        voltage     : voltage,
+        current     : current,
+        mot_temp    : mot_temp,
+        pcb_temp    : pcb_temp,
+        error       : error,
     };
     WriteBlock(&pkt, sizeof(pkt));
 }
diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h
index 61a6da48ca..d804e6ffbf 100644
--- a/libraries/AP_Logger/LogStructure.h
+++ b/libraries/AP_Logger/LogStructure.h
@@ -491,6 +491,12 @@ struct PACKED log_CSRV {
     float force;
     float speed;
     uint8_t power_pct;
+    float pos_cmd;
+    float voltage;
+    float current;
+    float mot_temp;
+    float pcb_temp;
+    uint8_t error;
 };
 
 struct PACKED log_ARSP {
@@ -754,6 +760,12 @@ struct PACKED log_VER {
 // @Field: Force: Force being applied
 // @Field: Speed: Current servo movement speed
 // @Field: Pow: Amount of rated power being applied
+// @Field: PosCmd: commanded servo position
+// @Field: V: Voltage
+// @Field: A: Current
+// @Field: MotT: motor temperature
+// @Field: PCBT: PCB temperature
+// @Field: Err: error flags
 
 // @LoggerMessage: DMS
 // @Description: DataFlash-Over-MAVLink statistics
@@ -1271,7 +1283,7 @@ LOG_STRUCTURE_FROM_AVOIDANCE \
       "TERR","QBLLHffHHf","TimeUS,Status,Lat,Lng,Spacing,TerrH,CHeight,Pending,Loaded,ROfs", "s-DU-mm--m", "F-GG-00--0", true }, \
 LOG_STRUCTURE_FROM_ESC_TELEM \
     { LOG_CSRV_MSG, sizeof(log_CSRV), \
-      "CSRV","QBfffB","TimeUS,Id,Pos,Force,Speed,Pow", "s#---%", "F-0000", true }, \
+      "CSRV","QBfffBfffffB","TimeUS,Id,Pos,Force,Speed,Pow,PosCmd,V,A,MotT,PCBT,Err", "s#---%dvAOO-", "F-000000000-", true }, \
     { LOG_PIDR_MSG, sizeof(log_PID), \
       "PIDR", PID_FMT,  PID_LABELS, PID_UNITS, PID_MULTS, true },  \
     { LOG_PIDP_MSG, sizeof(log_PID), \