fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-02 05:41:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

AirspeedValidator: keep data stuck test but only enable in FW mode

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2022-09-19 17:44:44 +02:00
parent 1a5d0f4347
commit 13fc5918cc
3 changed files with 10 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/airspeed_selector/AirspeedValidator.cpp
+7 -5
View File
@@ -197,7 +197,9 @@ AirspeedValidator::update_CAS_TAS(float air_pressure_pa, float air_temperature_c
void void
AirspeedValidator::check_airspeed_data_stuck(uint64_t time_now) AirspeedValidator::check_airspeed_data_stuck(uint64_t time_now)
{ {
// data stuck test: trigger when IAS is not changing for DATA_STUCK_TIMEOUT (2s) // Data stuck test: trigger when IAS is not changing for DATA_STUCK_TIMEOUT (2s) when in fixed-wing flight.
// Only consider fixed-wing flight as some airspeed sensors have a very low resolution around 0m/s and
// can output the exact same value for several seconds then.
if (!_data_stuck_check_enabled) { if (!_data_stuck_check_enabled) {
_data_stuck_test_failed = false; _data_stuck_test_failed = false;
@@ -209,7 +211,7 @@ AirspeedValidator::check_airspeed_data_stuck(uint64_t time_now)
_IAS_prev = _IAS; _IAS_prev = _IAS;
} }
_data_stuck_test_failed = hrt_elapsed_time(&_time_last_unequal_data) > DATA_STUCK_TIMEOUT; _data_stuck_test_failed = hrt_elapsed_time(&_time_last_unequal_data) > DATA_STUCK_TIMEOUT && _in_fixed_wing_flight;
} }
void void
@@ -226,15 +228,15 @@ AirspeedValidator::check_airspeed_data_variation(uint64_t time_now)
if (_time_first_data == 0) { if (_time_first_data == 0) {
// init // init
_time_first_data = time_now; _time_first_data = time_now;
_IAS_prev = _IAS; _data_variation_check_ias_prev = _IAS;
} }
if (!_variation_detected && (time_now - _time_first_data < VARIATION_CHECK_TIMEOUT)) { if (!_variation_detected && (time_now - _time_first_data < VARIATION_CHECK_TIMEOUT)) {
if (fabsf(_IAS - _IAS_prev) > FLT_EPSILON) { if (fabsf(_IAS - _data_variation_check_ias_prev) > FLT_EPSILON) {
_variation_detected = true; _variation_detected = true;
} }
_IAS_prev = _IAS; _data_variation_check_ias_prev = _IAS;
} else { } else {
// only update the test_failed flag once the timeout since first data is over // only update the test_failed flag once the timeout since first data is over
src/modules/airspeed_selector/AirspeedValidator.hpp
+1
View File
@@ -151,6 +151,7 @@ private:
bool _data_variation_test_failed{false}; bool _data_variation_test_failed{false};
static constexpr uint64_t VARIATION_CHECK_TIMEOUT{10_s}; ///< timeout to check for data variation after first data is received static constexpr uint64_t VARIATION_CHECK_TIMEOUT{10_s}; ///< timeout to check for data variation after first data is received
hrt_abstime _time_first_data{0}; hrt_abstime _time_first_data{0};
float _data_variation_check_ias_prev{0.f};
bool _variation_detected{false}; bool _variation_detected{false};
// states of innovation check // states of innovation check
src/modules/airspeed_selector/airspeed_selector_params.c
+2 -2
View File
@@ -153,13 +153,13 @@ PARAM_DEFINE_INT32(ASPD_PRIMARY, 1);
* @min 0 * @min 0
* @max 31 * @max 31
* @bit 0 Only data missing check (triggers if more than 1s no data) * @bit 0 Only data missing check (triggers if more than 1s no data)
* @bit 1 Data stuck (triggers if data is exactly constant for 2s) * @bit 1 Data stuck (triggers if data is exactly constant for 2s in FW mode)
* @bit 2 Innovation check (see ASPD_FS_INNOV) * @bit 2 Innovation check (see ASPD_FS_INNOV)
* @bit 3 Load factor check (triggers if measurement is below stall speed) * @bit 3 Load factor check (triggers if measurement is below stall speed)
* @bit 4 Check for data variation in first 10s after sensor connection * @bit 4 Check for data variation in first 10s after sensor connection
* @group Airspeed Validator * @group Airspeed Validator
*/ */
PARAM_DEFINE_INT32(ASPD_DO_CHECKS, 21); PARAM_DEFINE_INT32(ASPD_DO_CHECKS, 23);
/** /**
* Enable fallback to sensor-less airspeed estimation * Enable fallback to sensor-less airspeed estimation