FIBO
/
ReadingMag_HMC5883L_work
test module for fibo
Fork of ReadingMag_HMC5883L by
main.cpp@1:ed9abb368fcf, 2015-03-02 (annotated)
- Committer:
- masterholy
- Date:
- Mon Mar 02 09:06:19 2015 +0000
- Revision:
- 1:ed9abb368fcf
- Parent:
- 0:6bc5f85ca6fa
test module for fibo
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" |
masterholy | 1:ed9abb368fcf | 3 | #include "ms5611.h" |
jose_claudiojr | 0:6bc5f85ca6fa | 4 | #include <math.h> |
jose_claudiojr | 0:6bc5f85ca6fa | 5 | |
masterholy | 1:ed9abb368fcf | 6 | #define SDA PB_9 |
masterholy | 1:ed9abb368fcf | 7 | #define SCL PB_8 |
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; |
masterholy | 1:ed9abb368fcf | 13 | float p; |
jose_claudiojr | 0:6bc5f85ca6fa | 14 | /* |
jose_claudiojr | 0:6bc5f85ca6fa | 15 | float m_x, m_y, m_z; |
jose_claudiojr | 0:6bc5f85ca6fa | 16 | */ |
jose_claudiojr | 0:6bc5f85ca6fa | 17 | printf("Inicializing...\r\n"); |
jose_claudiojr | 0:6bc5f85ca6fa | 18 | HMC5883L hmc5883l(SDA, SCL); |
masterholy | 1:ed9abb368fcf | 19 | ms5611 baro(SDA, SCL); |
masterholy | 1:ed9abb368fcf | 20 | baro.cmd_reset(); |
jose_claudiojr | 0:6bc5f85ca6fa | 21 | //HMC5883L *hmc5883l = new HMC5883L(SDA, SCL); |
jose_claudiojr | 0:6bc5f85ca6fa | 22 | printf("OK...\r\n"); |
jose_claudiojr | 0:6bc5f85ca6fa | 23 | /* |
jose_claudiojr | 0:6bc5f85ca6fa | 24 | for (int i = 0 ; i < 100 ; i++) |
jose_claudiojr | 0:6bc5f85ca6fa | 25 | { |
jose_claudiojr | 0:6bc5f85ca6fa | 26 | x += hmc5883l.getMx()/100; |
jose_claudiojr | 0:6bc5f85ca6fa | 27 | y += hmc5883l.getMy()/100; |
jose_claudiojr | 0:6bc5f85ca6fa | 28 | z += hmc5883l.getMz()/100; |
jose_claudiojr | 0:6bc5f85ca6fa | 29 | wait_ms(100); |
jose_claudiojr | 0:6bc5f85ca6fa | 30 | } |
jose_claudiojr | 0:6bc5f85ca6fa | 31 | |
jose_claudiojr | 0:6bc5f85ca6fa | 32 | |
jose_claudiojr | 0:6bc5f85ca6fa | 33 | m_x = x/2; |
jose_claudiojr | 0:6bc5f85ca6fa | 34 | m_y = y/2; |
jose_claudiojr | 0:6bc5f85ca6fa | 35 | m_z = z/2; |
jose_claudiojr | 0:6bc5f85ca6fa | 36 | */ |
jose_claudiojr | 0:6bc5f85ca6fa | 37 | wait(1); |
jose_claudiojr | 0:6bc5f85ca6fa | 38 | |
jose_claudiojr | 0:6bc5f85ca6fa | 39 | while(1) |
jose_claudiojr | 0:6bc5f85ca6fa | 40 | { |
jose_claudiojr | 0:6bc5f85ca6fa | 41 | /* |
jose_claudiojr | 0:6bc5f85ca6fa | 42 | x = m_x - hmc5883l.getMx()*0.92; |
jose_claudiojr | 0:6bc5f85ca6fa | 43 | y = m_y - hmc5883l.getMy()*0.92; |
jose_claudiojr | 0:6bc5f85ca6fa | 44 | z = m_z - hmc5883l.getMz()*0.92; |
jose_claudiojr | 0:6bc5f85ca6fa | 45 | */ |
jose_claudiojr | 0:6bc5f85ca6fa | 46 | x = hmc5883l.getMx(); |
jose_claudiojr | 0:6bc5f85ca6fa | 47 | y = hmc5883l.getMy(); |
jose_claudiojr | 0:6bc5f85ca6fa | 48 | z = hmc5883l.getMz(); |
masterholy | 1:ed9abb368fcf | 49 | p = baro.getPressure(); |
jose_claudiojr | 0:6bc5f85ca6fa | 50 | heading = atan2(y, x); |
jose_claudiojr | 0:6bc5f85ca6fa | 51 | if(heading < 0) |
jose_claudiojr | 0:6bc5f85ca6fa | 52 | heading += 2*PI; |
jose_claudiojr | 0:6bc5f85ca6fa | 53 | if(heading > 2*PI) |
jose_claudiojr | 0:6bc5f85ca6fa | 54 | heading -= 2*PI; |
jose_claudiojr | 0:6bc5f85ca6fa | 55 | |
jose_claudiojr | 0:6bc5f85ca6fa | 56 | heading = heading * 180 / PI; |
jose_claudiojr | 0:6bc5f85ca6fa | 57 | |
jose_claudiojr | 0:6bc5f85ca6fa | 58 | // Correct for when signs are reversed. |
jose_claudiojr | 0:6bc5f85ca6fa | 59 | //if(heading < 0) |
jose_claudiojr | 0:6bc5f85ca6fa | 60 | // heading += 2*PI; |
jose_claudiojr | 0:6bc5f85ca6fa | 61 | //if(heading > 2*PI) |
jose_claudiojr | 0:6bc5f85ca6fa | 62 | // heading -= 2*PI; |
jose_claudiojr | 0:6bc5f85ca6fa | 63 | |
jose_claudiojr | 0:6bc5f85ca6fa | 64 | //while(heading < 0) heading += 360; |
jose_claudiojr | 0:6bc5f85ca6fa | 65 | //while(heading > 360) heading -= 360; |
jose_claudiojr | 0:6bc5f85ca6fa | 66 | |
jose_claudiojr | 0:6bc5f85ca6fa | 67 | |
jose_claudiojr | 0:6bc5f85ca6fa | 68 | printf("x: %f \t\ty: %f \t\t z: %f \t\t heading: %f \t\r\n", x, y, z, heading); |
masterholy | 1:ed9abb368fcf | 69 | printf("pressure : %f \n",p); |
jose_claudiojr | 0:6bc5f85ca6fa | 70 | wait_ms(200); |
jose_claudiojr | 0:6bc5f85ca6fa | 71 | |
jose_claudiojr | 0:6bc5f85ca6fa | 72 | } |
jose_claudiojr | 0:6bc5f85ca6fa | 73 | } |