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

AirspeedValidator: Simplify data missing check

Browse Source 
The data_stopped check should be independent of the innovation
and load factor check.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2021-01-22 14:16:22 +01:00
parent 083cd6f1c1
commit 7132e674d6
1 changed files with 6 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/airspeed_validator/AirspeedValidator.cpp
+6 -9
View File
@@ -212,18 +212,15 @@ AirspeedValidator::check_load_factor(float accel_z)
void void
AirspeedValidator::update_airspeed_valid_status(const uint64_t timestamp) AirspeedValidator::update_airspeed_valid_status(const uint64_t timestamp)
{ {
// Check if sensor data is missing - assume a minimum 5Hz data rate.
const bool data_missing = (timestamp - _time_last_airspeed) > 200_ms;
// Declare data stopped if not received for longer than 1 second // Declare data stopped if not received for longer than 1 second
_data_stopped_failed = (timestamp - _time_last_airspeed) > 1_s; _data_stopped_failed = (timestamp - _time_last_airspeed) > 1_s;
if (_innovations_check_failed || _load_factor_check_failed || data_missing) { if (_innovations_check_failed || _load_factor_check_failed) {
// either innovation, load factor or data missing check failed, so declare airspeed failing and record timestamp // either innovation or load factor check failed, so record timestamp
_time_checks_failed = timestamp; _time_checks_failed = timestamp;
} else if (!_innovations_check_failed && !_load_factor_check_failed && !data_missing) { } else if (!_innovations_check_failed && !_load_factor_check_failed) {
// All checks must pass to declare airspeed good // both innovation or load factor checks must pass to declare airspeed good
_time_checks_passed = timestamp; _time_checks_passed = timestamp;
} }
@@ -232,7 +229,7 @@ AirspeedValidator::update_airspeed_valid_status(const uint64_t timestamp)
// airspeed measurement fault has developed, so a fault should be declared immediately // airspeed measurement fault has developed, so a fault should be declared immediately
const bool both_checks_failed = (_innovations_check_failed && _load_factor_check_failed); const bool both_checks_failed = (_innovations_check_failed && _load_factor_check_failed);
// Because the innovation, load factor and data missing checks are subject to short duration false positives // Because the innovation and load factor checks are subject to short duration false positives
// a timeout period is applied. // a timeout period is applied.
const bool single_check_fail_timeout = (timestamp - _time_checks_passed) > _checks_fail_delay * 1_s; const bool single_check_fail_timeout = (timestamp - _time_checks_passed) > _checks_fail_delay * 1_s;
@@ -241,7 +238,7 @@ AirspeedValidator::update_airspeed_valid_status(const uint64_t timestamp)
_airspeed_valid = false; _airspeed_valid = false;
} }
} else if ((timestamp - _time_checks_failed) > _checks_clear_delay * 1_s) { } else if (!_data_stopped_failed && (timestamp - _time_checks_failed) > _checks_clear_delay * 1_s) {
_airspeed_valid = true; _airspeed_valid = true;
} }
} }