aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions 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

88
libraries/AP_Mission/AP_Mission.cpp
View File

@ -505,58 +505,58 @@ bool AP_Mission::read_cmd_from_storage(uint16_t index, Mission_Command& cmd) con
{
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
if (index == 0) {
cmd.index = 0;
cmd.id = MAV_CMD_NAV_WAYPOINT;
cmd.p1 = 0;
cmd.content.location = AP::ahrs().get_home();
}else{
// 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
// read WP position
uint16_t pos_in_storage = 4 + (index * AP_MISSION_EEPROM_COMMAND_SIZE);
PackedContent packed_content {};
uint8_t b1 = _storage.read_byte(pos_in_storage);
if (b1 == 0) {
cmd.id = _storage.read_uint16(pos_in_storage+1);
cmd.p1 = _storage.read_uint16(pos_in_storage+3);
_storage.read_block(packed_content.bytes, pos_in_storage+5, 10);
} else {
cmd.id = b1;
cmd.p1 = _storage.read_uint16(pos_in_storage+1);
_storage.read_block(packed_content.bytes, pos_in_storage+3, 12);
}
if (stored_in_location(cmd.id)) {
// Location is not PACKED; field-wise copy it:
cmd.content.location.relative_alt = packed_content.location.flags.relative_alt;
cmd.content.location.loiter_ccw = packed_content.location.flags.loiter_ccw;
cmd.content.location.terrain_alt = packed_content.location.flags.terrain_alt;
cmd.content.location.origin_alt = packed_content.location.flags.origin_alt;
cmd.content.location.loiter_xtrack = packed_content.location.flags.loiter_xtrack;
cmd.content.location.alt = packed_content.location.alt;
cmd.content.location.lat = packed_content.location.lat;
cmd.content.location.lng = packed_content.location.lng;
} else {
// all other options in Content are assumed to be packed:
static_assert(sizeof(cmd.content) >= 12,
"content is big enough to take bytes");
// (void *) cast to specify gcc that we know that we are copy byte into a non trivial type and leaving 4 bytes untouched
memcpy((void *)&cmd.content, packed_content.bytes, 12);
}
// set command's index to it's position in eeprom
cmd.index = index;
return true;
}
if (index >= (unsigned)_cmd_total) {
return false;
}
// 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
// read WP position
const uint16_t pos_in_storage = 4 + (index * AP_MISSION_EEPROM_COMMAND_SIZE);
PackedContent packed_content {};
const uint8_t b1 = _storage.read_byte(pos_in_storage);
if (b1 == 0) {
cmd.id = _storage.read_uint16(pos_in_storage+1);
cmd.p1 = _storage.read_uint16(pos_in_storage+3);
_storage.read_block(packed_content.bytes, pos_in_storage+5, 10);
} else {
cmd.id = b1;
cmd.p1 = _storage.read_uint16(pos_in_storage+1);
_storage.read_block(packed_content.bytes, pos_in_storage+3, 12);
}
if (stored_in_location(cmd.id)) {
// Location is not PACKED; field-wise copy it:
cmd.content.location.relative_alt = packed_content.location.flags.relative_alt;
cmd.content.location.loiter_ccw = packed_content.location.flags.loiter_ccw;
cmd.content.location.terrain_alt = packed_content.location.flags.terrain_alt;
cmd.content.location.origin_alt = packed_content.location.flags.origin_alt;
cmd.content.location.loiter_xtrack = packed_content.location.flags.loiter_xtrack;
cmd.content.location.alt = packed_content.location.alt;
cmd.content.location.lat = packed_content.location.lat;
cmd.content.location.lng = packed_content.location.lng;
} else {
// all other options in Content are assumed to be packed:
static_assert(sizeof(cmd.content) >= 12,
"content is big enough to take bytes");
// (void *) cast to specify gcc that we know that we are copy byte into a non trivial type and leaving 4 bytes untouched
memcpy((void *)&cmd.content, packed_content.bytes, 12);
}
// set command's index to it's position in eeprom
cmd.index = index;
// return success
return true;
}