File content as of revision 0:c15430f1895f:

#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;
}

}