Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

removed redundant FP calcs for speed up. 

added ability to alternate normalization and drift correction. Not fully implemented, needs further testing.


git-svn-id: https://arducopter.googlecode.com/svn/trunk@2691 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-06-26 22:54:08 +00:00
parent 4247586584
commit 418a4b3903
2 changed files with 24 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
libraries/AP_DCM/AP_DCM.cpp
View File

@ -48,8 +48,13 @@ AP_DCM::update_DCM(float _G_Dt)
_accel_vector = _imu->get_accel(); // 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 matrix_update(_G_Dt); // Integrate the DCM matrix
normalize(); // Normalize the DCM matrix
drift_correction(); // Perform drift correction //if(_toggle){
normalize(); // Normalize the DCM matrix
//}else{
drift_correction(); // Perform drift correction
//}
//_toggle = !_toggle;
euler_angles(); // Calculate pitch, roll, yaw for stabilization and navigation euler_angles(); // Calculate pitch, roll, yaw for stabilization and navigation
} }
@ -78,7 +83,7 @@ AP_DCM::matrix_update(float _G_Dt)
Matrix3f temp_matrix; Matrix3f temp_matrix;
//Record when you saturate any of the gyros. //Record when you saturate any of the gyros.
if((fabs(_gyro_vector.x) >= radians(300)) || if( (fabs(_gyro_vector.x) >= radians(300)) ||
(fabs(_gyro_vector.y) >= radians(300)) || (fabs(_gyro_vector.y) >= radians(300)) ||
(fabs(_gyro_vector.z) >= radians(300))){ (fabs(_gyro_vector.z) >= radians(300))){
gyro_sat_count++; gyro_sat_count++;
@ -92,14 +97,20 @@ AP_DCM::matrix_update(float _G_Dt)
} }
#if OUTPUTMODE == 1 #if OUTPUTMODE == 1
float tmp = _G_Dt * _omega.x;
update_matrix.b.z = -tmp; // -delta Theta x
update_matrix.c.y = tmp; // delta Theta x
tmp = _G_Dt * _omega.y;
update_matrix.c.x = -tmp; // -delta Theta y
update_matrix.a.z = tmp; // delta Theta y
tmp = _G_Dt * _omega.z;
update_matrix.b.x = tmp; // delta Theta z
update_matrix.a.y = -tmp; // -delta Theta z
update_matrix.a.x = 0; update_matrix.a.x = 0;
update_matrix.a.y = -_G_Dt * _omega.z; // -delta Theta z
update_matrix.a.z = _G_Dt * _omega.y; // delta Theta y
update_matrix.b.x = _G_Dt * _omega.z; // delta Theta z
update_matrix.b.y = 0; update_matrix.b.y = 0;
update_matrix.b.z = -_G_Dt * _omega.x; // -delta Theta x
update_matrix.c.x = -_G_Dt * _omega.y; // -delta Theta y
update_matrix.c.y = _G_Dt * _omega.x; // delta Theta x
update_matrix.c.z = 0; update_matrix.c.z = 0;
#else // Uncorrected data (no drift correction) #else // Uncorrected data (no drift correction)
update_matrix.a.x = 0; update_matrix.a.x = 0;
@ -115,7 +126,6 @@ AP_DCM::matrix_update(float _G_Dt)
temp_matrix = _dcm_matrix * update_matrix; temp_matrix = _dcm_matrix * update_matrix;
_dcm_matrix = _dcm_matrix + temp_matrix; // Equation 17 _dcm_matrix = _dcm_matrix + temp_matrix; // Equation 17
} }

4
libraries/AP_DCM/AP_DCM.h
View File

@ -33,7 +33,8 @@ public:
_health(1.), _health(1.),
_kp_roll_pitch(0.05967), _kp_roll_pitch(0.05967),
_ki_roll_pitch(0.00001278), _ki_roll_pitch(0.00001278),
_kp_yaw(0.8) _kp_yaw(0.8),
_toggle(0)
{} {}
// Accessors // Accessors
@ -115,6 +116,7 @@ private:
float _course_over_ground_y; // Course overground Y axis float _course_over_ground_y; // Course overground Y axis
float _health; float _health;
bool _centripetal; bool _centripetal;
bool _toggle;
}; };
#endif #endif