From 150faafaf4a74c4b1c2bcf9d3bee8796a89fbede Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 12 Mar 2014 17:17:13 +0900
Subject: [PATCH] Dataflash: add Log_Write_Cmd method

---
 libraries/DataFlash/DataFlash.h | 20 +++++++++++++++++++-
 libraries/DataFlash/LogFile.cpp | 33 +++++++++++++++++++++++++++++++++
 2 files changed, 52 insertions(+), 1 deletion(-)

diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h
index 82f95af6c6..c2c1372baa 100644
--- a/libraries/DataFlash/DataFlash.h
+++ b/libraries/DataFlash/DataFlash.h
@@ -12,6 +12,7 @@
 #include <AP_InertialSensor.h>
 #include <AP_Baro.h>
 #include <AP_AHRS.h>
+#include <AP_Mission.h>
 #include <stdint.h>
 
 class DataFlash_Class
@@ -58,6 +59,7 @@ public:
 #if AP_AHRS_NAVEKF_AVAILABLE
     void Log_Write_EKF(AP_AHRS_NavEKF &ahrs);
 #endif
+    void Log_Write_Cmd(uint16_t cmd_total, const AP_Mission::Mission_Command& cmd);
     void Log_Write_Message(const char *message);
     void Log_Write_Message_P(const prog_char_t *message);
 
@@ -318,6 +320,19 @@ struct PACKED log_EKF4 {
     int16_t sqrtvarVT;
 };
 
+struct PACKED log_Cmd {
+    LOG_PACKET_HEADER;
+    uint32_t time_ms;
+    uint16_t command_total;
+    uint16_t command_number;
+    uint8_t waypoint_id;
+    uint8_t waypoint_options;
+    uint16_t waypoint_param1;
+    int32_t waypoint_altitude;
+    int32_t waypoint_latitude;
+    int32_t waypoint_longitude;
+};
+
 #define LOG_COMMON_STRUCTURES \
     { LOG_FORMAT_MSG, sizeof(log_Format), \
       "FMT", "BBnNZ",      "Type,Length,Name,Format,Columns" },    \
@@ -352,7 +367,9 @@ struct PACKED log_EKF4 {
     { LOG_EKF3_MSG, sizeof(log_EKF3), \
       "EKF3","Icccccchhhc","TimeMS,IVN,IVE,IVD,IPN,IPE,IPD,IMX,IMY,IMZ,IVT" }, \
     { LOG_EKF4_MSG, sizeof(log_EKF4), \
-      "EKF4","Icccccchhhc","TimeMS,SVN,SVE,SVD,SPN,SPE,SPD,SMX,SMY,SMZ,SVT" }
+      "EKF4","Icccccchhhc","TimeMS,SVN,SVE,SVD,SPN,SPE,SPD,SMX,SMY,SMZ,SVT" }, \
+    { LOG_CMD_MSG, sizeof(log_Cmd), \
+      "CMD", "IHHBBHeLL","TimeMS,CTot,CNum,CId,COpt,Prm1,Alt,Lat,Lng" }
 
 // message types for common messages
 #define LOG_FORMAT_MSG	  128
@@ -372,6 +389,7 @@ struct PACKED log_EKF4 {
 #define LOG_EKF3_MSG      142
 #define LOG_EKF4_MSG      143
 #define LOG_GPS2_MSG	  144
+#define LOG_CMD_MSG       145
 
 #include "DataFlash_Block.h"
 #include "DataFlash_File.h"
diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp
index 3a38a58f92..ee65361a62 100644
--- a/libraries/DataFlash/LogFile.cpp
+++ b/libraries/DataFlash/LogFile.cpp
@@ -942,3 +942,36 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs)
 }
 #endif
 
+// Write a command processing packet
+void DataFlash_Class::Log_Write_Cmd(uint16_t cmd_total, const AP_Mission::Mission_Command& cmd)
+{
+    if (cmd.id == MAV_CMD_DO_JUMP) {
+        struct log_Cmd pkt = {
+            LOG_PACKET_HEADER_INIT(LOG_CMD_MSG),
+            time_ms             : hal.scheduler->millis(),
+            command_total       : cmd_total,
+            command_number      : cmd.index,
+            waypoint_id         : cmd.id,
+            waypoint_options    : 0,
+            waypoint_param1     : cmd.content.jump.target,
+            waypoint_altitude   : 0,
+            waypoint_latitude   : cmd.content.jump.num_times,
+            waypoint_longitude  : 0
+        };
+        WriteBlock(&pkt, sizeof(pkt));
+    }else{
+        struct log_Cmd pkt = {
+            LOG_PACKET_HEADER_INIT(LOG_CMD_MSG),
+            time_ms             : hal.scheduler->millis(),
+            command_total       : cmd_total,
+            command_number      : cmd.index,
+            waypoint_id         : cmd.id,
+            waypoint_options    : cmd.content.location.options,
+            waypoint_param1     : (uint16_t)cmd.p1,
+            waypoint_altitude   : cmd.content.location.alt,
+            waypoint_latitude   : cmd.content.location.lat,
+            waypoint_longitude  : cmd.content.location.lng
+        };
+        WriteBlock(&pkt, sizeof(pkt));
+    }
+}