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

Merge pull request #255 from PX4/pr-ekfBugFix

Browse Source 
EKF: fix bugs preventing preflight checks passing
This commit is contained in:
Paul Riseborough
2017-03-21 07:27:43 +10:00
committed by GitHub
parent 9ddfd66d01 a1ff219380
commit 6d9b556b04
3 changed files with 8 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/covariance.cpp
+1 -1
View File
@@ -737,7 +737,7 @@ void Ekf::fixCovarianceErrors()
// record the pass/fail // record the pass/fail
if (!bad_acc_bias) { if (!bad_acc_bias) {
_fault_status.flags.bad_acc_bias = true; _fault_status.flags.bad_acc_bias = false;
_time_acc_bias_check = _time_last_imu; _time_acc_bias_check = _time_last_imu;
} else { } else {
_fault_status.flags.bad_acc_bias = true; _fault_status.flags.bad_acc_bias = true;
EKF/estimator_interface.cpp
+1 -1
View File
@@ -435,7 +435,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
_time_last_range = 0; _time_last_range = 0;
_time_last_airspeed = 0; _time_last_airspeed = 0;
_time_last_optflow = 0; _time_last_optflow = 0;
memset(&_fault_status.flags, 0, sizeof(_fault_status.flags)); _fault_status.value = 0;
_time_last_ext_vision = 0; _time_last_ext_vision = 0;
return true; return true;
} }
EKF/gps_checks.cpp
+6 -5
View File
@@ -57,11 +57,12 @@
bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps) bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps)
{ {
// If we have defined the WGS-84 position of the NED origin, run gps quality checks until they pass, then define the origins WGS-84 position using the last GPS fix // Run GPS checks whenever the WGS-84 origin is not set or the vehicle is not using GPS
if (!_NED_origin_initialised) { // Also run checks if the vehicle is on-ground as the check data can be used by vehicle pre-flight checks
// we have good GPS data so can now set the origin's WGS-84 position if (!_control_status.flags.in_air || !_NED_origin_initialised || !_control_status.flags.gps) {
if (gps_is_good(gps) && !_NED_origin_initialised) { bool gps_checks_pass = gps_is_good(gps);
// Set the origin's WGS-84 position to the last gps fix if (!_NED_origin_initialised && gps_checks_pass) {
// If we have good GPS data set the origin's WGS-84 position to the last gps fix
double lat = gps->lat / 1.0e7; double lat = gps->lat / 1.0e7;
double lon = gps->lon / 1.0e7; double lon = gps->lon / 1.0e7;
map_projection_init_timestamped(&_pos_ref, lat, lon, _time_last_imu); map_projection_init_timestamped(&_pos_ref, lat, lon, _time_last_imu);