Port of http://dev.qu.tu-berlin.de/projects/sf-razor-9dof-ahrs to an mbed, tested with a 9DOF Sensor Stick, SEN-10724
Compass.cpp
- Committer:
- lpetre
- Date:
- 2011-12-28
- Revision:
- 2:5aa75c3d8cc3
- Parent:
- 0:9a72d42c0da3
File content as of revision 2:5aa75c3d8cc3:
/* 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); }