KL46 Accelerometer + Magnetometer, with 3-axis calibration. Readout through OpenSDA CDC.
Dependencies: MAG3110 MMA8451Q USBDevice mbed
main.cpp@0:c569d820861b, 2014-04-10 (annotated)
- Committer:
- wue
- Date:
- Thu Apr 10 07:48:46 2014 +0000
- Revision:
- 0:c569d820861b
KL46Z Accelerometer & Magnetometer readout over OpenSDA CDC.; 3-axis 4-value calibration taken from eCompass.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
wue | 0:c569d820861b | 1 | #include "mbed.h" |
wue | 0:c569d820861b | 2 | //#include "USBSerial.h" |
wue | 0:c569d820861b | 3 | #include "MAG3110.h" |
wue | 0:c569d820861b | 4 | #include "MMA8451Q.h" |
wue | 0:c569d820861b | 5 | |
wue | 0:c569d820861b | 6 | #include "magnetic.h" |
wue | 0:c569d820861b | 7 | |
wue | 0:c569d820861b | 8 | #define MMA8451_I2C_ADDRESS (0x1d<<1) |
wue | 0:c569d820861b | 9 | MMA8451Q acc(PTE25, PTE24, MMA8451_I2C_ADDRESS); |
wue | 0:c569d820861b | 10 | MAG3110 mag(PTE25, PTE24); |
wue | 0:c569d820861b | 11 | Serial usb_cdc(USBTX, USBRX); |
wue | 0:c569d820861b | 12 | |
wue | 0:c569d820861b | 13 | DigitalOut gLED(LED_GREEN); //PTD5 |
wue | 0:c569d820861b | 14 | DigitalOut rLED(LED_RED); //PTE29 |
wue | 0:c569d820861b | 15 | |
wue | 0:c569d820861b | 16 | |
wue | 0:c569d820861b | 17 | struct MagneticBuffer mbuf; |
wue | 0:c569d820861b | 18 | struct MagCalibration mcal; |
wue | 0:c569d820861b | 19 | |
wue | 0:c569d820861b | 20 | typedef struct tagV3i { |
wue | 0:c569d820861b | 21 | int x; |
wue | 0:c569d820861b | 22 | int y; |
wue | 0:c569d820861b | 23 | int z; |
wue | 0:c569d820861b | 24 | } v3i, *pv3i; |
wue | 0:c569d820861b | 25 | |
wue | 0:c569d820861b | 26 | typedef struct tagV3f { |
wue | 0:c569d820861b | 27 | float x; |
wue | 0:c569d820861b | 28 | float y; |
wue | 0:c569d820861b | 29 | float z; |
wue | 0:c569d820861b | 30 | } v3f, *pv3f; |
wue | 0:c569d820861b | 31 | |
wue | 0:c569d820861b | 32 | int main(void) { |
wue | 0:c569d820861b | 33 | |
wue | 0:c569d820861b | 34 | int i; |
wue | 0:c569d820861b | 35 | v3i mv; |
wue | 0:c569d820861b | 36 | v3f av; |
wue | 0:c569d820861b | 37 | |
wue | 0:c569d820861b | 38 | wait(0.2); |
wue | 0:c569d820861b | 39 | usb_cdc.baud(115200); |
wue | 0:c569d820861b | 40 | |
wue | 0:c569d820861b | 41 | //printf("Started.\r\n"); |
wue | 0:c569d820861b | 42 | |
wue | 0:c569d820861b | 43 | gLED = 1; |
wue | 0:c569d820861b | 44 | rLED = 1; |
wue | 0:c569d820861b | 45 | wait(0.2); |
wue | 0:c569d820861b | 46 | |
wue | 0:c569d820861b | 47 | //usb_cdc.printf("Reading %d mag vectors... ", MAGBUFFSIZE); |
wue | 0:c569d820861b | 48 | gLED = 0; |
wue | 0:c569d820861b | 49 | |
wue | 0:c569d820861b | 50 | // sample MAGBUFFSIZE vectors for cal. |
wue | 0:c569d820861b | 51 | for(i=0; i<MAGBUFFSIZE; i++) { |
wue | 0:c569d820861b | 52 | mag.getValues(&mv.x, &mv.y, &mv.z); |
wue | 0:c569d820861b | 53 | mbuf.iBx[i] = mv.x; |
wue | 0:c569d820861b | 54 | mbuf.iBy[i] = mv.y; |
wue | 0:c569d820861b | 55 | mbuf.iBz[i] = mv.z; |
wue | 0:c569d820861b | 56 | wait(0.04); |
wue | 0:c569d820861b | 57 | }; |
wue | 0:c569d820861b | 58 | |
wue | 0:c569d820861b | 59 | gLED = 1; |
wue | 0:c569d820861b | 60 | //usb_cdc.printf("done\r\n"); |
wue | 0:c569d820861b | 61 | wait(0.2); |
wue | 0:c569d820861b | 62 | |
wue | 0:c569d820861b | 63 | rLED = 0; |
wue | 0:c569d820861b | 64 | |
wue | 0:c569d820861b | 65 | //usb_cdc.printf("Calibrating... "); |
wue | 0:c569d820861b | 66 | ResetMagCalibration(&mcal); |
wue | 0:c569d820861b | 67 | magUpdateCalibration(&mcal, &mbuf); |
wue | 0:c569d820861b | 68 | //usb_cdc.printf("done\r\n"); |
wue | 0:c569d820861b | 69 | rLED = 1; |
wue | 0:c569d820861b | 70 | //printf("cal. result: % 3.2f % 3.2f % 3.2f\r\n", mcal.ftrVx, mcal.ftrVy, mcal.ftrVz); |
wue | 0:c569d820861b | 71 | |
wue | 0:c569d820861b | 72 | while(1) { |
wue | 0:c569d820861b | 73 | mag.getValues(&(mv.x), &(mv.y), &(mv.z)); |
wue | 0:c569d820861b | 74 | if(mv.x>=32768) mv.x |= 0xFFFF0000; |
wue | 0:c569d820861b | 75 | if(mv.y>=32768) mv.y |= 0xFFFF0000; |
wue | 0:c569d820861b | 76 | if(mv.z>=32768) mv.z |= 0xFFFF0000; |
wue | 0:c569d820861b | 77 | |
wue | 0:c569d820861b | 78 | av.x = acc.getAccX(); |
wue | 0:c569d820861b | 79 | av.y = acc.getAccY(); |
wue | 0:c569d820861b | 80 | av.z = acc.getAccZ(); |
wue | 0:c569d820861b | 81 | printf("% 3.3f % 3.3f % 3.3f % 3.3f % 3.3f % 3.3f\r\n", |
wue | 0:c569d820861b | 82 | mv.x*FUTPERCOUNT-mcal.ftrVx, |
wue | 0:c569d820861b | 83 | mv.y*FUTPERCOUNT-mcal.ftrVy, |
wue | 0:c569d820861b | 84 | mv.z*FUTPERCOUNT-mcal.ftrVz, |
wue | 0:c569d820861b | 85 | av.x, |
wue | 0:c569d820861b | 86 | av.y, |
wue | 0:c569d820861b | 87 | av.z); |
wue | 0:c569d820861b | 88 | wait(0.02); |
wue | 0:c569d820861b | 89 | |
wue | 0:c569d820861b | 90 | }; |
wue | 0:c569d820861b | 91 | } |