AP_MotorsUGV: throttle slew updates motor limit flags

this ensure I-term build-up doesn't happen when the slew
causes the throttle to react slowly
This commit is contained in:
Randy Mackay 2017-11-22 09:43:30 +09:00
parent b724884344
commit 78275ec4a1
2 changed files with 13 additions and 8 deletions

View File

@ -187,11 +187,12 @@ void AP_MotorsUGV::output(bool armed, float dt)
armed = false;
}
slew_limit_throttle(dt);
// clear and set limits based on input (limit flags may be set again by output_regular or output_skid_steering methods)
set_limits_from_input(armed, _steering, _throttle);
// slew limit throttle
slew_limit_throttle(dt);
// output for regular steering/throttle style frames
output_regular(armed, _steering, _throttle);
@ -203,7 +204,6 @@ void AP_MotorsUGV::output(bool armed, float dt)
SRV_Channels::cork();
SRV_Channels::output_ch_all();
SRV_Channels::push();
_last_throttle = _throttle;
}
// output to regular steering and throttle channels
@ -354,12 +354,17 @@ void AP_MotorsUGV::output_throttle(SRV_Channel::Aux_servo_function_t function, f
void AP_MotorsUGV::slew_limit_throttle(float dt)
{
if (_slew_rate > 0) {
float temp = _slew_rate * dt * 0.01f * (_throttle_max - _throttle_min);
if (temp < 1.0f) {
temp = 1.0f;
// slew throttle
const float throttle_change_max = MAX(1.0f, _slew_rate * dt * 0.01f * (_throttle_max - _throttle_min));
if (_throttle > _throttle_prev + throttle_change_max) {
_throttle = _throttle_prev + throttle_change_max;
limit.throttle_upper = true;
} else if (_throttle < _throttle_prev - throttle_change_max) {
_throttle = _throttle_prev - throttle_change_max;
limit.throttle_lower = true;
}
_throttle = constrain_int16(_throttle, _last_throttle - temp, _last_throttle + temp);
}
_throttle_prev = _throttle;
}
// set limits based on steering and throttle input

View File

@ -101,5 +101,5 @@ protected:
// internal variables
float _steering; // requested steering as a value from -4500 to +4500
float _throttle; // requested throttle as a value from -100 to 100
float _last_throttle;
float _throttle_prev; // throttle input from previous iteration
};