Merge pull request PX4#10 from PX4/delta_ang_bias_fix

fix delta angle bias usage:
TSC21 · Jan 14, 2016 · 2c80614 · 2c80614
2 parents 749156d + 0b5c905
commit 2c80614
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 12 deletions.
diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp
@@ -232,14 +232,14 @@ void Ekf::calculateOutputStates()
 {
 	imuSample imu_new = _imu_sample_new;
 	Vector3f delta_angle;
-	delta_angle(0) = imu_new.delta_ang(0) * _state.gyro_scale(0);
-	delta_angle(1) = imu_new.delta_ang(1) * _state.gyro_scale(1);
-	delta_angle(2) = imu_new.delta_ang(2) * _state.gyro_scale(2);
 
-	delta_angle -= _state.gyro_bias;
+	// Note: We do no not need to consider any bias or scale correction here
+	// since the base class has already corrected the imu sample
+	delta_angle(0) = imu_new.delta_ang(0);
+	delta_angle(1) = imu_new.delta_ang(1);
+	delta_angle(2) = imu_new.delta_ang(2);
 
 	Vector3f delta_vel = imu_new.delta_vel;
-	delta_vel(2) -= _state.accel_z_bias;
 
 	delta_angle += _delta_angle_corr;
 	Quaternion dq;
@@ -265,9 +265,7 @@ void Ekf::calculateOutputStates()
 		_imu_updated = false;
 	}
 
-	if (!_output_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_output_sample_delayed)) {
-		return;
-	}
+	_output_sample_delayed = _output_buffer.get_oldest();
 
 	Quaternion quat_inv = _state.quat_nominal.inversed();
 	Quaternion q_error =  _output_sample_delayed.quat_nominal * quat_inv;

diff --git a/EKF/estimator_base.cpp b/EKF/estimator_base.cpp
@@ -83,14 +83,15 @@ void EstimatorBase::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64
 
 	imu_sample_new.time_us = time_usec;
 
-	_imu_sample_new = imu_sample_new;
-
 	imu_sample_new.delta_ang(0) = imu_sample_new.delta_ang(0) * _state.gyro_scale(0);
 	imu_sample_new.delta_ang(1) = imu_sample_new.delta_ang(1) * _state.gyro_scale(1);
 	imu_sample_new.delta_ang(2) = imu_sample_new.delta_ang(2) * _state.gyro_scale(2);
 
-	imu_sample_new.delta_ang -= _state.gyro_bias;
-	imu_sample_new.delta_vel(2) -= _state.accel_z_bias;
+	imu_sample_new.delta_ang -= _state.gyro_bias * imu_sample_new.delta_ang_dt / (_dt_imu_avg > 0 ? _dt_imu_avg : 0.01f);
+	imu_sample_new.delta_vel(2) -= _state.accel_z_bias * imu_sample_new.delta_vel_dt / (_dt_imu_avg > 0 ? _dt_imu_avg : 0.01f);;
+
+	// store the new sample for the complementary filter prediciton
+	_imu_sample_new = imu_sample_new;
 
 	_imu_down_sampled.delta_ang_dt += imu_sample_new.delta_ang_dt;
 	_imu_down_sampled.delta_vel_dt += imu_sample_new.delta_vel_dt;