Tyler Weaver
AHRS library, modified version of Peter Bartz work.
Diff: Compass.cpp
- Revision:
- 0:014ee3239c80
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Compass.cpp Thu Nov 08 18:57:18 2012 +0000
@@ -0,0 +1,25 @@
+/* This file is part of the Razor AHRS Firmware */
+#include "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);
+}