Hey guys! I have a question to ask related to the integrator windup in tecs (integTHR_state).
My plane (which is powered) will climb to a “glide altitude”, after which it will enter a glide state with thr_max being set to a low throttle value (and with tecs_spdweight set to 2). After it reaches a minimum altitude it will go back to its regular mode (tecs_spdweight set to 1) and throttle max being set to 100. Is there any way to reset or mitigate an integrator windup? Would setting the desired airspeed and altitude to its current value help? Is there any params I could use (I currently see tecs_thr_damp, tecs_time_const, trim_throttle and tecs_integ_gain)?
Would putting thr_max = 0, and thr_min = desired value help in the sense that integTHR_state would not be touched during the glide as the is_glididng flag would be put to true?
} else if (_flags.is_gliding) {
_throttle_dem = 0.0f;
} else {
// Calculate gain scaler from specific energy error to throttle
// (_STEdot_max - _STEdot_min) / (_THRmaxf - _THRminf) is the derivative of STEdot wrt throttle measured across the max allowed throttle range.
const float K_STE2Thr = 1 / (timeConstant() * (_STEdot_max - _STEdot_min) / (_THRmaxf - _THRminf));
// Calculate feed-forward throttle
const float nomThr = aparm.throttle_cruise * 0.01f;
const Matrix3f &rotMat = _ahrs.get_rotation_body_to_ned();
// Use the demanded rate of change of total energy as the feed-forward demand, but add
// additional component which scales with (1/cos(bank angle) - 1) to compensate for induced
// drag increase during turns.
const float cosPhi = sqrtf((rotMat.a.y*rotMat.a.y) + (rotMat.b.y*rotMat.b.y));
STEdot_dem = STEdot_dem + _rollComp * (1.0f/constrain_float(cosPhi * cosPhi, 0.1f, 1.0f) - 1.0f);
const float ff_throttle = nomThr + STEdot_dem / (_STEdot_max - _STEdot_min) * (_THRmaxf - _THRminf);
// Calculate PD + FF throttle
float throttle_damp = _thrDamp;
if (_flags.is_doing_auto_land && !is_zero(_land_throttle_damp)) {
throttle_damp = _land_throttle_damp;
}
_throttle_dem = (_STE_error + STEdot_error * throttle_damp) * K_STE2Thr + ff_throttle;
float THRminf_clipped_to_zero = constrain_float(_THRminf, 0, _THRmaxf);
// Calculate integrator state upper and lower limits
// Set to a value that will allow 0.1 (10%) throttle saturation to allow for noise on the demand
// Additionally constrain the integrator state amplitude so that the integrator comes off limits faster.
const float maxAmp = 0.5f*(_THRmaxf - THRminf_clipped_to_zero);
const float integ_max = constrain_float((_THRmaxf - _throttle_dem + 0.1f),-maxAmp,maxAmp);
const float integ_min = constrain_float((_THRminf - _throttle_dem - 0.1f),-maxAmp,maxAmp);
// GCS_SEND_TEXT(MAV_SEVERITY_INFO, "integ_max : %f", integ_max);
// GCS_SEND_TEXT(MAV_SEVERITY_INFO, "integ_min : %f", integ_min);
// GCS_SEND_TEXT(MAV_SEVERITY_INFO, "STE_error : %f", _STE_error);
// Calculate integrator state, constraining state
// Set integrator to a max throttle value during climbout
_integTHR_state = _integTHR_state + (_STE_error * _get_i_gain()) * _DT * K_STE2Thr;
if (_flight_stage == AP_FixedWing::FlightStage::TAKEOFF || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) {
if (!_flags.reached_speed_takeoff) {
// ensure we run at full throttle until we reach the target airspeed
_throttle_dem = MAX(_throttle_dem, _THRmaxf - _integTHR_state);
}
_integTHR_state = integ_max;
}
else {
_integTHR_state = constrain_float(_integTHR_state, integ_min, integ_max);
}
I would really appreciate any help or advice, thanks in advance!