Copter: add acro throttle and yaw expo and smoother manual pilot throttle

ArduCopter/Attitude.cpp

@@ -121,15 +121,17 @@ void Copter::set_throttle_takeoff()
     pos_control.init_takeoff();
 }
 
-// get_pilot_desired_throttle - transform pilot's throttle input to make cruise throttle mid stick
+// transform pilot's manual throttle input to make hover throttle mid stick
 // used only for manual throttle modes
+// thr_mid should be in the range 0 to 1
 // returns throttle output 0 to 1
-float Copter::get_pilot_desired_throttle(int16_t throttle_control)
+float Copter::get_pilot_desired_throttle(int16_t throttle_control, float thr_mid)
 {
-    float throttle_out;
+    if (thr_mid <= 0.0f) {
+        thr_mid = motors.get_throttle_hover();
+    }
 
     int16_t mid_stick = channel_throttle->get_control_mid();
-
     // protect against unlikely divide by zero
     if (mid_stick <= 0) {
         mid_stick = 500;
@@ -138,21 +140,22 @@ float Copter::get_pilot_desired_throttle(int16_t throttle_control)
     // ensure reasonable throttle values
     throttle_control = constrain_int16(throttle_control,0,1000);
 
-    // ensure mid throttle is set within a reasonable range
-    float thr_mid = constrain_float(motors.get_throttle_hover(), 0.1f, 0.9f);
-
-    // check throttle is above, below or in the deadband
+    // calculate normalised throttle input
+    float throttle_in;
     if (throttle_control < mid_stick) {
         // below the deadband
-        throttle_out = ((float)throttle_control)*thr_mid/(float)mid_stick;
+        throttle_in = ((float)throttle_control)*0.5f/(float)mid_stick;
     }else if(throttle_control > mid_stick) {
         // above the deadband
-        throttle_out = (thr_mid) + ((float)(throttle_control-mid_stick)) * (1.0f - thr_mid) / (float)(1000-mid_stick);
+        throttle_in = 0.5f + ((float)(throttle_control-mid_stick)) * 0.5f / (float)(1000-mid_stick);
     }else{
         // must be in the deadband
-        throttle_out = thr_mid;
+        throttle_in = 0.5f;
     }
 
+    float expo = constrain_float(-(thr_mid-0.5)/0.375, -0.5f, 1.0f);
+    // calculate the output throttle using the given expo function
+    float throttle_out = throttle_in*(1.0f-expo) + expo*throttle_in*throttle_in*throttle_in;
     return throttle_out;
 }
 

ArduCopter/Copter.h

@@ -685,7 +685,7 @@ private:
     float get_look_ahead_yaw();
     void update_throttle_hover();
     void set_throttle_takeoff();
-    float get_pilot_desired_throttle(int16_t throttle_control);
+    float get_pilot_desired_throttle(int16_t throttle_control, float thr_mid);
     float get_pilot_desired_climb_rate(float throttle_control);
     float get_non_takeoff_throttle();
     float get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt);

ArduCopter/Parameters.cpp

@@ -611,12 +611,13 @@ const AP_Param::Info Copter::var_info[] = {
     // @User: Advanced
     GSCALAR(acro_trainer,   "ACRO_TRAINER",     ACRO_TRAINER_LIMITED),
 
-    // @Param: ACRO_EXPO
-    // @DisplayName: Acro Expo
+    // @Param: ACRO_RP_EXPO
+    // @DisplayName: Acro Roll/Pitch Expo
     // @Description: Acro roll/pitch Expo to allow faster rotation when stick at edges
     // @Values: 0:Disabled,0.1:Very Low,0.2:Low,0.3:Medium,0.4:High,0.5:Very High
+    // @Range: -0.5 1.0
     // @User: Advanced
-    GSCALAR(acro_expo,  "ACRO_EXPO",    ACRO_EXPO_DEFAULT),
+    GSCALAR(acro_rp_expo,  "ACRO_RP_EXPO",    ACRO_RP_EXPO_DEFAULT),
 
     // @Param: VEL_XY_P
     // @DisplayName: Velocity (horizontal) P gain
@@ -1014,6 +1015,22 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     AP_SUBGROUPINFO(proximity, "PRX_", 8, ParametersG2, AP_Proximity),
 #endif
 
+
+    // @Param: ACRO_Y_EXPO
+    // @DisplayName: Acro Yaw Expo
+    // @Description: Acro yaw expo to allow faster rotation when stick at edges
+    // @Values: 0:Disabled,0.1:Very Low,0.2:Low,0.3:Medium,0.4:High,0.5:Very High
+    // @Range: -0.5 1.0
+    // @User: Advanced
+    AP_GROUPINFO("ACRO_Y_EXPO", 9, ParametersG2, acro_y_expo, ACRO_Y_EXPO_DEFAULT),
+
+    // @Param: ACRO_THR_MID
+    // @DisplayName: Acro Thr Mid
+    // @Description: Acro Throttle Mid
+    // @Range: 0 1
+    // @User: Advanced
+    AP_GROUPINFO("ACRO_THR_MID", 10, ParametersG2, acro_thr_mid, ACRO_THR_MID_DEFAULT),
+
     AP_GROUPEND
 };
 

