fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 21:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Navigator and mavlink: more mavlink cleanup and set current waypoint option support added

Browse Source
This commit is contained in:
Julian Oes
2013-11-27 16:17:44 +01:00
parent 3f25298798
commit 81023fe5aa
4 changed files with 138 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/navigator_main.cpp
+5 -2
View File
@@ -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);
}