Plane: added manual input expo for MANUAL, ACRO and TRAINING

this is easier than setting up mode specific expo in radios for flying
in manual modes

ArduPlane/Attitude.cpp

@@ -312,26 +312,28 @@ void Plane::stabilize_yaw(float speed_scaler)
  */
 void Plane::stabilize_training(float speed_scaler)
 {
+    const float rexpo = roll_in_expo(false);
+    const float pexpo = pitch_in_expo(false);
     if (training_manual_roll) {
-        SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, channel_roll->get_control_in());
+        SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, rexpo);
     } else {
         // calculate what is needed to hold
         stabilize_roll(speed_scaler);
-        if ((nav_roll_cd > 0 && channel_roll->get_control_in() < SRV_Channels::get_output_scaled(SRV_Channel::k_aileron)) ||
-            (nav_roll_cd < 0 && channel_roll->get_control_in() > SRV_Channels::get_output_scaled(SRV_Channel::k_aileron))) {
+        if ((nav_roll_cd > 0 && rexpo < SRV_Channels::get_output_scaled(SRV_Channel::k_aileron)) ||
+            (nav_roll_cd < 0 && rexpo > SRV_Channels::get_output_scaled(SRV_Channel::k_aileron))) {
             // allow user to get out of the roll
-            SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, channel_roll->get_control_in());
+            SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, rexpo);
         }
     }
 
     if (training_manual_pitch) {
-        SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, channel_pitch->get_control_in());
+        SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pexpo);
     } else {
         stabilize_pitch(speed_scaler);
-        if ((nav_pitch_cd > 0 && channel_pitch->get_control_in() < SRV_Channels::get_output_scaled(SRV_Channel::k_elevator)) ||
-            (nav_pitch_cd < 0 && channel_pitch->get_control_in() > SRV_Channels::get_output_scaled(SRV_Channel::k_elevator))) {
+        if ((nav_pitch_cd > 0 && pexpo < SRV_Channels::get_output_scaled(SRV_Channel::k_elevator)) ||
+            (nav_pitch_cd < 0 && pexpo > SRV_Channels::get_output_scaled(SRV_Channel::k_elevator))) {
             // allow user to get back to level
-            SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, channel_pitch->get_control_in());
+            SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pexpo);
         }
     }
 
