SES / Mbed 2 deprecated LPC824mother

test

Dependencies:   mbed

Fork of BNO055_HelloWorld by Dave Turner

BNO055.cpp@1:b178e785986d, 2018-05-22 (annotated)

Committer:
SES01
Date:
Tue May 22 11:29:05 2018 +0000
Revision:
1:b178e785986d
test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
SES01 1:b178e785986d 1 #include "BNO055.h"
SES01 1:b178e785986d 2 #include "mbed.h"
SES01 1:b178e785986d 3
SES01 1:b178e785986d 4 BNO055::BNO055(PinName SDA, PinName SCL) : _i2c(SDA,SCL){
SES01 1:b178e785986d 5 //Set I2C fast and bring reset line high
SES01 1:b178e785986d 6 _i2c.frequency(100000);
SES01 1:b178e785986d 7 address = BNOAddress;
SES01 1:b178e785986d 8 accel_scale = 1.0f;
SES01 1:b178e785986d 9 rate_scale = 1.0f/16.0f;
SES01 1:b178e785986d 10 angle_scale = 1.0f/16.0f;
SES01 1:b178e785986d 11 temp_scale = 1;
SES01 1:b178e785986d 12 }
SES01 1:b178e785986d 13
SES01 1:b178e785986d 14 void BNO055::reset(){
SES01 1:b178e785986d 15 //Perform a power-on-reset
SES01 1:b178e785986d 16 readchar(BNO055_SYS_TRIGGER_ADDR);
SES01 1:b178e785986d 17 rx = rx | 0x20;
SES01 1:b178e785986d 18 writechar(BNO055_SYS_TRIGGER_ADDR,rx);
SES01 1:b178e785986d 19 //Wait for the system to come back up again (datasheet says 650ms)
SES01 1:b178e785986d 20 wait_ms(675);
SES01 1:b178e785986d 21 }
SES01 1:b178e785986d 22
SES01 1:b178e785986d 23 bool BNO055::check(){
SES01 1:b178e785986d 24 //Check we have communication link with the chip
SES01 1:b178e785986d 25 readchar(BNO055_CHIP_ID_ADDR);
SES01 1:b178e785986d 26 if (rx != 0xA0) return false;
SES01 1:b178e785986d 27 //Grab the chip ID and software versions
SES01 1:b178e785986d 28 tx[0] = BNO055_CHIP_ID_ADDR;
SES01 1:b178e785986d 29 _i2c.write(address,tx,1,true);
SES01 1:b178e785986d 30 _i2c.read(address+1,rawdata,7,false);
SES01 1:b178e785986d 31 ID.id = rawdata[0];
SES01 1:b178e785986d 32 ID.accel = rawdata[1];
SES01 1:b178e785986d 33 ID.mag = rawdata[2];
SES01 1:b178e785986d 34 ID.gyro = rawdata[3];
SES01 1:b178e785986d 35 ID.sw[0] = rawdata[4];
SES01 1:b178e785986d 36 ID.sw[1] = rawdata[5];
SES01 1:b178e785986d 37 ID.bootload = rawdata[6];
SES01 1:b178e785986d 38 setpage(1);
SES01 1:b178e785986d 39 tx[0] = BNO055_UNIQUE_ID_ADDR;
SES01 1:b178e785986d 40 _i2c.write(address,tx,1,true);
SES01 1:b178e785986d 41 _i2c.read(address+1,ID.serial,16,false);
SES01 1:b178e785986d 42 setpage(0);
SES01 1:b178e785986d 43 return true;
SES01 1:b178e785986d 44 }
SES01 1:b178e785986d 45
SES01 1:b178e785986d 46 void BNO055::SetExternalCrystal(bool yn){
SES01 1:b178e785986d 47 // Read the current status from the device
SES01 1:b178e785986d 48 readchar(BNO055_SYS_TRIGGER_ADDR);
SES01 1:b178e785986d 49 if (yn) rx = rx | 0x80;
SES01 1:b178e785986d 50 else rx = rx & 0x7F;
SES01 1:b178e785986d 51 writechar(BNO055_SYS_TRIGGER_ADDR,rx);
SES01 1:b178e785986d 52 }
SES01 1:b178e785986d 53
SES01 1:b178e785986d 54 void BNO055::set_accel_units(char units){
SES01 1:b178e785986d 55 readchar(BNO055_UNIT_SEL_ADDR);
SES01 1:b178e785986d 56 if(units == MPERSPERS){
SES01 1:b178e785986d 57 rx = rx & 0xFE;
SES01 1:b178e785986d 58 accel_scale = 0.01f;
SES01 1:b178e785986d 59 }
SES01 1:b178e785986d 60 else {
SES01 1:b178e785986d 61 rx = rx | units;
SES01 1:b178e785986d 62 accel_scale = 1.0f;
SES01 1:b178e785986d 63 }
SES01 1:b178e785986d 64 writechar(BNO055_UNIT_SEL_ADDR,rx);
SES01 1:b178e785986d 65 }
SES01 1:b178e785986d 66
SES01 1:b178e785986d 67 void BNO055::set_anglerate_units(char units){
SES01 1:b178e785986d 68 readchar(BNO055_UNIT_SEL_ADDR);
SES01 1:b178e785986d 69 if (units == DEG_PER_SEC){
SES01 1:b178e785986d 70 rx = rx & 0xFD;
SES01 1:b178e785986d 71 rate_scale = 1.0f/16.0f;
SES01 1:b178e785986d 72 }
SES01 1:b178e785986d 73 else {
SES01 1:b178e785986d 74 rx = rx | units;
SES01 1:b178e785986d 75 rate_scale = 1.0f/900.0f;
SES01 1:b178e785986d 76 }
SES01 1:b178e785986d 77 writechar(BNO055_UNIT_SEL_ADDR,rx);
SES01 1:b178e785986d 78 }
SES01 1:b178e785986d 79
SES01 1:b178e785986d 80 void BNO055::set_angle_units(char units){
SES01 1:b178e785986d 81 readchar(BNO055_UNIT_SEL_ADDR);
SES01 1:b178e785986d 82 if (units == DEGREES){
SES01 1:b178e785986d 83 rx = rx & 0xFB;
SES01 1:b178e785986d 84 angle_scale = 1.0f/16.0f;
SES01 1:b178e785986d 85 }
SES01 1:b178e785986d 86 else {
SES01 1:b178e785986d 87 rx = rx | units;
SES01 1:b178e785986d 88 rate_scale = 1.0f/900.0f;
SES01 1:b178e785986d 89 }
SES01 1:b178e785986d 90 writechar(BNO055_UNIT_SEL_ADDR,rx);
SES01 1:b178e785986d 91 }
SES01 1:b178e785986d 92
SES01 1:b178e785986d 93 void BNO055::set_temp_units(char units){
SES01 1:b178e785986d 94 readchar(BNO055_UNIT_SEL_ADDR);
SES01 1:b178e785986d 95 if (units == CENTIGRADE){
SES01 1:b178e785986d 96 rx = rx & 0xEF;
SES01 1:b178e785986d 97 temp_scale = 1;
SES01 1:b178e785986d 98 }
SES01 1:b178e785986d 99 else {
SES01 1:b178e785986d 100 rx = rx | units;
SES01 1:b178e785986d 101 temp_scale = 2;
SES01 1:b178e785986d 102 }
SES01 1:b178e785986d 103 writechar(BNO055_UNIT_SEL_ADDR,rx);
SES01 1:b178e785986d 104 }
SES01 1:b178e785986d 105
SES01 1:b178e785986d 106 void BNO055::set_orientation(char units){
SES01 1:b178e785986d 107 readchar(BNO055_UNIT_SEL_ADDR);
SES01 1:b178e785986d 108 if (units == WINDOWS) rx = rx &0x7F;
SES01 1:b178e785986d 109 else rx = rx | units;
SES01 1:b178e785986d 110 writechar(BNO055_UNIT_SEL_ADDR,rx);
SES01 1:b178e785986d 111 }
SES01 1:b178e785986d 112
SES01 1:b178e785986d 113 void BNO055::setmode(char omode){
SES01 1:b178e785986d 114 writechar(BNO055_OPR_MODE_ADDR,omode);
SES01 1:b178e785986d 115 op_mode = omode;
SES01 1:b178e785986d 116 }
SES01 1:b178e785986d 117
SES01 1:b178e785986d 118 void BNO055::setpowermode(char pmode){
SES01 1:b178e785986d 119 writechar(BNO055_PWR_MODE_ADDR,pmode);
SES01 1:b178e785986d 120 pwr_mode = pmode;
SES01 1:b178e785986d 121 }
SES01 1:b178e785986d 122
SES01 1:b178e785986d 123 void BNO055::get_accel(void){
SES01 1:b178e785986d 124 tx[0] = BNO055_ACCEL_DATA_X_LSB_ADDR;
SES01 1:b178e785986d 125 _i2c.write(address,tx,1,true);
SES01 1:b178e785986d 126 _i2c.read(address+1,rawdata,6,0);
SES01 1:b178e785986d 127 accel.rawx = (rawdata[1] << 8 | rawdata[0]);
SES01 1:b178e785986d 128 accel.rawy = (rawdata[3] << 8 | rawdata[2]);
SES01 1:b178e785986d 129 accel.rawz = (rawdata[5] << 8 | rawdata[4]);
SES01 1:b178e785986d 130 accel.x = float(accel.rawx)*accel_scale;
SES01 1:b178e785986d 131 accel.y = float(accel.rawy)*accel_scale;
SES01 1:b178e785986d 132 accel.z = float(accel.rawz)*accel_scale;
SES01 1:b178e785986d 133 }
SES01 1:b178e785986d 134
SES01 1:b178e785986d 135 void BNO055::get_gyro(void){
SES01 1:b178e785986d 136 tx[0] = BNO055_GYRO_DATA_X_LSB_ADDR;
SES01 1:b178e785986d 137 _i2c.write(address,tx,1,true);
SES01 1:b178e785986d 138 _i2c.read(address+1,rawdata,6,0);
SES01 1:b178e785986d 139 gyro.rawx = (rawdata[1] << 8 | rawdata[0]);
SES01 1:b178e785986d 140 gyro.rawy = (rawdata[3] << 8 | rawdata[2]);
SES01 1:b178e785986d 141 gyro.rawz = (rawdata[5] << 8 | rawdata[4]);
SES01 1:b178e785986d 142 gyro.x = float(gyro.rawx)*rate_scale;
SES01 1:b178e785986d 143 gyro.y = float(gyro.rawy)*rate_scale;
SES01 1:b178e785986d 144 gyro.z = float(gyro.rawz)*rate_scale;
SES01 1:b178e785986d 145 }
SES01 1:b178e785986d 146
SES01 1:b178e785986d 147 void BNO055::get_mag(void){
SES01 1:b178e785986d 148 tx[0] = BNO055_MAG_DATA_X_LSB_ADDR;
SES01 1:b178e785986d 149 _i2c.write(address,tx,1,true);
SES01 1:b178e785986d 150 _i2c.read(address+1,rawdata,6,0);
SES01 1:b178e785986d 151 mag.rawx = (rawdata[1] << 8 | rawdata[0]);
SES01 1:b178e785986d 152 mag.rawy = (rawdata[3] << 8 | rawdata[2]);
SES01 1:b178e785986d 153 mag.rawz = (rawdata[5] << 8 | rawdata[4]);
SES01 1:b178e785986d 154 mag.x = float(mag.rawx);
SES01 1:b178e785986d 155 mag.y = float(mag.rawy);
SES01 1:b178e785986d 156 mag.z = float(mag.rawz);
SES01 1:b178e785986d 157 }
SES01 1:b178e785986d 158
SES01 1:b178e785986d 159 void BNO055::get_lia(void){
SES01 1:b178e785986d 160 tx[0] = BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR;
SES01 1:b178e785986d 161 _i2c.write(address,tx,1,true);
SES01 1:b178e785986d 162 _i2c.read(address+1,rawdata,6,0);
SES01 1:b178e785986d 163 lia.rawx = (rawdata[1] << 8 | rawdata[0]);
SES01 1:b178e785986d 164 lia.rawy = (rawdata[3] << 8 | rawdata[2]);
SES01 1:b178e785986d 165 lia.rawz = (rawdata[5] << 8 | rawdata[4]);
SES01 1:b178e785986d 166 lia.x = float(lia.rawx)*accel_scale;
SES01 1:b178e785986d 167 lia.y = float(lia.rawy)*accel_scale;
SES01 1:b178e785986d 168 lia.z = float(lia.rawz)*accel_scale;
SES01 1:b178e785986d 169 }
SES01 1:b178e785986d 170
SES01 1:b178e785986d 171 void BNO055::get_grv(void){
SES01 1:b178e785986d 172 tx[0] = BNO055_GRAVITY_DATA_X_LSB_ADDR;
SES01 1:b178e785986d 173 _i2c.write(address,tx,1,true);
SES01 1:b178e785986d 174 _i2c.read(address+1,rawdata,6,0);
SES01 1:b178e785986d 175 gravity.rawx = (rawdata[1] << 8 | rawdata[0]);
SES01 1:b178e785986d 176 gravity.rawy = (rawdata[3] << 8 | rawdata[2]);
SES01 1:b178e785986d 177 gravity.rawz = (rawdata[5] << 8 | rawdata[4]);
SES01 1:b178e785986d 178 gravity.x = float(gravity.rawx)*accel_scale;
SES01 1:b178e785986d 179 gravity.y = float(gravity.rawy)*accel_scale;
SES01 1:b178e785986d 180 gravity.z = float(gravity.rawz)*accel_scale;
SES01 1:b178e785986d 181 }
SES01 1:b178e785986d 182
SES01 1:b178e785986d 183 void BNO055::get_quat(void){
SES01 1:b178e785986d 184 tx[0] = BNO055_QUATERNION_DATA_W_LSB_ADDR;
SES01 1:b178e785986d 185 _i2c.write(address,tx,1,true);
SES01 1:b178e785986d 186 _i2c.read(address+1,rawdata,8,0);
SES01 1:b178e785986d 187 quat.raww = (rawdata[1] << 8 | rawdata[0]);
SES01 1:b178e785986d 188 quat.rawx = (rawdata[3] << 8 | rawdata[2]);
SES01 1:b178e785986d 189 quat.rawy = (rawdata[5] << 8 | rawdata[4]);
SES01 1:b178e785986d 190 quat.rawz = (rawdata[7] << 8 | rawdata[6]);
SES01 1:b178e785986d 191 quat.w = float(quat.raww)/16384.0f;
SES01 1:b178e785986d 192 quat.x = float(quat.rawx)/16384.0f;
SES01 1:b178e785986d 193 quat.y = float(quat.rawy)/16384.0f;
SES01 1:b178e785986d 194 quat.z = float(quat.rawz)/16384.0f;
SES01 1:b178e785986d 195 }
SES01 1:b178e785986d 196
SES01 1:b178e785986d 197 void BNO055::get_angles(void){
SES01 1:b178e785986d 198 tx[0] = BNO055_EULER_H_LSB_ADDR;
SES01 1:b178e785986d 199 _i2c.write(address,tx,1,true);
SES01 1:b178e785986d 200 _i2c.read(address+1,rawdata,6,0);
SES01 1:b178e785986d 201 euler.rawyaw = (rawdata[1] << 8 | rawdata[0]);
SES01 1:b178e785986d 202 euler.rawroll = (rawdata[3] << 8 | rawdata[2]);
SES01 1:b178e785986d 203 euler.rawpitch = (rawdata[5] << 8 | rawdata[4]);
SES01 1:b178e785986d 204 euler.yaw = float(euler.rawyaw)*angle_scale;
SES01 1:b178e785986d 205 euler.roll = float(euler.rawroll)*angle_scale;
SES01 1:b178e785986d 206 euler.pitch = float(euler.rawpitch)*angle_scale;
SES01 1:b178e785986d 207 }
SES01 1:b178e785986d 208
SES01 1:b178e785986d 209
SES01 1:b178e785986d 210 void BNO055::get_temp(void){
SES01 1:b178e785986d 211 readchar(BNO055_TEMP_ADDR);
SES01 1:b178e785986d 212 temperature = rx / temp_scale;
SES01 1:b178e785986d 213 }
SES01 1:b178e785986d 214
SES01 1:b178e785986d 215 void BNO055::get_calib(void){
SES01 1:b178e785986d 216 readchar(BNO055_CALIB_STAT_ADDR);
SES01 1:b178e785986d 217 calib = rx;
SES01 1:b178e785986d 218 }
SES01 1:b178e785986d 219
SES01 1:b178e785986d 220 void BNO055::read_calibration_data(void){
SES01 1:b178e785986d 221 char tempmode = op_mode;
SES01 1:b178e785986d 222 setmode(OPERATION_MODE_CONFIG);
SES01 1:b178e785986d 223 wait_ms(20);
SES01 1:b178e785986d 224 tx[0] = ACCEL_OFFSET_X_LSB_ADDR;
SES01 1:b178e785986d 225 _i2c.write(address,tx,1,true);
SES01 1:b178e785986d 226 _i2c.read(address,calibration,22,false);
SES01 1:b178e785986d 227 setmode(tempmode);
SES01 1:b178e785986d 228 wait_ms(10);
SES01 1:b178e785986d 229 }
SES01 1:b178e785986d 230
SES01 1:b178e785986d 231 void BNO055::write_calibration_data(void){
SES01 1:b178e785986d 232 char tempmode = op_mode;
SES01 1:b178e785986d 233 setmode(OPERATION_MODE_CONFIG);
SES01 1:b178e785986d 234 wait_ms(20);
SES01 1:b178e785986d 235 tx[0] = ACCEL_OFFSET_X_LSB_ADDR;
SES01 1:b178e785986d 236 _i2c.write(address,tx,1,true);
SES01 1:b178e785986d 237 _i2c.write(address,calibration,22,false);
SES01 1:b178e785986d 238 setmode(tempmode);
SES01 1:b178e785986d 239 wait_ms(10);
SES01 1:b178e785986d 240 }
SES01 1:b178e785986d 241
SES01 1:b178e785986d 242 void BNO055::set_mapping(char orient){
SES01 1:b178e785986d 243 switch (orient){
SES01 1:b178e785986d 244 case 0:
SES01 1:b178e785986d 245 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21);
SES01 1:b178e785986d 246 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x04);
SES01 1:b178e785986d 247 break;
SES01 1:b178e785986d 248 case 1:
SES01 1:b178e785986d 249 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
SES01 1:b178e785986d 250 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x00);
SES01 1:b178e785986d 251 break;
SES01 1:b178e785986d 252 case 2:
SES01 1:b178e785986d 253 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
SES01 1:b178e785986d 254 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x00);
SES01 1:b178e785986d 255 break;
SES01 1:b178e785986d 256 case 3:
SES01 1:b178e785986d 257 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21);
SES01 1:b178e785986d 258 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x02);
SES01 1:b178e785986d 259 break;
SES01 1:b178e785986d 260 case 4:
SES01 1:b178e785986d 261 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
SES01 1:b178e785986d 262 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x03);
SES01 1:b178e785986d 263 break;
SES01 1:b178e785986d 264 case 5:
SES01 1:b178e785986d 265 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21);
SES01 1:b178e785986d 266 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x01);
SES01 1:b178e785986d 267 break;
SES01 1:b178e785986d 268 case 6:
SES01 1:b178e785986d 269 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21);
SES01 1:b178e785986d 270 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x07);
SES01 1:b178e785986d 271 break;
SES01 1:b178e785986d 272 case 7:
SES01 1:b178e785986d 273 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
SES01 1:b178e785986d 274 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x05);
SES01 1:b178e785986d 275 break;
SES01 1:b178e785986d 276 default:
SES01 1:b178e785986d 277 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
SES01 1:b178e785986d 278 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x00);
SES01 1:b178e785986d 279 }
SES01 1:b178e785986d 280 }