Skip to content

Commit

Permalink
ekf2: send airspeed data to ekf backend regardless of sign
Browse files Browse the repository at this point in the history
On ground the airspeed can sometimes be slightly negative but the ekf
should still know that airspeed data is flowing regularly
  • Loading branch information
bresch committed Aug 13, 2024
1 parent db8781e commit 2ac3642
Showing 1 changed file with 8 additions and 3 deletions.
11 changes: 8 additions & 3 deletions src/modules/ekf2/EKF2.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -2076,14 +2076,19 @@ void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps)
if (_airspeed_validated_sub.update(&airspeed_validated)) {

if (PX4_ISFINITE(airspeed_validated.true_airspeed_m_s)
&& PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s)
&& (airspeed_validated.calibrated_airspeed_m_s > 0.f)
&& (airspeed_validated.selected_airspeed_index > 0)
) {
const float cas2tas = 1.f;

if (PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s)
&& (airspeed_validated.calibrated_airspeed_m_s > FLT_EPSILON)) {
cas2tas = airspeed_validated.true_airspeed_m_s / airspeed_validated.calibrated_airspeed_m_s;
}

airspeedSample airspeed_sample {
.time_us = airspeed_validated.timestamp,
.true_airspeed = airspeed_validated.true_airspeed_m_s,
.eas2tas = airspeed_validated.true_airspeed_m_s / airspeed_validated.calibrated_airspeed_m_s,
.eas2tas = cas2tas,
};
_ekf.setAirspeedData(airspeed_sample);
}
Expand Down

0 comments on commit 2ac3642

Please sign in to comment.