Diff: main.cpp

Revision:

2:514af7294ad0

Parent:

1:7562cc147e26

Child:

3:c0d70871aa4d

diff -r 7562cc147e26 -r 514af7294ad0 main.cpp
--- a/main.cpp	Mon Jun 20 19:29:09 2016 +0000
+++ b/main.cpp	Mon Jun 20 20:10:37 2016 +0000
@@ -18,9 +18,9 @@
 #include "ble/BLE.h"
 
 #include "ble/services/UARTService.h"
-#include "LSM6DS3/LSM6DS3.h"
+#include "BNO055/BNO055.h"
 
-LSM6DS3 imu(p30, p7);
+BNO055 imu(p30, p7);
 Serial pc(p9, p11);
 
 #define NEED_CONSOLE_OUTPUT 1 /* Set this if you need debug messages on the console;
@@ -49,33 +49,33 @@
     if ((uartServicePtr != NULL) && (params->handle == uartServicePtr->getTXCharacteristicHandle())) {
         uint16_t bytesRead = params->len;
         uint8_t value = *(params->data);
-        uint16_t status;
         DEBUG("received %u bytes\n\r", bytesRead);
         DEBUG("recevied %d data\n\r", value);
         
+        // Set power mode, POWER_MODE_NORMAL(default), POWER_MODE_LOWPOWER, or POWER_MODE_SUSPEND(sleep)
+        // By default, I2C will pull high when bus is free(pg90 of datasheet)
         switch(value) {
             case 49:
                 DEBUG("Off power\n\r");
-                status = imu.begin(imu.G_SCALE_245DPS, imu.A_SCALE_2G,
-                                   imu.G_POWER_DOWN, imu.A_POWER_DOWN);
+                imu.setpowermode(POWER_MODE_SUSPEND);
                 break;
             case 50:
                 DEBUG("Low power\n\r");
-                status = imu.begin(imu.G_SCALE_245DPS, imu.A_SCALE_2G,
-                                   imu.G_ODR_13_BW_0, imu.A_ODR_13);
+                imu.setpowermode(POWER_MODE_LOWPOWER);
                 break;
             case 51:
                 DEBUG("High power\n\r");
-                status = imu.begin(imu.G_SCALE_2000DPS, imu.A_SCALE_8G,
-                                   imu.G_ODR_1660, imu.A_ODR_6660);
+                imu.setpowermode(POWER_MODE_NORMAL);
                 break;
             default:
                 DEBUG("Nothing happened\n\r");
                 break;
         }
                 
-        pc.printf("LSM6DS3 WHO_AM_I's returned: 0x%X\r\n", status);
-        pc.printf("Should be 0x69\r\n");
+        if (imu.check())
+        {
+            pc.printf("BNO055 found\r\n");
+        }
         
         ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), params->data, bytesRead);
     }
@@ -111,6 +111,10 @@
 
     UARTService uartService(ble);
     uartServicePtr = &uartService;
+    
+    // setup baud rate and reset sensor
+    pc.baud(115200);
+    imu.reset();
 
     while (true) {
         ble.waitForEvent();