AP_AHRS: use rotateXY for speed

Saves 0.1ms at 100hz
This commit is contained in:
Randy Mackay 2013-07-20 18:00:36 +09:00
parent 540ca25b84
commit 0325ad5d0d

View File

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