AP_TECS: Add /0 protection

This commit is contained in:
Paul Riseborough 2024-02-21 18:56:18 +11:00 committed by Andrew Tridgell
parent 846eaecc0b
commit aecbd116e7
1 changed files with 4 additions and 2 deletions

View File

@ -542,8 +542,10 @@ void AP_TECS::_update_height_demand(void)
_hgt_dem_rate_ltd = _hgt_dem_rate_ltd - _sink_rate_limit * _DT; _hgt_dem_rate_ltd = _hgt_dem_rate_ltd - _sink_rate_limit * _DT;
_sink_fraction = 1.0f; _sink_fraction = 1.0f;
} else { } else {
if (is_negative(hgt_dem - _hgt_dem_rate_ltd)) { const float numerator = hgt_dem - _hgt_dem_rate_ltd;
_sink_fraction = (hgt_dem - _hgt_dem_rate_ltd) / (-_sink_rate_limit * _DT); const float denominator = - _sink_rate_limit * _DT;
if (is_negative(numerator) && is_negative(denominator)) {
_sink_fraction = numerator / denominator;
} else { } else {
_sink_fraction = 0.0f; _sink_fraction = 0.0f;
} }