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

AP_Mission: add get_prev_nav_cmd_with_wp_index()

Browse Source 
This is different than get_prev_nav_cmd_index() in that it only stores the index if there is a valid lat/lng
This commit is contained in:
Tom Pittenger 2015-08-23 18:06:31 -07:00 committed by Andrew Tridgell
parent 722dd29370
commit 41508457e1
2 changed files with 17 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_Mission/AP_Mission.cpp
View File

@ -132,6 +132,7 @@ void AP_Mission::reset()
_nav_cmd.index = AP_MISSION_CMD_INDEX_NONE;
_do_cmd.index = AP_MISSION_CMD_INDEX_NONE;
_prev_nav_cmd_index = AP_MISSION_CMD_INDEX_NONE;
_prev_nav_cmd_wp_index = AP_MISSION_CMD_INDEX_NONE;
init_jump_tracking();
}
@ -312,6 +313,7 @@ bool AP_Mission::set_current_cmd(uint16_t index)
// if index is zero then the user wants to completely restart the mission
if (index == 0 || _flags.state == MISSION_COMPLETE) {
_prev_nav_cmd_index = AP_MISSION_CMD_INDEX_NONE;
_prev_nav_cmd_wp_index = AP_MISSION_CMD_INDEX_NONE;
// reset the jump tracking to zero
init_jump_tracking();
if (index == 0) {
@ -372,6 +374,10 @@ bool AP_Mission::set_current_cmd(uint16_t index)
if (is_nav_cmd(cmd)) {
// save previous nav command index
_prev_nav_cmd_index = _nav_cmd.index;
// save separate previous nav command index if it contains lat,long,alt
if (cmd.content.location.lat != 0 && cmd.content.location.lng != 0) {
_prev_nav_cmd_wp_index = _nav_cmd.index;
}
// set current navigation command and start it
_nav_cmd = cmd;
_flags.nav_cmd_loaded = true;
@ -1133,6 +1139,10 @@ bool AP_Mission::advance_current_nav_cmd()
if (is_nav_cmd(cmd)) {
// save previous nav command index
_prev_nav_cmd_index = _nav_cmd.index;
// save separate previous nav command index if it contains lat,long,alt
if (cmd.content.location.lat != 0 && cmd.content.location.lng != 0) {
_prev_nav_cmd_wp_index = _nav_cmd.index;
}
// set current navigation command and start it
_nav_cmd = cmd;
_flags.nav_cmd_loaded = true;

7
libraries/AP_Mission/AP_Mission.h
View File

@ -243,6 +243,7 @@ public:
_cmd_verify_fn(cmd_verify_fn),
_mission_complete_fn(mission_complete_fn),
_prev_nav_cmd_index(AP_MISSION_CMD_INDEX_NONE),
_prev_nav_cmd_wp_index(AP_MISSION_CMD_INDEX_NONE),
_last_change_time_ms(0)
{
// load parameter defaults
@ -333,6 +334,11 @@ public:
/// we do not return the entire command to save on RAM
uint16_t get_prev_nav_cmd_index() const { return _prev_nav_cmd_index; }
/// get_prev_nav_cmd_with_wp_index - returns the previous "navigation" commands index that contains a waypoint (i.e. position in the mission command list)
/// if there was no previous nav command it returns AP_MISSION_CMD_INDEX_NONE
/// we do not return the entire command to save on RAM
uint16_t get_prev_nav_cmd_with_wp_index() const { return _prev_nav_cmd_wp_index; }
/// get_next_nav_cmd - gets next "navigation" command found at or after start_index
/// returns true if found, false if not found (i.e. reached end of mission command list)
/// accounts for do_jump commands
@ -454,6 +460,7 @@ private:
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
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_wp_index; // index of the previous "navigation" command that contains a waypoint. Rarely used which is why we don't store the whole command
// jump related variables
struct jump_tracking_struct {