From 809fec8c0579835d1fa9f1315f7ebc4398800502 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 17 Apr 2017 11:03:34 -0400 Subject: [PATCH] navigator treat TAKEOFF like POSITION if already flying --- src/modules/navigator/mission_block.cpp | 18 +++++++++++++++--- 1 file changed, 15 insertions(+), 3 deletions(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 01249e4311..de3ef25447 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -542,10 +542,22 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite break; case NAV_CMD_TAKEOFF: - // set pitch and ensure that the hold time is zero - sp->pitch_min = item->pitch_min; - // fall through + // if already flying (armed and !landed) treat TAKEOFF like regular POSITION + if ((_navigator->get_vstatus()->arming_state == vehicle_status_s::ARMING_STATE_ARMED) + && !_navigator->get_land_detected()->landed) { + + sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION; + + } else { + sp->type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF; + + // set pitch and ensure that the hold time is zero + sp->pitch_min = item->pitch_min; + } + + break; + case NAV_CMD_VTOL_TAKEOFF: sp->type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;