Copter: control_acro, drift use trig from ahrs

ArduCopter/control_acro.pde

@@ -94,7 +94,7 @@ static void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int
         rate_bf_request.y += rate_bf_level.y;
         rate_bf_request.z += rate_bf_level.z;
     }else{
-        acro_level_mix = constrain_float(1-max(max(abs(roll_in), abs(pitch_in)), abs(yaw_in))/4500.0, 0, 1)*cos_pitch_x;
+        acro_level_mix = constrain_float(1-max(max(abs(roll_in), abs(pitch_in)), abs(yaw_in))/4500.0, 0, 1)*ahrs.cos_pitch();
 
         // Scale leveling rates by stick input
         rate_bf_level = rate_bf_level*acro_level_mix;

ArduCopter/control_drift.pde

@@ -40,8 +40,8 @@ static void drift_run()
     Vector3f vel = inertial_nav.get_velocity();
 
     // rotate roll, pitch input from north facing to vehicle's perspective
-    float roll_vel =  vel.y * cos_yaw - vel.x * sin_yaw; // body roll vel
-    float pitch_vel = vel.y * sin_yaw + vel.x * cos_yaw; // body pitch vel
+    float roll_vel =  vel.y * ahrs.cos_yaw() - vel.x * ahrs.sin_yaw(); // body roll vel
+    float pitch_vel = vel.y * ahrs.sin_yaw() + vel.x * ahrs.cos_yaw(); // body pitch vel
 
     float pitch_vel2 = min(fabs(pitch_vel), 800);