Browse Source

Compass: fixed the order of rotations in the compass driver

this should fix the massive heading issues that people have been
reporting. Please test!
mission-4.1.18
Andrew Tridgell 13 years ago
parent
commit
7daaadf776
  1. 2
      libraries/AP_Compass/AP_Compass_HMC5843.cpp

2
libraries/AP_Compass/AP_Compass_HMC5843.cpp

@ -264,10 +264,10 @@ bool AP_Compass_HMC5843::read() @@ -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;

Loading…
Cancel
Save