From aa408655f864b3d98f7ce8ab5279162e6aac8438 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 12 Mar 2012 17:28:07 +1100 Subject: [PATCH] Compass: fixed the order of rotations in the compass driver this should fix the massive heading issues that people have been reporting. Please test! --- libraries/AP_Compass/AP_Compass_HMC5843.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.cpp b/libraries/AP_Compass/AP_Compass_HMC5843.cpp index 6ec074d28b..7310b1cd47 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5843.cpp +++ b/libraries/AP_Compass/AP_Compass_HMC5843.cpp @@ -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;