fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-20 15:17:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix disable airspeed check with negative ASPD_FS_INTEG (#18186)

Browse Source 
* fix disable airspeed check with negative ASPD_FS_INTEG

* improve logic when nav velocity data is not good

* simplify logic. Reset integrator state when the check is not run.
This commit is contained in:
Thomas Stauber
2021-09-14 12:13:56 +02:00
committed by GitHub
parent e7a90bf367
commit 2b80a6958a
1 changed files with 21 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/airspeed_selector/AirspeedValidator.cpp
+21 -20
View File
@@ -145,34 +145,35 @@ AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_
}
// reset states if we are not flying or wind estimator was just initialized/reset
if (!_in_fixed_wing_flight || (time_now - _time_wind_estimator_initialized) < 10_s) {
if (!_in_fixed_wing_flight || (time_now - _time_wind_estimator_initialized) < 10_s
|| _tas_innov_integ_threshold <= 0.f) {
_innovations_check_failed = false;
_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) {
//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;
_apsd_innov_integ_state = 0.f;
} else {
// nav velocity data is likely good so airspeed innovations are able to be used
// compute the ratio of innovation to gate size
const float dt_s = math::constrain((time_now - _time_last_aspd_innov_check) / 1e6f, 0.01f, 0.2f); // limit to [100,5] Hz
const float tas_test_ratio = _wind_estimator.get_tas_innov() * _wind_estimator.get_tas_innov()
/ (fmaxf(_tas_gate, 1.0f) * fmaxf(_tas_gate, 1.f) * _wind_estimator.get_tas_innov_var());
if ((estimator_status_vel_test_ratio < 1.f) && (estimator_status_mag_test_ratio < 1.f)) {
// nav velocity data is likely good so airspeed innovations are able to be used
// compute the ratio of innovation to gate size
const float tas_test_ratio = _wind_estimator.get_tas_innov() * _wind_estimator.get_tas_innov()
/ (fmaxf(_tas_gate, 1.0f) * fmaxf(_tas_gate, 1.f) * _wind_estimator.get_tas_innov_var());
if (tas_test_ratio > _tas_innov_threshold) {
_apsd_innov_integ_state += dt_s * (tas_test_ratio - _tas_innov_threshold); // integrate exceedance
if (tas_test_ratio > _tas_innov_threshold) {
_apsd_innov_integ_state += dt_s * (tas_test_ratio - _tas_innov_threshold); // integrate exceedance
} else {
// reset integrator used to trigger and record pass if integrator check is disabled
_apsd_innov_integ_state = 0.f;
}
} else {
// reset integrator used to trigger and record pass if integrator check is disabled
_apsd_innov_integ_state = 0.f;
if (_tas_innov_integ_threshold <= 0.f) {
_time_last_tas_pass = time_now;
}
}
if (_tas_innov_integ_threshold > 0.f && _apsd_innov_integ_state < _tas_innov_integ_threshold) {
_time_last_tas_pass = time_now;
}
if (_tas_innov_integ_threshold > 0.f && _apsd_innov_integ_state < _tas_innov_integ_threshold) {
_time_last_tas_pass = time_now;
}
_innovations_check_failed = (time_now - _time_last_tas_pass) > TAS_INNOV_FAIL_DELAY;