From 869f325c66b6f80028999ecbccf8a62d357bb348 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Sun, 16 Mar 2014 17:40:30 +0900
Subject: [PATCH] DataFlash: Log_Write_MavCmd replaces Log_Write_Cmd

---
 libraries/DataFlash/DataFlash.h | 23 +++++++++--------
 libraries/DataFlash/LogFile.cpp | 46 ++++++++++++---------------------
 2 files changed, 28 insertions(+), 41 deletions(-)

diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h
index c2c1372baa..239218abd7 100644
--- a/libraries/DataFlash/DataFlash.h
+++ b/libraries/DataFlash/DataFlash.h
@@ -12,7 +12,6 @@
 #include <AP_InertialSensor.h>
 #include <AP_Baro.h>
 #include <AP_AHRS.h>
-#include <AP_Mission.h>
 #include <stdint.h>
 
 class DataFlash_Class
@@ -59,7 +58,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_MavCmd(uint16_t cmd_total, const mavlink_mission_item_t& mav_cmd);
     void Log_Write_Message(const char *message);
     void Log_Write_Message_P(const prog_char_t *message);
 
@@ -324,13 +323,15 @@ 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;
+    uint16_t sequence;
+    uint16_t command;
+    float param1;
+    float param2;
+    float param3;
+    float param4;
+    float latitude;
+    float longitude;
+    float altitude;
 };
 
 #define LOG_COMMON_STRUCTURES \
@@ -369,7 +370,7 @@ struct PACKED log_Cmd {
     { LOG_EKF4_MSG, sizeof(log_EKF4), \
       "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" }
+      "CMD", "IHHHfffffff","TimeMS,CTot,CNum,CId,Prm1,Prm2,Prm3,Prm4,Lat,Lng,Alt" }
 
 // message types for common messages
 #define LOG_FORMAT_MSG	  128
@@ -389,7 +390,7 @@ struct PACKED log_Cmd {
 #define LOG_EKF3_MSG      142
 #define LOG_EKF4_MSG      143
 #define LOG_GPS2_MSG	  144
-#define LOG_CMD_MSG       145
+#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 ee65361a62..be8156bd8f 100644
--- a/libraries/DataFlash/LogFile.cpp
+++ b/libraries/DataFlash/LogFile.cpp
@@ -943,35 +943,21 @@ 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)
+void DataFlash_Class::Log_Write_MavCmd(uint16_t cmd_total, const mavlink_mission_item_t& mav_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));
-    }
+    struct log_Cmd pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_CMD_MSG),
+        time_ms         : hal.scheduler->millis(),
+        command_total   : (uint16_t)cmd_total,
+        sequence        : (uint16_t)mav_cmd.seq,
+        command         : (uint16_t)mav_cmd.command,
+        param1          : (float)mav_cmd.param1,
+        param2          : (float)mav_cmd.param2,
+        param3          : (float)mav_cmd.param3,
+        param4          : (float)mav_cmd.param4,
+        latitude        : (float)mav_cmd.x,
+        longitude       : (float)mav_cmd.y,
+        altitude        : (float)mav_cmd.z
+    };
+    WriteBlock(&pkt, sizeof(pkt));
 }