From 9f545ae3cff6b05da7a275e5585cd74076c593d6 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Fri, 22 Nov 2019 16:54:01 +0100 Subject: [PATCH] navigator: convert to radians in configuration getter --- src/modules/navigator/mission_block.cpp | 2 +- src/modules/navigator/navigator.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 55c0773a8c..8b1883624e 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -345,7 +345,7 @@ MissionBlock::is_mission_item_reached() float yaw_err = wrap_pi(_mission_item.yaw - cog); /* accept yaw if reached or if timeout is set in which case we ignore not forced headings */ - if (fabsf(yaw_err) < math::radians(_navigator->get_yaw_threshold()) + if (fabsf(yaw_err) < _navigator->get_yaw_threshold() || (_navigator->get_yaw_timeout() >= FLT_EPSILON && !_mission_item.force_heading)) { _waypoint_yaw_reached = true; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 3511acc8a8..77d593ab4e 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -286,7 +286,7 @@ public: float get_takeoff_min_alt() const { return _param_mis_takeoff_alt.get(); } bool get_takeoff_required() const { return _param_mis_takeoff_req.get(); } float get_yaw_timeout() const { return _param_mis_yaw_tmt.get(); } - float get_yaw_threshold() const { return _param_mis_yaw_err.get(); } + float get_yaw_threshold() const { return math::radians(_param_mis_yaw_err.get()); } float get_vtol_back_trans_deceleration() const { return _param_back_trans_dec_mss; } float get_vtol_reverse_delay() const { return _param_reverse_delay; }