From fc08c538da70ffae225399ec73bd64357eae5b1e Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Mon, 7 Mar 2022 07:44:22 +0300 Subject: [PATCH] mission: on activation in fw mode make sure mission index > transition point index Signed-off-by: RomanBapst --- src/modules/navigator/mission.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 62e16571d7..fe73ab4b38 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -208,6 +208,15 @@ Mission::on_activation() cmd.param1 = -1.0f; cmd.param3 = 0.0f; _navigator->publish_vehicle_cmd(&cmd); + + if (_navigator->get_vstatus()->is_vtol + && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + // if we are already in fixed wing flight then make sure that the current active waypoint + // is after the transition to forward flight waypoint + if (_first_fixed_wing_waypoint_index > 0 && _current_mission_index < _first_fixed_wing_waypoint_index) { + set_current_mission_index(_first_fixed_wing_waypoint_index); + } + } } void @@ -585,8 +594,8 @@ Mission::update_mission() // if we are already in fixed wing flight then make sure that the current active waypoint // is after the transition to forward flight waypoint - if (_current_mission_index < _first_fixed_wing_waypoint_index) { - _current_mission_index = _first_fixed_wing_waypoint_index; + if (_first_fixed_wing_waypoint_index > 0 && _current_mission_index < _first_fixed_wing_waypoint_index) { + set_current_mission_index(_first_fixed_wing_waypoint_index); } }