Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Mission: convert to using StorageManager

This commit is contained in:
Andrew Tridgell 2014-08-13 14:43:37 +10:00
parent 7cbb326405
commit 7a12ff0271
2 changed files with 28 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
libraries/AP_Mission/AP_Mission.cpp
View File

@ -27,6 +27,9 @@ const AP_Param::GroupInfo AP_Mission::var_info[] PROGMEM = {
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
// storage object
StorageAccess AP_Mission::_storage(StorageManager::StorageMission);
/// ///
/// public mission methods /// public mission methods
/// ///
@ -377,8 +380,6 @@ bool AP_Mission::set_current_cmd(uint16_t index)
/// true is return if successful /// true is return if successful
bool AP_Mission::read_cmd_from_storage(uint16_t index, Mission_Command& cmd) const bool AP_Mission::read_cmd_from_storage(uint16_t index, Mission_Command& cmd) const
{ {
uint16_t pos_in_storage; // position in storage from where we will read the next byte
// exit immediately if index is beyond last command but we always let cmd #0 (i.e. home) be read // 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) { if (index > (unsigned)_cmd_total && index != 0) {
return false; return false;
@ -394,11 +395,11 @@ bool AP_Mission::read_cmd_from_storage(uint16_t index, Mission_Command& cmd) con
// 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
pos_in_storage = _storage_start + 4 + (index * AP_MISSION_EEPROM_COMMAND_SIZE); uint16_t pos_in_storage = 4 + (index * AP_MISSION_EEPROM_COMMAND_SIZE);
cmd.id = hal.storage->read_byte(pos_in_storage); cmd.id = _storage.read_byte(pos_in_storage);
cmd.p1 = hal.storage->read_word(pos_in_storage+1); cmd.p1 = _storage.read_uint16(pos_in_storage+1);
hal.storage->read_block(cmd.content.bytes, pos_in_storage+3, 12); _storage.read_block(cmd.content.bytes, pos_in_storage+3, 12);
// 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;
@ -414,16 +415,16 @@ bool AP_Mission::read_cmd_from_storage(uint16_t index, Mission_Command& cmd) con
bool AP_Mission::write_cmd_to_storage(uint16_t index, Mission_Command& cmd) bool AP_Mission::write_cmd_to_storage(uint16_t index, Mission_Command& cmd)
{ {
// range check cmd's index // range check cmd's index
if (index >= _cmd_total_max) { if (index >= num_commands_max()) {
return false; return false;
} }
// calculate where in storage the command should be placed // calculate where in storage the command should be placed
uint16_t pos_in_storage = _storage_start + 4 + (index * AP_MISSION_EEPROM_COMMAND_SIZE); uint16_t pos_in_storage = 4 + (index * AP_MISSION_EEPROM_COMMAND_SIZE);
hal.storage->write_byte(pos_in_storage, cmd.id); _storage.write_byte(pos_in_storage, cmd.id);
hal.storage->write_word(pos_in_storage+1, cmd.p1); _storage.write_uint16(pos_in_storage+1, cmd.p1);
hal.storage->write_block(pos_in_storage+3, cmd.content.bytes, 12); _storage.write_block(pos_in_storage+3, cmd.content.bytes, 12);
// remember when the mission last changed // remember when the mission last changed
_last_change_time_ms = hal.scheduler->millis(); _last_change_time_ms = hal.scheduler->millis();
@ -1193,12 +1194,22 @@ void AP_Mission::increment_jump_times_run(Mission_Command& cmd)
// command list will be cleared if they do not match // command list will be cleared if they do not match
void AP_Mission::check_eeprom_version() void AP_Mission::check_eeprom_version()
{ {
uint32_t eeprom_version = hal.storage->read_dword(_storage_start); uint32_t eeprom_version = _storage.read_uint32(0);
// if eeprom version does not match, clear the command list and update the eeprom version // if eeprom version does not match, clear the command list and update the eeprom version
if (eeprom_version != AP_MISSION_EEPROM_VERSION) { if (eeprom_version != AP_MISSION_EEPROM_VERSION) {
if (clear()) { if (clear()) {
hal.storage->write_dword(_storage_start, AP_MISSION_EEPROM_VERSION); _storage.write_uint32(0, AP_MISSION_EEPROM_VERSION);
} }
} }
} }
/*
return total number of commands that can fit in storage space
*/
uint16_t AP_Mission::num_commands_max(void) const
{
// -4 to remove space for eeprom version number
return (_storage.size() - 4) / AP_MISSION_EEPROM_COMMAND_SIZE;
}

12
libraries/AP_Mission/AP_Mission.h
View File

@ -21,6 +21,7 @@
#include <AP_Param.h> #include <AP_Param.h>
#include <AP_AHRS.h> #include <AP_AHRS.h>
#include <AP_HAL.h> #include <AP_HAL.h>
#include <../StorageManager/StorageManager.h>
// definitions // definitions
#define AP_MISSION_EEPROM_VERSION 0x65AE // version number stored in first four bytes of eeprom. increment this by one when eeprom format is changed #define AP_MISSION_EEPROM_VERSION 0x65AE // version number stored in first four bytes of eeprom. increment this by one when eeprom format is changed
@ -184,7 +185,7 @@ public:
}; };
/// constructor /// constructor
AP_Mission(AP_AHRS &ahrs, mission_cmd_fn_t cmd_start_fn, mission_cmd_fn_t cmd_verify_fn, mission_complete_fn_t mission_complete_fn, uint16_t storage_start, uint16_t storage_end) : AP_Mission(AP_AHRS &ahrs, mission_cmd_fn_t cmd_start_fn, mission_cmd_fn_t cmd_verify_fn, mission_complete_fn_t mission_complete_fn) :
_ahrs(ahrs), _ahrs(ahrs),
_cmd_start_fn(cmd_start_fn), _cmd_start_fn(cmd_start_fn),
_cmd_verify_fn(cmd_verify_fn), _cmd_verify_fn(cmd_verify_fn),
@ -195,10 +196,6 @@ public:
// load parameter defaults // load parameter defaults
AP_Param::setup_object_defaults(this, var_info); AP_Param::setup_object_defaults(this, var_info);
// calculate
_storage_start = storage_start;
_cmd_total_max = ((storage_end - storage_start - 4) / AP_MISSION_EEPROM_COMMAND_SIZE) -1; // -4 to remove space for eeprom version number, -1 to be safe
// clear commands // clear commands
_nav_cmd.index = AP_MISSION_CMD_INDEX_NONE; _nav_cmd.index = AP_MISSION_CMD_INDEX_NONE;
_do_cmd.index = AP_MISSION_CMD_INDEX_NONE; _do_cmd.index = AP_MISSION_CMD_INDEX_NONE;
@ -223,7 +220,7 @@ public:
uint16_t num_commands() const { return _cmd_total; } uint16_t num_commands() const { return _cmd_total; }
/// num_commands_max - returns maximum number of commands that can be stored /// num_commands_max - returns maximum number of commands that can be stored
uint16_t num_commands_max() const {return _cmd_total_max; } uint16_t num_commands_max() const;
/// start - resets current commands to point to the beginning of the mission /// start - resets current commands to point to the beginning of the mission
/// To-Do: should we validate the mission first and return true/false? /// To-Do: should we validate the mission first and return true/false?
@ -328,6 +325,7 @@ public:
static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info[];
private: private:
static StorageAccess _storage;
struct Mission_Flags { struct Mission_Flags {
mission_state state; mission_state state;
@ -396,8 +394,6 @@ private:
mission_complete_fn_t _mission_complete_fn; // pointer to function which will be called when mission completes mission_complete_fn_t _mission_complete_fn; // pointer to function which will be called when mission completes
// internal variables // internal variables
uint16_t _storage_start; // first position we are free to use in eeprom storage
uint16_t _cmd_total_max; // maximum number of commands we can store
struct Mission_Command _nav_cmd; // current "navigation" command. It's position in the command list is held in _nav_cmd.index struct Mission_Command _nav_cmd; // current "navigation" command. It's position in the command list is held in _nav_cmd.index
struct Mission_Command _do_cmd; // current "do" command. It's position in the command list is held in _do_cmd.index struct Mission_Command _do_cmd; // current "do" command. It's position in the command list is held in _do_cmd.index
uint16_t _prev_nav_cmd_index; // index of the previous "navigation" command. Rarely used which is why we don't store the whole command uint16_t _prev_nav_cmd_index; // index of the previous "navigation" command. Rarely used which is why we don't store the whole command