From bf48004fb9118bd90794bd121a97166839363e90 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 21 Oct 2019 11:12:14 +0200 Subject: [PATCH] ekf: move "reset" part of the init function into a separate function add getter for the vehicle at rest flag --- EKF/ekf.cpp | 9 +++++++-- EKF/ekf.h | 3 +++ EKF/estimator_interface.h | 3 +++ 3 files changed, 13 insertions(+), 2 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 85d9511ef3..5a5cf76f7f 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -47,6 +47,13 @@ bool Ekf::init(uint64_t timestamp) { bool ret = initialise_interface(timestamp); + reset(timestamp); + + return ret; +} + +void Ekf::reset(uint64_t timestamp) +{ _state.vel.setZero(); _state.pos.setZero(); _state.gyro_bias.setZero(); @@ -94,8 +101,6 @@ bool Ekf::init(uint64_t timestamp) _accel_mag_filt = 0.0f; _ang_rate_mag_filt = 0.0f; _prev_dvel_bias_var.zero(); - - return ret; } bool Ekf::update() diff --git a/EKF/ekf.h b/EKF/ekf.h index ddf6cdeb92..cea8166fae 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -54,6 +54,9 @@ public: // initialise variables to sane values (also interface class) bool init(uint64_t timestamp) override; + // set the internal states and status to their default value + void reset(uint64_t timestamp) override; + // should be called every time new data is pushed into the filter bool update() override; diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 3c7d6501c1..79cacdef97 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -59,6 +59,7 @@ public: virtual ~EstimatorInterface() = default; virtual bool init(uint64_t timestamp) = 0; + virtual void reset(uint64_t timestamp) = 0; virtual bool update() = 0; // gets the innovations of velocity and position measurements @@ -357,6 +358,8 @@ public: *val = _fault_status.value; } + bool isVehicleAtRest() const { return _vehicle_at_rest; } + // get GPS check status virtual void get_gps_check_status(uint16_t *val) = 0;