diff --git a/libraries/AC_WPNav/AC_Circle.cpp b/libraries/AC_WPNav/AC_Circle.cpp index 1943bf8893..a9cdb99e00 100644 --- a/libraries/AC_WPNav/AC_Circle.cpp +++ b/libraries/AC_WPNav/AC_Circle.cpp @@ -35,9 +35,7 @@ AC_Circle::AC_Circle(const AP_InertialNav& inav, const AP_AHRS& ahrs, AC_PosCont _ahrs(ahrs), _pos_control(pos_control), _last_update(0), - _angle(0), - _cos_yaw(1.0), - _sin_yaw(0.0) + _angle(0) { AP_Param::setup_object_defaults(this, var_info); } @@ -66,8 +64,8 @@ void AC_Circle::init_center() _pos_control.get_stopping_point_z(stopping_point); // set circle center to circle_radius ahead of stopping point - _center.x = stopping_point.x + _radius * _cos_yaw; - _center.y = stopping_point.y + _radius * _sin_yaw; + _center.x = stopping_point.x + _radius * _ahrs.cos_yaw(); + _center.y = stopping_point.y + _radius * _ahrs.sin_yaw(); _center.z = stopping_point.z; // update pos_control target to stopping point diff --git a/libraries/AC_WPNav/AC_Circle.h b/libraries/AC_WPNav/AC_Circle.h index 1f2e99759d..b877cdbc6f 100644 --- a/libraries/AC_WPNav/AC_Circle.h +++ b/libraries/AC_WPNav/AC_Circle.h @@ -47,12 +47,6 @@ public: int32_t get_pitch() const { return _pos_control.get_pitch(); }; int32_t get_yaw() const { return _yaw; }; - /// set_cos_sin_yaw - short-cut to save on calculations to convert from roll-pitch frame to lat-lon frame - void set_cos_sin_yaw(float cos_yaw, float sin_yaw) { - _cos_yaw = cos_yaw; - _sin_yaw = sin_yaw; - } - static const struct AP_Param::GroupInfo var_info[]; private: @@ -86,9 +80,5 @@ private: float _angular_vel; // angular velocity in radians/sec float _angular_vel_max; // maximum velocity in radians/sec float _angular_accel; // angular acceleration in radians/sec/sec - - // helper variables - float _cos_yaw; // short-cut to save on calcs required to convert roll-pitch frame to lat-lon frame - float _sin_yaw; // To-Do: move these to ahrs or attitude control class to save on memory }; #endif // AC_CIRCLE_H