Witek Ewert / Mbed 2 deprecated kl46_accel_mag

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?

UserRevisionLine numberNew 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 }