Diff: main.cpp

Revision:

0:1b3e1eec2577

Child:

1:622d7b18a1c0

diff -r 000000000000 -r 1b3e1eec2577 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Dec 23 08:46:06 2013 +0000
@@ -0,0 +1,122 @@
+#include "mbed.h"
+#include "rtos.h"
+#include "FreeIMU.h"
+
+Serial pc(USBTX, USBRX); // tx, rx
+
+typedef struct {
+    float q[4];
+} imu_t;
+
+FreeIMU imu;
+Mail<imu_t, 8> imu_queue;
+int i = 0;
+
+void getIMUdata(void const *n)
+{
+    i++;
+    if (i%4==0) {
+        float *q = (float *)imu_queue.alloc();
+        imu.getQ(q);
+        imu_queue.put((imu_t*)q);
+    } else {
+        imu.getQ(NULL);
+    }
+}
+
+// thanks to Francesco Ferrara and the Simplo project for the following code!
+void writeVar(void * val, uint8_t type_bytes)
+{
+    char * addr=(char *)(val);
+    for(uint8_t i=0; i<type_bytes; i++) {
+        //Serial1.write(addr[i]);
+        pc.putc(addr[i]);
+    }
+}
+
+void writeArr(void * varr, uint8_t arr_length, uint8_t type_bytes)
+{
+    char * arr = (char*) varr;
+    for(uint8_t i=0; i<arr_length; i++) {
+        writeVar(&arr[i * type_bytes], type_bytes);
+    }
+}
+
+void serialFloatPrint(float f)
+{
+    char * b = (char *) &f;
+    for(int i=0; i<4; i++) {
+
+        char b1 = (b[i] >> 4) & 0x0f;
+        char b2 = (b[i] & 0x0f);
+
+        char c1 = (b1 < 10) ? ('0' + b1) : 'A' + b1 - 10;
+        char c2 = (b2 < 10) ? ('0' + b2) : 'A' + b2 - 10;
+
+        pc.putc(c1);
+        pc.putc(c2);
+    }
+}
+
+void serialPrintFloatArr(float * arr, int length)
+{
+    for(int i=0; i<length; i++) {
+        serialFloatPrint(arr[i]);
+        pc.putc(',');
+    }
+}
+
+int main()
+{
+    pc.baud(115200);
+    imu.init(true);
+    pc.baud(115200);
+    RtosTimer IMUTimer(getIMUdata, osTimerPeriodic, (void *)0);
+    IMUTimer.start(5);
+
+    while(true) {
+        int16_t raw_values[9];
+
+        Thread::wait(10);
+        char cmd = 'q';//pc.getc();
+
+        if(cmd=='v') {
+            char str[256];
+            sprintf(str, "FreeIMU library by %s, FREQ:%s, LIB_VERSION: %s, IMU: %s", FREEIMU_DEVELOPER, 0x10000000, FREEIMU_LIB_VERSION, 0);
+            pc.printf("%s", str);
+            pc.printf("\n");
+        }
+
+        if (cmd=='q') {
+            uint8_t count = 32;//pc.getc();
+
+            for(uint8_t i=0; i<count; i++) {
+                osEvent evt = imu_queue.get();
+                if (evt.status == osEventMail) {
+                    imu_t *obj = (imu_t*)evt.value.p;
+                    serialPrintFloatArr(obj->q, 4);
+                    imu_queue.free(obj);
+                    pc.printf("\r\n");
+                }
+                Thread::yield();
+            }
+        }
+
+        if(cmd=='b') {
+            uint8_t count = pc.getc();
+            for(uint8_t i=0; i<count; i++) {
+
+                imu.accgyro.getMotion6(&raw_values[0], &raw_values[1], &raw_values[2], &raw_values[3], &raw_values[4], &raw_values[5]);
+
+                writeArr(raw_values, 6, sizeof(int16_t)); // writes accelerometer and gyro values
+
+                imu.magn.getValues(&raw_values[0], &raw_values[1], &raw_values[2]);
+                writeArr(raw_values, 3, sizeof(int16_t));
+
+                pc.printf("\r\n");
+                wait_us(4000);
+            }
+        }
+    }
+
+}