José Claudio
/
ReadingMag_HMC5883L
Example of reading and magnetometer sensor (HMC5883L)
main.cpp@0:6bc5f85ca6fa, 2013-05-21 (annotated)
- Committer:
- jose_claudiojr
- Date:
- Tue May 21 13:48:10 2013 +0000
- Revision:
- 0:6bc5f85ca6fa
Example of reading and magnetometer sensor (HMC5883L)
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jose_claudiojr | 0:6bc5f85ca6fa | 1 | #include "mbed.h" |
jose_claudiojr | 0:6bc5f85ca6fa | 2 | #include "HMC5883L.h" |
jose_claudiojr | 0:6bc5f85ca6fa | 3 | |
jose_claudiojr | 0:6bc5f85ca6fa | 4 | #include <math.h> |
jose_claudiojr | 0:6bc5f85ca6fa | 5 | |
jose_claudiojr | 0:6bc5f85ca6fa | 6 | #define SDA p9 |
jose_claudiojr | 0:6bc5f85ca6fa | 7 | #define SCL p10 |
jose_claudiojr | 0:6bc5f85ca6fa | 8 | #define PI 3.14159265 |
jose_claudiojr | 0:6bc5f85ca6fa | 9 | |
jose_claudiojr | 0:6bc5f85ca6fa | 10 | int main() |
jose_claudiojr | 0:6bc5f85ca6fa | 11 | { |
jose_claudiojr | 0:6bc5f85ca6fa | 12 | float x, y, z, heading; |
jose_claudiojr | 0:6bc5f85ca6fa | 13 | /* |
jose_claudiojr | 0:6bc5f85ca6fa | 14 | float m_x, m_y, m_z; |
jose_claudiojr | 0:6bc5f85ca6fa | 15 | */ |
jose_claudiojr | 0:6bc5f85ca6fa | 16 | printf("Inicializing...\r\n"); |
jose_claudiojr | 0:6bc5f85ca6fa | 17 | HMC5883L hmc5883l(SDA, SCL); |
jose_claudiojr | 0:6bc5f85ca6fa | 18 | //HMC5883L *hmc5883l = new HMC5883L(SDA, SCL); |
jose_claudiojr | 0:6bc5f85ca6fa | 19 | printf("OK...\r\n"); |
jose_claudiojr | 0:6bc5f85ca6fa | 20 | /* |
jose_claudiojr | 0:6bc5f85ca6fa | 21 | for (int i = 0 ; i < 100 ; i++) |
jose_claudiojr | 0:6bc5f85ca6fa | 22 | { |
jose_claudiojr | 0:6bc5f85ca6fa | 23 | x += hmc5883l.getMx()/100; |
jose_claudiojr | 0:6bc5f85ca6fa | 24 | y += hmc5883l.getMy()/100; |
jose_claudiojr | 0:6bc5f85ca6fa | 25 | z += hmc5883l.getMz()/100; |
jose_claudiojr | 0:6bc5f85ca6fa | 26 | wait_ms(100); |
jose_claudiojr | 0:6bc5f85ca6fa | 27 | } |
jose_claudiojr | 0:6bc5f85ca6fa | 28 | |
jose_claudiojr | 0:6bc5f85ca6fa | 29 | |
jose_claudiojr | 0:6bc5f85ca6fa | 30 | m_x = x/2; |
jose_claudiojr | 0:6bc5f85ca6fa | 31 | m_y = y/2; |
jose_claudiojr | 0:6bc5f85ca6fa | 32 | m_z = z/2; |
jose_claudiojr | 0:6bc5f85ca6fa | 33 | */ |
jose_claudiojr | 0:6bc5f85ca6fa | 34 | wait(1); |
jose_claudiojr | 0:6bc5f85ca6fa | 35 | |
jose_claudiojr | 0:6bc5f85ca6fa | 36 | while(1) |
jose_claudiojr | 0:6bc5f85ca6fa | 37 | { |
jose_claudiojr | 0:6bc5f85ca6fa | 38 | /* |
jose_claudiojr | 0:6bc5f85ca6fa | 39 | x = m_x - hmc5883l.getMx()*0.92; |
jose_claudiojr | 0:6bc5f85ca6fa | 40 | y = m_y - hmc5883l.getMy()*0.92; |
jose_claudiojr | 0:6bc5f85ca6fa | 41 | z = m_z - hmc5883l.getMz()*0.92; |
jose_claudiojr | 0:6bc5f85ca6fa | 42 | */ |
jose_claudiojr | 0:6bc5f85ca6fa | 43 | x = hmc5883l.getMx(); |
jose_claudiojr | 0:6bc5f85ca6fa | 44 | y = hmc5883l.getMy(); |
jose_claudiojr | 0:6bc5f85ca6fa | 45 | z = hmc5883l.getMz(); |
jose_claudiojr | 0:6bc5f85ca6fa | 46 | |
jose_claudiojr | 0:6bc5f85ca6fa | 47 | heading = atan2(y, x); |
jose_claudiojr | 0:6bc5f85ca6fa | 48 | if(heading < 0) |
jose_claudiojr | 0:6bc5f85ca6fa | 49 | heading += 2*PI; |
jose_claudiojr | 0:6bc5f85ca6fa | 50 | if(heading > 2*PI) |
jose_claudiojr | 0:6bc5f85ca6fa | 51 | heading -= 2*PI; |
jose_claudiojr | 0:6bc5f85ca6fa | 52 | |
jose_claudiojr | 0:6bc5f85ca6fa | 53 | heading = heading * 180 / PI; |
jose_claudiojr | 0:6bc5f85ca6fa | 54 | |
jose_claudiojr | 0:6bc5f85ca6fa | 55 | // Correct for when signs are reversed. |
jose_claudiojr | 0:6bc5f85ca6fa | 56 | //if(heading < 0) |
jose_claudiojr | 0:6bc5f85ca6fa | 57 | // heading += 2*PI; |
jose_claudiojr | 0:6bc5f85ca6fa | 58 | //if(heading > 2*PI) |
jose_claudiojr | 0:6bc5f85ca6fa | 59 | // heading -= 2*PI; |
jose_claudiojr | 0:6bc5f85ca6fa | 60 | |
jose_claudiojr | 0:6bc5f85ca6fa | 61 | //while(heading < 0) heading += 360; |
jose_claudiojr | 0:6bc5f85ca6fa | 62 | //while(heading > 360) heading -= 360; |
jose_claudiojr | 0:6bc5f85ca6fa | 63 | |
jose_claudiojr | 0:6bc5f85ca6fa | 64 | |
jose_claudiojr | 0:6bc5f85ca6fa | 65 | printf("x: %f \t\ty: %f \t\t z: %f \t\t heading: %f \t\r\n", x, y, z, heading); |
jose_claudiojr | 0:6bc5f85ca6fa | 66 | wait_ms(200); |
jose_claudiojr | 0:6bc5f85ca6fa | 67 | |
jose_claudiojr | 0:6bc5f85ca6fa | 68 | } |
jose_claudiojr | 0:6bc5f85ca6fa | 69 | } |