FIBO
/
ReadingMag_HMC5883L_work
test module for fibo
Fork of ReadingMag_HMC5883L by
Diff: main.cpp
- Revision:
- 0:6bc5f85ca6fa
- Child:
- 1:ed9abb368fcf
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Tue May 21 13:48:10 2013 +0000 @@ -0,0 +1,69 @@ +#include "mbed.h" +#include "HMC5883L.h" + +#include <math.h> + +#define SDA p9 +#define SCL p10 +#define PI 3.14159265 + +int main() +{ + float x, y, z, heading; + /* + float m_x, m_y, m_z; + */ + printf("Inicializing...\r\n"); + HMC5883L hmc5883l(SDA, SCL); + //HMC5883L *hmc5883l = new HMC5883L(SDA, SCL); + printf("OK...\r\n"); + /* + for (int i = 0 ; i < 100 ; i++) + { + x += hmc5883l.getMx()/100; + y += hmc5883l.getMy()/100; + z += hmc5883l.getMz()/100; + wait_ms(100); + } + + + m_x = x/2; + m_y = y/2; + m_z = z/2; + */ + wait(1); + + while(1) + { + /* + x = m_x - hmc5883l.getMx()*0.92; + y = m_y - hmc5883l.getMy()*0.92; + z = m_z - hmc5883l.getMz()*0.92; + */ + x = hmc5883l.getMx(); + y = hmc5883l.getMy(); + z = hmc5883l.getMz(); + + heading = atan2(y, x); + if(heading < 0) + heading += 2*PI; + if(heading > 2*PI) + heading -= 2*PI; + + heading = heading * 180 / PI; + + // Correct for when signs are reversed. + //if(heading < 0) + // heading += 2*PI; + //if(heading > 2*PI) + // heading -= 2*PI; + + //while(heading < 0) heading += 360; + //while(heading > 360) heading -= 360; + + + printf("x: %f \t\ty: %f \t\t z: %f \t\t heading: %f \t\r\n", x, y, z, heading); + wait_ms(200); + + } +}