From b944d9df26233b89427d0bb3603095f17dbbc444 Mon Sep 17 00:00:00 2001
From: rmackay9 <rmackay9@yahoo.com>
Date: Wed, 17 Oct 2012 19:15:25 +0900
Subject: [PATCH] ArduCopter: bug fix in earthframe->body frame translation.

Provided by Leonard Hall.

Also removed large unnecessary comments.
---
 ArduCopter/Attitude.pde | 104 +---------------------------------------
 1 file changed, 2 insertions(+), 102 deletions(-)

diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde
index 1560d6229d..52bf9c0882 100644
--- a/ArduCopter/Attitude.pde
+++ b/ArduCopter/Attitude.pde
@@ -178,80 +178,6 @@ get_yaw_rate_stabilized_ef(int32_t stick_angle)
 	set_yaw_rate_target(g.pi_stabilize_yaw.get_p(angle_error)+target_rate, EARTH_FRAME);
 }
 
-/*
- *  static int16_t
- *  get_acro_yaw2(int32_t target_rate)
- *  {
- *       int32_t p,i,d;									// used to capture pid values for logging
- *       int32_t	rate_error;								// current yaw rate error
- *       int32_t	current_rate;							// current real yaw rate
- *       int32_t decel_boost;							// gain scheduling if we are overshooting
- *       int32_t output;									// output to rate controller
- *
- *       target_rate = g.pi_stabilize_yaw.get_p(target_rate);
- *       current_rate = omega.z * DEGX100;
- *       rate_error = target_rate - current_rate;
- *
- *       //Gain Scheduling:
- *       //If the yaw input is to the right, but stick is moving to the middle
- *       //and actual rate is greater than the target rate then we are
- *       //going to overshoot the yaw target to the left side, so we should
- *       //strengthen the yaw output to slow down the yaw!
- *
- * #if (FRAME_CONFIG == HELI_FRAME	|| FRAME_CONFIG == TRI_FRAME)
- *       static int32_t last_target_rate = 0;			// last iteration's target rate
- *       if ( target_rate > 0 && last_target_rate > target_rate && rate_error < 0 ){
- *               decel_boost = 1;
- *       } else if (target_rate < 0 && last_target_rate < target_rate && rate_error > 0 ){
- *               decel_boost = 1;
- *       } else if (target_rate == 0 && labs(current_rate) > 1000){
- *               decel_boost = 1;
- *       } else {
- *               decel_boost = 0;
- *       }
- *
- *       last_target_rate = target_rate;
- *
- * #else
- *
- *       decel_boost = 0;
- *
- * #endif
- *
- *       // separately calculate p, i, d values for logging
- *       // we will use d=0, and hold i at it's last value
- *       // since manual inputs are never steady state
- *
- *       p = g.pid_rate_yaw.get_p(rate_error);
- *       i = g.pid_rate_yaw.get_integrator();
- *       d = 0;
- *
- *       if (decel_boost){
- *               p *= 2;
- *       }
- *
- *       output     = p+i+d;
- *
- *       // output control:
- *       // constrain output
- *       output = constrain(output, -4500, 4500);
- *
- * #if LOGGING_ENABLED == ENABLED
- *       static int8_t log_counter = 0;					// used to slow down logging of PID values to dataflash
- *       // log output if PID loggins is on and we are tuning the yaw
- *       if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_YAW_KP || g.radio_tuning == CH6_YAW_RATE_KP) ) {
- *               log_counter++;
- *               if( log_counter >= 10 ) {	// (update rate / desired output rate) = (100hz / 10hz) = 10
- *                       log_counter = 0;
- *                       Log_Write_PID(CH6_YAW_RATE_KP, rate_error, p, i, d, output, tuning_value);
- *               }
- *       }
- * #endif
- *
- *       return output;
- *  }
- */
-
 // set_roll_rate_target - to be called by upper controllers to set roll rate targets in the earth frame
 void set_roll_rate_target( int32_t desired_rate, uint8_t earth_or_body_frame ) {
     rate_targets_frame = earth_or_body_frame;
@@ -289,8 +215,8 @@ update_rate_contoller_targets()
     if( rate_targets_frame == EARTH_FRAME ) {
         // convert earth frame rates to body frame rates
         roll_rate_target_bf = roll_rate_target_ef - sin_pitch * yaw_rate_target_ef;
-        pitch_rate_target_bf = cos_roll_x * pitch_rate_target_ef + sin_roll * yaw_rate_target_ef;
-        yaw_rate_target_bf = cos_pitch_x * cos_roll_x * yaw_rate_target_ef + sin_roll * pitch_rate_target_ef;
+        pitch_rate_target_bf = cos_roll_x * pitch_rate_target_ef + sin_roll * cos_pitch_x * yaw_rate_target_ef;
+        yaw_rate_target_bf = cos_pitch_x * cos_roll_x * yaw_rate_target_ef - sin_roll * pitch_rate_target_ef;
     }
 }
 
@@ -615,32 +541,6 @@ static void reset_stability_I(void)
  *  throttle control
  ****************************************************************/
 
-/* Depricated
- *
- *  static long
- *  //get_nav_yaw_offset(int yaw_input, int reset)
- *  {
- *       int32_t _yaw;
- *
- *       if(reset == 0){
- *               // we are on the ground
- *               return ahrs.yaw_sensor;
- *
- *       }else{
- *               // re-define nav_yaw if we have stick input
- *               if(yaw_input != 0){
- *                       // set nav_yaw + or - the current location
- *                       _yaw = yaw_input + ahrs.yaw_sensor;
- *                       // we need to wrap our value so we can be 0 to 360 (*100)
- *                       return wrap_360(_yaw);
- *               }else{
- *                       // no stick input, lets not change nav_yaw
- *                       return nav_yaw;
- *               }
- *       }
- *  }
- */
-
 static int16_t get_angle_boost(int16_t value)
 {
     float temp = cos_pitch_x * cos_roll_x;