diff --git a/ArduPlane/Log.pde b/ArduPlane/Log.pde index 796bb36f02..a258b83504 100644 --- a/ArduPlane/Log.pde +++ b/ArduPlane/Log.pde @@ -227,35 +227,6 @@ static void Log_Write_Performance() DataFlash.WriteBlock(&pkt, sizeof(pkt)); } -struct PACKED log_Cmd { - LOG_PACKET_HEADER; - 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), - 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 gps_time; @@ -304,9 +275,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); } } } @@ -564,8 +535,6 @@ static const struct LogStructure log_structure[] PROGMEM = { "ATT", "IccCCC", "TimeMS,Roll,Pitch,Yaw,ErrorRP,ErrorYaw" }, { LOG_PERFORMANCE_MSG, sizeof(log_Performance), "PM", "IHIhhhBH", "LTime,MLC,gDt,GDx,GDy,GDz,I2CErr,INSErr" }, - { LOG_CMD_MSG, sizeof(log_Cmd), - "CMD", "HHBBBeLL", "CTot,CNum,CId,COpt,Prm1,Alt,Lat,Lng" }, { LOG_CAMERA_MSG, sizeof(log_Camera), "CAM", "IHLLeccC", "GPSTime,GPSWeek,Lat,Lng,Alt,Roll,Pitch,Yaw" }, { LOG_STARTUP_MSG, sizeof(log_Startup), @@ -625,7 +594,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_TECS_Tuning() {} diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index 10e0de23dc..8067af67a5 100644 --- a/ArduPlane/commands_logic.pde +++ b/ArduPlane/commands_logic.pde @@ -22,7 +22,7 @@ start_command(const AP_Mission::Mission_Command& cmd) { // log when new commands start if (g.log_bitmask & MASK_LOG_CMD) { - Log_Write_Cmd(cmd); + DataFlash.Log_Write_Cmd(mission.num_commands(),cmd); } // special handling for nav vs non-nav commands diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 7c45a708fd..735ace5c89 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -114,7 +114,7 @@ enum log_messages { LOG_CTUN_MSG, LOG_NTUN_MSG, LOG_PERFORMANCE_MSG, - LOG_CMD_MSG, + LOG_CMD_MSG_DEPRECATED, // deprecated LOG_CURRENT_MSG, LOG_STARTUP_MSG, TYPE_AIRSTART_MSG,