Skip to content

Commit

Permalink
TECS: in _calcPitchControlOutput guard against invalid airspeed inputs
Browse files Browse the repository at this point in the history
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
  • Loading branch information
sfuhrer authored and MaEtUgR committed Dec 12, 2023
1 parent 9db86f7 commit 91ab09e
Showing 1 changed file with 8 additions and 1 deletion.
9 changes: 8 additions & 1 deletion src/lib/tecs/TECS.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -481,8 +481,15 @@ void TECSControl::_calcPitchControlUpdate(float dt, const Input &input, const Co
float TECSControl::_calcPitchControlOutput(const Input &input, const ControlValues &seb_rate, const Param &param,
const Flag &flag) const
{
float airspeed_for_seb_rate = param.equivalent_airspeed_trim;

// avoid division by zero by checking if airspeed is finite and greater than zero
if (flag.airspeed_enabled && PX4_ISFINITE(input.tas) && input.tas > FLT_EPSILON) {
airspeed_for_seb_rate = input.tas;
}

// Calculate derivative from change in climb angle to rate of change of specific energy balance
const float climb_angle_to_SEB_rate = input.tas * CONSTANTS_ONE_G;
const float climb_angle_to_SEB_rate = airspeed_for_seb_rate * CONSTANTS_ONE_G;

// Calculate a specific energy correction that doesn't include the integrator contribution
float SEB_rate_correction = _getControlError(seb_rate) * param.pitch_damping_gain +
Expand Down

0 comments on commit 91ab09e

Please sign in to comment.