diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index 604a21287f14d..07ae5c1deb398 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -799,6 +799,8 @@ void AP_TECS::_update_throttle_with_airspeed(void) _last_throttle_dem = _throttle_dem; } + _throttle_dem_pre_integ = _throttle_dem; + // Sum the components. _throttle_dem = _throttle_dem + _integTHR_state; @@ -914,6 +916,10 @@ void AP_TECS::_update_throttle_without_airspeed(int16_t throttle_nudge, float pi float cosPhi = sqrtf((rotMat.a.y*rotMat.a.y) + (rotMat.b.y*rotMat.b.y)); float STEdot_dem = _rollComp * (1.0f/constrain_float(cosPhi * cosPhi, 0.1f, 1.0f) - 1.0f); _throttle_dem = _throttle_dem + STEdot_dem / (_STEdot_max - _STEdot_min) * (_THRmaxf - _THRminf); + + // integrator not used without airspeed, but store pre-integrator + // value anyway in case we enable airspeed + _throttle_dem_pre_integ = _throttle_dem; } void AP_TECS::_detect_bad_descent(void) @@ -1197,6 +1203,16 @@ void AP_TECS::_update_STE_rate_lim(void) _STEdot_neg_max = - _maxSinkRate * GRAVITY_MSS; } +// reset throttle integrator to give trim throttle +// used when overriding throttle +void AP_TECS::reset_throttle_I_cruise(void) +{ + // get the last throttle output without the integrator + const float nomThr = aparm.throttle_cruise * 0.01f; + // set integrator so that we would have gotten cruise throttle + _integTHR_state = nomThr - _throttle_dem_pre_integ; +} + void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, int32_t EAS_dem_cm, diff --git a/libraries/AP_TECS/AP_TECS.h b/libraries/AP_TECS/AP_TECS.h index fcd3fe97f9bfe..63bd9fa8c79eb 100644 --- a/libraries/AP_TECS/AP_TECS.h +++ b/libraries/AP_TECS/AP_TECS.h @@ -98,6 +98,9 @@ class AP_TECS { _integTHR_state = 0.0; } + // reset throttle integrator to give trim throttle + void reset_throttle_I_cruise(void); + // return landing sink rate float get_land_sinkrate(void) const { return _land_sink; @@ -215,6 +218,9 @@ class AP_TECS { // throttle demand in the range from -1.0 to 1.0, usually positive unless reverse thrust is enabled via _THRminf < 0 float _throttle_dem; + // pre-integrator throttle demand + float _throttle_dem_pre_integ; + // pitch angle demand in radians float _pitch_dem;