Chawankorn Tiemprasert / BNO055

imu libraries use in class project

Dependents:   Ros_STM32_IMU_BNO055 baseControl_ackermannCar

BNO055.cpp@0:445290b98598, 2018-12-15 (annotated)

Committer:
chawankorn
Date:
Sat Dec 15 08:43:30 2018 +0000
Revision:
0:445290b98598
class project nucleo_IMU

Who changed what in which revision?

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