yotaro morizumi
/
zoomy_customLibrary
my new gear...
sensor/BNO055.cpp@20:4e0fa6fba27c, 2022-09-30 (annotated)
- Committer:
- yootee
- Date:
- Fri Sep 30 00:04:11 2022 +0000
- Revision:
- 20:4e0fa6fba27c
- Parent:
- 7:1c09bcc18c14
oko
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
yootee | 0:1456b6f84c75 | 1 | #include "BNO055.h" |
yootee | 0:1456b6f84c75 | 2 | #include "mbed.h" |
yootee | 0:1456b6f84c75 | 3 | |
yootee | 20:4e0fa6fba27c | 4 | BNO055::BNO055(PinName SDA, PinName SCL, int freq = 400000) : _i2c(SDA,SCL) |
yootee | 0:1456b6f84c75 | 5 | { |
yootee | 0:1456b6f84c75 | 6 | //Set I2C fast and bring reset line high |
yootee | 20:4e0fa6fba27c | 7 | _i2c.frequency(freq); |
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 | 7:1c09bcc18c14 | 219 | euler.yaw = 360 - euler.yaw; |
yootee | 0:1456b6f84c75 | 220 | //euler.roll = float(euler.rawroll)*angle_scale; |
yootee | 0:1456b6f84c75 | 221 | //euler.pitch = float(euler.rawpitch)*angle_scale; |
yootee | 0:1456b6f84c75 | 222 | } |
yootee | 0:1456b6f84c75 | 223 | |
yootee | 0:1456b6f84c75 | 224 | |
yootee | 0:1456b6f84c75 | 225 | void BNO055::get_temp(void) |
yootee | 0:1456b6f84c75 | 226 | { |
yootee | 0:1456b6f84c75 | 227 | readchar(BNO055_TEMP_ADDR); |
yootee | 0:1456b6f84c75 | 228 | temperature = rx / temp_scale; |
yootee | 0:1456b6f84c75 | 229 | } |
yootee | 0:1456b6f84c75 | 230 | |
yootee | 0:1456b6f84c75 | 231 | void BNO055::get_calib(void) |
yootee | 0:1456b6f84c75 | 232 | { |
yootee | 0:1456b6f84c75 | 233 | readchar(BNO055_CALIB_STAT_ADDR); |
yootee | 0:1456b6f84c75 | 234 | calib = rx; |
yootee | 0:1456b6f84c75 | 235 | } |
yootee | 0:1456b6f84c75 | 236 | |
yootee | 0:1456b6f84c75 | 237 | void BNO055::read_calibration_data(void) |
yootee | 0:1456b6f84c75 | 238 | { |
yootee | 0:1456b6f84c75 | 239 | char tempmode = op_mode; |
yootee | 0:1456b6f84c75 | 240 | setmode(OPERATION_MODE_CONFIG); |
yootee | 0:1456b6f84c75 | 241 | wait_ms(20); |
yootee | 0:1456b6f84c75 | 242 | tx[0] = ACCEL_OFFSET_X_LSB_ADDR; |
yootee | 0:1456b6f84c75 | 243 | _i2c.write(address,tx,1,true); |
yootee | 0:1456b6f84c75 | 244 | _i2c.read(address,calibration,22,false); |
yootee | 0:1456b6f84c75 | 245 | setmode(tempmode); |
yootee | 0:1456b6f84c75 | 246 | wait_ms(10); |
yootee | 0:1456b6f84c75 | 247 | } |
yootee | 0:1456b6f84c75 | 248 | |
yootee | 0:1456b6f84c75 | 249 | void BNO055::write_calibration_data(void) |
yootee | 0:1456b6f84c75 | 250 | { |
yootee | 0:1456b6f84c75 | 251 | char tempmode = op_mode; |
yootee | 0:1456b6f84c75 | 252 | setmode(OPERATION_MODE_CONFIG); |
yootee | 0:1456b6f84c75 | 253 | wait_ms(20); |
yootee | 0:1456b6f84c75 | 254 | tx[0] = ACCEL_OFFSET_X_LSB_ADDR; |
yootee | 0:1456b6f84c75 | 255 | _i2c.write(address,tx,1,true); |
yootee | 0:1456b6f84c75 | 256 | _i2c.write(address,calibration,22,false); |
yootee | 0:1456b6f84c75 | 257 | setmode(tempmode); |
yootee | 0:1456b6f84c75 | 258 | wait_ms(10); |
yootee | 0:1456b6f84c75 | 259 | } |
yootee | 0:1456b6f84c75 | 260 | |
yootee | 0:1456b6f84c75 | 261 | void BNO055::set_mapping(char orient) |
yootee | 0:1456b6f84c75 | 262 | { |
yootee | 0:1456b6f84c75 | 263 | switch (orient) { |
yootee | 0:1456b6f84c75 | 264 | case 0: |
yootee | 0:1456b6f84c75 | 265 | writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21); |
yootee | 0:1456b6f84c75 | 266 | writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x04); |
yootee | 0:1456b6f84c75 | 267 | break; |
yootee | 0:1456b6f84c75 | 268 | case 1: |
yootee | 0:1456b6f84c75 | 269 | writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24); |
yootee | 0:1456b6f84c75 | 270 | writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x00); |
yootee | 0:1456b6f84c75 | 271 | break; |
yootee | 0:1456b6f84c75 | 272 | case 2: |
yootee | 0:1456b6f84c75 | 273 | writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24); |
yootee | 0:1456b6f84c75 | 274 | writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x00); |
yootee | 0:1456b6f84c75 | 275 | break; |
yootee | 0:1456b6f84c75 | 276 | case 3: |
yootee | 0:1456b6f84c75 | 277 | writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21); |
yootee | 0:1456b6f84c75 | 278 | writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x02); |
yootee | 0:1456b6f84c75 | 279 | break; |
yootee | 0:1456b6f84c75 | 280 | case 4: |
yootee | 0:1456b6f84c75 | 281 | writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24); |
yootee | 0:1456b6f84c75 | 282 | writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x03); |
yootee | 0:1456b6f84c75 | 283 | break; |
yootee | 0:1456b6f84c75 | 284 | case 5: |
yootee | 0:1456b6f84c75 | 285 | writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21); |
yootee | 0:1456b6f84c75 | 286 | writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x01); |
yootee | 0:1456b6f84c75 | 287 | break; |
yootee | 0:1456b6f84c75 | 288 | case 6: |
yootee | 0:1456b6f84c75 | 289 | writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21); |
yootee | 0:1456b6f84c75 | 290 | writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x07); |
yootee | 0:1456b6f84c75 | 291 | break; |
yootee | 0:1456b6f84c75 | 292 | case 7: |
yootee | 0:1456b6f84c75 | 293 | writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24); |
yootee | 0:1456b6f84c75 | 294 | writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x05); |
yootee | 0:1456b6f84c75 | 295 | break; |
yootee | 0:1456b6f84c75 | 296 | default: |
yootee | 0:1456b6f84c75 | 297 | writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24); |
yootee | 0:1456b6f84c75 | 298 | writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x00); |
yootee | 0:1456b6f84c75 | 299 | } |
yootee | 0:1456b6f84c75 | 300 | } |