From 0325ad5d0d5ccddd326e1b96f7bd201cf12a1fab Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 20 Jul 2013 18:00:36 +0900 Subject: [PATCH] AP_AHRS: use rotateXY for speed Saves 0.1ms at 100hz --- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 1e44cd51bc..863705c86a 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -440,7 +440,7 @@ AP_AHRS_DCM::drift_correction(float deltat) drift_correction_yaw(); // apply trim - temp_dcm.rotate(_trim); + temp_dcm.rotateXY(_trim); // rotate accelerometer values into the earth frame _accel_ef = temp_dcm * _accel_vector;