Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 00:58:37 -04:00
Code Issues Packages Projects Releases Wiki Activity

ACM: removed a lot of the special case code for quaternions

Browse Source
This commit is contained in:
Andrew Tridgell 2012-03-07 15:11:19 +11:00
parent e780dd2b44
commit 34d25ab298
1 changed files with 4 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
ArduCopter/ArduCopter.pde
View File

@ -973,12 +973,9 @@ static void medium_loop()
if(g.compass_enabled){ if(g.compass_enabled){
if (compass.read()) { if (compass.read()) {
// Calculate heading // Calculate heading
#if QUATERNION_ENABLE == ENABLED Matrix3f m = dcm.get_dcm_matrix();
compass.calculate(dcm.roll, dcm.pitch); compass.calculate(m);
#else compass.null_offsets(m);
compass.calculate(dcm.get_dcm_matrix());
compass.null_offsets(dcm.get_dcm_matrix());
#endif
} }
} }
#endif #endif
@ -1856,12 +1853,11 @@ static void read_AHRS(void)
gcs_update(); gcs_update();
#endif #endif
dcm.update_DCM_fast(); dcm.update_DCM();
omega = imu.get_gyro(); omega = imu.get_gyro();
} }
static void update_trig(void){ static void update_trig(void){
#if QUATERNION_ENABLE == DISABLED
Vector2f yawvector; Vector2f yawvector;
Matrix3f temp = dcm.get_dcm_matrix(); Matrix3f temp = dcm.get_dcm_matrix();
@ -1880,23 +1876,6 @@ static void update_trig(void){
sin_yaw_y = yawvector.x; // 1y = north sin_yaw_y = yawvector.x; // 1y = north
cos_yaw_x = yawvector.y; // 0x = north cos_yaw_x = yawvector.y; // 0x = north
#else
// we need a cheaper way to calculate this ....
Vector2f yawvector;
float cp = cos(dcm.pitch);
float cr = cos(dcm.roll);
float sy = sin(dcm.yaw);
float cy = cos(dcm.yaw);
yawvector.x = cp * cy;
yawvector.y = cp * sy;
yawvector.normalize();
cos_pitch_x = cp;
cos_roll_x = cr;
sin_yaw_y = yawvector.x;
cos_yaw_x = yawvector.y;
#endif
//flat: //flat:
// 0 ° = cos_yaw: 0.00, sin_yaw: 1.00, // 0 ° = cos_yaw: 0.00, sin_yaw: 1.00,
// 90° = cos_yaw: 1.00, sin_yaw: 0.00, // 90° = cos_yaw: 1.00, sin_yaw: 0.00,