Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_TECS: prevent large glitches in the specific energy integrator 

this tries to prevent problems like in issue#4066, or at least
reducing the impact when it happens
This commit is contained in:
Andrew Tridgell 2016-05-13 15:46:27 +10:00
parent c52451b01b
commit 9a6ac77c64
1 changed files with 12 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_TECS/AP_TECS.cpp
View File

@ -788,7 +788,7 @@ void AP_TECS::_update_pitch(void)
{
integSEB_input = MAX(integSEB_input, _PITCHminf - _pitch_dem);
}
_integSEB_state = _integSEB_state + integSEB_input * _DT;
float integSEB_delta = integSEB_input * _DT;
#if 0
if (_flight_stage == FLIGHT_LAND_FINAL && fabsf(_climb_rate) > 0.2f) {
@ -820,7 +820,17 @@ void AP_TECS::_update_pitch(void)
if (_flight_stage == AP_TECS::FLIGHT_TAKEOFF || _flight_stage == AP_TECS::FLIGHT_LAND_ABORT) {
temp += _PITCHminf * gainInv;
}
_integSEB_state = constrain_float(_integSEB_state, (gainInv * (_PITCHminf - 0.0783f)) - temp, (gainInv * (_PITCHmaxf + 0.0783f)) - temp);
float integSEB_min = (gainInv * (_PITCHminf - 0.0783f)) - temp;
float integSEB_max = (gainInv * (_PITCHmaxf + 0.0783f)) - temp;
float integSEB_range = integSEB_max - integSEB_min;
// don't allow the integrator to rise by more than 20% of its full
// range in one step. This prevents single value glitches from
// causing massive integrator changes. See Issue#4066
integSEB_delta = constrain_float(integSEB_delta, -integSEB_range*0.1f, integSEB_range*0.1f);
// integrate
_integSEB_state = constrain_float(_integSEB_state + integSEB_delta, integSEB_min, integSEB_max);
// Calculate pitch demand from specific energy balance signals
_pitch_dem_unc = (temp + _integSEB_state) / gainInv;