diff options
author | Julian Oes <julian@oes.ch> | 2014-06-11 23:16:22 +0200 |
---|---|---|
committer | Julian Oes <julian@oes.ch> | 2014-06-11 23:16:22 +0200 |
commit | d9a64bb58720300417f190b8a8b610ab2966a11f (patch) | |
tree | 4a85ee17b216d4980be24c28e91f3aabd1850f38 /src/modules | |
parent | cfcba921a1a06b6f102ad227c3f6da3f977891c0 (diff) | |
download | px4-firmware-d9a64bb58720300417f190b8a8b610ab2966a11f.tar.gz px4-firmware-d9a64bb58720300417f190b8a8b610ab2966a11f.tar.bz2 px4-firmware-d9a64bb58720300417f190b8a8b610ab2966a11f.zip |
navigator: don't give up after DO_JUMPS
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/navigator/mission.cpp | 43 |
1 files changed, 23 insertions, 20 deletions
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 33a1399b1..a551df9a2 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -391,29 +391,32 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio /* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */ if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) { - if (new_mission_item->do_jump_current_count >= new_mission_item->do_jump_repeat_count) { + /* do DO_JUMP as many times as requested */ + if (new_mission_item->do_jump_current_count < new_mission_item->do_jump_repeat_count) { + + /* only raise the repeat count if this is for the current mission item + * but not for the next mission item */ + if (is_current) { + (new_mission_item->do_jump_current_count)++; + /* save repeat count */ + if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET, + new_mission_item, len) != len) { + /* not supposed to happen unless the datamanager can't access the + * dataman */ + mavlink_log_critical(_navigator->get_mavlink_fd(), + "#audio: ERROR DO JUMP waypoint could not be written"); + return false; + } + } + /* set new mission item index and repeat + * we don't have to validate here, if it's invalid, we should realize this later .*/ + *mission_index = new_mission_item->do_jump_mission_index; + } else { mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: DO JUMP repetitions completed"); - return false; - } - - /* only raise the repeat count if this is for the current mission item - * but not for the next mission item */ - if (is_current) { - (new_mission_item->do_jump_current_count)++; - - /* save repeat count */ - if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET, - new_mission_item, len) != len) { - /* not supposed to happen unless the datamanager can't access the dataman */ - mavlink_log_critical(_navigator->get_mavlink_fd(), - "#audio: ERROR DO JUMP waypoint could not be written"); - return false; - } + /* no more DO_JUMPS, therefore just try to continue with next mission item */ + (*mission_index)++; } - /* set new mission item index and repeat - * we don't have to validate here, if it's invalid, we should realize this later .*/ - *mission_index = new_mission_item->do_jump_mission_index; } else { /* if it's not a DO_JUMP, then we were successful */ |