From a779d45cd0e2f888983c96d7fe2af2f68529076c Mon Sep 17 00:00:00 2001
From: bugobliterator <siddharthbharatpurohit@gmail.com>
Date: Fri, 29 Jan 2016 23:45:50 -0800
Subject: [PATCH] ekf2: fix bug causing faulty imu data storage

---
 EKF/ekf.cpp            | 3 +--
 EKF/estimator_base.cpp | 3 +--
 2 files changed, 2 insertions(+), 4 deletions(-)

diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp
index 7d76aed542..09f8aaddf3 100644
--- a/EKF/ekf.cpp
+++ b/EKF/ekf.cpp
@@ -87,7 +87,6 @@ bool Ekf::update()
 			return false;
 		}
 	}
-
 	//printStates();
 	//printStatesFast();
 	// prediction
@@ -289,7 +288,7 @@ bool Ekf::collect_imu(imuSample &imu)
 
 	if ((_dt_imu_avg * _imu_ticks >= (float)(FILTER_UPDATE_PERRIOD_MS) / 1000 && _start_predict_enabled) || 
 		_dt_imu_avg * _imu_ticks >= 0.02f){
-		_imu_sample_new = {
+		imu = {
 			delta_ang		: _q_down_sampled.to_axis_angle(),
 			delta_vel		: _imu_down_sampled.delta_vel,
 			delta_ang_dt	: _imu_down_sampled.delta_ang_dt,
diff --git a/EKF/estimator_base.cpp b/EKF/estimator_base.cpp
index 1996fb6b56..dfff630dd9 100644
--- a/EKF/estimator_base.cpp
+++ b/EKF/estimator_base.cpp
@@ -74,8 +74,6 @@ void EstimatorBase::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64
 	if (_time_last_imu > 0) {
 		_dt_imu_avg = 0.8f * _dt_imu_avg + 0.2f * dt;
 	}
-	delta_ang_dt = delta_ang_dt / 1e6f;
-	delta_vel_dt = delta_vel_dt / 1e6f;
 
 	// copy data
 	imuSample imu_sample_new = {};
@@ -88,6 +86,7 @@ void EstimatorBase::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64
 	imu_sample_new.time_us = time_usec;
 	_imu_ticks++;
 
+
 	if (collect_imu(imu_sample_new)) {
 		_imu_buffer.push(imu_sample_new);
 		_imu_ticks = 0;