From 6ba6e11e7b162450db649e95d6e248fb757ad7b4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 1 Mar 2012 23:37:18 +1100 Subject: [PATCH] DCM: added a small amount of accel smoothing to update_DCM_fast() --- libraries/AP_DCM/AP_DCM.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_DCM/AP_DCM.cpp b/libraries/AP_DCM/AP_DCM.cpp index d8bbc708e5..d24426b2c7 100644 --- a/libraries/AP_DCM/AP_DCM.cpp +++ b/libraries/AP_DCM/AP_DCM.cpp @@ -57,7 +57,7 @@ AP_DCM::update_DCM_fast(void) _gyro_vector = _imu->get_gyro(); // Get current values for IMU sensors accel = _imu->get_accel(); - _accel_vector = (_accel_vector * 0.0) + (accel * 1.0); + _accel_vector = (_accel_vector * 0.5) + (accel * 0.5); delta_t = _imu->get_delta_time();