Marek Korczak / nrf51_microbit

succes

Dependencies:   microbit

BNO055.cpp@0:c15430f1895f, 2017-02-11 (annotated)

Committer:
mk1
Date:
Sat Feb 11 15:55:34 2017 +0000
Revision:
0:c15430f1895f
succes

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mk1 0:c15430f1895f 1 #include "BNO055.h"
mk1 0:c15430f1895f 2 #include <vector>
mk1 0:c15430f1895f 3
mk1 0:c15430f1895f 4 namespace BNO055
mk1 0:c15430f1895f 5 {
mk1 0:c15430f1895f 6 /*
mk1 0:c15430f1895f 7 * Sensor constructor. This will take care of all necessary initialization and sensor calibration
mk1 0:c15430f1895f 8 */
mk1 0:c15430f1895f 9 Sensor::Sensor(MicroBit & bit) :
mk1 0:c15430f1895f 10 _bit(bit),
mk1 0:c15430f1895f 11 _I2CAddress(0x52), // 0x28 : 0x29
mk1 0:c15430f1895f 12 _currentPage(0xFF)
mk1 0:c15430f1895f 13 {
mk1 0:c15430f1895f 14 LogDeviceId();
mk1 0:c15430f1895f 15 Init();
mk1 0:c15430f1895f 16 }
mk1 0:c15430f1895f 17
mk1 0:c15430f1895f 18 /*
mk1 0:c15430f1895f 19 * This is just a sanity check. Verfies chip/accelerometer/magnetometer and gyro id.
mk1 0:c15430f1895f 20 */
mk1 0:c15430f1895f 21 void Sensor::LogDeviceId()
mk1 0:c15430f1895f 22 {
mk1 0:c15430f1895f 23 SelectPage(0);
mk1 0:c15430f1895f 24
mk1 0:c15430f1895f 25 char error[200];
mk1 0:c15430f1895f 26 char cmd[7] = {CHIP_ID_REGISTER, 0, 0, 0, 0, 0, 0};
mk1 0:c15430f1895f 27 _bit.i2c.write(_I2CAddress, cmd, 1, true);
mk1 0:c15430f1895f 28 _bit.i2c.read(_I2CAddress, cmd, 7, false);
mk1 0:c15430f1895f 29
mk1 0:c15430f1895f 30 if (cmd[0] == BNO055_CHIP_ID) {
mk1 0:c15430f1895f 31 _bit.serial.send("BNO055 CHIP ID - OK\n");
mk1 0:c15430f1895f 32 } else {
mk1 0:c15430f1895f 33 sprintf(error, "BNO055 CHIP ID - ERROR. Got: %d\n", cmd[0]);
mk1 0:c15430f1895f 34 _bit.serial.send(error);
mk1 0:c15430f1895f 35 }
mk1 0:c15430f1895f 36 if (cmd[1] == BNO055_ACC_ID) {
mk1 0:c15430f1895f 37 _bit.serial.send("BNO055 ACCELEROMETER ID - OK\n");
mk1 0:c15430f1895f 38 } else {
mk1 0:c15430f1895f 39 sprintf(error, "BNO055 ACCELEROMETER ID - ERROR. Got: %d\n", cmd[1]);
mk1 0:c15430f1895f 40 _bit.serial.send(error);
mk1 0:c15430f1895f 41 }
mk1 0:c15430f1895f 42 if (cmd[2] == BNO055_MAG_ID) {
mk1 0:c15430f1895f 43 _bit.serial.send("BNO055 MAGNETOMETER ID - OK\n");
mk1 0:c15430f1895f 44 } else {
mk1 0:c15430f1895f 45 sprintf(error, "BNO055 ACCELERMAGNETOMETEROMETER ID - ERROR. Got: %d\n", cmd[2]);
mk1 0:c15430f1895f 46 _bit.serial.send(error);
mk1 0:c15430f1895f 47 }
mk1 0:c15430f1895f 48 if (cmd[3] == BNO055_GYRO_ID) {
mk1 0:c15430f1895f 49 _bit.serial.send("BNO055 GYRO ID - OK\n");
mk1 0:c15430f1895f 50 } else {
mk1 0:c15430f1895f 51 sprintf(error, "BNO055 GYRO ID - ERROR. Got: %d\n", cmd[3]);
mk1 0:c15430f1895f 52 _bit.serial.send(error);
mk1 0:c15430f1895f 53 }
mk1 0:c15430f1895f 54 }
mk1 0:c15430f1895f 55
mk1 0:c15430f1895f 56 uint8_t Sensor::CalibrationStatus(void)
mk1 0:c15430f1895f 57 {
mk1 0:c15430f1895f 58 SelectPage(0);
mk1 0:c15430f1895f 59 char cmd[1] = {CALIBRATION_STATUS};
mk1 0:c15430f1895f 60 _bit.i2c.write(_I2CAddress, cmd, 1, true);
mk1 0:c15430f1895f 61 _bit.i2c.read(_I2CAddress, cmd, 1, false);
mk1 0:c15430f1895f 62 return cmd[0];
mk1 0:c15430f1895f 63 }
mk1 0:c15430f1895f 64
mk1 0:c15430f1895f 65 void Sensor::Calibrate()
mk1 0:c15430f1895f 66 {
mk1 0:c15430f1895f 67 _bit.display.scroll("CALIBRATE");
mk1 0:c15430f1895f 68 while (true) {
mk1 0:c15430f1895f 69 uint8_t status = (CalibrationStatus() & 0b11000000) >> 6;
mk1 0:c15430f1895f 70 _bit.display.scroll(status);
mk1 0:c15430f1895f 71 if (3 == status)
mk1 0:c15430f1895f 72 break;
mk1 0:c15430f1895f 73 wait(0.1);
mk1 0:c15430f1895f 74 }
mk1 0:c15430f1895f 75
mk1 0:c15430f1895f 76 _bit.display.scroll("OK");
mk1 0:c15430f1895f 77
mk1 0:c15430f1895f 78 for (int i=5; i>= 0; i--) {
mk1 0:c15430f1895f 79 _bit.display.printChar(i+48);
mk1 0:c15430f1895f 80 wait(0.5);
mk1 0:c15430f1895f 81 }
mk1 0:c15430f1895f 82
mk1 0:c15430f1895f 83
mk1 0:c15430f1895f 84 }
mk1 0:c15430f1895f 85
mk1 0:c15430f1895f 86
mk1 0:c15430f1895f 87 void Sensor::SelectNDOFFusion()
mk1 0:c15430f1895f 88 {
mk1 0:c15430f1895f 89 SelectPage(0);
mk1 0:c15430f1895f 90 char cmd[2] = { OPR_MODE_REGISTER, NDOF_FUSION_MODE };
mk1 0:c15430f1895f 91 _bit.i2c.write(_I2CAddress, cmd, 2);
mk1 0:c15430f1895f 92 }
mk1 0:c15430f1895f 93
mk1 0:c15430f1895f 94 void Sensor::SelectUnits()
mk1 0:c15430f1895f 95 {
mk1 0:c15430f1895f 96 SelectPage(0);
mk1 0:c15430f1895f 97 char cmd[2] = {UNIT_SELECTION_REGISTER, UNIT_ORI_WIN + UNIT_ACC_MSS + UNIT_GYR_DPS + UNIT_EULER_DEG + UNIT_TEMP_C};
mk1 0:c15430f1895f 98 _bit.i2c.write(_I2CAddress, cmd, 2, false);
mk1 0:c15430f1895f 99 }
mk1 0:c15430f1895f 100
mk1 0:c15430f1895f 101 void Sensor::Init()
mk1 0:c15430f1895f 102 {
mk1 0:c15430f1895f 103 SelectUnits();
mk1 0:c15430f1895f 104 SelectNDOFFusion();
mk1 0:c15430f1895f 105 }
mk1 0:c15430f1895f 106
mk1 0:c15430f1895f 107 void Sensor::SelectPage(uint8_t page)
mk1 0:c15430f1895f 108 {
mk1 0:c15430f1895f 109 if (page != _currentPage)
mk1 0:c15430f1895f 110 {
mk1 0:c15430f1895f 111 char cmd[2] = {PAGE_ID_REGISTER, page};
mk1 0:c15430f1895f 112 _bit.i2c.write(_I2CAddress, cmd, 2, false);
mk1 0:c15430f1895f 113 _currentPage = page; // Happy path ;)
mk1 0:c15430f1895f 114 }
mk1 0:c15430f1895f 115 }
mk1 0:c15430f1895f 116
mk1 0:c15430f1895f 117 void Sensor::ReadEulerAngles(double & heading, double & roll, double & pitch)
mk1 0:c15430f1895f 118 {
mk1 0:c15430f1895f 119 char cmd[6] = {EULER_H_REGISTER_LSB, 0, 0, 0, 0, 0};
mk1 0:c15430f1895f 120 int16_t _heading;
mk1 0:c15430f1895f 121 int16_t _roll;
mk1 0:c15430f1895f 122 int16_t _pitch;
mk1 0:c15430f1895f 123 SelectPage(0);
mk1 0:c15430f1895f 124 _bit.i2c.write(_I2CAddress, cmd, 1, true);
mk1 0:c15430f1895f 125 _bit.i2c.read(_I2CAddress, cmd, 6, false);
mk1 0:c15430f1895f 126 _heading = cmd[1] << 8 | cmd[0];
mk1 0:c15430f1895f 127 _pitch = cmd[3] << 8 | cmd[2];
mk1 0:c15430f1895f 128 _roll = cmd[5] << 8 | cmd[4];
mk1 0:c15430f1895f 129 heading = (double)_heading / 16;
mk1 0:c15430f1895f 130 roll = (double)_roll / 16;
mk1 0:c15430f1895f 131 pitch = (double)_pitch / 16;
mk1 0:c15430f1895f 132 }
mk1 0:c15430f1895f 133
mk1 0:c15430f1895f 134 }