Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 07:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Mission: Refactor read_cmd_from_storage for clarity

Browse Source
This commit is contained in:
Michael du Breuil 2019-07-30 00:34:18 -07:00 committed by Peter Barker
parent be20f458bf
commit c19e4f2147
1 changed files with 44 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_Mission/AP_Mission.cpp
View File

@ -505,26 +505,27 @@ bool AP_Mission::read_cmd_from_storage(uint16_t index, Mission_Command& cmd) con
{ {
WITH_SEMAPHORE(_rsem); WITH_SEMAPHORE(_rsem);
// exit immediately if index is beyond last command but we always let cmd #0 (i.e. home) be read
if (index >= (unsigned)_cmd_total && index != 0) {
return false;
}
// special handling for command #0 which is home // special handling for command #0 which is home
if (index == 0) { if (index == 0) {
cmd.index = 0; cmd.index = 0;
cmd.id = MAV_CMD_NAV_WAYPOINT; cmd.id = MAV_CMD_NAV_WAYPOINT;
cmd.p1 = 0; cmd.p1 = 0;
cmd.content.location = AP::ahrs().get_home(); cmd.content.location = AP::ahrs().get_home();
}else{ return true;
}
if (index >= (unsigned)_cmd_total) {
return false;
}
// Find out proper location in memory by using the start_byte position + the index // Find out proper location in memory by using the start_byte position + the index
// we can load a command, we don't process it yet // we can load a command, we don't process it yet
// read WP position // read WP position
uint16_t pos_in_storage = 4 + (index * AP_MISSION_EEPROM_COMMAND_SIZE); const uint16_t pos_in_storage = 4 + (index * AP_MISSION_EEPROM_COMMAND_SIZE);
PackedContent packed_content {}; PackedContent packed_content {};
uint8_t b1 = _storage.read_byte(pos_in_storage); const uint8_t b1 = _storage.read_byte(pos_in_storage);
if (b1 == 0) { if (b1 == 0) {
cmd.id = _storage.read_uint16(pos_in_storage+1); cmd.id = _storage.read_uint16(pos_in_storage+1);
cmd.p1 = _storage.read_uint16(pos_in_storage+3); cmd.p1 = _storage.read_uint16(pos_in_storage+3);
@ -555,7 +556,6 @@ bool AP_Mission::read_cmd_from_storage(uint16_t index, Mission_Command& cmd) con
// set command's index to it's position in eeprom // set command's index to it's position in eeprom
cmd.index = index; cmd.index = index;
}
// return success // return success
return true; return true;