diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index a0c41c7b62..661e422ad2 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -1005,7 +1005,7 @@ private: void calc_nav_yaw_coordinated(float speed_scaler); void calc_nav_yaw_course(void); void calc_nav_yaw_ground(void); - void throttle_slew_limit(void); + void throttle_slew_limit(SRV_Channel::Aux_servo_function_t func); bool suppress_throttle(void); void channel_function_mixer(SRV_Channel::Aux_servo_function_t func1_in, SRV_Channel::Aux_servo_function_t func2_in, SRV_Channel::Aux_servo_function_t func1_out, SRV_Channel::Aux_servo_function_t func2_out); diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 870a7158fd..b12ae9d4ba 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -22,7 +22,7 @@ /***************************************** * Throttle slew limit *****************************************/ -void Plane::throttle_slew_limit(void) +void Plane::throttle_slew_limit(SRV_Channel::Aux_servo_function_t func) { uint8_t slewrate = aparm.throttle_slewrate; if (control_mode==AUTO) { @@ -34,9 +34,7 @@ void Plane::throttle_slew_limit(void) } // if slew limit rate is set to zero then do not slew limit if (slewrate) { - SRV_Channels::limit_slew_rate(SRV_Channel::k_throttle, slewrate, G_Dt); - SRV_Channels::limit_slew_rate(SRV_Channel::k_throttleLeft, slewrate, G_Dt); - SRV_Channels::limit_slew_rate(SRV_Channel::k_throttleRight, slewrate, G_Dt); + SRV_Channels::limit_slew_rate(func, slewrate, G_Dt); } } @@ -590,6 +588,8 @@ void Plane::servos_twin_engine_mix(void) } else { SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, throttle_left); SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, throttle_right); + throttle_slew_limit(SRV_Channel::k_throttleLeft); + throttle_slew_limit(SRV_Channel::k_throttleRight); } } @@ -675,7 +675,7 @@ void Plane::set_servos(void) quadplane.in_vtol_mode()) { /* only do throttle slew limiting in modes where throttle * control is automatic */ - throttle_slew_limit(); + throttle_slew_limit(SRV_Channel::k_throttle); } if (!arming.is_armed()) {