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
Compass.cpp@2:5aa75c3d8cc3, 2011-12-28 (annotated)
- Committer:
- lpetre
- Date:
- Wed Dec 28 18:09:14 2011 +0000
- Revision:
- 2:5aa75c3d8cc3
- Parent:
- 0:9a72d42c0da3
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| lpetre | 0:9a72d42c0da3 | 1 | /* This file is part of the Razor AHRS Firmware */ |
| lpetre | 0:9a72d42c0da3 | 2 | #include "Razor_AHRS.h" |
| lpetre | 0:9a72d42c0da3 | 3 | #include <math.h> |
| lpetre | 0:9a72d42c0da3 | 4 | |
| lpetre | 0:9a72d42c0da3 | 5 | void IMU::Compass_Heading() |
| lpetre | 0:9a72d42c0da3 | 6 | { |
| lpetre | 0:9a72d42c0da3 | 7 | float mag_x; |
| lpetre | 0:9a72d42c0da3 | 8 | float mag_y; |
| lpetre | 0:9a72d42c0da3 | 9 | float cos_roll; |
| lpetre | 0:9a72d42c0da3 | 10 | float sin_roll; |
| lpetre | 0:9a72d42c0da3 | 11 | float cos_pitch; |
| lpetre | 0:9a72d42c0da3 | 12 | float sin_pitch; |
| lpetre | 0:9a72d42c0da3 | 13 | |
| lpetre | 0:9a72d42c0da3 | 14 | cos_roll = cos(roll); |
| lpetre | 0:9a72d42c0da3 | 15 | sin_roll = sin(roll); |
| lpetre | 0:9a72d42c0da3 | 16 | cos_pitch = cos(pitch); |
| lpetre | 0:9a72d42c0da3 | 17 | sin_pitch = sin(pitch); |
| lpetre | 0:9a72d42c0da3 | 18 | |
| lpetre | 0:9a72d42c0da3 | 19 | // Tilt compensated magnetic field X |
| lpetre | 0:9a72d42c0da3 | 20 | mag_x = magnetom[0]*cos_pitch + magnetom[1]*sin_roll*sin_pitch + magnetom[2]*cos_roll*sin_pitch; |
| lpetre | 0:9a72d42c0da3 | 21 | // Tilt compensated magnetic field Y |
| lpetre | 0:9a72d42c0da3 | 22 | mag_y = magnetom[1]*cos_roll - magnetom[2]*sin_roll; |
| lpetre | 0:9a72d42c0da3 | 23 | // Magnetic Heading |
| lpetre | 0:9a72d42c0da3 | 24 | MAG_Heading = atan2(-mag_y, mag_x); |
| lpetre | 0:9a72d42c0da3 | 25 | } |