Bo Carøe
Test BMA180, HMC5883, ITG3200 sensor on my All_in_one II
Dependencies: mbed ITG3200 BMA180 HMC5883
main.cpp@0:f4e9301d548b, 2012-05-30 (annotated)
- Committer:
- caroe
- Date:
- Wed May 30 10:51:50 2012 +0000
- Revision:
- 0:f4e9301d548b
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| caroe | 0:f4e9301d548b | 1 | #include "mbed.h" |
| caroe | 0:f4e9301d548b | 2 | #include "HMC5883.h" |
| caroe | 0:f4e9301d548b | 3 | #include "BMA180.h" |
| caroe | 0:f4e9301d548b | 4 | #include "ITG3200.h" |
| caroe | 0:f4e9301d548b | 5 | Serial pc(USBTX, USBRX); |
| caroe | 0:f4e9301d548b | 6 | |
| caroe | 0:f4e9301d548b | 7 | DigitalOut l1(LED1); |
| caroe | 0:f4e9301d548b | 8 | I2C I2CBus(p28, p27); |
| caroe | 0:f4e9301d548b | 9 | Timer GlobalTime; |
| caroe | 0:f4e9301d548b | 10 | |
| caroe | 0:f4e9301d548b | 11 | #define PI 3.1415926535897932384626433832795 |
| caroe | 0:f4e9301d548b | 12 | HMC5883 Mag(I2CBus, GlobalTime); |
| caroe | 0:f4e9301d548b | 13 | BMA180 Acc(I2CBus, GlobalTime); |
| caroe | 0:f4e9301d548b | 14 | ITG3200 Gyro(I2CBus, GlobalTime); |
| caroe | 0:f4e9301d548b | 15 | |
| caroe | 0:f4e9301d548b | 16 | int HMC5883_getAngle(float x, float y) |
| caroe | 0:f4e9301d548b | 17 | { |
| caroe | 0:f4e9301d548b | 18 | float heading = atan2((float)y,(float)x); |
| caroe | 0:f4e9301d548b | 19 | // Your mrad result / 1000.00 (to turn it into radians). |
| caroe | 0:f4e9301d548b | 20 | float declinationAngle = 21.18 / 1000.0; |
| caroe | 0:f4e9301d548b | 21 | // If you have an EAST declination, use += declinationAngle, if you have a WEST declination, use -= declinationAngle |
| caroe | 0:f4e9301d548b | 22 | heading += declinationAngle; |
| caroe | 0:f4e9301d548b | 23 | if(heading < 0) heading += 2*PI;// Correct for when signs are reversed. |
| caroe | 0:f4e9301d548b | 24 | if(heading > 2*PI) heading -= 2*PI; // Check for wrap due to addition of declination. |
| caroe | 0:f4e9301d548b | 25 | return(heading * 180/PI); // Convert radians to degrees for readability. |
| caroe | 0:f4e9301d548b | 26 | } |
| caroe | 0:f4e9301d548b | 27 | |
| caroe | 0:f4e9301d548b | 28 | |
| caroe | 0:f4e9301d548b | 29 | int main() |
| caroe | 0:f4e9301d548b | 30 | { |
| caroe | 0:f4e9301d548b | 31 | pc.baud(115200); |
| caroe | 0:f4e9301d548b | 32 | |
| caroe | 0:f4e9301d548b | 33 | I2CBus.frequency(400000); |
| caroe | 0:f4e9301d548b | 34 | GlobalTime.start(); |
| caroe | 0:f4e9301d548b | 35 | //*** |
| caroe | 0:f4e9301d548b | 36 | Acc.Init(); |
| caroe | 0:f4e9301d548b | 37 | wait_ms(500); |
| caroe | 0:f4e9301d548b | 38 | //0.5 Sekunden kalibrieren |
| caroe | 0:f4e9301d548b | 39 | //1 g sind ca -2870 auf der 3. Achse |
| caroe | 0:f4e9301d548b | 40 | short Raw1g[3]= {0, 0, 0}; // -2870 |
| caroe | 0:f4e9301d548b | 41 | Acc.Calibrate(500, Raw1g); |
| caroe | 0:f4e9301d548b | 42 | //*** |
| caroe | 0:f4e9301d548b | 43 | Gyro.Init(); |
| caroe | 0:f4e9301d548b | 44 | wait_ms(500); |
| caroe | 0:f4e9301d548b | 45 | |
| caroe | 0:f4e9301d548b | 46 | //0.5 Sekunden kalibrieren |
| caroe | 0:f4e9301d548b | 47 | Gyro.Calibrate(500); |
| caroe | 0:f4e9301d548b | 48 | //*** |
| caroe | 0:f4e9301d548b | 49 | |
| caroe | 0:f4e9301d548b | 50 | Mag.Init(); |
| caroe | 0:f4e9301d548b | 51 | #if 1 |
| caroe | 0:f4e9301d548b | 52 | Mag.AutoCalibration= 1; //In Echtzeit kalibrieren |
| caroe | 0:f4e9301d548b | 53 | #else |
| caroe | 0:f4e9301d548b | 54 | short MagRawMin[3]= {-400, -400, -400}; //Gespeicherte Werte |
| caroe | 0:f4e9301d548b | 55 | short MagRawMax[3]= {400, 400, 400}; |
| caroe | 0:f4e9301d548b | 56 | Mag.Calibrate(MagRawMin, MagRawMax); |
| caroe | 0:f4e9301d548b | 57 | #endif |
| caroe | 0:f4e9301d548b | 58 | |
| caroe | 0:f4e9301d548b | 59 | while(1) |
| caroe | 0:f4e9301d548b | 60 | { |
| caroe | 0:f4e9301d548b | 61 | Mag.Update(); |
| caroe | 0:f4e9301d548b | 62 | |
| caroe | 0:f4e9301d548b | 63 | l1= Mag.MeasurementError; |
| caroe | 0:f4e9301d548b | 64 | Acc.Update(); |
| caroe | 0:f4e9301d548b | 65 | Gyro.Update(); |
| caroe | 0:f4e9301d548b | 66 | |
| caroe | 0:f4e9301d548b | 67 | pc.printf("Gyro:%.3f %.3f %.3f ", |
| caroe | 0:f4e9301d548b | 68 | (Gyro.Rate[0]), |
| caroe | 0:f4e9301d548b | 69 | (Gyro.Rate[1]), |
| caroe | 0:f4e9301d548b | 70 | (Gyro.Rate[2])); |
| caroe | 0:f4e9301d548b | 71 | |
| caroe | 0:f4e9301d548b | 72 | |
| caroe | 0:f4e9301d548b | 73 | pc.printf("BMA180:%.3f %.3f %.3f ", |
| caroe | 0:f4e9301d548b | 74 | (Acc.Acc[0]), |
| caroe | 0:f4e9301d548b | 75 | (Acc.Acc[1]), |
| caroe | 0:f4e9301d548b | 76 | (Acc.Acc[2])); |
| caroe | 0:f4e9301d548b | 77 | |
| caroe | 0:f4e9301d548b | 78 | |
| caroe | 0:f4e9301d548b | 79 | |
| caroe | 0:f4e9301d548b | 80 | // pc.printf("Min: %i %i %i Max: %i %i %i Scale: %.3f %.3f %.3f Offset: %.3i %.3i %.3i Mag: %.3f %.3f %.3f\r\n", |
| caroe | 0:f4e9301d548b | 81 | /* Mag.RawMin[0], Mag.RawMin[1], Mag.RawMin[2], |
| caroe | 0:f4e9301d548b | 82 | Mag.RawMax[0], Mag.RawMax[1], Mag.RawMax[2], |
| caroe | 0:f4e9301d548b | 83 | Mag.Scale[0] , Mag.Scale[1] , Mag.Scale[2], |
| caroe | 0:f4e9301d548b | 84 | Mag.Offset[0], Mag.Offset[1], Mag.Offset[2], |
| caroe | 0:f4e9301d548b | 85 | Mag.Mag[0] , Mag.Mag[1] , Mag.Mag[2]);*/ |
| caroe | 0:f4e9301d548b | 86 | printf("grader = %i \n",HMC5883_getAngle(Mag.Mag[0],Mag.Mag[1])); |
| caroe | 0:f4e9301d548b | 87 | |
| caroe | 0:f4e9301d548b | 88 | wait_ms(20); |
| caroe | 0:f4e9301d548b | 89 | } |
| caroe | 0:f4e9301d548b | 90 | } |