fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 07:43:34 +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 21:46:45 -04:00
committed by Daniel Agar
parent 1e4fcfc614
commit 4cd078409d
2 changed files with 4 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/airspeed_selector/AirspeedValidator.cpp
+3 -3
View File
@@ -59,7 +59,7 @@ AirspeedValidator::update_airspeed_validator(const airspeed_validator_update_dat
check_airspeed_data_stuck(input_data.timestamp); check_airspeed_data_stuck(input_data.timestamp);
check_load_factor(input_data.accel_z); check_load_factor(input_data.accel_z);
check_airspeed_innovation(input_data.timestamp, input_data.vel_test_ratio, input_data.mag_test_ratio, 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); update_airspeed_valid_status(input_data.timestamp);
} }
@@ -212,7 +212,7 @@ AirspeedValidator::check_airspeed_data_stuck(uint64_t time_now)
void void
AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_status_vel_test_ratio, 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 // Check normalised innovation levels with requirement for continuous data and use of hysteresis
// to prevent false triggering. // to prevent false triggering.
@@ -228,7 +228,7 @@ AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_
_time_last_tas_pass = time_now; _time_last_tas_pass = time_now;
_apsd_innov_integ_state = 0.f; _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 //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 //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; _time_last_tas_pass = time_now;
src/modules/airspeed_selector/AirspeedValidator.hpp
+1 -1
View File
@@ -185,7 +185,7 @@ private:
void update_CAS_TAS(float air_pressure_pa, float air_temperature_celsius); void update_CAS_TAS(float air_pressure_pa, float air_temperature_celsius);
void check_airspeed_data_stuck(uint64_t timestamp); void check_airspeed_data_stuck(uint64_t timestamp);
void check_airspeed_innovation(uint64_t timestamp, float estimator_status_vel_test_ratio, 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 check_load_factor(float accel_z);
void update_airspeed_valid_status(const uint64_t timestamp); void update_airspeed_valid_status(const uint64_t timestamp);
void reset(); void reset();