ArduCopter/Parameters.h

@@ -126,7 +126,7 @@ public:
         k_param_rangefinder, // rangefinder object
         k_param_fs_ekf_thresh,
         k_param_terrain,
-        k_param_acro_expo,
+        k_param_acro_rp_expo,
         k_param_throttle_deadzone,
         k_param_optflow,
         k_param_dcmcheck_thresh,        // deprecated - remove
@@ -484,7 +484,7 @@ public:
     AP_Float                acro_balance_roll;
     AP_Float                acro_balance_pitch;
     AP_Int8                 acro_trainer;
-    AP_Float                acro_expo;
+    AP_Float                acro_rp_expo;
 
     // PI/D controllers
     AC_PI_2D                pi_vel_xy;
@@ -570,6 +570,10 @@ public:
 
     // developer options
     AP_Int32 dev_options;
+
+    // acro exponent parameters
+    AP_Float acro_y_expo;
+    AP_Float acro_thr_mid;
 };
 
 extern const AP_Param::Info        var_info[];

ArduCopter/config.h

@@ -443,8 +443,16 @@
  #define ACRO_BALANCE_PITCH         1.0f
 #endif
 
-#ifndef ACRO_EXPO_DEFAULT
- #define ACRO_EXPO_DEFAULT          0.3f
+#ifndef ACRO_RP_EXPO_DEFAULT
+ #define ACRO_RP_EXPO_DEFAULT       0.3f
+#endif
+
+#ifndef ACRO_Y_EXPO_DEFAULT
+ #define ACRO_Y_EXPO_DEFAULT        0.3f
+#endif
+
+#ifndef ACRO_THR_MID_DEFAULT
+ #define ACRO_THR_MID_DEFAULT       0.0f
 #endif
 
 // RTL Mode

ArduCopter/control_acro.cpp

@@ -10,7 +10,8 @@
 bool Copter::acro_init(bool ignore_checks)
 {
    // if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high
-   if (motors.armed() && ap.land_complete && !mode_has_manual_throttle(control_mode) && (get_pilot_desired_throttle(channel_throttle->get_control_in()) > get_non_takeoff_throttle())) {
+   if (motors.armed() && ap.land_complete && !mode_has_manual_throttle(control_mode) &&
+           (get_pilot_desired_throttle(channel_throttle->get_control_in(), g2.acro_thr_mid) > get_non_takeoff_throttle())) {
        return false;
    }
    // set target altitude to zero for reporting
@@ -42,7 +43,7 @@ void Copter::acro_run()
     get_pilot_desired_angle_rates(channel_roll->get_control_in(), channel_pitch->get_control_in(), channel_yaw->get_control_in(), target_roll, target_pitch, target_yaw);
 
     // get pilot's desired throttle
-    pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->get_control_in());
+    pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->get_control_in(), g2.acro_thr_mid);
 
     // run attitude controller
     attitude_control.input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw);
@@ -69,7 +70,7 @@ void Copter::get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, in
     }
     
     // calculate roll, pitch rate requests
