From 00872fcaa35f5ba298b555b4be5e16bee77cdc3b Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Tue, 18 Feb 2020 13:20:52 -0500
Subject: [PATCH] EKF: ImuDownSampler pass imuSample as const reference

 - inline ImuDownSampler::getDownSampledImuAndTriggerReset()
 - minor formatting and initialization cleanup
---
 EKF/imu_down_sampler.cpp | 16 +++-------------
 EKF/imu_down_sampler.hpp | 24 ++++++++++++++----------
 2 files changed, 17 insertions(+), 23 deletions(-)

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};
 };