From 631725f817a17e9973f0ebe60f358b3a9f6f76a9 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Fri, 23 Aug 2019 11:21:39 +1000
Subject: [PATCH] AP_Logger: add Write_Command for writing out received mavlink
 commands

---
 libraries/AP_Logger/AP_Logger.h    |  1 +
 libraries/AP_Logger/LogFile.cpp    | 26 ++++++++++++++++++++++++++
 libraries/AP_Logger/LogStructure.h | 23 +++++++++++++++++++++++
 3 files changed, 50 insertions(+)

diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h
index 7db179bc0a..5a1483e834 100644
--- a/libraries/AP_Logger/AP_Logger.h
+++ b/libraries/AP_Logger/AP_Logger.h
@@ -257,6 +257,7 @@ public:
     void Write_Mode(uint8_t mode, uint8_t reason);
 
     void Write_EntireMission();
+    void Write_Command(const mavlink_command_int_t &packet, MAV_RESULT result, bool was_command_long=false);
     void Write_Mission_Cmd(const AP_Mission &mission,
                                const AP_Mission::Mission_Command &cmd);
     void Write_Origin(uint8_t origin_type, const Location &loc);
diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp
index 2a822f82b9..048ea56eb2 100644
--- a/libraries/AP_Logger/LogFile.cpp
+++ b/libraries/AP_Logger/LogFile.cpp
@@ -398,6 +398,32 @@ void AP_Logger::Write_Vibration()
     WriteBlock(&pkt, sizeof(pkt));
 }
 
+void AP_Logger::Write_Command(const mavlink_command_int_t &packet,
+                              const MAV_RESULT result,
+                              bool was_command_long)
+{
+    const struct log_MAVLink_Command pkt{
+        LOG_PACKET_HEADER_INIT(LOG_MAVLINK_COMMAND_MSG),
+        time_us         : AP_HAL::micros64(),
+        target_system   : packet.target_system,
+        target_component: packet.target_component,
+        frame           : packet.frame,
+        command         : packet.command,
+        current         : packet.current,
+        autocontinue    : packet.autocontinue,
+        param1          : packet.param1,
+        param2          : packet.param2,
+        param3          : packet.param3,
+        param4          : packet.param4,
+        x               : packet.x,
+        y               : packet.y,
+        z               : packet.z,
+        result          : (uint8_t)result,
+        was_command_long:was_command_long,
+    };
+    return WriteBlock(&pkt, sizeof(pkt));
+}
+
 bool AP_Logger_Backend::Write_Mission_Cmd(const AP_Mission &mission,
                                               const AP_Mission::Mission_Command &cmd)
 {
diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h
index c41259924b..a6e03304ef 100644
--- a/libraries/AP_Logger/LogStructure.h
+++ b/libraries/AP_Logger/LogStructure.h
@@ -682,6 +682,26 @@ struct PACKED log_Cmd {
     uint8_t frame;
 };
 
+struct PACKED log_MAVLink_Command {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    uint8_t target_system;
+    uint8_t target_component;
+    uint8_t frame;
+    uint16_t command;
+    uint8_t current;
+    uint8_t autocontinue;
+    float param1;
+    float param2;
+    float param3;
+    float param4;
+    int32_t x;
+    int32_t y;
+    float z;
+    uint8_t result;
+    bool was_command_long;
+};
+
 struct PACKED log_Radio {
     LOG_PACKET_HEADER;
     uint64_t time_us;
@@ -1307,6 +1327,8 @@ struct PACKED log_Arm_Disarm {
       "POWR","QffHB","TimeUS,Vcc,VServo,Flags,Safety", "svv--", "F00--" },  \
     { LOG_CMD_MSG, sizeof(log_Cmd), \
       "CMD", "QHHHffffLLfB","TimeUS,CTot,CNum,CId,Prm1,Prm2,Prm3,Prm4,Lat,Lng,Alt,Frame", "s-------DUm-", "F-------GG0-" }, \
+    { LOG_MAVLINK_COMMAND_MSG, sizeof(log_MAVLink_Command), \
+      "MAVC", "QBBBHBBffffiifBB","TimeUS,TS,TC,Fr,Cmd,Cur,AC,P1,P2,P3,P4,X,Y,Z,Res,WL", "s---------------", "F---------------" }, \
     { LOG_RADIO_MSG, sizeof(log_Radio), \
       "RAD", "QBBBBBHH", "TimeUS,RSSI,RemRSSI,TxBuf,Noise,RemNoise,RxErrors,Fixed", "s-------", "F-------" }, \
     { LOG_CAMERA_MSG, sizeof(log_Camera), \
@@ -1637,6 +1659,7 @@ enum LogMessages : uint8_t {
     LOG_AHR2_MSG,
     LOG_SIMSTATE_MSG,
     LOG_CMD_MSG,
+    LOG_MAVLINK_COMMAND_MSG,
     LOG_RADIO_MSG,
     LOG_ATRP_MSG,
     LOG_CAMERA_MSG,