@@ -345,8 +347,10 @@ void Plane::stabilize_training(float speed_scaler)
  */
 void Plane::stabilize_acro(float speed_scaler)
 {
-    float roll_rate = (channel_roll->get_control_in()/4500.0f) * g.acro_roll_rate;
-    float pitch_rate = (channel_pitch->get_control_in()/4500.0f) * g.acro_pitch_rate;
+    const float rexpo = roll_in_expo(true);
+    const float pexpo = pitch_in_expo(true);
+    float roll_rate = (rexpo/float(SERVO_MAX)) * g.acro_roll_rate;
+    float pitch_rate = (pexpo/float(SERVO_MAX)) * g.acro_pitch_rate;
 
     /*
       check for special roll handling near the pitch poles

ArduPlane/Parameters.cpp

@@ -1223,6 +1223,30 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     AP_SUBGROUPINFO(guidedHeading, "GUIDED_", 28, ParametersG2, AC_PID),
 #endif // OFFBOARD_GUIDED == ENABLED
 
+    // @Param: MAN_EXPO_ROLL
+    // @DisplayName: Manual control expo for roll
+    // @Description: Percentage exponential for roll input in MANUAL, ACRO and TRAINING modes
+    // @Range: 0 100
+    // @Increment: 1
+    // @User: Standard
+    AP_GROUPINFO("MAN_EXPO_ROLL", 29, ParametersG2, man_expo_roll, 0),
+
+    // @Param: MAN_EXPO_PITCH
+    // @DisplayName: Manual input expo for pitch
+    // @Description: Percentage exponential for pitch input in MANUAL, ACRO and TRAINING modes
+    // @Range: 0 100
+    // @Increment: 1
+    // @User: Standard
+    AP_GROUPINFO("MAN_EXPO_PITCH", 30, ParametersG2, man_expo_pitch, 0),
+
+    // @Param: MAN_EXPO_RUDDER
+    // @DisplayName: Manual input expo for rudder
+    // @Description: Percentage exponential for rudder input in MANUAL, ACRO and TRAINING modes
+    // @Range: 0 100
+    // @Increment: 1
+    // @User: Standard
+    AP_GROUPINFO("MAN_EXPO_RUDDER", 31, ParametersG2, man_expo_rudder, 0),
+    
     AP_GROUPEND
 };
 

ArduPlane/Parameters.h

@@ -569,6 +569,10 @@ public:
 
     // min initial climb in RTL
     AP_Int16        rtl_climb_min;
+
+    AP_Int8         man_expo_roll;
+    AP_Int8         man_expo_pitch;
+    AP_Int8         man_expo_rudder;
 };
 
 extern const AP_Param::Info var_info[];

ArduPlane/Plane.h

@@ -1139,6 +1139,11 @@ private:
 
     FlareMode flare_mode;
 
+    // expo handling
+    float roll_in_expo(bool use_dz) const;
+    float pitch_in_expo(bool use_dz) const;
+    float rudder_in_expo(bool use_dz) const;
+
 public:
     void failsafe_check(void);
     bool set_target_location(const Location& target_loc) override;

ArduPlane/failsafe.cpp

@@ -63,10 +63,10 @@ void Plane::failsafe_check(void)
         // pass RC inputs to outputs every 20ms
         RC_Channels::clear_overrides();
 
-        int16_t roll = channel_roll->get_control_in_zero_dz();
-        int16_t pitch = channel_pitch->get_control_in_zero_dz();
+        int16_t roll = roll_in_expo(false);
+        int16_t pitch = pitch_in_expo(false);
         int16_t throttle = get_throttle_input(true);
-        int16_t rudder = channel_rudder->get_control_in_zero_dz();
+        int16_t rudder = rudder_in_expo(false);
 
         if (!hal.util->get_soft_armed()) {
             throttle = 0;

ArduPlane/mode_manual.cpp

@@ -10,8 +10,8 @@ void ModeManual::_exit()
 
 void ModeManual::update()
 {
-    SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, plane.channel_roll->get_control_in_zero_dz());
-    SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, plane.channel_pitch->get_control_in_zero_dz());
-    plane.steering_control.steering = plane.steering_control.rudder = plane.channel_rudder->get_control_in_zero_dz();
+    SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, plane.roll_in_expo(false));
+    SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, plane.pitch_in_expo(false));
+    plane.steering_control.steering = plane.steering_control.rudder = plane.rudder_in_expo(false);
 }
 

ArduPlane/radio.cpp

@@ -390,3 +390,30 @@ bool Plane::rc_failsafe_active(void) const
     }
     return false;
 }
+
+/*
+  expo handling for MANUAL, ACRO and TRAINING modes
+ */
+static float channel_expo(RC_Channel *chan, int8_t expo, bool use_dz)
+{
+    if (chan == nullptr) {
+        return 0;
+    }
+    float rin = use_dz? chan->get_control_in() : chan->get_control_in_zero_dz();
+    return SERVO_MAX * expo_curve(constrain_float(expo*0.01, 0, 1), rin/SERVO_MAX);
+}
+
+float Plane::roll_in_expo(bool use_dz) const
+{
+    return channel_expo(channel_roll, g2.man_expo_roll, use_dz);
+}
+
+float Plane::pitch_in_expo(bool use_dz) const
+{
+    return channel_expo(channel_pitch, g2.man_expo_roll, use_dz);
+}
+
+float Plane::rudder_in_expo(bool use_dz) const
+{
+    return channel_expo(channel_rudder, g2.man_expo_roll, use_dz);
+}

ArduPlane/servos.cpp

@@ -362,9 +362,9 @@ void Plane::set_servos_idle(void)
  */
 void Plane::set_servos_manual_passthrough(void)
 {
-    SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, channel_roll->get_control_in_zero_dz());
-    SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, channel_pitch->get_control_in_zero_dz());
-    SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, channel_rudder->get_control_in_zero_dz());
+    SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, roll_in_expo(false));
+    SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pitch_in_expo(false));
+    SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, rudder_in_expo(false));
     int8_t throttle = get_throttle_input(true);
     SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle);
 
@@ -818,7 +818,7 @@ void Plane::set_servos(void)
     steering_control.ground_steering = false;
 
     if (control_mode == &mode_training) {
-        steering_control.rudder = channel_rudder->get_control_in();
+        steering_control.rudder = rudder_in_expo(false);
     }
     
     SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, steering_control.rudder);