succes
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?
User | Revision | Line number | New 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 | } |