Rover: use Dataflash lib's Log_Write_Cmd

APMrover2/Log.pde

@@ -194,37 +194,6 @@ static void Log_Write_Performance()
     DataFlash.WriteBlock(&pkt, sizeof(pkt));
 }
 
-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;
-    uint8_t waypoint_param1;
-    int32_t waypoint_altitude;
-    int32_t waypoint_latitude;
-    int32_t waypoint_longitude;
-};
-
-// Write a command processing packet. Total length : 19 bytes
-static void Log_Write_Cmd(const AP_Mission::Mission_Command& cmd)
-{
-    struct log_Cmd pkt = {
-        LOG_PACKET_HEADER_INIT(LOG_CMD_MSG),
-        time_ms             : millis(),
-        command_total       : mission.num_commands(),
-        command_number      : cmd.index,
-        waypoint_id         : cmd.id,
-        waypoint_options    : cmd.content.location.options,
-        waypoint_param1     : cmd.p1,
-        waypoint_altitude   : cmd.content.location.alt,
-        waypoint_latitude   : cmd.content.location.lat,
-        waypoint_longitude  : cmd.content.location.lng
-    };
-    DataFlash.WriteBlock(&pkt, sizeof(pkt));
-}
-
 struct PACKED log_Camera {
     LOG_PACKET_HEADER;
     uint32_t time_ms;
@@ -294,9 +263,9 @@ static void Log_Write_Startup(uint8_t type)
 
     // write all commands to the dataflash as well
     AP_Mission::Mission_Command cmd;
-    for (uint8_t i = 0; i < mission.num_commands(); i++) {
+    for (uint16_t i = 0; i < mission.num_commands(); i++) {
         if(mission.read_cmd_from_storage(i,cmd)) {
-            Log_Write_Cmd(cmd);
+            DataFlash.Log_Write_Cmd(mission.num_commands(), cmd);
         }
     }
 }
@@ -535,8 +504,6 @@ static const struct LogStructure log_structure[] PROGMEM = {
       "ATT", "IccC",        "TimeMS,Roll,Pitch,Yaw" },
     { LOG_PERFORMANCE_MSG, sizeof(log_Performance), 
       "PM",  "IIHIhhhBH", "TimeMS,LTime,MLC,gDt,GDx,GDy,GDz,I2CErr,INSErr" },
-    { LOG_CMD_MSG, sizeof(log_Cmd),                 
-      "CMD", "IHHBBBeLL",   "TimeMS,CTot,CNum,CId,COpt,Prm1,Alt,Lat,Lng" },
     { LOG_CAMERA_MSG, sizeof(log_Camera),                 
       "CAM", "IIHLLeccC",   "TimeMS,GPSTime,GPSWeek,Lat,Lng,Alt,Roll,Pitch,Yaw" },
     { LOG_STARTUP_MSG, sizeof(log_Startup),         
@@ -597,7 +564,6 @@ static void start_logging()
 
 // dummy functions
 static void Log_Write_Startup(uint8_t type) {}
-static void Log_Write_Cmd(const AP_Mission::Mission_Command& cmd) {}
 static void Log_Write_Current() {}
 static void Log_Write_Nav_Tuning() {}
 static void Log_Write_Performance() {}

APMrover2/defines.h

@@ -76,7 +76,6 @@ enum mode {
 #define LOG_CTUN_MSG	        0x01
 #define LOG_NTUN_MSG    		0x02
 #define LOG_PERFORMANCE_MSG		0x03
-#define LOG_CMD_MSG			    0x04
 #define LOG_CURRENT_MSG 		0x05
 #define LOG_STARTUP_MSG 		0x06
 #define LOG_SONAR_MSG 		    0x07