diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index e5bc099229..7b349bc216 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -281,7 +281,6 @@ private: // Ground speed // The amount current ground speed is below min ground speed. meters per second float ground_speed; - int16_t throttle_last; int16_t throttle; // CH7 control @@ -469,7 +468,7 @@ private: void Log_Arm_Disarm(); void load_parameters(void); - void throttle_slew_limit(int16_t last_throttle); + void throttle_slew_limit(void); bool auto_check_trigger(void); bool use_pivot_steering(void); void calc_throttle(float target_speed); diff --git a/APMrover2/Steering.cpp b/APMrover2/Steering.cpp index 2c109cdf9e..b6b48d2581 100644 --- a/APMrover2/Steering.cpp +++ b/APMrover2/Steering.cpp @@ -3,18 +3,12 @@ /***************************************** Throttle slew limit *****************************************/ -void Rover::throttle_slew_limit(int16_t last_throttle) { - // if slew limit rate is set to zero then do not slew limit - if (g.throttle_slewrate && last_throttle != 0) { - // limit throttle change by the given percentage per second - float temp = g.throttle_slewrate * G_Dt * 0.01f * fabsf(channel_throttle->get_radio_max() - channel_throttle->get_radio_min()); - // allow a minimum change of 1 PWM per cycle - if (temp < 1) { - temp = 1; - } - uint16_t pwm; - if (SRV_Channels::get_output_pwm(SRV_Channel::k_throttle, pwm)) { - SRV_Channels::set_output_pwm(SRV_Channel::k_throttle, constrain_int16(pwm, last_throttle - temp, last_throttle + temp)); +void Rover::throttle_slew_limit(void) { + if (g.throttle_slewrate > 0) { + SRV_Channels::limit_slew_rate(SRV_Channel::k_throttle, g.throttle_slewrate, G_Dt); + if (g.skid_steer_out) { + // when skid steering also limit 2nd channel + SRV_Channels::limit_slew_rate(SRV_Channel::k_steering, g.throttle_slewrate, G_Dt); } } } @@ -284,8 +278,6 @@ void Rover::mix_skid_steering(void) Set the flight control servos based on the current calculated values *****************************************/ void Rover::set_servos(void) { - static uint16_t last_throttle; - if (control_mode == MANUAL || control_mode == LEARNING) { // do a direct pass through of radio values SRV_Channels::set_output_pwm(SRV_Channel::k_steering, channel_steer->read()); @@ -325,13 +317,12 @@ void Rover::set_servos(void) { SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0); } } - - // limit throttle movement speed - throttle_slew_limit(last_throttle); } - // record last throttle before we apply skid steering - SRV_Channels::get_output_pwm(SRV_Channel::k_throttle, last_throttle); + if (control_mode != MANUAL && control_mode != LEARNING) { + // limit throttle movement speed + throttle_slew_limit(); + } if (have_skid_steering()) { mix_skid_steering(); diff --git a/APMrover2/system.cpp b/APMrover2/system.cpp index ebfe31315a..e13c5fa4d7 100644 --- a/APMrover2/system.cpp +++ b/APMrover2/system.cpp @@ -293,7 +293,6 @@ void Rover::set_mode(enum mode mode) } control_mode = mode; - throttle_last = 0; throttle = 500; if (!in_auto_reverse) { set_reverse(false);