fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-07 18:00:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

AirspeedSelector: only update with lpos if coming from GNSS (#23268)

Browse Source 
Compared to GNSS, alternate position observation methods are less accurate
and thus generally not good enough to do airspeed validation with.
Airspeed validation is thus disabled if no GNSS fusion is happening.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2024-06-14 17:10:08 +02:00
committed by GitHub
parent c0d6b67633
commit f646f1ba98
3 changed files with 22 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/airspeed_selector/AirspeedValidator.cpp
+9 -9
View File
@@ -50,16 +50,16 @@ AirspeedValidator::update_airspeed_validator(const airspeed_validator_update_dat
// get indicated airspeed from input data (raw airspeed)
_IAS = input_data.airspeed_indicated_raw;
update_CAS_scale_validated(input_data.lpos_valid, input_data.ground_velocity, input_data.airspeed_true_raw);
update_CAS_scale_validated(input_data.gnss_valid, input_data.ground_velocity, input_data.airspeed_true_raw);
update_CAS_scale_applied();
update_CAS_TAS(input_data.air_pressure_pa, input_data.air_temperature_celsius);
update_wind_estimator(input_data.timestamp, input_data.airspeed_true_raw, input_data.lpos_valid,
update_wind_estimator(input_data.timestamp, input_data.airspeed_true_raw, input_data.gnss_valid,
input_data.ground_velocity, input_data.lpos_evh, input_data.lpos_evv, input_data.q_att);
update_in_fixed_wing_flight(input_data.in_fixed_wing_flight);
check_airspeed_data_stuck(input_data.timestamp);
check_load_factor(input_data.accel_z);
check_airspeed_innovation(input_data.timestamp, input_data.vel_test_ratio, input_data.mag_test_ratio,
input_data.ground_velocity, input_data.lpos_valid);
input_data.ground_velocity, input_data.gnss_valid);
update_airspeed_valid_status(input_data.timestamp);
}
@@ -71,12 +71,12 @@ AirspeedValidator::reset_airspeed_to_invalid(const uint64_t timestamp)
}
void
AirspeedValidator::update_wind_estimator(const uint64_t time_now_usec, float airspeed_true_raw, bool lpos_valid,
AirspeedValidator::update_wind_estimator(const uint64_t time_now_usec, float airspeed_true_raw, bool gnss_valid,
const matrix::Vector3f &vI, float lpos_evh, float lpos_evv, const Quatf &q_att)
{
_wind_estimator.update(time_now_usec);
if (lpos_valid && _in_fixed_wing_flight) {
if (gnss_valid && _in_fixed_wing_flight) {
// airspeed fusion (with raw TAS)
const float hor_vel_variance = lpos_evh * lpos_evh;
@@ -109,9 +109,9 @@ AirspeedValidator::get_wind_estimator_states(uint64_t timestamp)
}
void
AirspeedValidator::update_CAS_scale_validated(bool lpos_valid, const matrix::Vector3f &vI, float airspeed_true_raw)
AirspeedValidator::update_CAS_scale_validated(bool gnss_valid, const matrix::Vector3f &vI, float airspeed_true_raw)
{
if (!_in_fixed_wing_flight || !lpos_valid) {
if (!_in_fixed_wing_flight || !gnss_valid) {
return;
}
@@ -212,7 +212,7 @@ AirspeedValidator::check_airspeed_data_stuck(uint64_t time_now)
void
AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_status_vel_test_ratio,
float estimator_status_mag_test_ratio, const matrix::Vector3f &vI, bool lpos_valid)
float estimator_status_mag_test_ratio, const matrix::Vector3f &vI, bool gnss_valid)
{
// Check normalised innovation levels with requirement for continuous data and use of hysteresis
// to prevent false triggering.
@@ -226,7 +226,7 @@ AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_
_innovations_check_failed = false;
_aspd_innov_integ_state = 0.f;
} else if (!lpos_valid || estimator_status_vel_test_ratio > 1.f || estimator_status_mag_test_ratio > 1.f) {
} else if (!gnss_valid || estimator_status_vel_test_ratio > 1.f || estimator_status_mag_test_ratio > 1.f) {
//nav velocity data is likely not good
//don't run the test but don't reset the check if it had previously failed when nav velocity data was still likely good
_aspd_innov_integ_state = 0.f;