From 2426f1dd3a50e1658157ff6fa28a88854c7dc4b3 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 18 Mar 2017 08:57:49 +1100 Subject: [PATCH 1/3] EKF: remove use of memset to initialise variable When using a union of flags and integer value it is safer to initialise the value to 0 rather than memset the flags because the flags may not define all bits in the integer. --- EKF/estimator_interface.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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; } From ed2938c8a40357f1711ee3fd7760289f5849ae5e Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 18 Mar 2017 11:29:25 +1100 Subject: [PATCH 2/3] EKF: fix bug causing bad accel bias status to latch --- EKF/covariance.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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; From a1ff219380587801d7b795e1ad65bfc308d58a73 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 18 Mar 2017 17:40:18 +1100 Subject: [PATCH 3/3] EKF: Extend range of conditions over which we run GPS quality checks Previously GPS quality checks were only run until the EKF origin was set. This meant that they could not be used by other pre-flight checks. This change ensures that checks will always be run when the vehicle on-ground or not using GPS to enable use by external preflight checks. --- EKF/gps_checks.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) 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);