Tyler Weaver / AHRS

AHRS library, modified version of Peter Bartz work.

Dependencies:   MODSERIAL

Dependents:   AHRS_demo

Compass.cpp@1:da3b20b5d38a, 2012-11-08 (annotated)

Committer:
tylerjw
Date:
Thu Nov 08 20:13:35 2012 +0000
Revision:
1:da3b20b5d38a
Parent:
0:014ee3239c80 
Modified AHRS library.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tylerjw 0:014ee3239c80 1 /* This file is part of the Razor AHRS Firmware */
tylerjw 0:014ee3239c80 2 #include "AHRS.h"
tylerjw 0:014ee3239c80 3 #include <math.h>
tylerjw 0:014ee3239c80 4
tylerjw 0:014ee3239c80 5 void IMU::Compass_Heading()
tylerjw 0:014ee3239c80 6 {
tylerjw 0:014ee3239c80 7 float mag_x;
tylerjw 0:014ee3239c80 8 float mag_y;
tylerjw 0:014ee3239c80 9 float cos_roll;
tylerjw 0:014ee3239c80 10 float sin_roll;
tylerjw 0:014ee3239c80 11 float cos_pitch;
tylerjw 0:014ee3239c80 12 float sin_pitch;
tylerjw 0:014ee3239c80 13
tylerjw 0:014ee3239c80 14 cos_roll = cos(roll);
tylerjw 0:014ee3239c80 15 sin_roll = sin(roll);
tylerjw 0:014ee3239c80 16 cos_pitch = cos(pitch);
tylerjw 0:014ee3239c80 17 sin_pitch = sin(pitch);
tylerjw 0:014ee3239c80 18
tylerjw 0:014ee3239c80 19 // Tilt compensated magnetic field X
tylerjw 0:014ee3239c80 20 mag_x = magnetom[0]*cos_pitch + magnetom[1]*sin_roll*sin_pitch + magnetom[2]*cos_roll*sin_pitch;
tylerjw 0:014ee3239c80 21 // Tilt compensated magnetic field Y
tylerjw 0:014ee3239c80 22 mag_y = magnetom[1]*cos_roll - magnetom[2]*sin_roll;
tylerjw 0:014ee3239c80 23 // Magnetic Heading
tylerjw 0:014ee3239c80 24 MAG_Heading = atan2(-mag_y, mag_x);
tylerjw 0:014ee3239c80 25 }