AP_TECS: fixed a bug in changes from rate-limited to non-limited airspeed

The calculation of the non-limited airspeed rate demand used the last
non-limited airspeed, whereas it should have used the last adjusted
value. This led to a single frame spike in airspeed demand, which fed
through to a sudden change in pitch integrator.

libraries/AP_TECS/AP_TECS.cpp

@@ -409,28 +409,31 @@ void AP_TECS::_update_speed_demand(void)
     // calculate velocity rate limits based on physical performance limits
     // provision to use a different rate limit if bad descent or underspeed condition exists
     // Use 50% of maximum energy rate to allow margin for total energy contgroller
-    float velRateMax = 0.5f * _STEdot_max / _TAS_state;
-    float velRateMin = 0.5f * _STEdot_min / _TAS_state;
+    const float velRateMax = 0.5f * _STEdot_max / _TAS_state;
+    const float velRateMin = 0.5f * _STEdot_min / _TAS_state;
+    const float TAS_dem_previous = _TAS_dem_adj;
+
+    // assume fixed 10Hz call rate
+    const float dt = 0.1;
 
     // Apply rate limit
-    if ((_TAS_dem - _TAS_dem_adj) > (velRateMax * 0.1f))
+    if ((_TAS_dem - TAS_dem_previous) > (velRateMax * dt))
     {
-        _TAS_dem_adj = _TAS_dem_adj + velRateMax * 0.1f;
+        _TAS_dem_adj = TAS_dem_previous + velRateMax * dt;
         _TAS_rate_dem = velRateMax;
     }
-    else if ((_TAS_dem - _TAS_dem_adj) < (velRateMin * 0.1f))
+    else if ((_TAS_dem - TAS_dem_previous) < (velRateMin * dt))
     {
-        _TAS_dem_adj = _TAS_dem_adj + velRateMin * 0.1f;
+        _TAS_dem_adj = TAS_dem_previous + velRateMin * dt;
         _TAS_rate_dem = velRateMin;
     }
     else
     {
+        _TAS_rate_dem = (_TAS_dem - TAS_dem_previous) / dt;
         _TAS_dem_adj = _TAS_dem;
-        _TAS_rate_dem = (_TAS_dem - _TAS_dem_last) / 0.1f;
     }
     // Constrain speed demand again to protect against bad values on initialisation.
     _TAS_dem_adj = constrain_float(_TAS_dem_adj, _TASmin, _TASmax);
-    _TAS_dem_last = _TAS_dem;
 }
 
 void AP_TECS::_update_height_demand(void)
@@ -908,7 +911,6 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe)
         _hgt_dem_adj       = _hgt_dem_adj_last;
         _hgt_dem_prev      = _hgt_dem_adj_last;
         _hgt_dem_in_old    = _hgt_dem_adj_last;
-        _TAS_dem_last      = _TAS_dem;
         _TAS_dem_adj       = _TAS_dem;
         _flags.underspeed        = false;
         _flags.badDescent        = false;
@@ -922,7 +924,6 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe)
         _hgt_dem_adj_last  = hgt_afe;
         _hgt_dem_adj       = _hgt_dem_adj_last;
         _hgt_dem_prev      = _hgt_dem_adj_last;
-        _TAS_dem_last      = _TAS_dem;
         _TAS_dem_adj       = _TAS_dem;
         _flags.underspeed        = false;
         _flags.badDescent  = false;

libraries/AP_TECS/AP_TECS.h

@@ -222,9 +222,8 @@ private:
     float _TASmax;
     float _TASmin;
 
-    // Current and last true airspeed demand
+    // Current true airspeed demand
     float _TAS_dem;
-    float _TAS_dem_last;
 
     // Equivalent airspeed demand
     float _EAS_dem;