From e1e15add01bf9292bd636581037a8673b376af15 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 May 2016 13:35:53 +0200 Subject: [PATCH] Reposition: Do not yaw for small dispsplacements --- src/modules/navigator/loiter.cpp | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index e56355393d..f1af2fbbdd 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -121,12 +121,17 @@ Loiter::reposition() // set yaw - // calculate direction the vehicle should point to. - pos_sp_triplet->current.yaw = get_bearing_to_next_waypoint( - _navigator->get_global_position()->lat, - _navigator->get_global_position()->lon, - pos_sp_triplet->current.lat, - pos_sp_triplet->current.lon); + float travel_dist = get_distance_to_next_waypoint(_navigator->get_global_position()->lat, _navigator->get_global_position()->lon, + pos_sp_triplet->current.lat, pos_sp_triplet->current.lon); + + if (travel_dist > 1.0f) { + // calculate direction the vehicle should point to. + pos_sp_triplet->current.yaw = get_bearing_to_next_waypoint( + _navigator->get_global_position()->lat, + _navigator->get_global_position()->lon, + pos_sp_triplet->current.lat, + pos_sp_triplet->current.lon); + } _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER);