From 26668531002b105fd70892745f95c6cb3cdee2ac Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 4 Jan 2020 15:25:55 +1100
Subject: [PATCH] AP_Logger: added CSRV logging for CAN servo status

---
 libraries/AP_Logger/AP_Logger.h    |  1 +
 libraries/AP_Logger/LogFile.cpp    | 17 +++++++++++++++++
 libraries/AP_Logger/LogStructure.h | 13 +++++++++++++
 3 files changed, 31 insertions(+)

diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h
index eeabfe5153..3ca16b20f5 100644
--- a/libraries/AP_Logger/AP_Logger.h
+++ b/libraries/AP_Logger/AP_Logger.h
@@ -259,6 +259,7 @@ public:
     void Write_Camera(const Location &current_loc, uint64_t timestamp_us=0);
     void Write_Trigger(const Location &current_loc);
     void Write_ESC(uint8_t id, uint64_t time_us, int32_t rpm, uint16_t voltage, uint16_t current, int16_t esc_temp, uint16_t current_tot, int16_t motor_temp);
+    void Write_ServoStatus(uint64_t time_us, uint8_t id, float position, float force, float speed, uint8_t power_pct);
     void Write_Attitude(const Vector3f &targets);
     void Write_AttitudeView(AP_AHRS_View &ahrs, const Vector3f &targets);
     void Write_Current();
diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp
index df2bfd0a85..e7b8eb19eb 100644
--- a/libraries/AP_Logger/LogFile.cpp
+++ b/libraries/AP_Logger/LogFile.cpp
@@ -799,6 +799,23 @@ void AP_Logger::Write_ESC(uint8_t id, uint64_t time_us, int32_t rpm, uint16_t vo
     WriteBlock(&pkt, sizeof(pkt));
 }
 
+/*
+  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)
+{
+    const struct log_CSRV pkt {
+        LOG_PACKET_HEADER_INIT(LOG_CSRV_MSG),
+        time_us     : time_us,
+        id          : id,
+        position    : position,
+        force       : force,
+        speed       : speed,
+        power_pct   : power_pct
+    };
+    WriteBlock(&pkt, sizeof(pkt));
+}
+
 // Write a Yaw PID packet
 void AP_Logger::Write_PID(uint8_t msg_type, const PID_Info &info)
 {
diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h
index ed47ca9d07..a58cf20ff7 100644
--- a/libraries/AP_Logger/LogStructure.h
+++ b/libraries/AP_Logger/LogStructure.h
@@ -951,6 +951,16 @@ struct PACKED log_Esc {
     int16_t motor_temp;
 };
 
+struct PACKED log_CSRV {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;     
+    uint8_t id;
+    float position;
+    float force;
+    float speed;
+    uint8_t power_pct;
+};
+
 struct PACKED log_AIRSPEED {
     LOG_PACKET_HEADER;
     uint64_t time_us;
@@ -1484,6 +1494,8 @@ struct PACKED log_Arm_Disarm {
       "ESC7",  ESC_FMT, ESC_LABELS, ESC_UNITS, ESC_MULTS }, \
     { LOG_ESC8_MSG, sizeof(log_Esc), \
       "ESC8",  ESC_FMT, ESC_LABELS, ESC_UNITS, ESC_MULTS }, \
+    { LOG_CSRV_MSG, sizeof(log_CSRV), \
+      "CSRV","QBfffB","TimeUS,Id,Pos,Force,Speed,Pow", "s#---%", "F-0000" }, \
     { LOG_COMPASS2_MSG, sizeof(log_Compass), \
       "MAG2",MAG_FMT,    MAG_LABELS, MAG_UNITS, MAG_MULTS }, \
     { LOG_COMPASS3_MSG, sizeof(log_Compass), \
@@ -1631,6 +1643,7 @@ enum LogMessages : uint8_t {
     LOG_ESC6_MSG,
     LOG_ESC7_MSG,
     LOG_ESC8_MSG,
+    LOG_CSRV_MSG,
     LOG_BAR2_MSG,
     LOG_ARSP_MSG,
     LOG_ATTITUDE_MSG,