Luke Petre
Port of http://dev.qu.tu-berlin.de/projects/sf-razor-9dof-ahrs to an mbed, tested with a 9DOF Sensor Stick, SEN-10724
Diff: Compass.cpp
- Revision:
- 0:9a72d42c0da3
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Compass.cpp Tue Dec 27 17:20:06 2011 +0000
@@ -0,0 +1,25 @@
+/* This file is part of the Razor AHRS Firmware */
+#include "Razor_AHRS.h"
+#include <math.h>
+
+void IMU::Compass_Heading()
+{
+ float mag_x;
+ float mag_y;
+ float cos_roll;
+ float sin_roll;
+ float cos_pitch;
+ float sin_pitch;
+
+ cos_roll = cos(roll);
+ sin_roll = sin(roll);
+ cos_pitch = cos(pitch);
+ sin_pitch = sin(pitch);
+
+ // Tilt compensated magnetic field X
+ mag_x = magnetom[0]*cos_pitch + magnetom[1]*sin_roll*sin_pitch + magnetom[2]*cos_roll*sin_pitch;
+ // Tilt compensated magnetic field Y
+ mag_y = magnetom[1]*cos_roll - magnetom[2]*sin_roll;
+ // Magnetic Heading
+ MAG_Heading = atan2(-mag_y, mag_x);
+}