Browse Source

Rover: if receiving last mission item then log new mission to dataflash

master
Tom Pittenger 10 years ago committed by Andrew Tridgell
parent
commit
06796cd994
  1. 4
      APMrover2/GCS_Mavlink.pde
  2. 10
      APMrover2/Log.pde

4
APMrover2/GCS_Mavlink.pde

@ -1068,7 +1068,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) @@ -1068,7 +1068,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// XXX receive a WP from GCS and store in EEPROM
case MAVLINK_MSG_ID_MISSION_ITEM:
{
handle_mission_item(msg, mission);
if (handle_mission_item(msg, mission)) {
Log_Write_EntireMission();
}
break;
}

10
APMrover2/Log.pde

@ -244,9 +244,16 @@ static void Log_Write_Startup(uint8_t type) @@ -244,9 +244,16 @@ static void Log_Write_Startup(uint8_t type)
DataFlash.WriteBlock(&pkt, sizeof(pkt));
// write all commands to the dataflash as well
Log_Write_EntireMission();
}
static void Log_Write_EntireMission()
{
gcs_send_text_P(SEVERITY_LOW, PSTR("New mission"));
AP_Mission::Mission_Command cmd;
for (uint16_t i = 0; i < mission.num_commands(); i++) {
if(mission.read_cmd_from_storage(i,cmd)) {
if (mission.read_cmd_from_storage(i,cmd)) {
Log_Write_Cmd(cmd);
}
}
@ -430,6 +437,7 @@ static void start_logging() @@ -430,6 +437,7 @@ static void start_logging()
// dummy functions
static void Log_Write_Startup(uint8_t type) {}
static void Log_Write_EntireMission() {}
static void Log_Write_Current() {}
static void Log_Write_Nav_Tuning() {}
static void Log_Write_Performance() {}

Loading…
Cancel
Save