AP_TECS: added reset_throttle_I_cruise()

for resetting integrator during a fwd transition with min throttle
override

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,

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;