From d67f82e02d355190320d84a08b71639080fc6be3 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 17 Jul 2021 16:40:38 +1000 Subject: [PATCH] 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 | 24 ++++++++++++++---------- ArduPlane/Parameters.cpp | 24 ++++++++++++++++++++++++ ArduPlane/Parameters.h | 4 ++++ ArduPlane/Plane.h | 5 +++++ ArduPlane/failsafe.cpp | 6 +++--- ArduPlane/mode_manual.cpp | 6 +++--- ArduPlane/radio.cpp | 27 +++++++++++++++++++++++++++ ArduPlane/servos.cpp | 8 ++++---- 8 files changed, 84 insertions(+), 20 deletions(-) diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 49bfa9b777..06a1acc020 100644 --- a/ArduPlane/Attitude.cpp +++ b/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 diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 1cfb056ba5..c2a8aa6260 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1225,6 +1225,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 }; diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index c5684734a5..8b7fa63cf3 100644 --- a/ArduPlane/Parameters.h +++ b/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[]; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index b48f85044a..932464f1d4 100644 --- a/ArduPlane/Plane.h +++ b/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); #ifdef ENABLE_SCRIPTING diff --git a/ArduPlane/failsafe.cpp b/ArduPlane/failsafe.cpp index 28bc7a46a3..b72c1b554e 100644 --- a/ArduPlane/failsafe.cpp +++ b/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; diff --git a/ArduPlane/mode_manual.cpp b/ArduPlane/mode_manual.cpp index 56df31bf53..9ddddc45ec 100644 --- a/ArduPlane/mode_manual.cpp +++ b/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); } diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index 9ae91e51f7..cbdbf05876 100644 --- a/ArduPlane/radio.cpp +++ b/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); +} diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 0c1de29ef0..4d197eff6c 100644 --- a/ArduPlane/servos.cpp +++ b/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);