Copter: THR_MID used to scale manual throttle

ArduCopter/ArduCopter.pde

@@ -1875,6 +1875,7 @@ bool set_throttle_mode( uint8_t new_throttle_mode )
 void update_throttle_mode(void)
 {
     int16_t pilot_climb_rate;
+    int16_t pilot_throttle_scaled;
 
     if(ap.do_flip)     // this is pretty bad but needed to flip in AP modes.
         return;
@@ -1902,19 +1903,20 @@ void update_throttle_mode(void)
             set_throttle_out(0, false);
         }else{
             // send pilot's output directly to motors
-            set_throttle_out(g.rc_3.control_in, false);
+            pilot_throttle_scaled = get_pilot_desired_throttle(g.rc_3.control_in);
+            set_throttle_out(pilot_throttle_scaled, false);
 
             // update estimate of throttle cruise
 			#if FRAME_CONFIG == HELI_FRAME
             update_throttle_cruise(motors.coll_out);
 			#else
-			update_throttle_cruise(g.rc_3.control_in);
+			update_throttle_cruise(pilot_throttle_scaled);
 			#endif  //HELI_FRAME
 
 
             // check if we've taken off yet
             if (!ap.takeoff_complete && motors.armed()) {
-                if (g.rc_3.control_in > g.throttle_cruise) {
+                if (pilot_throttle_scaled > g.throttle_cruise) {
                     // we must be in the air by now
                     set_takeoff_complete(true);
                 }
@@ -1927,17 +1929,18 @@ void update_throttle_mode(void)
         if (g.rc_3.control_in <= 0) {
             set_throttle_out(0, false); // no need for angle boost with zero throttle
         }else{
-            set_throttle_out(g.rc_3.control_in, true);
+            pilot_throttle_scaled = get_pilot_desired_throttle(g.rc_3.control_in);
+            set_throttle_out(pilot_throttle_scaled, true);
 
             // update estimate of throttle cruise
             #if FRAME_CONFIG == HELI_FRAME
             update_throttle_cruise(motors.coll_out);
 			#else
-			update_throttle_cruise(g.rc_3.control_in);
+			update_throttle_cruise(pilot_throttle_scaled);
 			#endif  //HELI_FRAME
 
             if (!ap.takeoff_complete && motors.armed()) {
-                if (g.rc_3.control_in > g.throttle_cruise) {
+                if (pilot_throttle_scaled > g.throttle_cruise) {
                     // we must be in the air by now
                     set_takeoff_complete(true);
                 }

ArduCopter/Attitude.pde

@@ -833,10 +833,41 @@ get_throttle_accel(int16_t z_target_accel)
     return output;
 }
 
+// get_pilot_desired_throttle - transform pilot's throttle input to make cruise throttle mid stick
+// used only for manual throttle modes
+// returns throttle output 0 to 1000
+#define THROTTLE_IN_MIDDLE 500          // the throttle mid point
+static int16_t get_pilot_desired_throttle(int16_t throttle_control)
+{
+    int16_t throttle_out;
+
+    // exit immediately in the simple cases
+    if( throttle_control == 0 || g.throttle_mid == 500) {
+        return throttle_control;
+    }
+
+    // ensure reasonable throttle values
+    throttle_control = constrain(throttle_control,0,1000);
+    g.throttle_mid = constrain(g.throttle_mid,300,700);
+
+    // check throttle is above, below or in the deadband
+    if (throttle_control < THROTTLE_IN_MIDDLE) {
+        // below the deadband
+        throttle_out = (float)throttle_control * (float)g.throttle_mid / 500.0f;
+    }else if(throttle_control > THROTTLE_IN_MIDDLE) {
+        // above the deadband
+        throttle_out = g.throttle_mid + ((float)(throttle_control-500))*(float)(1000-g.throttle_mid)/500.0f;
+    }else{
+        // must be in the deadband
+        throttle_out = g.throttle_mid;
+    }
+
+    return throttle_out;
+}
+
 // get_pilot_desired_climb_rate - transform pilot's throttle input to
 // climb rate in cm/s.  we use radio_in instead of control_in to get the full range
 // without any deadzone at the bottom
-#define THROTTLE_IN_MIDDLE 500          // the throttle mid point
 #define THROTTLE_IN_DEADBAND 100        // the throttle input channel's deadband in PWM
 #define THROTTLE_IN_DEADBAND_TOP (THROTTLE_IN_MIDDLE+THROTTLE_IN_DEADBAND)  // top of the deadband
 #define THROTTLE_IN_DEADBAND_BOTTOM (THROTTLE_IN_MIDDLE-THROTTLE_IN_DEADBAND)  // bottom of the deadband

ArduCopter/Parameters.h

@@ -188,7 +188,8 @@ public:
         k_param_radio_tuning_high,
         k_param_radio_tuning_low,
         k_param_rc_speed = 192,
-        k_param_failsafe_battery_enabled, // 193
+        k_param_failsafe_battery_enabled,
+        k_param_throttle_mid, // 194
 
         //
         // 200: flight modes
@@ -305,6 +306,7 @@ public:
     AP_Int8         failsafe_throttle;
     AP_Int16        failsafe_throttle_value;
     AP_Int16        throttle_cruise;
+    AP_Int16        throttle_mid;
 
     // Flight modes
     //

ArduCopter/Parameters.pde

@@ -334,6 +334,14 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @User: Standard
     GSCALAR(throttle_cruise,        "TRIM_THROTTLE",    THROTTLE_CRUISE),
 
+    // @Param: THR_MID
+    // @DisplayName: Throttle Mid Position
+    // @Description: The throttle output (0 ~ 1000) when throttle stick is in mid position.  Used to scale the manual throttle so that the mid throttle stick position is close to the throttle required to hover
+    // @User: Standard
+    // @Range: 300 700
+    // @Increment: 1
+    GSCALAR(throttle_mid,        "THR_MID",    THR_MID),
+
     // @Param: FLTMODE1
     // @DisplayName: Flight Mode 1
     // @Description: Flight mode when Channel 5 pwm is <= 1230

ArduCopter/config.h

@@ -1005,6 +1005,10 @@
  # define THROTTLE_CRUISE       450            //
 #endif
 
+#ifndef THR_MID
+ # define THR_MID        500                            // Throttle output (0 ~ 1000) when throttle stick is in mid position
+#endif
+
 #ifndef ALT_HOLD_P
  # define ALT_HOLD_P            2.0f
 #endif