diff --git a/EKF/imu_down_sampler.cpp b/EKF/imu_down_sampler.cpp index 1c39c7fc4e..e09eec51e6 100644 --- a/EKF/imu_down_sampler.cpp +++ b/EKF/imu_down_sampler.cpp @@ -1,20 +1,15 @@ #include "imu_down_sampler.hpp" -ImuDownSampler::ImuDownSampler(float target_dt_sec) : _target_dt{target_dt_sec}, _imu_collection_time_adj{0.0f} { - reset(); - _imu_down_sampled.time_us = 0.0f; -} - -ImuDownSampler::~ImuDownSampler() {} +ImuDownSampler::ImuDownSampler(float target_dt_sec) : _target_dt{target_dt_sec} { reset(); } // integrate imu samples until target dt reached // assumes that dt of the gyroscope is close to the dt of the accelerometer // returns true if target dt is reached -bool ImuDownSampler::update(imuSample imu_sample_new) { - +bool ImuDownSampler::update(const imuSample &imu_sample_new) { if (_do_reset) { reset(); } + // accumulate time deltas _imu_down_sampled.delta_ang_dt += imu_sample_new.delta_ang_dt; _imu_down_sampled.delta_vel_dt += imu_sample_new.delta_vel_dt; @@ -51,11 +46,6 @@ bool ImuDownSampler::update(imuSample imu_sample_new) { } } -imuSample ImuDownSampler::getDownSampledImuAndTriggerReset() { - _do_reset = true; - return _imu_down_sampled; -} - void ImuDownSampler::reset() { _imu_down_sampled.delta_ang.setZero(); _imu_down_sampled.delta_vel.setZero(); diff --git a/EKF/imu_down_sampler.hpp b/EKF/imu_down_sampler.hpp index 3409cf1309..d61e7fe9b3 100644 --- a/EKF/imu_down_sampler.hpp +++ b/EKF/imu_down_sampler.hpp @@ -46,18 +46,22 @@ using namespace estimator; class ImuDownSampler { public: - ImuDownSampler(float target_dt_sec); - ~ImuDownSampler(); + explicit ImuDownSampler(float target_dt_sec); + ~ImuDownSampler() = default; - bool update(imuSample imu_sample_new); - imuSample getDownSampledImuAndTriggerReset(); + bool update(const imuSample &imu_sample_new); + + imuSample getDownSampledImuAndTriggerReset() { + _do_reset = true; + return _imu_down_sampled; + } private: - imuSample _imu_down_sampled; - Quatf _delta_angle_accumulated; - const float _target_dt; // [sec] - float _imu_collection_time_adj; - bool _do_reset; - void reset(); + + imuSample _imu_down_sampled{}; + Quatf _delta_angle_accumulated{}; + const float _target_dt; // [sec] + float _imu_collection_time_adj{0.f}; + bool _do_reset{true}; };