diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-09-30 09:08:31 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-09-30 09:08:31 +0400 |
commit | 70e5d4027a3b1465d5128dbf9a04cbb6545e043d (patch) | |
tree | 84ac7831e7bfa12ff771a7ead847fc5a51a5334d | |
parent | 1cc5f3778dd6c18f3a14c64959d28f978b0305ef (diff) | |
download | px4-firmware-70e5d4027a3b1465d5128dbf9a04cbb6545e043d.tar.gz px4-firmware-70e5d4027a3b1465d5128dbf9a04cbb6545e043d.tar.bz2 px4-firmware-70e5d4027a3b1465d5128dbf9a04cbb6545e043d.zip |
navigator: autocontinue fix
-rw-r--r-- | src/modules/navigator/mission.cpp | 26 |
1 files changed, 17 insertions, 9 deletions
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 104065132..8bacaa425 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -150,8 +150,19 @@ Mission::on_active() /* lets check if we reached the current mission item */ if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) { - advance_mission(); - set_mission_items(); + if (_mission_item.autocontinue) { + /* switch to next waypoint if 'autocontinue' flag set */ + advance_mission(); + set_mission_items(); + + } else { + /* else just report that item reached */ + if (_mission_type == MISSION_TYPE_OFFBOARD) { + if (!(_mission_result.seq_reached == _current_offboard_mission_index && _mission_result.reached)) { + report_mission_item_reached(); + } + } + } } else if (_mission_type != MISSION_TYPE_NONE &&_param_altmode.get() == MISSION_ALTMODE_FOH) { altitude_sp_foh_update(); @@ -694,10 +705,8 @@ Mission::save_offboard_mission_state() void Mission::report_mission_item_reached() { - if (_mission_type == MISSION_TYPE_OFFBOARD) { - _mission_result.reached = true; - _mission_result.seq_reached = _current_offboard_mission_index; - } + _mission_result.reached = true; + _mission_result.seq_reached = _current_offboard_mission_index; publish_mission_result(); } @@ -705,6 +714,8 @@ void Mission::report_current_offboard_mission_item() { warnx("current offboard mission index: %d", _current_offboard_mission_index); + _mission_result.reached = false; + _mission_result.finished = false; _mission_result.seq_current = _current_offboard_mission_index; publish_mission_result(); @@ -730,7 +741,4 @@ Mission::publish_mission_result() /* advertise and publish */ _mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result); } - /* reset reached bool */ - _mission_result.reached = false; - _mission_result.finished = false; } |