fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-14 03:53:35 +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;
src/lib/airspeed_validator/AirspeedValidator.hpp
+1 -2
View File
@@ -134,8 +134,7 @@ private:
float _tas_innov_integ_threshold{-1.0}; ///< integrator innovation error threshold for triggering innovation check failure
uint64_t _time_last_aspd_innov_check{0}; ///< time airspeed innovation was last checked (uSec)
uint64_t _time_last_tas_pass{0}; ///< last time innovation checks passed
uint64_t _time_last_tas_fail{0}; ///< last time innovation checks failed
float _apsd_innov_integ_state{0.0f}; ///< inegral of excess normalised airspeed innovation (sec)
float _apsd_innov_integ_state{0.0f}; ///< integral of excess normalised airspeed innovation (sec)
static constexpr uint64_t TAS_INNOV_FAIL_DELAY{1_s}; ///< time required for innovation levels to pass or fail (usec)
uint64_t _time_wind_estimator_initialized{0}; ///< time last time wind estimator was initialized (uSec)