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

EKF: Add reporting of bad accel status

Browse Source
This commit is contained in:
Paul Riseborough
2017-04-07 08:47:13 +10:00
parent 2f2ac5be43
commit f0bbbc8dc6
2 changed files with 2 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/common.h
+1
View File
@@ -506,6 +506,7 @@ union ekf_solution_status {
uint16_t pred_pos_horiz_rel : 1; // 8 - True if the EKF has sufficient data to enter a mode that will provide a (relative) position estimate uint16_t pred_pos_horiz_rel : 1; // 8 - True if the EKF has sufficient data to enter a mode that will provide a (relative) position estimate
uint16_t pred_pos_horiz_abs : 1; // 9 - True if the EKF has sufficient data to enter a mode that will provide a (absolute) position estimate uint16_t pred_pos_horiz_abs : 1; // 9 - True if the EKF has sufficient data to enter a mode that will provide a (absolute) position estimate
uint16_t gps_glitch : 1; // 10 - True if the EKF has detected a GPS glitch uint16_t gps_glitch : 1; // 10 - True if the EKF has detected a GPS glitch
uint16_t accel_error : 1; // 11 - True if the EKF has detected bad accelerometer data
} flags; } flags;
uint16_t value; uint16_t value;
}; };
EKF/ekf_helper.cpp
+1
View File
@@ -862,6 +862,7 @@ void Ekf::get_ekf_soln_status(uint16_t *status)
bool gps_pos_innov_bad = (_vel_pos_test_ratio[3] > 1.0f) || (_vel_pos_test_ratio[4] > 1.0f); bool gps_pos_innov_bad = (_vel_pos_test_ratio[3] > 1.0f) || (_vel_pos_test_ratio[4] > 1.0f);
bool mag_innov_good = (_mag_test_ratio[0] < 1.0f) && (_mag_test_ratio[1] < 1.0f) && (_mag_test_ratio[2] < 1.0f) && (_yaw_test_ratio < 1.0f); bool mag_innov_good = (_mag_test_ratio[0] < 1.0f) && (_mag_test_ratio[1] < 1.0f) && (_mag_test_ratio[2] < 1.0f) && (_yaw_test_ratio < 1.0f);
soln_status.flags.gps_glitch = (gps_vel_innov_bad || gps_pos_innov_bad) && mag_innov_good; soln_status.flags.gps_glitch = (gps_vel_innov_bad || gps_pos_innov_bad) && mag_innov_good;
soln_status.flags.accel_error = _bad_vert_accel_detected;
*status = soln_status.value; *status = soln_status.value;
} }