fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 02:03:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

AirspeedValidator: change logic for innovation checks

Browse Source 
Previously, the logic was not consistent with the description in the params.
It now, if the integral is enabled, declares innovation check failed if
integral is above threshold.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2021-01-22 14:16:22 +01:00
parent 0c2099e788
commit 98d057b8c9
2 changed files with 16 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/airspeed_validator/AirspeedValidator.cpp
+15 -23
View File
@@ -144,46 +144,38 @@ AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_
_time_wind_estimator_initialized = time_now;
}
// reset states if we are not flying
if (!_in_fixed_wing_flight) {
// 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) {
_innovations_check_failed = false;
_time_last_tas_pass = time_now;
_time_last_tas_fail = 0;
} else {
const float dt_s = math::max((time_now - _time_last_aspd_innov_check) / 1e6f, 0.01f); // limit to 100Hz
const float dt_s = math::constrain((time_now - _time_last_aspd_innov_check) / 1e6f, 0.01f, 0.2f); // limit to [100,5] Hz
if (dt_s < 1.0f) {
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.0f) * _wind_estimator.get_tas_innov_var());
/ (fmaxf(_tas_gate, 1.0f) * fmaxf(_tas_gate, 1.f) * _wind_estimator.get_tas_innov_var());
if (tas_test_ratio <= _tas_innov_threshold) {
// record pass and reset integrator used to trigger
_time_last_tas_pass = time_now;
_apsd_innov_integ_state = 0.0f;
if (tas_test_ratio > _tas_innov_threshold) {
_apsd_innov_integ_state += dt_s * (tas_test_ratio - _tas_innov_threshold); // integrate exceedance
} else {
// integrate exceedance
_apsd_innov_integ_state += dt_s * (tas_test_ratio - _tas_innov_threshold);
}
// reset integrator used to trigger and record pass if integrator check is disabled
_apsd_innov_integ_state = 0.f;
if ((estimator_status_vel_test_ratio < 1.0f) && (estimator_status_mag_test_ratio < 1.0f)) {
// nav velocity data is likely good so airspeed innovations are able to be used
if ((_tas_innov_integ_threshold > 0.0f) && (_apsd_innov_integ_state > _tas_innov_integ_threshold)) {
_time_last_tas_fail = time_now;
if (_tas_innov_integ_threshold <= 0.f) {
_time_last_tas_pass = time_now;
}
}
if (!_innovations_check_failed) {
_innovations_check_failed = (time_now - _time_last_tas_pass) > TAS_INNOV_FAIL_DELAY;
} else {
_innovations_check_failed = ((time_now - _time_last_tas_fail) < TAS_INNOV_FAIL_DELAY * 100)
|| (time_now - _time_wind_estimator_initialized) < TAS_INNOV_FAIL_DELAY * 100;
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;
}
_time_last_aspd_innov_check = time_now;