AC_AttControl: use trig values from ahrs

libraries/AC_AttitudeControl/AC_AttitudeControl.cpp

@@ -352,9 +352,9 @@ void AC_AttitudeControl::rate_stab_bf_to_rate_bf_yaw()
 void AC_AttitudeControl::rate_ef_targets_to_bf(const Vector3f& rate_ef_target, Vector3f& rate_bf_target)
 {
     // convert earth frame rates to body frame rates
-    rate_bf_target.x = rate_ef_target.x - _sin_pitch * rate_ef_target.z;
-    rate_bf_target.y = _cos_roll  * rate_ef_target.y + _sin_roll * _cos_pitch * rate_ef_target.z;
-    rate_bf_target.z = _cos_pitch * _cos_roll * rate_ef_target.z - _sin_roll * rate_ef_target.y;
+    rate_bf_target.x = rate_ef_target.x - _ahrs.sin_pitch() * rate_ef_target.z;
+    rate_bf_target.y = _ahrs.cos_roll()  * rate_ef_target.y + _ahrs.sin_roll() * _ahrs.cos_pitch() * rate_ef_target.z;
+    rate_bf_target.z = _ahrs.cos_pitch() * _ahrs.cos_roll() * rate_ef_target.z - _ahrs.sin_roll() * rate_ef_target.y;
 }
 
 //
@@ -505,7 +505,7 @@ void AC_AttitudeControl::set_throttle_out(int16_t throttle_out, bool apply_angle
 // throttle value should be 0 ~ 1000
 int16_t AC_AttitudeControl::get_angle_boost(int16_t throttle_pwm)
 {
-    float temp = _cos_pitch * _cos_roll;
+    float temp = _ahrs.cos_pitch() * _ahrs.cos_roll();
     int16_t throttle_out;
 
     temp = constrain_float(temp, 0.5f, 1.0f);

libraries/AC_AttitudeControl/AC_AttitudeControl.h

@@ -59,11 +59,7 @@ public:
         _motor_yaw(motor_yaw),
         _motor_throttle(motor_throttle),
         _dt(AC_ATTITUDE_100HZ_DT),
-        _angle_boost(0),
-        _cos_roll(1.0),
-        _cos_pitch(1.0),
-        _sin_roll(0.0),
-        _sin_pitch(0.0)
+        _angle_boost(0)
 		{
 			AP_Param::setup_object_defaults(this, var_info);
 
@@ -191,15 +187,6 @@ public:
     // helper functions
     //
 
-    /// set_cos_sin_yaw - short-cut to save on calculations to convert from roll-pitch frame to lat-lon frame
-    /// To-Do: make these references or calculate in ahrs
-    void set_cos_sin_yaw(float cos_roll, float cos_pitch, float sin_roll, float sin_pitch) {
-        _cos_roll = cos_roll;
-        _cos_pitch = cos_pitch;
-        _sin_roll = sin_roll;
-        _sin_pitch = sin_pitch;
-    }
-
     // lean_angle_max - maximum lean angle of the copter in centi-degrees
     int16_t lean_angle_max() { return _aparm.angle_max; }
 
@@ -278,13 +265,6 @@ protected:
     Vector3f            _rate_stab_bf_error;    // stabilized rate controller body-frame angle errors
     Vector3f            _rate_bf_target;        // rate controller body-frame targets
     int16_t             _angle_boost;           // used only for logging
-
-    // precalculated values for efficiency saves
-    // To-Do: could these be changed to references and passed into the constructor?
-    float _cos_roll;
-    float _cos_pitch;
-    float _sin_roll;
-    float _sin_pitch;
 };
 
 #define AC_ATTITUDE_CONTROL_LOG_FORMAT(msg) { msg, sizeof(AC_AttitudeControl::log_Attitude),	\