From 8b8d6a8e0172badfb152e2a48e89415f4ac99d4b Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sun, 9 Feb 2014 12:34:59 +0900 Subject: [PATCH] AC_PosControl: use trig values from ahrs --- libraries/AC_AttitudeControl/AC_PosControl.cpp | 9 +++------ libraries/AC_AttitudeControl/AC_PosControl.h | 7 ------- 2 files changed, 3 insertions(+), 13 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 5e945c5147..562c5c7225 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -46,9 +46,6 @@ AC_PosControl::AC_PosControl(const AP_AHRS& ahrs, const AP_InertialNav& inav, _accel_z_cms(POSCONTROL_ACCEL_XY_MAX), // To-Do: check this default _accel_cms(POSCONTROL_ACCEL_XY_MAX), // To-Do: check this default _leash(POSCONTROL_LEASH_LENGTH_MIN), - _cos_yaw(1.0), - _sin_yaw(0.0), - _cos_pitch(1.0), _roll_target(0.0), _pitch_target(0.0), _vel_target_filt_z(0), @@ -639,11 +636,11 @@ void AC_PosControl::accel_to_lean_angles() // To-Do: add 1hz filter to accel_lat, accel_lon // rotate accelerations into body forward-right frame - accel_forward = _accel_target.x*_cos_yaw + _accel_target.y*_sin_yaw; - accel_right = -_accel_target.x*_sin_yaw + _accel_target.y*_cos_yaw; + accel_forward = _accel_target.x*_ahrs.cos_yaw() + _accel_target.y*_ahrs.sin_yaw(); + accel_right = -_accel_target.x*_ahrs.sin_yaw() + _accel_target.y*_ahrs.cos_yaw(); // update angle targets that will be passed to stabilize controller - _roll_target = constrain_float(fast_atan(accel_right*_cos_pitch/(GRAVITY_MSS * 100))*(18000/M_PI), -lean_angle_max, lean_angle_max); + _roll_target = constrain_float(fast_atan(accel_right*_ahrs.cos_pitch()/(GRAVITY_MSS * 100))*(18000/M_PI), -lean_angle_max, lean_angle_max); _pitch_target = constrain_float(fast_atan(-accel_forward/(GRAVITY_MSS * 100))*(18000/M_PI),-lean_angle_max, lean_angle_max); } diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 995641a046..d201ccb55b 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -186,13 +186,6 @@ public: const Vector3f get_vel_target() { return _vel_target; } const Vector3f get_accel_target() { return _accel_target; } - /// 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, float cos_pitch) { - _cos_yaw = cos_yaw; - _sin_yaw = sin_yaw; - _cos_pitch = cos_pitch; - } - static const struct AP_Param::GroupInfo var_info[]; private: