From dfb31f225216384df6bafaa242a8374f7d868656 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Thu, 28 Jan 2016 14:20:30 +0100 Subject: [PATCH] finish moving to waypoint after a back transition --- src/modules/navigator/mission.cpp | 25 +++++++++++++++++++++++++ src/modules/navigator/mission.h | 3 ++- 2 files changed, 27 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index b90ef946c1..fb6a300f94 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -501,6 +501,31 @@ Mission::set_mission_items() new_work_item_type = WORK_ITEM_TYPE_DEFAULT; } + /* don't advance mission after FW to MC command */ + if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION + && _work_item_type != WORK_ITEM_TYPE_CMD_BEFORE_MOVE + && !_navigator->get_vstatus()->is_rotary_wing + && !_navigator->get_vstatus()->condition_landed + && pos_sp_triplet->previous.valid) { + + new_work_item_type = WORK_ITEM_TYPE_CMD_BEFORE_MOVE; + } + + /* after FW to MC transition finish moving to the waypoint */ + if (_work_item_type == WORK_ITEM_TYPE_CMD_BEFORE_MOVE + && pos_sp_triplet->previous.valid) { + + new_work_item_type = WORK_ITEM_TYPE_DEFAULT; + + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.lat = pos_sp_triplet->previous.lat; + _mission_item.lon = pos_sp_triplet->previous.lon; + _mission_item.altitude = pos_sp_triplet->previous.alt; + _mission_item.altitude_is_relative = false; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0; + } + } /*********************************** set setpoints and check next *********************************************/ diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 136475df67..dff8d32540 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -233,7 +233,8 @@ private: WORK_ITEM_TYPE_DEFAULT, /**< default mission item */ WORK_ITEM_TYPE_TAKEOFF, /**< takeoff before moving to waypoint */ WORK_ITEM_TYPE_MOVE_TO_LAND, /**< move to land waypoint before descent */ - WORK_ITEM_TYPE_ALIGN /**< align for next waypoint */ + WORK_ITEM_TYPE_ALIGN, /**< align for next waypoint */ + WORK_ITEM_TYPE_CMD_BEFORE_MOVE /**< */ } _work_item_type; /**< current type of work to do (sub mission item) */ };