diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index d49a48699e..c25eb5c532 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/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; diff --git a/libraries/AP_TECS/AP_TECS.h b/libraries/AP_TECS/AP_TECS.h index 9e1d678d70..737f2a0166 100644 --- a/libraries/AP_TECS/AP_TECS.h +++ b/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;