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