diff options
author | Julian Oes <julian@oes.ch> | 2014-11-12 12:11:23 +1000 |
---|---|---|
committer | Julian Oes <julian@oes.ch> | 2014-11-12 12:11:23 +1000 |
commit | 703fcec62d3430b75876d2c06183bbf4451f0dd6 (patch) | |
tree | 771913ce8d22dd1f20be15636fab595773b85c29 /src/modules/navigator/mission.cpp | |
parent | f1058bdf0816cda718cee4d5a69c4ac3d53ea781 (diff) | |
download | px4-firmware-703fcec62d3430b75876d2c06183bbf4451f0dd6.tar.gz px4-firmware-703fcec62d3430b75876d2c06183bbf4451f0dd6.tar.bz2 px4-firmware-703fcec62d3430b75876d2c06183bbf4451f0dd6.zip |
navigator: fix do jumps: after the repetitions are completed the current mission index is no longer invalid
Diffstat (limited to 'src/modules/navigator/mission.cpp')
-rw-r--r-- | src/modules/navigator/mission.cpp | 21 |
1 files changed, 12 insertions, 9 deletions
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 7fac69a61..840c9ec0e 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -371,7 +371,7 @@ Mission::set_mission_items() } else { /* no mission available or mission finished, switch to loiter */ if (_mission_type != MISSION_TYPE_NONE) { - mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished"); /* use last setpoint for loiter */ _navigator->set_can_loiter_at_sp(true); @@ -595,13 +595,14 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); } - if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) { - /* mission item index out of bounds */ - return false; - } - /* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */ for (int i = 0; i < 10; i++) { + + if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) { + /* mission item index out of bounds */ + return false; + } + const ssize_t len = sizeof(struct mission_item_s); /* read mission item to temp storage first to not overwrite current mission item if data damaged */ @@ -630,7 +631,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s /* not supposed to happen unless the datamanager can't access the * dataman */ mavlink_log_critical(_navigator->get_mavlink_fd(), - "ERROR DO JUMP waypoint could not be written"); + "ERROR DO JUMP waypoint could not be written"); return false; } } @@ -639,8 +640,10 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_index_ptr = mission_item_tmp.do_jump_mission_index; } else { - mavlink_log_critical(_navigator->get_mavlink_fd(), - "DO JUMP repetitions completed"); + if (is_current) { + mavlink_log_critical(_navigator->get_mavlink_fd(), + "DO JUMP repetitions completed"); + } /* no more DO_JUMPS, therefore just try to continue with next mission item */ (*mission_index_ptr)++; } |