Diff: RTOS-Setup/src/setup.cpp

Revision:

1:43f8ac7ca6d7

Parent:

0:8c28fac22d27

Child:

2:ab967d7b4346

diff -r 8c28fac22d27 -r 43f8ac7ca6d7 RTOS-Setup/src/setup.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RTOS-Setup/src/setup.cpp	Tue Apr 29 11:02:18 2014 +0000
@@ -0,0 +1,77 @@
+#include "setup.h"
+#include "tasks.h"
+
+Serial BT(p13, p14);
+DigitalOut BT_CMD(p12);
+
+MPU6050 imu(p9, p10);
+
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+
+bool imu_available = false;
+
+// MPU control/status vars
+bool dmpReady = false; // set true if DMP init was successful
+uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
+uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
+
+bool setup_led(void)
+{
+    ledReset();
+    return true;
+}
+
+bool setup_bt(void)
+{
+    BT.baud(115200);
+    BT_CMD = 0;
+    BT.printf("Bluetooth online!\n");
+    return true;
+}
+
+bool setup_mpu6050(void)
+{
+    imu.reset();
+    imu.debugSerial.baud(115200);
+    wait_ms(5);
+
+    imu.initialize();
+    imu_available = imu.testConnection();
+
+    imu.debugSerial.printf("IMU status...\t\t\t");
+    imu_available ? imu.debugSerial.printf("OK!\n") : imu.debugSerial.printf("NOT OK!\n");
+
+    if (imu_available) {
+        devStatus = imu.dmpInitialize();
+
+        /*
+         // supply your own gyro offsets here, scaled for min sensitivity
+        mpu.setXGyroOffset(-61);
+        mpu.setYGyroOffset(-127);
+        mpu.setZGyroOffset(19);
+        mpu.setZAccelOffset(16282); // 1688 factory default for my test chip
+        */
+
+        if(!devStatus) {
+            imu.setDMPEnabled(true);
+            while (!imu.getIntDataReadyStatus());
+            packetSize = imu.dmpGetFIFOPacketSize();
+            dmpReady = true;
+        } else {
+            imu.debugSerial.printf("\tDMP setup failed!\n");
+            return false;
+        }
+
+        imu.resetFIFO();
+    } else {
+        return false;
+    }
+
+    imu.debugSerial.printf("IMU setup routine done!");
+
+    return dmpReady;
+}
+