From 1d6396b41826af45fa7577ff6e8312aa59301efe Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Wed, 9 Feb 2022 11:08:43 +0100 Subject: [PATCH] Navigator: VTOL: track virtual WP 1m ahead of vehicle during backtransition in Land mode Signed-off-by: Silvan Fuhrer --- src/modules/navigator/land.cpp | 8 ++++++-- src/modules/navigator/mission_block.cpp | 2 +- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/src/modules/navigator/land.cpp b/src/modules/navigator/land.cpp index 7a17c8cd6b..38fba5136a 100644 --- a/src/modules/navigator/land.cpp +++ b/src/modules/navigator/land.cpp @@ -74,8 +74,12 @@ Land::on_active() if (_navigator->get_vstatus()->is_vtol && _navigator->get_vstatus()->in_transition_mode) { struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - pos_sp_triplet->current.lat = _navigator->get_global_position()->lat; - pos_sp_triplet->current.lon = _navigator->get_global_position()->lon; + + // create a virtual wp 1m in front of the vehicle to track during the backtransition + waypoint_from_heading_and_distance(_navigator->get_global_position()->lat, _navigator->get_global_position()->lon, + _navigator->get_position_setpoint_triplet()->current.yaw, 1.f, + &pos_sp_triplet->current.lat, &pos_sp_triplet->current.lon); + _navigator->set_position_setpoint_triplet_updated(); } diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index f75a2700ed..d9e799ace2 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -795,7 +795,7 @@ MissionBlock::set_vtol_transition_item(struct mission_item_s *item, const uint8_ item->nav_cmd = NAV_CMD_DO_VTOL_TRANSITION; item->params[0] = (float) new_mode; item->params[1] = 0.0f; - item->yaw = _navigator->get_local_position()->heading; + item->yaw = _navigator->get_local_position()->heading; // ideally that would be course and not heading item->autocontinue = true; }