Copter: Leonard's control_acro fixes

get_pilot_desired_angle_rates returns bf rate targets as floats

ArduCopter/control_acro.pde

@@ -16,7 +16,7 @@ static bool acro_init(bool ignore_checks)
 // should be called at 100hz or more
 static void acro_run()
 {
-    int16_t target_roll, target_pitch, target_yaw;
+    float target_roll, target_pitch, target_yaw;
     int16_t pilot_throttle_scaled;
 
     // if motors not running reset angle targets
@@ -42,7 +42,7 @@ static void acro_run()
 
 // get_pilot_desired_angle_rates - transform pilot's roll pitch and yaw input into a desired lean angle rates
 // returns desired angle rates in centi-degrees-per-second
-static void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, int16_t &roll_out, int16_t &pitch_out, int16_t &yaw_out)
+static void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out)
 {
 
     // Calculate trainer mode earth frame rate command for roll
@@ -57,15 +57,14 @@ static void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int
 
     // calculate earth frame rate corrections to pull the copter back to level while in ACRO mode
 
+    if (g.acro_trainer != ACRO_TRAINER_DISABLED) {
         // Calculate trainer mode earth frame rate command for roll
         int32_t roll_angle = wrap_180_cd(ahrs.roll_sensor);
-    roll_angle   = constrain_int32(roll_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE);
-    rate_ef_level.x = -roll_angle * g.acro_balance_roll;
+        rate_ef_level.x = -constrain_int32(roll_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_roll;
 
         // Calculate trainer mode earth frame rate command for pitch
         int32_t pitch_angle = wrap_180_cd(ahrs.pitch_sensor);
-    pitch_angle  = constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE);
-    rate_ef_level.y = -pitch_angle * g.acro_balance_pitch;
+        rate_ef_level.y = -constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_pitch;
 
         // Calculate trainer mode earth frame rate command for yaw
         rate_ef_level.z = 0;
@@ -73,15 +72,15 @@ static void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int
         // Calculate angle limiting earth frame rate commands
         if (g.acro_trainer == ACRO_TRAINER_LIMITED) {
             if (roll_angle > aparm.angle_max){
-            rate_ef_level.x +=  g.pi_stabilize_roll.get_p(aparm.angle_max-roll_angle);
+                rate_ef_level.x -=  g.pi_stabilize_roll.get_p(roll_angle-aparm.angle_max);
             }else if (roll_angle < -aparm.angle_max) {
-            rate_ef_level.x +=  g.pi_stabilize_roll.get_p(-aparm.angle_max-roll_angle);
+                rate_ef_level.x -=  g.pi_stabilize_roll.get_p(roll_angle+aparm.angle_max);
             }
 
             if (pitch_angle > aparm.angle_max){
-            rate_ef_level.y +=  g.pi_stabilize_pitch.get_p(aparm.angle_max-pitch_angle);
+                rate_ef_level.y -=  g.pi_stabilize_pitch.get_p(pitch_angle-aparm.angle_max);
             }else if (pitch_angle < -aparm.angle_max) {
-            rate_ef_level.y +=  g.pi_stabilize_pitch.get_p(-aparm.angle_max-pitch_angle);
+                rate_ef_level.y -=  g.pi_stabilize_pitch.get_p(pitch_angle+aparm.angle_max);
             }
         }
 
@@ -114,6 +113,7 @@ static void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int
             rate_bf_request.z += rate_bf_level.z;
             rate_bf_request.z = constrain_float(rate_bf_request.z, -rate_limit, rate_limit);
         }
+    }
 
     // hand back rate request
     roll_out = rate_bf_request.x;