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

AirspeedValidator: disable innovation checks if lpos invalid

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2023-05-08 14:58:23 +02:00 committed by Daniel Agar
parent 1e4fcfc614
commit 4cd078409d
2 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/airspeed_selector/AirspeedValidator.cpp
View File

@ -59,7 +59,7 @@ AirspeedValidator::update_airspeed_validator(const airspeed_validator_update_dat
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.ground_velocity, input_data.lpos_valid);
update_airspeed_valid_status(input_data.timestamp);
}
@ -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)
float estimator_status_mag_test_ratio, const matrix::Vector3f &vI, bool lpos_valid)
{
// Check normalised innovation levels with requirement for continuous data and use of hysteresis
// to prevent false triggering.
@ -228,7 +228,7 @@ AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_
_time_last_tas_pass = time_now;
_apsd_innov_integ_state = 0.f;
} else if (estimator_status_vel_test_ratio > 1.f || estimator_status_mag_test_ratio > 1.f) {
} else if (!lpos_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
_time_last_tas_pass = time_now;

2
src/modules/airspeed_selector/AirspeedValidator.hpp
View File

@ -185,7 +185,7 @@ private:
void update_CAS_TAS(float air_pressure_pa, float air_temperature_celsius);
void check_airspeed_data_stuck(uint64_t timestamp);
void check_airspeed_innovation(uint64_t timestamp, float estimator_status_vel_test_ratio,
float estimator_status_mag_test_ratio, const matrix::Vector3f &vI);
float estimator_status_mag_test_ratio, const matrix::Vector3f &vI, bool lpos_valid);
void check_load_factor(float accel_z);
void update_airspeed_valid_status(const uint64_t timestamp);
void reset();