Diff: BNO055.cpp

Revision:

0:c15430f1895f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BNO055.cpp	Sat Feb 11 15:55:34 2017 +0000
@@ -0,0 +1,134 @@
+#include "BNO055.h"
+#include <vector>
+
+namespace BNO055
+{
+/*
+*  Sensor constructor. This will take care of all necessary initialization and sensor calibration
+*/    
+Sensor::Sensor(MicroBit & bit) : 
+    _bit(bit),
+    _I2CAddress(0x52),  // 0x28 : 0x29
+    _currentPage(0xFF)       
+ {
+    LogDeviceId();
+    Init();
+ }
+ 
+ /*
+ *  This is just a sanity check. Verfies chip/accelerometer/magnetometer and gyro id.
+ */
+void Sensor::LogDeviceId()
+{
+      SelectPage(0); 
+ 
+    char error[200];
+    char cmd[7] = {CHIP_ID_REGISTER, 0, 0, 0, 0, 0, 0};
+    _bit.i2c.write(_I2CAddress, cmd, 1, true);
+    _bit.i2c.read(_I2CAddress, cmd, 7, false);
+    
+    if (cmd[0] == BNO055_CHIP_ID) {
+        _bit.serial.send("BNO055 CHIP ID - OK\n");
+    } else {
+        sprintf(error, "BNO055 CHIP ID - ERROR. Got: %d\n", cmd[0]);
+        _bit.serial.send(error);
+    }
+    if (cmd[1] == BNO055_ACC_ID) {
+        _bit.serial.send("BNO055 ACCELEROMETER ID - OK\n");
+    } else {
+        sprintf(error, "BNO055 ACCELEROMETER ID - ERROR. Got: %d\n", cmd[1]);
+        _bit.serial.send(error);
+    }
+    if (cmd[2] == BNO055_MAG_ID) {
+        _bit.serial.send("BNO055 MAGNETOMETER ID - OK\n");
+    } else {
+        sprintf(error, "BNO055 ACCELERMAGNETOMETEROMETER ID - ERROR. Got: %d\n", cmd[2]);
+        _bit.serial.send(error);
+    }
+    if (cmd[3] == BNO055_GYRO_ID) {
+      _bit.serial.send("BNO055 GYRO ID - OK\n");
+    } else {
+        sprintf(error, "BNO055 GYRO ID - ERROR. Got: %d\n", cmd[3]);
+        _bit.serial.send(error);
+    }
+}
+
+uint8_t Sensor::CalibrationStatus(void)
+{
+    SelectPage(0);
+    char cmd[1] = {CALIBRATION_STATUS};
+    _bit.i2c.write(_I2CAddress, cmd, 1, true);
+    _bit.i2c.read(_I2CAddress, cmd, 1, false);
+    return cmd[0];
+}
+
+void Sensor::Calibrate()
+{
+    _bit.display.scroll("CALIBRATE");
+    while  (true) {
+        uint8_t status = (CalibrationStatus() & 0b11000000) >> 6;
+        _bit.display.scroll(status);
+        if (3 == status)
+            break;
+        wait(0.1);
+    }
+    
+    _bit.display.scroll("OK");
+    
+    for (int i=5; i>= 0; i--) {
+        _bit.display.printChar(i+48);
+        wait(0.5);
+    }
+  
+    
+}
+
+
+void Sensor::SelectNDOFFusion()
+{
+   SelectPage(0); 
+    char cmd[2] = { OPR_MODE_REGISTER, NDOF_FUSION_MODE };
+    _bit.i2c.write(_I2CAddress, cmd, 2); 
+}
+   
+void Sensor::SelectUnits()
+{
+     SelectPage(0);
+     char cmd[2] = {UNIT_SELECTION_REGISTER, UNIT_ORI_WIN + UNIT_ACC_MSS + UNIT_GYR_DPS + UNIT_EULER_DEG + UNIT_TEMP_C};
+    _bit.i2c.write(_I2CAddress, cmd, 2, false);
+}
+
+void Sensor::Init()
+{
+    SelectUnits();
+    SelectNDOFFusion();
+}
+
+void Sensor::SelectPage(uint8_t page)
+{
+    if (page != _currentPage)
+    {
+        char cmd[2] = {PAGE_ID_REGISTER, page};
+        _bit.i2c.write(_I2CAddress, cmd, 2, false);
+        _currentPage = page; // Happy path ;)
+    }
+}
+
+void Sensor::ReadEulerAngles(double & heading, double & roll, double & pitch)
+{
+    char cmd[6] = {EULER_H_REGISTER_LSB, 0, 0, 0, 0, 0};
+    int16_t _heading;
+    int16_t _roll;
+    int16_t _pitch;
+    SelectPage(0);
+    _bit.i2c.write(_I2CAddress, cmd, 1, true);
+    _bit.i2c.read(_I2CAddress, cmd, 6, false);
+    _heading = cmd[1] << 8 | cmd[0];
+    _pitch = cmd[3] << 8 | cmd[2];
+    _roll = cmd[5] << 8 | cmd[4];
+    heading = (double)_heading / 16;
+    roll = (double)_roll / 16;
+    pitch = (double)_pitch / 16;
+}
+
+}