AHRS library, modified version of Peter Bartz work.
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?
User | Revision | Line number | New 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 | } |