From 9e383f2cd31fd24ad59673a844dded485ef4eb0f Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Wed, 3 May 2017 17:13:33 +0200 Subject: [PATCH] navigator: when landed only exit idle if mission/takoff --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 1 + src/modules/navigator/navigator_main.cpp | 13 +++++++++++++ 2 files changed, 14 insertions(+) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index edce8af294..c5a7b4542b 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1605,6 +1605,7 @@ void MulticopterPositionControl::control_auto(float dt) } else { /* no waypoint, do nothing, setpoint was already reset */ + /* we are in idle */ } } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index bca60bc574..372ca92a3e 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -200,6 +200,7 @@ void Navigator::vehicle_land_detected_update() { orb_copy(ORB_ID(vehicle_land_detected), _land_detected_sub, &_land_detected); + } void @@ -633,6 +634,18 @@ Navigator::task_main() _navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]); } + /* if we landed and have not received takoff setpoint then stay in idle */ + if (_land_detected.landed && + !((_vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF) + || (_vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION))) { + + _pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_IDLE; + _pos_sp_triplet.current.valid = true; + _pos_sp_triplet.previous.valid = false; + _pos_sp_triplet.next.valid = false; + + } + /* if nothing is running, set position setpoint triplet invalid once */ if (_navigation_mode == nullptr && !_pos_sp_triplet_published_invalid_once) { _pos_sp_triplet_published_invalid_once = true;