dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

AirspeedValidator: check_airspeed_innovation() check absolute innovations 

Do no longer use tas_innovation from wind estimator and test ratio, but calculate
the innovation  based on wind estimate, TAS measurement (including currently applied scale)
and ground velocity. Use innovations directly to trigger failure.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2021-10-12 23:38:15 +02:00
parent f6d37ecacf
commit a2faac148f
5 changed files with 37 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
src/lib/wind_estimator/WindEstimator.hpp
View File

@ -61,17 +61,13 @@ public:
const matrix::Vector2f &velIvar);
void fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const matrix::Quatf &q_att);
void get_wind(float wind[2])
{
wind[0] = _state(INDEX_W_N);
wind[1] = _state(INDEX_W_E);
}
bool is_estimate_valid() { return _initialised; }
bool check_if_meas_is_rejected(uint64_t time_now, float innov, float innov_var, uint8_t gate_size,
uint64_t &time_meas_rejected, bool &reinit_filter);
matrix::Vector2f get_wind() { return matrix::Vector2f{_state(INDEX_W_N), _state(INDEX_W_E)}; }
// invert scale (CAS = IAS * scale), protect agains division by 0, constrain to [0.1, 10]
float get_tas_scale() { return math::constrain(1.f / (_state(INDEX_TAS_SCALE) + 0.0001f), 0.1f, 10.f); }
float get_tas_scale_var() { return _P(2, 2); }
@ -79,11 +75,7 @@ public:
float get_tas_innov_var() { return _tas_innov_var; }
float get_beta_innov() { return _beta_innov; }
float get_beta_innov_var() { return _beta_innov_var; }
void get_wind_var(float wind_var[2])
{
wind_var[0] = _P(0, 0);
wind_var[1] = _P(1, 1);
}
matrix::Vector2f get_wind_var() { return matrix::Vector2f{_P(0, 0), _P(1, 1)}; }
bool get_wind_estimator_reset() { return _wind_estimator_reset; }
void set_wind_p_noise(float wind_sigma) { _wind_p_var = wind_sigma * wind_sigma; }

31
src/modules/airspeed_selector/AirspeedValidator.cpp
View File

@ -57,8 +57,9 @@ AirspeedValidator::update_airspeed_validator(const airspeed_validator_update_dat
input_data.ground_velocity, input_data.lpos_evh, input_data.lpos_evv, input_data.att_q);
update_in_fixed_wing_flight(input_data.in_fixed_wing_flight);
check_airspeed_data_stuck(input_data.timestamp);
check_airspeed_innovation(input_data.timestamp, input_data.vel_test_ratio, input_data.mag_test_ratio);
check_load_factor(input_data.accel_z);
check_airspeed_innovation(input_data.timestamp, input_data.vel_test_ratio, input_data.mag_test_ratio,
input_data.ground_velocity);
update_airspeed_valid_status(input_data.timestamp);
}
@ -95,14 +96,10 @@ AirspeedValidator::get_wind_estimator_states(uint64_t timestamp)
airspeed_wind_s wind_est = {};
wind_est.timestamp = timestamp;
float wind[2];
_wind_estimator.get_wind(wind);
wind_est.windspeed_north = wind[0];
wind_est.windspeed_east = wind[1];
float wind_cov[2];
_wind_estimator.get_wind_var(wind_cov);
wind_est.variance_north = wind_cov[0];
wind_est.variance_east = wind_cov[1];
wind_est.windspeed_north = _wind_estimator.get_wind()(0);
wind_est.windspeed_east = _wind_estimator.get_wind()(1);
wind_est.variance_north = _wind_estimator.get_wind_var()(0);
wind_est.variance_east = _wind_estimator.get_wind_var()(1);
wind_est.tas_innov = _wind_estimator.get_tas_innov();
wind_est.tas_innov_var = _wind_estimator.get_tas_innov_var();
wind_est.beta_innov = _wind_estimator.get_beta_innov();
@ -217,7 +214,7 @@ AirspeedValidator::check_airspeed_data_stuck(uint64_t time_now)
void
AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_status_vel_test_ratio,
float estimator_status_mag_test_ratio)
float estimator_status_mag_test_ratio, matrix::Vector3f vI)
{
// Check normalised innovation levels with requirement for continuous data and use of hysteresis
// to prevent false triggering.
@ -227,7 +224,7 @@ 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) < 5_s
|| _tas_innov_integ_threshold <= 0.f) {
_innovations_check_failed = false;
_time_last_tas_pass = time_now;
@ -241,13 +238,13 @@ AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_
} 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());
const float dt_s = math::constrain((time_now - _time_last_aspd_innov_check) / 1e6f, 0.01f, 0.2f); // limit to [5,100] Hz
matrix::Vector2f wind_2d(_wind_estimator.get_wind());
matrix::Vector3f air_vel = vI - matrix::Vector3f {wind_2d(0), wind_2d(1), 0.f};
const float tas_innov = abs(_TAS - air_vel.norm());
if (tas_test_ratio > _tas_innov_threshold) {
_apsd_innov_integ_state += dt_s * (tas_test_ratio - _tas_innov_threshold); // integrate exceedance
if (tas_innov > _tas_innov_threshold) {
_apsd_innov_integ_state += dt_s * (tas_innov - _tas_innov_threshold); // integrate exceedance
} else {
// reset integrator used to trigger and record pass if integrator check is disabled

2
src/modules/airspeed_selector/AirspeedValidator.hpp
View File

@ -178,7 +178,7 @@ private:
void update_CAS_TAS(float air_pressure_pa, float air_temperature_celsius);
void check_airspeed_data_stuck(uint64_t timestamp);
void check_airspeed_innovation(uint64_t timestamp, float estimator_status_vel_test_ratio,
float estimator_status_mag_test_ratio);
float estimator_status_mag_test_ratio, matrix::Vector3f vI);
void check_load_factor(float accel_z);
void update_airspeed_valid_status(const uint64_t timestamp);
void reset();

12
src/modules/airspeed_selector/airspeed_selector_main.cpp
View File

@ -505,14 +505,10 @@ void AirspeedModule::update_wind_estimator_sideslip()
}
_wind_estimate_sideslip.timestamp = _time_now_usec;
float wind[2];
_wind_estimator_sideslip.get_wind(wind);
_wind_estimate_sideslip.windspeed_north = wind[0];
_wind_estimate_sideslip.windspeed_east = wind[1];
float wind_cov[2];
_wind_estimator_sideslip.get_wind_var(wind_cov);
_wind_estimate_sideslip.variance_north = wind_cov[0];
_wind_estimate_sideslip.variance_east = wind_cov[1];
_wind_estimate_sideslip.windspeed_north = _wind_estimator_sideslip.get_wind()(0);
_wind_estimate_sideslip.windspeed_east = _wind_estimator_sideslip.get_wind()(1);
_wind_estimate_sideslip.variance_north = _wind_estimator_sideslip.get_wind_var()(0);
_wind_estimate_sideslip.variance_east = _wind_estimator_sideslip.get_wind_var()(1);
_wind_estimate_sideslip.tas_innov = NAN;
_wind_estimate_sideslip.tas_innov_var = NAN;
_wind_estimate_sideslip.beta_innov = _wind_estimator_sideslip.get_beta_innov();

