aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Rover: Mission integration into dataflash Logging

Browse Source
This commit is contained in:
Randy Mackay 2014-03-10 17:45:49 +09:00
parent 4ca3a92655
commit c83733048f
1 changed files with 19 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
APMrover2/Log.pde
View File

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