Diff: main.cpp

Revision:

0:c569d820861b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Apr 10 07:48:46 2014 +0000
@@ -0,0 +1,91 @@
+#include "mbed.h"
+//#include "USBSerial.h"
+#include "MAG3110.h"
+#include "MMA8451Q.h"
+
+#include "magnetic.h"
+
+#define MMA8451_I2C_ADDRESS (0x1d<<1)
+MMA8451Q acc(PTE25, PTE24, MMA8451_I2C_ADDRESS);
+MAG3110 mag(PTE25, PTE24);
+Serial usb_cdc(USBTX, USBRX);
+
+DigitalOut gLED(LED_GREEN); //PTD5
+DigitalOut rLED(LED_RED);       //PTE29
+
+
+struct MagneticBuffer mbuf;
+struct MagCalibration mcal;
+
+typedef struct tagV3i {
+    int x;
+    int y;
+    int z;   
+} v3i, *pv3i;
+
+typedef struct tagV3f {
+    float x;
+    float y;
+    float z;   
+} v3f, *pv3f;
+
+int main(void) {
+    
+    int i;
+    v3i mv;
+    v3f av;
+
+    wait(0.2);
+    usb_cdc.baud(115200);
+    
+    //printf("Started.\r\n");
+    
+    gLED = 1;
+    rLED = 1;
+    wait(0.2);
+    
+    //usb_cdc.printf("Reading %d mag vectors... ", MAGBUFFSIZE);
+    gLED = 0;
+    
+    // sample MAGBUFFSIZE vectors for cal.
+    for(i=0; i<MAGBUFFSIZE; i++) {
+        mag.getValues(&mv.x, &mv.y, &mv.z);
+        mbuf.iBx[i] = mv.x;
+        mbuf.iBy[i] = mv.y;
+        mbuf.iBz[i] = mv.z;
+        wait(0.04);
+    };
+    
+    gLED = 1;
+    //usb_cdc.printf("done\r\n");
+    wait(0.2);
+    
+    rLED = 0;
+    
+    //usb_cdc.printf("Calibrating... ");
+    ResetMagCalibration(&mcal);
+    magUpdateCalibration(&mcal, &mbuf);
+    //usb_cdc.printf("done\r\n");
+    rLED = 1;
+    //printf("cal. result: % 3.2f % 3.2f % 3.2f\r\n", mcal.ftrVx, mcal.ftrVy, mcal.ftrVz);
+    
+    while(1) {
+        mag.getValues(&(mv.x), &(mv.y), &(mv.z));
+        if(mv.x>=32768) mv.x |= 0xFFFF0000;
+        if(mv.y>=32768) mv.y |= 0xFFFF0000;
+        if(mv.z>=32768) mv.z |= 0xFFFF0000;
+        
+        av.x = acc.getAccX();
+        av.y = acc.getAccY();
+        av.z = acc.getAccZ();
+        printf("% 3.3f % 3.3f % 3.3f % 3.3f % 3.3f % 3.3f\r\n", 
+            mv.x*FUTPERCOUNT-mcal.ftrVx, 
+            mv.y*FUTPERCOUNT-mcal.ftrVy, 
+            mv.z*FUTPERCOUNT-mcal.ftrVz, 
+            av.x, 
+            av.y, 
+            av.z);
+        wait(0.02);
+        
+    };
+}
\ No newline at end of file