From ed37cd37a4c06e870eaddf4cf725af58595cffac Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 4 Jan 2025 09:03:01 +1100 Subject: [PATCH] Plane: slew limit tilted motor throttles this reduces the sudden change in throttle on fwd transition completion and allows for THR_SLEWRATE to work on tilt quadplanes --- ArduPlane/servos.cpp | 1 + ArduPlane/tiltrotor.cpp | 14 +++++++++++--- 2 files changed, 12 insertions(+), 3 deletions(-) diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 2ded238cf2f5ba..6725369b31295f 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -56,6 +56,7 @@ void Plane::throttle_slew_limit() if (g.takeoff_throttle_slewrate != 0 && quadplane.in_frwd_transition()) { slewrate = g.takeoff_throttle_slewrate; } + SRV_Channels::set_slew_rate(SRV_Channel::k_throttle_tilt, quadplane.tiltrotor.fully_fwd()?slewrate:0, 100, G_Dt); #endif SRV_Channels::set_slew_rate(SRV_Channel::k_throttle, slewrate, 100, G_Dt); SRV_Channels::set_slew_rate(SRV_Channel::k_throttleLeft, slewrate, 100, G_Dt); diff --git a/ArduPlane/tiltrotor.cpp b/ArduPlane/tiltrotor.cpp index 239e6df7b49b03..596b1462d64cc7 100644 --- a/ArduPlane/tiltrotor.cpp +++ b/ArduPlane/tiltrotor.cpp @@ -150,6 +150,8 @@ void Tiltrotor::setup() } quadplane.transition = transition; + SRV_Channels::set_range(SRV_Channel::k_throttle_tilt, 100); // match k_throttle + setup_complete = true; } @@ -229,7 +231,9 @@ void Tiltrotor::continuous_update(void) max_change = tilt_max_change(false); - float new_throttle = constrain_float(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle)*0.01, 0, 1); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle_tilt, SRV_Channels::get_output_scaled(SRV_Channel::k_throttle)); + float new_throttle = constrain_float(SRV_Channels::get_slew_limited_output_scaled(SRV_Channel::k_throttle_tilt)*0.01, 0, 1); + if (current_tilt < get_fully_forward_tilt()) { current_throttle = constrain_float(new_throttle, current_throttle-max_change, @@ -245,7 +249,7 @@ void Tiltrotor::continuous_update(void) } if (!quadplane.motor_test.running) { // the motors are all the way forward, start using them for fwd thrust - const uint16_t mask = is_zero(current_throttle)?0U:tilt_mask.get(); + const uint16_t mask = tilt_mask.get(); motors->output_motor_mask(current_throttle, mask, plane.rudder_dt); } return; @@ -363,7 +367,8 @@ void Tiltrotor::binary_update(void) // all the way forward and run them as a forward motor binary_slew(true); - float new_throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle)*0.01f; + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle_tilt, SRV_Channels::get_output_scaled(SRV_Channel::k_throttle)); + float new_throttle = constrain_float(SRV_Channels::get_slew_limited_output_scaled(SRV_Channel::k_throttle_tilt)*0.01, 0, 1); if (current_tilt >= 1) { const uint16_t mask = is_zero(new_throttle)?0U:tilt_mask.get(); // the motors are all the way forward, start using them for fwd thrust @@ -489,6 +494,9 @@ void Tiltrotor::tilt_compensate_angle(float *thrust, uint8_t num_motors, float n thrust[i] *= scale; } } + if (!fully_fwd()) { + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle_tilt, largest_tilted*100); + } } /*