Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 10:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

DCM: re-work the DCM to use the new IMU/ADC features

Browse Source 
this gives us higher resolution DCM calculations, with much more
accurate timing of the update delta time.
This commit is contained in:
Andrew Tridgell 2011-09-15 13:04:08 +10:00
parent bb35fdec10
commit d96fbf8acf
4 changed files with 17 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_DCM/AP_DCM.cpp
View File

@ -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

4
libraries/AP_DCM/AP_DCM.h
View File

@ -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);

2
libraries/AP_DCM/AP_DCM_HIL.cpp
View File

@ -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

2
libraries/AP_DCM/AP_DCM_HIL.h
View File

@ -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; }