From fa4427fbcea39c0d3d46f341f0023a30e9407acd Mon Sep 17 00:00:00 2001 From: "Dr.-Ing. Amilcar Do Carmo Lucas" Date: Mon, 27 Nov 2017 13:21:13 +0100 Subject: [PATCH] AC_AttitudeControl: Add AC_PosControl::get_bearing_to_target() method --- libraries/AC_AttitudeControl/AC_PosControl.cpp | 8 +++++++- libraries/AC_AttitudeControl/AC_PosControl.h | 3 +++ 2 files changed, 10 insertions(+), 1 deletion(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 8fac81be40..e3f746200f 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -617,12 +617,18 @@ void AC_PosControl::get_stopping_point_xy(Vector3f &stopping_point) const stopping_point.y = curr_pos.y + (stopping_dist * curr_vel.y / vel_total); } -/// get_distance_to_target - get horizontal distance to loiter target in cm +/// get_distance_to_target - get horizontal distance to target position in cm float AC_PosControl::get_distance_to_target() const { return _distance_to_target; } +/// get_bearing_to_target - get bearing to target position in centi-degrees +int32_t AC_PosControl::get_bearing_to_target() const +{ + return get_bearing_cd(_inav.get_position(), _pos_target); +} + // is_active_xy - returns true if the xy position controller has been run very recently bool AC_PosControl::is_active_xy() const { diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 510a3d2749..82758f641c 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -262,6 +262,9 @@ public: /// get_distance_to_target - get horizontal distance to position target in cm (used for reporting) float get_distance_to_target() const; + /// get_bearing_to_target - get bearing to target position in centi-degrees + int32_t get_bearing_to_target() const; + /// xyz velocity controller /// init_vel_controller_xyz - initialise the velocity controller - should be called once before the caller attempts to use the controller