yotaro morizumi / Mbed 2 deprecated zoomy_customLibrary

my new gear...

Dependencies:   mbed

BNO055.cpp@0:1456b6f84c75, 2022-02-25 (annotated)

Committer:
yootee
Date:
Fri Feb 25 05:20:11 2022 +0000
Revision:
0:1456b6f84c75
my_custom

Who changed what in which revision?

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