diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 08452e09eb..f17ce2e681 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -737,7 +737,7 @@ void Ekf::fixCovarianceErrors() // record the pass/fail 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; } else { _fault_status.flags.bad_acc_bias = true; diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 3e3e317f8e..6545edc45b 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -435,7 +435,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) _time_last_range = 0; _time_last_airspeed = 0; _time_last_optflow = 0; - memset(&_fault_status.flags, 0, sizeof(_fault_status.flags)); + _fault_status.value = 0; _time_last_ext_vision = 0; return true; } diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index 9a96a8af40..94478c379c 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -57,11 +57,12 @@ 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 - if (!_NED_origin_initialised) { - // we have good GPS data so can now set the origin's WGS-84 position - if (gps_is_good(gps) && !_NED_origin_initialised) { - // Set the origin's WGS-84 position to the last gps fix + // Run GPS checks whenever the WGS-84 origin is not set or the vehicle is not using GPS + // Also run checks if the vehicle is on-ground as the check data can be used by vehicle pre-flight checks + if (!_control_status.flags.in_air || !_NED_origin_initialised || !_control_status.flags.gps) { + bool gps_checks_pass = gps_is_good(gps); + 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 lon = gps->lon / 1.0e7; map_projection_init_timestamped(&_pos_ref, lat, lon, _time_last_imu);