Compass: fixed the order of rotations in the compass driver

this should fix the massive heading issues that people have been
reporting. Please test!
This commit is contained in:
Andrew Tridgell 2012-03-12 17:28:07 +11:00
parent 0e6037322a
commit aa408655f8

View File

@ -264,10 +264,10 @@ bool AP_Compass_HMC5843::read()
// rotate to the desired orientation
Vector3f rot_mag = Vector3f(mag_x,mag_y,mag_z);
rot_mag.rotate(_orientation);
if (product_id == AP_COMPASS_TYPE_HMC5883L) {
rot_mag.rotate(ROTATION_YAW_90);
}
rot_mag.rotate(_orientation);
rot_mag += _offset.get();
mag_x = rot_mag.x;