Copter: Acro trainer separated into limited and leveled

ArduCopter/Attitude.pde

@@ -101,11 +101,19 @@ get_acro_yaw(int32_t target_rate)
 static void
 get_acro_level_rates()
 {
+    // zero earth frame leveling if trainer is disabled
+    if (g.acro_trainer == ACRO_TRAINER_DISABLED) {
+        set_roll_rate_target(0, BODY_EARTH_FRAME);
+        set_pitch_rate_target(0, BODY_EARTH_FRAME);
+        set_yaw_rate_target(0, BODY_EARTH_FRAME);
+        return;
+    }
+
     // Calculate trainer mode earth frame rate command for roll
     int32_t roll_angle = wrap_180_cd(ahrs.roll_sensor);
     int32_t target_rate = 0;
 
-    if (g.acro_trainer_enabled) {
+    if (g.acro_trainer == ACRO_TRAINER_LIMITED) {
         if (roll_angle > 4500){
             target_rate =  g.pi_stabilize_roll.get_p(4500-roll_angle);
         }else if (roll_angle < -4500) {
@@ -113,7 +121,7 @@ get_acro_level_rates()
         }
     }
     roll_angle   = constrain_int32(roll_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE);
-    target_rate -=  (roll_angle * g.acro_balance_roll)/100;
+    target_rate -= roll_angle * g.acro_balance_roll;
 
     // add earth frame targets for roll rate controller
     set_roll_rate_target(target_rate, BODY_EARTH_FRAME);
@@ -122,7 +130,7 @@ get_acro_level_rates()
     int32_t pitch_angle = wrap_180_cd(ahrs.pitch_sensor);
     target_rate = 0;
 
-    if (g.acro_trainer_enabled) {
+    if (g.acro_trainer == ACRO_TRAINER_LIMITED) {
         if (pitch_angle > 4500){
             target_rate =  g.pi_stabilize_pitch.get_p(4500-pitch_angle);
         }else if (pitch_angle < -4500) {
@@ -130,7 +138,7 @@ get_acro_level_rates()
         }
     }
     pitch_angle  = constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE);
-    target_rate -=  (pitch_angle * g.acro_balance_pitch)/100;
+    target_rate -= pitch_angle * g.acro_balance_pitch;
 
     // add earth frame targets for pitch rate controller
     set_pitch_rate_target(target_rate, BODY_EARTH_FRAME);
@@ -148,7 +156,9 @@ get_roll_rate_stabilized_bf(int32_t stick_angle)
     // convert the input to the desired body frame roll rate
     int32_t rate_request = stick_angle * g.acro_p;
 
-    if (!g.acro_trainer_enabled) {
+    if (g.acro_trainer == ACRO_TRAINER_LIMITED) {
+        rate_request += acro_roll_rate;
+    }else{        
         // Scale pitch leveling by stick input
         acro_roll_rate = (float)acro_roll_rate*acro_level_mix;
 
@@ -157,8 +167,6 @@ get_roll_rate_stabilized_bf(int32_t stick_angle)
 
         rate_request += acro_roll_rate;
         rate_request = constrain_int32(rate_request, -rate_limit, rate_limit);
-    } else {
-        rate_request += acro_roll_rate;
     }
 
     // add automatic correction
@@ -187,7 +195,9 @@ get_pitch_rate_stabilized_bf(int32_t stick_angle)
     // convert the input to the desired body frame pitch rate
     int32_t rate_request = stick_angle * g.acro_p;
 
-    if (!g.acro_trainer_enabled) {
+    if (g.acro_trainer == ACRO_TRAINER_LIMITED) {
+        rate_request += acro_pitch_rate;
+    }else{
         // Scale pitch leveling by stick input
         acro_pitch_rate = (float)acro_pitch_rate*acro_level_mix;
         
@@ -196,8 +206,6 @@ get_pitch_rate_stabilized_bf(int32_t stick_angle)
         
         rate_request += acro_pitch_rate;
         rate_request = constrain_int32(rate_request, -rate_limit, rate_limit);
-    } else {
-        rate_request += acro_pitch_rate;
     }
 
     // add automatic correction
@@ -226,7 +234,9 @@ get_yaw_rate_stabilized_bf(int32_t stick_angle)
     // convert the input to the desired body frame yaw rate
     int32_t rate_request = stick_angle * g.acro_p;
 
-    if (!g.acro_trainer_enabled) {
+    if (g.acro_trainer == ACRO_TRAINER_LIMITED) {
+        rate_request += acro_yaw_rate;
+    }else{
         // Scale pitch leveling by stick input
         acro_yaw_rate = (float)acro_yaw_rate*acro_level_mix;
 
@@ -235,8 +245,6 @@ get_yaw_rate_stabilized_bf(int32_t stick_angle)
 
         rate_request += acro_yaw_rate;
         rate_request = constrain_int32(rate_request, -rate_limit, rate_limit);
-    } else {
-        rate_request += acro_yaw_rate;
     }
 
     // add automatic correction
@@ -263,14 +271,14 @@ get_roll_rate_stabilized_ef(int32_t stick_angle)
     int32_t angle_error = 0;
 
     // convert the input to the desired roll rate
-    int32_t target_rate = stick_angle * g.acro_p - (acro_roll * g.acro_balance_roll)/100;
+    int32_t target_rate = stick_angle * g.acro_p - (acro_roll * g.acro_balance_roll);
 
     // convert the input to the desired roll rate
     acro_roll += target_rate * G_Dt;
     acro_roll = wrap_180_cd(acro_roll);
 
     // ensure that we don't reach gimbal lock
-    if (labs(acro_roll) > 4500 && g.acro_trainer_enabled) {
+    if (labs(acro_roll) > 4500 && g.acro_trainer == ACRO_TRAINER_LIMITED) {
         acro_roll  = constrain_int32(acro_roll, -4500, 4500);
         angle_error = wrap_180_cd(acro_roll - ahrs.roll_sensor);
     } else {
@@ -304,7 +312,7 @@ get_pitch_rate_stabilized_ef(int32_t stick_angle)
     int32_t angle_error = 0;
 
     // convert the input to the desired pitch rate
-    int32_t target_rate = stick_angle * g.acro_p - (acro_pitch * g.acro_balance_pitch)/100;
+    int32_t target_rate = stick_angle * g.acro_p - (acro_pitch * g.acro_balance_pitch);
 
     // convert the input to the desired pitch rate
     acro_pitch += target_rate * G_Dt;

ArduCopter/Parameters.h

@@ -82,7 +82,7 @@ public:
         k_param_rssi_pin,
         k_param_throttle_accel_enabled,     // deprecated - remove
         k_param_wp_yaw_behavior,
-        k_param_acro_trainer_enabled,
+        k_param_acro_trainer,
         k_param_pilot_velocity_z_max,
         k_param_circle_rate,
         k_param_sonar_gain,
@@ -251,9 +251,11 @@ public:
         k_param_pid_throttle_rate,
         k_param_pid_optflow_roll,
         k_param_pid_optflow_pitch,
-        k_param_acro_balance_roll,      // scalar (not PID)
-        k_param_acro_balance_pitch,     // scalar (not PID)
-        k_param_pid_throttle_accel, // 241
+        k_param_acro_balance_roll_old,  // 239 - remove
+        k_param_acro_balance_pitch_old, // 240 - remove
+        k_param_pid_throttle_accel,
+        k_param_acro_balance_roll,
+        k_param_acro_balance_pitch, // 243
 
         // 254,255: reserved
     };
@@ -377,9 +379,9 @@ public:
 
     // Acro parameters
     AP_Float                acro_p;
-    AP_Int16                acro_balance_roll;
-    AP_Int16                acro_balance_pitch;
-    AP_Int8                 acro_trainer_enabled;
+    AP_Float                acro_balance_roll;
+    AP_Float                acro_balance_pitch;
+    AP_Int8                 acro_trainer;
 
     // PI/D controllers
     AC_PID                  pid_rate_roll;

ArduCopter/Parameters.pde

@@ -531,25 +531,25 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @Param: ACRO_BAL_ROLL
     // @DisplayName: Acro Balance Roll
     // @Description: rate at which roll angle returns to level in acro mode
-    // @Range: 0 300
-    // @Increment: 1
+    // @Range: 0 3
+    // @Increment: 0.1
     // @User: Advanced
     GSCALAR(acro_balance_roll,      "ACRO_BAL_ROLL",    ACRO_BALANCE_ROLL),
 
     // @Param: ACRO_BAL_PITCH
     // @DisplayName: Acro Balance Pitch
     // @Description: rate at which pitch angle returns to level in acro mode
-    // @Range: 0 300
-    // @Increment: 1
+    // @Range: 0 3
+    // @Increment: 0.1
     // @User: Advanced
     GSCALAR(acro_balance_pitch,     "ACRO_BAL_PITCH",   ACRO_BALANCE_PITCH),
 
     // @Param: ACRO_TRAINER
-    // @DisplayName: Acro Trainer Enabled
-    // @Description: Set to 1 (Enabled) to make roll return to within 45 degrees of level automatically
-    // @Values: 0:Disabled,1:Enabled
+    // @DisplayName: Acro Trainer
+    // @Description: Type of trainer used in acro mode
+    // @Values: 0:Disabled,1:Leveling,2:Leveling and Limited
     // @User: Advanced
-    GSCALAR(acro_trainer_enabled,   "ACRO_TRAINER",     ACRO_TRAINER_ENABLED),
+    GSCALAR(acro_trainer,   "ACRO_TRAINER",     ACRO_TRAINER_LIMITED),
 
     // @Param: LED_MODE
     // @DisplayName: Copter LED Mode

ArduCopter/config.h

@@ -869,15 +869,11 @@
 #endif
 
 #ifndef ACRO_BALANCE_ROLL
- #define ACRO_BALANCE_ROLL			200
+ #define ACRO_BALANCE_ROLL			1.0f
 #endif
 
 #ifndef ACRO_BALANCE_PITCH
- #define ACRO_BALANCE_PITCH			200
-#endif
-
-#ifndef ACRO_TRAINER_ENABLED
- #define ACRO_TRAINER_ENABLED       ENABLED
+ #define ACRO_BALANCE_PITCH			1.0f
 #endif
 
 //////////////////////////////////////////////////////////////////////////////

ArduCopter/defines.h

@@ -175,6 +175,10 @@
 #define CH6_DECLINATION                 38  // compass declination in radians
 #define CH6_CIRCLE_RATE                 39  // circle turn rate in degrees (hard coded to about 45 degrees in either direction)
 
+// Acro Trainer types
+#define ACRO_TRAINER_DISABLED   0
+#define ACRO_TRAINER_LEVELING   1
+#define ACRO_TRAINER_LIMITED    2
 
 // Commands - Note that APM now uses a subset of the MAVLink protocol
 // commands.  See enum MAV_CMD in the GCS_Mavlink library