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

Merge pull request #122 from CarlOlsson/airspeed_fix

Browse Source 
fixed_airspeed_healthy_bug
This commit is contained in:
Paul Riseborough
2016-05-09 10:24:22 +10:00
parent e06c0f41d7 ee7d7aeb8a
commit 3cdf56e57c
2 changed files with 8 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/airspeed_fusion.cpp
+7 -4
View File
@@ -124,9 +124,9 @@ void Ekf::fuseAirspeed()
Kfusion[23] = SK_TAS[0]*(P[23][4]*SH_TAS[2] - P[23][22]*SH_TAS[2] + P[23][5]*SK_TAS[1] - P[23][23]*SK_TAS[1] + P[23][6]*vd*SH_TAS[0]); Kfusion[23] = SK_TAS[0]*(P[23][4]*SH_TAS[2] - P[23][22]*SH_TAS[2] + P[23][5]*SK_TAS[1] - P[23][23]*SK_TAS[1] + P[23][6]*vd*SH_TAS[0]);
// calculate measurement innovation // Calculate measurement innovation
_airspeed_innov = v_tas_pred - _airspeed_innov = v_tas_pred -
_airspeed_sample_delayed.true_airspeed; // This is TAS, maybe we should indicate that in some way _airspeed_sample_delayed.true_airspeed;
// Calculate the innovation variance // Calculate the innovation variance
_airspeed_innov_var = 1.0f / SK_TAS[0]; _airspeed_innov_var = 1.0f / SK_TAS[0];
@@ -134,13 +134,16 @@ void Ekf::fuseAirspeed()
// Compute the ratio of innovation to gate size // Compute the ratio of innovation to gate size
_tas_test_ratio = sq(_airspeed_innov) / (sq(fmaxf(_params.tas_innov_gate, 1.0f)) * _airspeed_innov_var); _tas_test_ratio = sq(_airspeed_innov) / (sq(fmaxf(_params.tas_innov_gate, 1.0f)) * _airspeed_innov_var);
// if the innocation consistency check fails then don't fuse the sample and indicate bad airspeed health // If the innovation consistency check fails then don't fuse the sample and indicate bad airspeed health
if (_tas_test_ratio > 1.0f) { if (_tas_test_ratio > 1.0f) {
_airspeed_healthy = false; _airspeed_healthy = false;
return; return;
} }
else {
_airspeed_healthy = true;
}
// airspeed measurement sample has passed check so record it // Airspeed measurement sample has passed check so record it
_time_last_arsp_fuse = _time_last_imu; _time_last_arsp_fuse = _time_last_imu;
// update covariance matrix via Pnew = (I - KH)P = P - KHP // update covariance matrix via Pnew = (I - KH)P = P - KHP
EKF/estimator_interface.h
+1 -1
View File
@@ -268,7 +268,7 @@ protected:
struct map_projection_reference_s _pos_ref = {}; // Contains WGS-84 position latitude and longitude (radians) struct map_projection_reference_s _pos_ref = {}; // Contains WGS-84 position latitude and longitude (radians)
bool _mag_healthy; // computed by mag innovation test bool _mag_healthy; // computed by mag innovation test
float _airspeed_healthy; // computed by airspeed innovation test bool _airspeed_healthy; // computed by airspeed innovation test
float _yaw_test_ratio; // yaw innovation consistency check ratio float _yaw_test_ratio; // yaw innovation consistency check ratio
float _mag_test_ratio[3]; // magnetometer XYZ innovation consistency check ratios float _mag_test_ratio[3]; // magnetometer XYZ innovation consistency check ratios
float _vel_pos_test_ratio[6]; // velocity and position innovation consistency check ratios float _vel_pos_test_ratio[6]; // velocity and position innovation consistency check ratios