30
src/modules/airspeed_selector/airspeed_selector_params.c
View File

@ -167,34 +167,34 @@ PARAM_DEFINE_INT32(ASPD_DO_CHECKS, 1);
PARAM_DEFINE_INT32(ASPD_FALLBACK_GW, 0);
/**
* Airspeed failsafe consistency threshold
* Airspeed failure innovation threshold
*
* This specifies the minimum airspeed test ratio required to trigger a failsafe. Larger values make the check less sensitive,
* smaller values make it more sensitive. Start with a value of 1.0 when tuning. When tas_test_ratio is > 1.0 it indicates the
* inconsistency between predicted and measured airspeed is large enough to cause the wind EKF to reject airspeed measurements.
* This specifies the minimum airspeed innovation required to trigger a failsafe. Larger values make the check less sensitive,
* smaller values make it more sensitive. Large innovations indicate an inconsistency between predicted (groundspeed - windspeeed)
* and measured airspeed.
* The time required to detect a fault when the threshold is exceeded depends on the size of the exceedance and is controlled by the ASPD_FS_INTEG parameter.
*
*
* @unit m/s
* @min 0.5
* @max 3.0
* @max 10.0
* @decimal 1
* @group Airspeed Validator
*/
PARAM_DEFINE_FLOAT(ASPD_FS_INNOV, 1.0f);
PARAM_DEFINE_FLOAT(ASPD_FS_INNOV, 5.f);
/**
* Airspeed failsafe consistency delay
* Airspeed failure innovation integral threshold
*
* This sets the time integral of airspeed test ratio exceedance above ASPD_FS_INNOV required to trigger a failsafe.
* For example if ASPD_FS_INNOV is 1 and estimator_status.tas_test_ratio is 2.0, then the exceedance is 1.0 and the integral will
* rise at a rate of 1.0/second. A negative value disables the check. Larger positive values make the check less sensitive, smaller positive values
* make it more sensitive.
* This sets the time integral of airspeed innovation exceedance above ASPD_FS_INNOV required to trigger a failsafe.
* Larger values make the check less sensitive, smaller positive values make it more sensitive.
*
* @unit s
* @max 30.0
* @unit m
* @min 0.0
* @max 50.0
* @decimal 1
* @group Airspeed Validator
*/
PARAM_DEFINE_FLOAT(ASPD_FS_INTEG, 5.0f);
PARAM_DEFINE_FLOAT(ASPD_FS_INTEG, 10.f);
/**
* Airspeed failsafe stop delay