From b6420d0b79e61964069272b8878352ab54dafcd7 Mon Sep 17 00:00:00 2001 From: CarlOlsson Date: Mon, 10 Feb 2020 17:14:27 +0100 Subject: [PATCH] ekf: add vehicle at rest to flags --- EKF/common.h | 1 + EKF/ekf_helper.cpp | 2 +- EKF/estimator_interface.cpp | 2 +- EKF/estimator_interface.h | 3 +-- EKF/gps_checks.cpp | 2 +- EKF/mag_fusion.cpp | 2 +- 6 files changed, 6 insertions(+), 6 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index 3458274936..cb3753a54d 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -457,6 +457,7 @@ union filter_control_status_u { uint32_t mag_aligned_in_flight : 1; ///< 23 - true when the in-flight mag field alignment has been completed uint32_t ev_vel : 1; ///< 24 - true when local frame velocity data fusion from external vision measurements is intended uint32_t synthetic_mag_z : 1; ///< 25 - true when we are using a synthesized measurement for the magnetometer Z component + uint32_t vehicle_at_rest : 1; ///< 26 - true when the vehicle is at rest } flags; uint32_t value; }; diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index cd3a96a72a..b8a819cafd 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -1127,7 +1127,7 @@ void Ekf::get_imu_vibe_metrics(float vibe[3]) bool Ekf::get_gps_drift_metrics(float drift[3], bool *blocked) { memcpy(drift, _gps_drift_metrics, 3 * sizeof(float)); - *blocked = !_vehicle_at_rest; + *blocked = !_control_status.flags.vehicle_at_rest; if (_gps_drift_updated) { _gps_drift_updated = false; return true; diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index bbc6ba5a87..f16672c0de 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -66,7 +66,7 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample) // Do not change order of computeVibrationMetric and checkIfVehicleAtRest computeVibrationMetric(); - _vehicle_at_rest = checkIfVehicleAtRest(dt); + _control_status.flags.vehicle_at_rest = checkIfVehicleAtRest(dt); const bool new_downsampled_imu_sample_ready = _imu_down_sampler.update(_newest_high_rate_imu_sample); _imu_updated = new_downsampled_imu_sample_ready; diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index a820277f91..36d527aaa6 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -364,7 +364,7 @@ public: *val = _fault_status.value; } - bool isVehicleAtRest() const { return _vehicle_at_rest; } + bool isVehicleAtRest() const { return _control_status.flags.vehicle_at_rest; } // get GPS check status virtual void get_gps_check_status(uint16_t *val) = 0; @@ -521,7 +521,6 @@ protected: // [0] Horizontal position drift rate (m/s) // [1] Vertical position drift rate (m/s) // [2] Filtered horizontal velocity (m/s) - bool _vehicle_at_rest{false}; // true when the vehicle is at rest uint64_t _time_last_move_detect_us{0}; // timestamp of last movement detection event in microseconds bool _gps_drift_updated{false}; // true when _gps_drift_metrics has been updated and is ready for retrieval diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index e198ffdd8c..fe4db910f0 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -143,7 +143,7 @@ bool Ekf::gps_is_good(const gps_message &gps) // The following checks are only valid when the vehicle is at rest const double lat = gps.lat * 1.0e-7; const double lon = gps.lon * 1.0e-7; - if (!_control_status.flags.in_air && _vehicle_at_rest) { + if (!_control_status.flags.in_air && _control_status.flags.vehicle_at_rest) { // Calculate position movement since last measurement float delta_pos_n = 0.0f; float delta_pos_e = 0.0f; diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index dea3f67f74..da012ee5b2 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -716,7 +716,7 @@ void Ekf::fuseHeading() if (_mag_use_inhibit) { // The magnetometer cannot be trusted but we need to fuse a heading to prevent a badly // conditioned covariance matrix developing over time. - if (!_vehicle_at_rest) { + if (!_control_status.flags.vehicle_at_rest) { // Vehicle is not at rest so fuse a zero innovation and record the // predicted heading to use as an observation when movement ceases. _heading_innov = 0.0f;