From a78b00513ba2fb2c9be038e6087cca702e33d1b4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 1 Mar 2012 22:52:47 +1100 Subject: [PATCH] DCM: added a small amount of gyro and accel smoothing possibly not needed, but convenient to be able to test with different values --- libraries/AP_DCM/AP_DCM.cpp | 25 +++++++++++++++++-------- libraries/AP_DCM/AP_DCM.h | 2 ++ 2 files changed, 19 insertions(+), 8 deletions(-) diff --git a/libraries/AP_DCM/AP_DCM.cpp b/libraries/AP_DCM/AP_DCM.cpp index d338e7577c..ac53ca4dfd 100644 --- a/libraries/AP_DCM/AP_DCM.cpp +++ b/libraries/AP_DCM/AP_DCM.cpp @@ -51,10 +51,13 @@ void AP_DCM::update_DCM_fast(void) { float delta_t; + Vector3f accel; _imu->update(); _gyro_vector = _imu->get_gyro(); // Get current values for IMU sensors - _accel_vector = _imu->get_accel(); // Get current values for IMU sensors + + accel = _imu->get_accel(); + _accel_vector = (_accel_vector * 0.0) + (accel * 1.0); delta_t = _imu->get_delta_time(); @@ -96,10 +99,13 @@ void AP_DCM::update_DCM(void) { float delta_t; + Vector3f accel; _imu->update(); _gyro_vector = _imu->get_gyro(); // Get current values for IMU sensors - _accel_vector = _imu->get_accel(); // Get current values for IMU sensors + + accel = _imu->get_accel(); + _accel_vector = (_accel_vector * 0.5) + (accel * 0.5); delta_t = _imu->get_delta_time(); @@ -133,7 +139,8 @@ AP_DCM::matrix_update(float _G_Dt) Matrix3f temp_matrix; _omega_integ_corr = _gyro_vector + _omega_I; // Used for _centripetal correction (theoretically better than _omega) - _omega = _omega_integ_corr + _omega_P; // Equation 16, adding proportional and integral correction terms + _omega = _omega_integ_corr + _omega_P; // Equation 16, adding proportional and integral correction terms + _omega_smoothed = (_omega_smoothed * 0.5) + (_omega_integ_corr * 0.5); if(_centripetal && _gps != NULL && @@ -179,16 +186,17 @@ AP_DCM::matrix_update(float _G_Dt) void AP_DCM::accel_adjust(void) { - Vector3f veloc, temp; + Vector3f temp; + float veloc; - veloc.x = _gps->ground_speed / 100; // We are working with acceleration in m/s^2 units + veloc = _gps->ground_speed / 100; // We are working with acceleration in m/s^2 units // We are working with a modified version of equation 26 as our IMU object reports acceleration in the positive axis direction as positive - //_accel_vector -= _omega_integ_corr % _veloc; // Equation 26 This line is giving the compiler a problem so we break it up below + // _accel_vector -= _omega_integ_corr % veloc; // Equation 26 This line is giving the compiler a problem so we break it up below temp.x = 0; - temp.y = _omega_integ_corr.z * veloc.x; // only computing the non-zero terms - temp.z = -1.0f * _omega_integ_corr.y * veloc.x; // After looking at the compiler issue lets remove _veloc and simlify + temp.y = _omega_smoothed.z * veloc; // only computing the non-zero terms + temp.z = _omega_smoothed.y * (-veloc); // After looking at the compiler issue lets remove _veloc and simlify _accel_vector -= temp; } @@ -210,6 +218,7 @@ AP_DCM::matrix_reset(bool recover_eulers) _omega_I.z = 0.0f; _omega_P = _omega_I; _omega_integ_corr = _omega_I; + _omega_smoothed = _omega_I; _omega = _omega_I; // if the caller wants us to try to recover to the current diff --git a/libraries/AP_DCM/AP_DCM.h b/libraries/AP_DCM/AP_DCM.h index adcb331901..5294b8a06c 100644 --- a/libraries/AP_DCM/AP_DCM.h +++ b/libraries/AP_DCM/AP_DCM.h @@ -126,11 +126,13 @@ private: Matrix3f _dcm_matrix; Vector3f _accel_vector; // Store the acceleration in a vector + Vector3f _accel_smoothed; Vector3f _gyro_vector; // Store the gyros turn rate in a vector Vector3f _omega_P; // Omega Proportional correction Vector3f _omega_I; // Omega Integrator correction Vector3f _omega_integ_corr; // Partially corrected Gyro_Vector data - used for centrepetal correction Vector3f _omega; // Corrected Gyro_Vector data + Vector3f _omega_smoothed; float _health; bool _centripetal; uint8_t _toggle;