From 08fdc90175c5c2b897f9883a8c31f6d1f8d3bc8d Mon Sep 17 00:00:00 2001 From: Dusan Zivkovic Date: Fri, 29 May 2020 00:48:32 +0200 Subject: [PATCH] mission: keep current heading if close to ROI while heading sp is invalid --- src/modules/navigator/mission.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 27fe65ab88..ddfe8a267d 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -1321,6 +1321,7 @@ Mission::heading_sp_update() case vehicle_roi_s::ROI_TARGET: case vehicle_roi_s::ROI_ENUM_END: default: { + return; } } @@ -1339,6 +1340,13 @@ Mission::heading_sp_update() _mission_item.yaw = yaw; pos_sp_triplet->current.yaw = _mission_item.yaw; pos_sp_triplet->current.yaw_valid = true; + + } else { + if (!pos_sp_triplet->current.yaw_valid) { + _mission_item.yaw = _navigator->get_local_position()->yaw; + pos_sp_triplet->current.yaw = _mission_item.yaw; + pos_sp_triplet->current.yaw_valid = true; + } } // we set yaw directly so we can run this in parallel to the FOH update