-    if (g.acro_expo <= 0) {
+    if (g.acro_rp_expo <= 0) {
         rate_bf_request.x = roll_in * g.acro_rp_p;
         rate_bf_request.y = pitch_in * g.acro_rp_p;
     } else {
@@ -77,25 +78,42 @@ void Copter::get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, in
         float rp_in, rp_in3, rp_out;
 
         // range check expo
-        if (g.acro_expo > 1.0f) {
-            g.acro_expo = 1.0f;
+        if (g.acro_rp_expo > 1.0f) {
+            g.acro_rp_expo = 1.0f;
         }
 
         // roll expo
         rp_in = float(roll_in)/ROLL_PITCH_INPUT_MAX;
         rp_in3 = rp_in*rp_in*rp_in;
-        rp_out = (g.acro_expo * rp_in3) + ((1 - g.acro_expo) * rp_in);
+        rp_out = (g.acro_rp_expo * rp_in3) + ((1.0f - g.acro_rp_expo) * rp_in);
         rate_bf_request.x = ROLL_PITCH_INPUT_MAX * rp_out * g.acro_rp_p;
 
         // pitch expo
         rp_in = float(pitch_in)/ROLL_PITCH_INPUT_MAX;
         rp_in3 = rp_in*rp_in*rp_in;
-        rp_out = (g.acro_expo * rp_in3) + ((1 - g.acro_expo) * rp_in);
+        rp_out = (g.acro_rp_expo * rp_in3) + ((1.0f - g.acro_rp_expo) * rp_in);
         rate_bf_request.y = ROLL_PITCH_INPUT_MAX * rp_out * g.acro_rp_p;
     }
 
     // calculate yaw rate request
+    if (g.acro_rp_expo <= 0) {
         rate_bf_request.z = yaw_in * g.acro_yaw_p;
+    } else {
+        // expo variables
+        float y_in, y_in3, y_out;
+
+        // range check expo
+        if (g2.acro_y_expo > 1.0f || g2.acro_y_expo < 0.5f) {
+            g2.acro_y_expo = 1.0f;
+        }
+
+        // yaw expo
+        y_in = float(yaw_in)/ROLL_PITCH_INPUT_MAX;
+        y_in3 = y_in*y_in*y_in;
+        y_out = (g2.acro_y_expo * y_in3) + ((1.0f - g2.acro_y_expo) * y_in);
+        rate_bf_request.z = ROLL_PITCH_INPUT_MAX * y_out * g.acro_yaw_p;
+    }
+
 
     // calculate earth frame rate corrections to pull the copter back to level while in ACRO mode
 

ArduCopter/control_drift.cpp

@@ -64,7 +64,7 @@ void Copter::drift_run()
     get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max);
 
     // get pilot's desired throttle
-    pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->get_control_in());
+    pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->get_control_in(), 0.0f);
 
     // Grab inertial velocity
     const Vector3f& vel = inertial_nav.get_velocity();

ArduCopter/control_flip.cpp

@@ -105,7 +105,7 @@ void Copter::flip_run()
     }
 
     // get pilot's desired throttle
-    throttle_out = get_pilot_desired_throttle(channel_throttle->get_control_in());
+    throttle_out = get_pilot_desired_throttle(channel_throttle->get_control_in(), 0.0f);
 
     // get corrected angle based on direction and axis of rotation
     // we flip the sign of flip_angle to minimize the code repetition

ArduCopter/control_stabilize.cpp

@@ -10,7 +10,8 @@
 bool Copter::stabilize_init(bool ignore_checks)
 {
     // if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high
-    if (motors.armed() && ap.land_complete && !mode_has_manual_throttle(control_mode) && (get_pilot_desired_throttle(channel_throttle->get_control_in()) > get_non_takeoff_throttle())) {
+    if (motors.armed() && ap.land_complete && !mode_has_manual_throttle(control_mode) &&
+            (get_pilot_desired_throttle(channel_throttle->get_control_in(), 0.0f) > get_non_takeoff_throttle())) {
         return false;
     }
     // set target altitude to zero for reporting
@@ -50,7 +51,7 @@ void Copter::stabilize_run()
     target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
 
     // get pilot's desired throttle
-    pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->get_control_in());
+    pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->get_control_in(), 0.0f);
 
     // call attitude controller
     attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());

ArduCopter/flight_mode.cpp

@@ -274,7 +274,7 @@ void Copter::exit_mode(control_mode_t old_control_mode, control_mode_t new_contr
     // smooth throttle transition when switching from manual to automatic flight modes
     if (mode_has_manual_throttle(old_control_mode) && !mode_has_manual_throttle(new_control_mode) && motors.armed() && !ap.land_complete) {
         // this assumes all manual flight modes use get_pilot_desired_throttle to translate pilot input to output throttle
-        set_accel_throttle_I_from_pilot_throttle(get_pilot_desired_throttle(channel_throttle->get_control_in()));
+        set_accel_throttle_I_from_pilot_throttle(get_pilot_desired_throttle(channel_throttle->get_control_in(), 0.0f));
     }
 
     // cancel any takeoffs in progress