BNO055.cpp

00001 #include "BNO055.h"
00002 #include <vector>
00003 
00004 namespace BNO055
00005 {
00006 /*
00007 *  Sensor constructor. This will take care of all necessary initialization and sensor calibration
00008 */    
00009 Sensor::Sensor(MicroBit & bit) : 
00010     _bit(bit),
00011     _I2CAddress(0x52),  // 0x28 : 0x29
00012     _currentPage(0xFF)       
00013  {
00014     LogDeviceId();
00015     Init();
00016  }
00017  
00018  /*
00019  *  This is just a sanity check. Verfies chip/accelerometer/magnetometer and gyro id.
00020  */
00021 void Sensor::LogDeviceId()
00022 {
00023       SelectPage(0); 
00024  
00025     char error[200];
00026     char cmd[7] = {CHIP_ID_REGISTER, 0, 0, 0, 0, 0, 0};
00027     _bit.i2c.write(_I2CAddress, cmd, 1, true);
00028     _bit.i2c.read(_I2CAddress, cmd, 7, false);
00029     
00030     if (cmd[0] == BNO055_CHIP_ID) {
00031         _bit.serial.send("BNO055 CHIP ID - OK\n");
00032     } else {
00033         sprintf(error, "BNO055 CHIP ID - ERROR. Got: %d\n", cmd[0]);
00034         _bit.serial.send(error);
00035     }
00036     if (cmd[1] == BNO055_ACC_ID) {
00037         _bit.serial.send("BNO055 ACCELEROMETER ID - OK\n");
00038     } else {
00039         sprintf(error, "BNO055 ACCELEROMETER ID - ERROR. Got: %d\n", cmd[1]);
00040         _bit.serial.send(error);
00041     }
00042     if (cmd[2] == BNO055_MAG_ID) {
00043         _bit.serial.send("BNO055 MAGNETOMETER ID - OK\n");
00044     } else {
00045         sprintf(error, "BNO055 ACCELERMAGNETOMETEROMETER ID - ERROR. Got: %d\n", cmd[2]);
00046         _bit.serial.send(error);
00047     }
00048     if (cmd[3] == BNO055_GYRO_ID) {
00049       _bit.serial.send("BNO055 GYRO ID - OK\n");
00050     } else {
00051         sprintf(error, "BNO055 GYRO ID - ERROR. Got: %d\n", cmd[3]);
00052         _bit.serial.send(error);
00053     }
00054 }
00055 
00056 uint8_t Sensor::CalibrationStatus(void)
00057 {
00058     SelectPage(0);
00059     char cmd[1] = {CALIBRATION_STATUS};
00060     _bit.i2c.write(_I2CAddress, cmd, 1, true);
00061     _bit.i2c.read(_I2CAddress, cmd, 1, false);
00062     return cmd[0];
00063 }
00064 
00065 void Sensor::Calibrate()
00066 {
00067     _bit.display.scroll("CALIBRATE");
00068     while  (true) {
00069         uint8_t status = (CalibrationStatus() & 0b11000000) >> 6;
00070         _bit.display.scroll(status);
00071         if (3 == status)
00072             break;
00073         wait(0.1);
00074     }
00075     
00076     _bit.display.scroll("OK");
00077     
00078     for (int i=5; i>= 0; i--) {
00079         _bit.display.printChar(i+48);
00080         wait(0.5);
00081     }
00082   
00083     
00084 }
00085 
00086 
00087 void Sensor::SelectNDOFFusion()
00088 {
00089    SelectPage(0); 
00090     char cmd[2] = { OPR_MODE_REGISTER, NDOF_FUSION_MODE };
00091     _bit.i2c.write(_I2CAddress, cmd, 2); 
00092 }
00093    
00094 void Sensor::SelectUnits()
00095 {
00096      SelectPage(0);
00097      char cmd[2] = {UNIT_SELECTION_REGISTER, UNIT_ORI_WIN + UNIT_ACC_MSS + UNIT_GYR_DPS + UNIT_EULER_DEG + UNIT_TEMP_C};
00098     _bit.i2c.write(_I2CAddress, cmd, 2, false);
00099 }
00100 
00101 void Sensor::Init()
00102 {
00103     SelectUnits();
00104     SelectNDOFFusion();
00105 }
00106 
00107 void Sensor::SelectPage(uint8_t page)
00108 {
00109     if (page != _currentPage)
00110     {
00111         char cmd[2] = {PAGE_ID_REGISTER, page};
00112         _bit.i2c.write(_I2CAddress, cmd, 2, false);
00113         _currentPage = page; // Happy path ;)
00114     }
00115 }
00116 
00117 void Sensor::ReadEulerAngles(double & heading, double & roll, double & pitch)
00118 {
00119     char cmd[6] = {EULER_H_REGISTER_LSB, 0, 0, 0, 0, 0};
00120     int16_t _heading;
00121     int16_t _roll;
00122     int16_t _pitch;
00123     SelectPage(0);
00124     _bit.i2c.write(_I2CAddress, cmd, 1, true);
00125     _bit.i2c.read(_I2CAddress, cmd, 6, false);
00126     _heading = cmd[1] << 8 | cmd[0];
00127     _pitch = cmd[3] << 8 | cmd[2];
00128     _roll = cmd[5] << 8 | cmd[4];
00129     heading = (double)_heading / 16;
00130     roll = (double)_roll / 16;
00131     pitch = (double)_pitch / 16;
00132 }
00133 
00134 }