diff options
author | Julian Oes <julian@oes.ch> | 2013-11-27 16:17:44 +0100 |
---|---|---|
committer | Julian Oes <julian@oes.ch> | 2013-11-27 16:17:44 +0100 |
commit | 81023fe5aafc33477a9e16d044d2b5a1420ade76 (patch) | |
tree | a67c2449695dca3bfc0ccc73f26fd5ab531b7d8e /src/modules/navigator/navigator_main.cpp | |
parent | 3f252987988738d9246eec9716b780d23cb8b0f7 (diff) | |
download | px4-firmware-81023fe5aafc33477a9e16d044d2b5a1420ade76.tar.gz px4-firmware-81023fe5aafc33477a9e16d044d2b5a1420ade76.tar.bz2 px4-firmware-81023fe5aafc33477a9e16d044d2b5a1420ade76.zip |
Navigator and mavlink: more mavlink cleanup and set current waypoint option support added
Diffstat (limited to 'src/modules/navigator/navigator_main.cpp')
-rw-r--r-- | src/modules/navigator/navigator_main.cpp | 7 |
1 files changed, 5 insertions, 2 deletions
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 353629962..b88fc804c 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -358,7 +358,7 @@ Navigator::mission_update() * if the first part changed: start again at first waypoint * if the first part remained unchanged: continue with the (possibly changed second part) */ - if (_current_mission_index < _mission_item_count && _current_mission_index < mission.count) { //check if not finished and if the new mission is not a shorter mission + if (mission.current_index == -1 && _current_mission_index < _mission_item_count && _current_mission_index < mission.count) { //check if not finished and if the new mission is not a shorter mission for (unsigned i = 0; i < _current_mission_index; i++) { if (!cmp_mission_item_equivalent(_mission_item[i], mission.items[i])) { /* set flag to restart mission next we're in auto */ @@ -371,8 +371,11 @@ Navigator::mission_update() // warnx("Mission item is equivalent i=%d", i); // } } - } else { + } else if (mission.current_index >= 0 && mission.current_index < mission.count) { /* set flag to restart mission next we're in auto */ + _current_mission_index = mission.current_index; + mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index); + } else { _current_mission_index = 0; mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index); } |