Plane: dataflash logging to use Mission command

ArduPlane/Log.pde

@@ -240,18 +240,18 @@ struct PACKED log_Cmd {
 };
 
 // Write a command processing packet. Total length : 19 bytes
-static void Log_Write_Cmd(uint8_t num, const struct Location *wp)
+static void Log_Write_Cmd(const AP_Mission::Mission_Command& cmd)
 {
     struct log_Cmd pkt = {
         LOG_PACKET_HEADER_INIT(LOG_CMD_MSG),
-        command_total       : g.command_total,
-        command_number      : num,
-        waypoint_id         : wp->id,
-        waypoint_options    : wp->options,
-        waypoint_param1     : wp->p1,
-        waypoint_altitude   : wp->alt,
-        waypoint_latitude   : wp->lat,
-        waypoint_longitude  : wp->lng
+        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));
 }
@@ -298,15 +298,15 @@ static void Log_Write_Startup(uint8_t type)
     struct log_Startup pkt = {
         LOG_PACKET_HEADER_INIT(LOG_STARTUP_MSG),
         startup_type    : type,
-        command_total   : g.command_total
+        command_total   : mission.num_commands()
     };
     DataFlash.WriteBlock(&pkt, sizeof(pkt));
 
     // write all commands to the dataflash as well
-    struct Location cmd;
-    for (uint8_t i = 0; i <= g.command_total; i++) {
-        cmd = get_cmd_with_index(i);
-        Log_Write_Cmd(i, &cmd);
+    AP_Mission::Mission_Command cmd;
+    for (uint8_t i = 0; i < mission.num_commands(); i++) {
+        mission.read_cmd_from_storage(i,cmd);
+        Log_Write_Cmd(cmd);
     }
 }