Takushima Yukimasa / Mbed 2 deprecated R19_ManuTakukAHWGeo

a

Dependencies:   mbed

BNO055/BNO055.cpp@20:3f68738c4019, 2019-10-11 (annotated)

Committer:
Tom0108
Date:
Fri Oct 11 07:29:15 2019 +0000
Revision:
20:3f68738c4019
Parent:
11:c1a22e2e3534 
Gyro reset measures

Who changed what in which revision?

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