Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-04 06:58:39 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Mission: replace block read/write for eeprom

Browse Source 
Also reserve command #0 for home position
This commit is contained in:
Randy Mackay 2014-02-27 22:02:12 +09:00
parent fd009d0704
commit e8db57ced3
2 changed files with 40 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
libraries/AP_Mission/AP_Mission.cpp
View File

@ -217,12 +217,26 @@ bool AP_Mission::read_cmd_from_storage(uint8_t index, Mission_Command& cmd) cons
// read WP position
pos_in_storage = (AP_MISSION_EEPROM_START_BYTE) + (index * AP_MISSION_EEPROM_COMMAND_SIZE);
cmd.content.location.id = hal.storage->read_byte(pos_in_storage);
pos_in_storage++;
cmd.content.location.options = hal.storage->read_byte(pos_in_storage);
pos_in_storage++;
cmd.content.location.p1 = hal.storage->read_byte(pos_in_storage);
pos_in_storage++;
cmd.content.location.alt = hal.storage->read_dword(pos_in_storage);
pos_in_storage += 4;
cmd.content.location.lat = hal.storage->read_dword(pos_in_storage);
pos_in_storage += 4;
cmd.content.location.lng = hal.storage->read_dword(pos_in_storage);
// set command's index to it's position in eeprom
cmd.index = index;
// read command from eeprom
hal.storage->read_block(cmd.content.bytes, pos_in_storage, AP_MISSION_EEPROM_COMMAND_SIZE);
// set command from location's command
// To-Do: remove id (and p1?) from Location structure
cmd.id = cmd.content.location.id;
@ -244,12 +258,23 @@ bool AP_Mission::write_cmd_to_storage(uint8_t index, Mission_Command& cmd)
// calculate where in storage the command should be placed
uint16_t pos_in_storage = AP_MISSION_EEPROM_START_BYTE + (index * AP_MISSION_EEPROM_COMMAND_SIZE);
// set location id to cmd.id
// To-Do: remove id (and p1?) from Location structure
cmd.content.location.id = cmd.id;
hal.storage->write_byte(pos_in_storage, cmd.content.location.id);
// write block to eeprom
hal.storage->write_block(pos_in_storage, cmd.content.bytes, AP_MISSION_EEPROM_COMMAND_SIZE);
pos_in_storage++;
hal.storage->write_byte(pos_in_storage, cmd.content.location.options);
pos_in_storage++;
hal.storage->write_byte(pos_in_storage, cmd.content.location.p1);
pos_in_storage++;
hal.storage->write_dword(pos_in_storage, cmd.content.location.alt);
pos_in_storage += 4;
hal.storage->write_dword(pos_in_storage, cmd.content.location.lat);
pos_in_storage += 4;
hal.storage->write_dword(pos_in_storage, cmd.content.location.lng);
// return success
return true;
@ -295,7 +320,7 @@ bool AP_Mission::advance_current_nav_cmd()
// get starting point for search
cmd_index = _nav_cmd.index;
if (cmd_index == AP_MISSION_CMD_INDEX_NONE) {
cmd_index = 0;
cmd_index = AP_MISSION_FIRST_REAL_COMMAND;
}else{
// start from one position past the current nav command
cmd_index++;
@ -303,7 +328,6 @@ bool AP_Mission::advance_current_nav_cmd()
// search until we find next nav command or reach end of command list
while (!_flags.nav_cmd_loaded) {
// get next command
if (!get_next_cmd(cmd_index, cmd, true)) {
return false;
@ -348,7 +372,7 @@ void AP_Mission::advance_current_do_cmd()
// get starting point for search
cmd_index = _do_cmd.index;
if (cmd_index == AP_MISSION_CMD_INDEX_NONE) {
cmd_index = 0;
cmd_index = AP_MISSION_FIRST_REAL_COMMAND;
}else{
// start from one position past the current do command
cmd_index++;
@ -383,7 +407,6 @@ bool AP_Mission::get_next_cmd(uint8_t start_index, Mission_Command& cmd, bool in
// search until the end of the mission command list
while(cmd_index < _cmd_total) {
// load the next command
read_cmd_from_storage(cmd_index, temp_cmd);

8
libraries/AP_Mission/AP_Mission.h
View File

@ -37,6 +37,8 @@
#define AP_MISSION_CMD_INDEX_NONE 255 // command index of 255 means invalid or missing command
#define AP_MISSION_JUMP_TIMES_MAX 255 // maximum number of times a jump can be executed. Used when jump tracking fails (i.e. when too many jumps in mission)
#define AP_MISSION_FIRST_REAL_COMMAND 1 // command #0 reserved to hold home position
/// @class AP_Mission
/// @brief Object managing Mission
class AP_Mission {
@ -56,8 +58,6 @@ public:
// location
Location location; // Waypoint location
uint8_t bytes[AP_MISSION_EEPROM_COMMAND_SIZE];
};
// command structure
@ -154,6 +154,10 @@ public:
/// get_active_do_cmd - returns active "do" command
const Mission_Command& get_current_do_cmd() const { return _do_cmd; }
// jump_to_cmd - jumps to command specified by index
// To-Do: implement this function!
bool jump_to_cmd(uint8_t index) { return false; }
/// load_cmd_from_storage - load command from storage
/// true is return if successful
bool read_cmd_from_storage(uint8_t index, Mission_Command& cmd) const;