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