fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-06 04:10:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

AirspeedValidator: check_airspeed_innovation() check absolute innovations

Browse Source 
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
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/wind_estimator/WindEstimator.hpp
+3 -11
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; }