From 1424994bc0b219fc2acef7e9bbb7d884886c04ae Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 8 Dec 2016 11:19:08 +0100 Subject: [PATCH] navigator: don't takeoff in loiter on ground This fixes the following corner case: 1. Upload a mission. 2. Set mission mode. 3. Set loiter mode. 4. Arm. At this point it will shoot up and go to the takeoff waypoint even though we're not in mission but in loiter mode. The fix makes sure that the triplet is reset to invalid (and idle) in loiter mode if we're landed and disarmed. It will lead to the vehcle sit in idle on the ground until you issue a start mission (or takeoff) command. --- src/modules/navigator/loiter.cpp | 20 +++++++++++++++----- 1 file changed, 15 insertions(+), 5 deletions(-) diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index d773b9bb74..d8ff78afa2 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -102,15 +102,25 @@ Loiter::on_active() void Loiter::set_loiter_position() { - // not setting loiter position until armed - if (_navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED || - _loiter_pos_set) { + if (_navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED && + _navigator->get_land_detected()->landed) { + + // Not setting loiter position if disarmed and landed, instead mark the current + // setpoint as invalid and idle (both, just to be sure). + + _navigator->set_can_loiter_at_sp(false); + _navigator->get_position_setpoint_triplet()->current.type = position_setpoint_s::SETPOINT_TYPE_IDLE; + _navigator->set_position_setpoint_triplet_updated(); + _loiter_pos_set = false; return; - } else { - _loiter_pos_set = true; + } else if (_loiter_pos_set) { + // Already set, nothing to do. + return; } + _loiter_pos_set = true; + // set current mission item to loiter set_loiter_item(&_mission_item, _param_min_alt.get());