Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_MotorsUGV: throttle slew updates motor limit flags

Browse Source 
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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
APMrover2/AP_MotorsUGV.cpp
View File

@ -187,11 +187,12 @@ void AP_MotorsUGV::output(bool armed, float dt)
armed = false; 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) // 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); set_limits_from_input(armed, _steering, _throttle);
// slew limit throttle
slew_limit_throttle(dt);
// output for regular steering/throttle style frames // output for regular steering/throttle style frames
output_regular(armed, _steering, _throttle); output_regular(armed, _steering, _throttle);
@ -203,7 +204,6 @@ void AP_MotorsUGV::output(bool armed, float dt)
SRV_Channels::cork(); SRV_Channels::cork();
SRV_Channels::output_ch_all(); SRV_Channels::output_ch_all();
SRV_Channels::push(); SRV_Channels::push();
_last_throttle = _throttle;
} }
// output to regular steering and throttle channels // 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) void AP_MotorsUGV::slew_limit_throttle(float dt)
{ {
if (_slew_rate > 0) { if (_slew_rate > 0) {
float temp = _slew_rate * dt * 0.01f * (_throttle_max - _throttle_min); // slew throttle
if (temp < 1.0f) { const float throttle_change_max = MAX(1.0f, _slew_rate * dt * 0.01f * (_throttle_max - _throttle_min));
temp = 1.0f; 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 // set limits based on steering and throttle input

2
APMrover2/AP_MotorsUGV.h
View File

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