From d96fbf8acff099098886297ba64b2c8137ca8165 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 15 Sep 2011 13:04:08 +1000 Subject: [PATCH] DCM: re-work the DCM to use the new IMU/ADC features this gives us higher resolution DCM calculations, with much more accurate timing of the update delta time. --- libraries/AP_DCM/AP_DCM.cpp | 18 +++++++++++++----- libraries/AP_DCM/AP_DCM.h | 4 ++-- libraries/AP_DCM/AP_DCM_HIL.cpp | 2 +- libraries/AP_DCM/AP_DCM_HIL.h | 2 +- 4 files changed, 17 insertions(+), 9 deletions(-) diff --git a/libraries/AP_DCM/AP_DCM.cpp b/libraries/AP_DCM/AP_DCM.cpp index b6dcaaf4b1..4cbcdfeb02 100644 --- a/libraries/AP_DCM/AP_DCM.cpp +++ b/libraries/AP_DCM/AP_DCM.cpp @@ -10,7 +10,7 @@ version 2.1 of the License, or (at your option) any later version. Methods: - update_DCM(_G_Dt) : Updates the AHRS by integrating the rotation matrix over time _G_Dt using the IMU object data + update_DCM() : Updates the AHRS by integrating the rotation matrix over time using the IMU object data get_gyro() : Returns gyro vector corrected for bias get_accel() : Returns accelerometer vector get_dcm_matrix() : Returns dcm matrix @@ -41,13 +41,17 @@ AP_DCM::set_compass(Compass *compass) /**************************************************/ void -AP_DCM::update_DCM_fast(float _G_Dt) +AP_DCM::update_DCM_fast(void) { + float delta_t; + _imu->update(); _gyro_vector = _imu->get_gyro(); // Get current values for IMU sensors _accel_vector = _imu->get_accel(); // Get current values for IMU sensors - matrix_update(_G_Dt); // Integrate the DCM matrix + delta_t = _imu->get_delta_time(); + + matrix_update(delta_t); // Integrate the DCM matrix switch(_toggle++){ case 0: @@ -82,13 +86,17 @@ AP_DCM::update_DCM_fast(float _G_Dt) /**************************************************/ void -AP_DCM::update_DCM(float _G_Dt) +AP_DCM::update_DCM(void) { + float delta_t; + _imu->update(); _gyro_vector = _imu->get_gyro(); // Get current values for IMU sensors _accel_vector = _imu->get_accel(); // Get current values for IMU sensors - matrix_update(_G_Dt); // Integrate the DCM matrix + delta_t = _imu->get_delta_time(); + + matrix_update(delta_t); // Integrate the DCM matrix normalize(); // Normalize the DCM matrix drift_correction(); // Perform drift correction euler_angles(); // Calculate pitch, roll, yaw for stabilization and navigation diff --git a/libraries/AP_DCM/AP_DCM.h b/libraries/AP_DCM/AP_DCM.h index 8e8af7cb36..503f2ab1fa 100644 --- a/libraries/AP_DCM/AP_DCM.h +++ b/libraries/AP_DCM/AP_DCM.h @@ -49,8 +49,8 @@ public: void set_compass(Compass *compass); // Methods - void update_DCM(float _G_Dt); - void update_DCM_fast(float _G_Dt); + void update_DCM(void); + void update_DCM_fast(void); float get_health(void); diff --git a/libraries/AP_DCM/AP_DCM_HIL.cpp b/libraries/AP_DCM/AP_DCM_HIL.cpp index 2597d9eb58..66adbbee7e 100644 --- a/libraries/AP_DCM/AP_DCM_HIL.cpp +++ b/libraries/AP_DCM/AP_DCM_HIL.cpp @@ -8,7 +8,7 @@ version 2.1 of the License, or (at your option) any later version. Methods: - update_DCM(_G_Dt) : Updates the AHRS by integrating the rotation matrix over time _G_Dt using the IMU object data + update_DCM() : Updates the AHRS by integrating the rotation matrix over time using the IMU object data get_gyro() : Returns gyro vector corrected for bias get_accel() : Returns accelerometer vector get_dcm_matrix() : Returns dcm matrix diff --git a/libraries/AP_DCM/AP_DCM_HIL.h b/libraries/AP_DCM/AP_DCM_HIL.h index 447f4476f6..071370afa7 100644 --- a/libraries/AP_DCM/AP_DCM_HIL.h +++ b/libraries/AP_DCM/AP_DCM_HIL.h @@ -31,7 +31,7 @@ public: void set_compass(Compass *compass) {} // Methods - void update_DCM(float _G_Dt) {} + void update_DCM(void) {} float get_health(void) { return 1.0; }