aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Mission: special handling for cmd

Browse Source
This commit is contained in:
Randy Mackay 2014-02-28 11:58:27 +09:00
parent 37fe229ded
commit 87c7d19f83
2 changed files with 32 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
libraries/AP_Mission/AP_Mission.cpp
View File

@ -212,34 +212,41 @@ bool AP_Mission::read_cmd_from_storage(uint8_t index, Mission_Command& cmd) cons
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
pos_in_storage = (AP_MISSION_EEPROM_START_BYTE) + (index * AP_MISSION_EEPROM_COMMAND_SIZE);
// special handling for command #0 which is home
if (index == 0) {
cmd.index = 0;
cmd.id = MAV_CMD_NAV_WAYPOINT;
cmd.content.location = _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
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.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.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.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.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.lat = hal.storage->read_dword(pos_in_storage);
pos_in_storage += 4;
cmd.content.location.lng = hal.storage->read_dword(pos_in_storage);
cmd.content.location.lng = hal.storage->read_dword(pos_in_storage);
// set command's index to it's position in eeprom
cmd.index = index;
// set command's index to it's position in eeprom
cmd.index = index;
// set command from location's command
// To-Do: remove id (and p1?) from Location structure
cmd.id = cmd.content.location.id;
// set command from location's command
// To-Do: remove id (and p1?) from Location structure
cmd.id = cmd.content.location.id;
}
// return success
return true;

6
libraries/AP_Mission/AP_Mission.h
View File

@ -79,7 +79,8 @@ public:
};
/// constructor
AP_Mission(mission_cmd_fn_t cmd_start_fn, mission_cmd_fn_t cmd_verify_fn, mission_complete_fn_t mission_complete_fn) :
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),
_cmd_start_fn(cmd_start_fn),
_cmd_verify_fn(cmd_verify_fn),
_mission_complete_fn(mission_complete_fn),
@ -220,6 +221,9 @@ private:
/// increment_jump_times_run - increments the recorded number of times the jump command has been run
void increment_jump_times_run(Mission_Command& cmd);
// references to external libraries
const AP_AHRS& _ahrs; // used only for home position
// parameters
AP_Int16 _cmd_total; // total number of commands in the mission