クオータニオンを出力してくれる9軸センサ,BNO055のライブラリ
BNO055_lib.cpp@1:6de1be86a23f, 2019-02-19 (annotated)
- Committer:
- Sigma884
- Date:
- Tue Feb 19 14:57:28 2019 +0000
- Revision:
- 1:6de1be86a23f
- Parent:
- 0:4c82133c291f
MODE -> BNO_MODE
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Sigma884 | 0:4c82133c291f | 1 | #include "mbed.h" |
Sigma884 | 0:4c82133c291f | 2 | #include "BNO055_lib.h" |
Sigma884 | 0:4c82133c291f | 3 | |
Sigma884 | 0:4c82133c291f | 4 | BNO055_lib::BNO055_lib(I2C &user_i2c, AD0 ad0){ |
Sigma884 | 0:4c82133c291f | 5 | slave = ad0; |
Sigma884 | 0:4c82133c291f | 6 | i2c = &user_i2c; |
Sigma884 | 0:4c82133c291f | 7 | |
Sigma884 | 0:4c82133c291f | 8 | i2c -> frequency(400000); |
Sigma884 | 0:4c82133c291f | 9 | |
Sigma884 | 0:4c82133c291f | 10 | setAccRange(_16G); |
Sigma884 | 0:4c82133c291f | 11 | setOperationMode(NDOF); |
Sigma884 | 0:4c82133c291f | 12 | } |
Sigma884 | 0:4c82133c291f | 13 | |
Sigma884 | 0:4c82133c291f | 14 | int BNO055_lib::connectCheck(){ |
Sigma884 | 0:4c82133c291f | 15 | cmd[0] = BNO055_CHIP_ID; |
Sigma884 | 0:4c82133c291f | 16 | i2c -> write(slave << 1, cmd, 1); |
Sigma884 | 0:4c82133c291f | 17 | i2c -> read(slave << 1 | 1, buff, 1); |
Sigma884 | 0:4c82133c291f | 18 | |
Sigma884 | 0:4c82133c291f | 19 | if(buff[0] == 0xA0){ |
Sigma884 | 0:4c82133c291f | 20 | return 1; |
Sigma884 | 0:4c82133c291f | 21 | } |
Sigma884 | 0:4c82133c291f | 22 | else{ |
Sigma884 | 0:4c82133c291f | 23 | return 0; |
Sigma884 | 0:4c82133c291f | 24 | } |
Sigma884 | 0:4c82133c291f | 25 | } |
Sigma884 | 0:4c82133c291f | 26 | |
Sigma884 | 0:4c82133c291f | 27 | void BNO055_lib::setAccRange(ACC_RANGE range){ |
Sigma884 | 1:6de1be86a23f | 28 | cmd[0] = BNO055_PAGE_ID; |
Sigma884 | 1:6de1be86a23f | 29 | cmd[1] = 0x01; |
Sigma884 | 1:6de1be86a23f | 30 | i2c -> write(slave << 1, cmd, 2); |
Sigma884 | 1:6de1be86a23f | 31 | |
Sigma884 | 0:4c82133c291f | 32 | cmd[0] = BNO055_ACC_CONFIG; |
Sigma884 | 1:6de1be86a23f | 33 | cmd[1] = (char)range | 0x0C; |
Sigma884 | 1:6de1be86a23f | 34 | i2c -> write(slave << 1, cmd, 2); |
Sigma884 | 1:6de1be86a23f | 35 | i2c -> write(slave << 1, cmd, 1); |
Sigma884 | 1:6de1be86a23f | 36 | i2c -> read(slave << 1 | 1, buff, 1); |
Sigma884 | 1:6de1be86a23f | 37 | |
Sigma884 | 1:6de1be86a23f | 38 | cmd[0] = BNO055_PAGE_ID; |
Sigma884 | 1:6de1be86a23f | 39 | cmd[1] = 0x00; |
Sigma884 | 0:4c82133c291f | 40 | i2c -> write(slave << 1, cmd, 2); |
Sigma884 | 0:4c82133c291f | 41 | } |
Sigma884 | 0:4c82133c291f | 42 | |
Sigma884 | 0:4c82133c291f | 43 | void BNO055_lib::setOperationMode(OPERATION_MODE mode){ |
Sigma884 | 1:6de1be86a23f | 44 | wait(0.05f); |
Sigma884 | 0:4c82133c291f | 45 | cmd[0] = BNO055_OPR_MODE; |
Sigma884 | 1:6de1be86a23f | 46 | cmd[1] = (char)mode; |
Sigma884 | 1:6de1be86a23f | 47 | i2c -> write(slave << 1, cmd, 2); |
Sigma884 | 1:6de1be86a23f | 48 | wait(0.05f); |
Sigma884 | 1:6de1be86a23f | 49 | } |
Sigma884 | 1:6de1be86a23f | 50 | |
Sigma884 | 1:6de1be86a23f | 51 | void BNO055_lib::setAxis(AXIS x, AXIS y, AXIS z){ |
Sigma884 | 1:6de1be86a23f | 52 | if((x != y) && (y != z) && (z != x)){ |
Sigma884 | 1:6de1be86a23f | 53 | cmd[0] = BNO055_AXIS_MAP_CONFIG; |
Sigma884 | 1:6de1be86a23f | 54 | cmd[1] = ((char)z << 4) | ((char)y << 2) | (char)x; |
Sigma884 | 1:6de1be86a23f | 55 | i2c -> write(slave << 1, cmd, 2); |
Sigma884 | 1:6de1be86a23f | 56 | } |
Sigma884 | 1:6de1be86a23f | 57 | } |
Sigma884 | 1:6de1be86a23f | 58 | |
Sigma884 | 1:6de1be86a23f | 59 | void BNO055_lib::setAxisPM(int x_pm, int y_pm, int z_pm){ |
Sigma884 | 1:6de1be86a23f | 60 | cmd[0] = BNO055_AXIS_MAP_SIGN; |
Sigma884 | 1:6de1be86a23f | 61 | cmd[1] = 0x00; |
Sigma884 | 1:6de1be86a23f | 62 | if(x_pm < 0){ |
Sigma884 | 1:6de1be86a23f | 63 | cmd[1] = cmd[1] | 0x04; |
Sigma884 | 1:6de1be86a23f | 64 | } |
Sigma884 | 1:6de1be86a23f | 65 | if(y_pm < 0){ |
Sigma884 | 1:6de1be86a23f | 66 | cmd[1] = cmd[1] | 0x02; |
Sigma884 | 1:6de1be86a23f | 67 | } |
Sigma884 | 1:6de1be86a23f | 68 | if(z_pm < 0){ |
Sigma884 | 1:6de1be86a23f | 69 | cmd[1] = cmd[1] | 0x01; |
Sigma884 | 1:6de1be86a23f | 70 | } |
Sigma884 | 0:4c82133c291f | 71 | i2c -> write(slave << 1, cmd, 2); |
Sigma884 | 0:4c82133c291f | 72 | } |
Sigma884 | 1:6de1be86a23f | 73 | |
Sigma884 | 0:4c82133c291f | 74 | void BNO055_lib::getAcc(float *acc){ |
Sigma884 | 0:4c82133c291f | 75 | cmd[0] = BNO055_ACC_DATA; |
Sigma884 | 0:4c82133c291f | 76 | i2c -> write(slave << 1, cmd, 1); |
Sigma884 | 0:4c82133c291f | 77 | i2c -> read(slave << 1 | 1, buff, 6); |
Sigma884 | 0:4c82133c291f | 78 | data[0] = (short)buff[1] << 8 | (short)buff[0]; |
Sigma884 | 0:4c82133c291f | 79 | data[1] = (short)buff[3] << 8 | (short)buff[2]; |
Sigma884 | 0:4c82133c291f | 80 | data[2] = (short)buff[5] << 8 | (short)buff[4]; |
Sigma884 | 0:4c82133c291f | 81 | |
Sigma884 | 0:4c82133c291f | 82 | acc[0] = (float)data[0] / 100.0; |
Sigma884 | 0:4c82133c291f | 83 | acc[1] = (float)data[1] / 100.0; |
Sigma884 | 0:4c82133c291f | 84 | acc[2] = (float)data[2] / 100.0; |
Sigma884 | 0:4c82133c291f | 85 | } |
Sigma884 | 0:4c82133c291f | 86 | |
Sigma884 | 0:4c82133c291f | 87 | void BNO055_lib::getAcc(double *acc){ |
Sigma884 | 0:4c82133c291f | 88 | cmd[0] = BNO055_ACC_DATA; |
Sigma884 | 0:4c82133c291f | 89 | i2c -> write(slave << 1, cmd, 1); |
Sigma884 | 0:4c82133c291f | 90 | i2c -> read(slave << 1 | 1, buff, 6); |
Sigma884 | 0:4c82133c291f | 91 | data[0] = (short)buff[1] << 8 | (short)buff[0]; |
Sigma884 | 0:4c82133c291f | 92 | data[1] = (short)buff[3] << 8 | (short)buff[2]; |
Sigma884 | 0:4c82133c291f | 93 | data[2] = (short)buff[5] << 8 | (short)buff[4]; |
Sigma884 | 0:4c82133c291f | 94 | |
Sigma884 | 0:4c82133c291f | 95 | acc[0] = (double)data[0] / 100.0; |
Sigma884 | 0:4c82133c291f | 96 | acc[1] = (double)data[1] / 100.0; |
Sigma884 | 0:4c82133c291f | 97 | acc[2] = (double)data[2] / 100.0; |
Sigma884 | 0:4c82133c291f | 98 | } |
Sigma884 | 0:4c82133c291f | 99 | |
Sigma884 | 0:4c82133c291f | 100 | void BNO055_lib::getMag(float *mag){ |
Sigma884 | 0:4c82133c291f | 101 | cmd[0] = BNO055_MAG_DATA; |
Sigma884 | 0:4c82133c291f | 102 | i2c -> write(slave << 1, cmd, 1); |
Sigma884 | 0:4c82133c291f | 103 | i2c -> read(slave << 1 | 1, buff, 6); |
Sigma884 | 0:4c82133c291f | 104 | data[0] = (short)buff[1] << 8 | (short)buff[0]; |
Sigma884 | 0:4c82133c291f | 105 | data[1] = (short)buff[3] << 8 | (short)buff[2]; |
Sigma884 | 0:4c82133c291f | 106 | data[2] = (short)buff[5] << 8 | (short)buff[4]; |
Sigma884 | 0:4c82133c291f | 107 | |
Sigma884 | 0:4c82133c291f | 108 | mag[0] = (float)data[0] / 16.0; |
Sigma884 | 0:4c82133c291f | 109 | mag[1] = (float)data[1] / 16.0; |
Sigma884 | 0:4c82133c291f | 110 | mag[2] = (float)data[2] / 16.0; |
Sigma884 | 0:4c82133c291f | 111 | } |
Sigma884 | 0:4c82133c291f | 112 | |
Sigma884 | 0:4c82133c291f | 113 | void BNO055_lib::getMag(double *mag){ |
Sigma884 | 0:4c82133c291f | 114 | cmd[0] = BNO055_MAG_DATA; |
Sigma884 | 0:4c82133c291f | 115 | i2c -> write(slave << 1, cmd, 1); |
Sigma884 | 0:4c82133c291f | 116 | i2c -> read(slave << 1 | 1, buff, 6); |
Sigma884 | 0:4c82133c291f | 117 | data[0] = (short)buff[1] << 8 | (short)buff[0]; |
Sigma884 | 0:4c82133c291f | 118 | data[1] = (short)buff[3] << 8 | (short)buff[2]; |
Sigma884 | 0:4c82133c291f | 119 | data[2] = (short)buff[5] << 8 | (short)buff[4]; |
Sigma884 | 0:4c82133c291f | 120 | |
Sigma884 | 0:4c82133c291f | 121 | mag[0] = (double)data[0] / 16.0; |
Sigma884 | 0:4c82133c291f | 122 | mag[1] = (double)data[1] / 16.0; |
Sigma884 | 0:4c82133c291f | 123 | mag[2] = (double)data[2] / 16.0; |
Sigma884 | 0:4c82133c291f | 124 | } |
Sigma884 | 0:4c82133c291f | 125 | |
Sigma884 | 0:4c82133c291f | 126 | void BNO055_lib::getGyro(float *gyro){ |
Sigma884 | 0:4c82133c291f | 127 | cmd[0] = BNO055_GYR_DATA; |
Sigma884 | 0:4c82133c291f | 128 | i2c -> write(slave << 1, cmd, 1); |
Sigma884 | 0:4c82133c291f | 129 | i2c -> read(slave << 1 | 1, buff, 6); |
Sigma884 | 0:4c82133c291f | 130 | data[0] = (short)buff[1] << 8 | (short)buff[0]; |
Sigma884 | 0:4c82133c291f | 131 | data[1] = (short)buff[3] << 8 | (short)buff[2]; |
Sigma884 | 0:4c82133c291f | 132 | data[2] = (short)buff[5] << 8 | (short)buff[4]; |
Sigma884 | 0:4c82133c291f | 133 | |
Sigma884 | 0:4c82133c291f | 134 | gyro[0] = (float)data[0] / 16.0; |
Sigma884 | 0:4c82133c291f | 135 | gyro[1] = (float)data[1] / 16.0; |
Sigma884 | 0:4c82133c291f | 136 | gyro[2] = (float)data[2] / 16.0; |
Sigma884 | 0:4c82133c291f | 137 | } |
Sigma884 | 0:4c82133c291f | 138 | |
Sigma884 | 0:4c82133c291f | 139 | void BNO055_lib::getGyro(double *gyro){ |
Sigma884 | 0:4c82133c291f | 140 | cmd[0] = BNO055_GYR_DATA; |
Sigma884 | 0:4c82133c291f | 141 | i2c -> write(slave << 1, cmd, 1); |
Sigma884 | 0:4c82133c291f | 142 | i2c -> read(slave << 1 | 1, buff, 6); |
Sigma884 | 0:4c82133c291f | 143 | data[0] = (short)buff[1] << 8 | (short)buff[0]; |
Sigma884 | 0:4c82133c291f | 144 | data[1] = (short)buff[3] << 8 | (short)buff[2]; |
Sigma884 | 0:4c82133c291f | 145 | data[2] = (short)buff[5] << 8 | (short)buff[4]; |
Sigma884 | 0:4c82133c291f | 146 | |
Sigma884 | 0:4c82133c291f | 147 | gyro[0] = (double)data[0] / 16.0; |
Sigma884 | 0:4c82133c291f | 148 | gyro[1] = (double)data[1] / 16.0; |
Sigma884 | 0:4c82133c291f | 149 | gyro[2] = (double)data[2] / 16.0; |
Sigma884 | 0:4c82133c291f | 150 | } |
Sigma884 | 0:4c82133c291f | 151 | |
Sigma884 | 0:4c82133c291f | 152 | void BNO055_lib::getAMG(float *amg){ |
Sigma884 | 0:4c82133c291f | 153 | cmd[0] = BNO055_ACC_DATA; |
Sigma884 | 0:4c82133c291f | 154 | i2c -> write(slave << 1, cmd, 1); |
Sigma884 | 0:4c82133c291f | 155 | i2c -> read(slave << 1 | 1, buff, 18); |
Sigma884 | 0:4c82133c291f | 156 | |
Sigma884 | 0:4c82133c291f | 157 | data[0] = (short)buff[1] << 8 | (short)buff[0]; |
Sigma884 | 0:4c82133c291f | 158 | data[1] = (short)buff[3] << 8 | (short)buff[2]; |
Sigma884 | 0:4c82133c291f | 159 | data[2] = (short)buff[5] << 8 | (short)buff[4]; |
Sigma884 | 0:4c82133c291f | 160 | amg[0] = (float)data[0] / 100.0; |
Sigma884 | 0:4c82133c291f | 161 | amg[1] = (float)data[1] / 100.0; |
Sigma884 | 0:4c82133c291f | 162 | amg[2] = (float)data[2] / 100.0; |
Sigma884 | 0:4c82133c291f | 163 | |
Sigma884 | 0:4c82133c291f | 164 | data[0] = (short)buff[7] << 8 | (short)buff[6]; |
Sigma884 | 0:4c82133c291f | 165 | data[1] = (short)buff[9] << 8 | (short)buff[8]; |
Sigma884 | 0:4c82133c291f | 166 | data[2] = (short)buff[11] << 8 | (short)buff[10]; |
Sigma884 | 0:4c82133c291f | 167 | amg[3] = (float)data[0] / 16.0; |
Sigma884 | 0:4c82133c291f | 168 | amg[4] = (float)data[1] / 16.0; |
Sigma884 | 0:4c82133c291f | 169 | amg[5] = (float)data[2] / 16.0; |
Sigma884 | 0:4c82133c291f | 170 | |
Sigma884 | 0:4c82133c291f | 171 | data[0] = (short)buff[13] << 8 | (short)buff[12]; |
Sigma884 | 0:4c82133c291f | 172 | data[1] = (short)buff[15] << 8 | (short)buff[14]; |
Sigma884 | 0:4c82133c291f | 173 | data[2] = (short)buff[17] << 8 | (short)buff[16]; |
Sigma884 | 0:4c82133c291f | 174 | amg[6] = (float)data[0] / 16.0; |
Sigma884 | 0:4c82133c291f | 175 | amg[7] = (float)data[1] / 16.0; |
Sigma884 | 0:4c82133c291f | 176 | amg[8] = (float)data[2] / 16.0; |
Sigma884 | 0:4c82133c291f | 177 | } |
Sigma884 | 0:4c82133c291f | 178 | |
Sigma884 | 0:4c82133c291f | 179 | void BNO055_lib::getAMG(double *amg){ |
Sigma884 | 0:4c82133c291f | 180 | cmd[0] = BNO055_ACC_DATA; |
Sigma884 | 0:4c82133c291f | 181 | i2c -> write(slave << 1, cmd, 1); |
Sigma884 | 0:4c82133c291f | 182 | i2c -> read(slave << 1 | 1, buff, 18); |
Sigma884 | 0:4c82133c291f | 183 | |
Sigma884 | 0:4c82133c291f | 184 | data[0] = (short)buff[1] << 8 | (short)buff[0]; |
Sigma884 | 0:4c82133c291f | 185 | data[1] = (short)buff[3] << 8 | (short)buff[2]; |
Sigma884 | 0:4c82133c291f | 186 | data[2] = (short)buff[5] << 8 | (short)buff[4]; |
Sigma884 | 0:4c82133c291f | 187 | amg[0] = (double)data[0] / 100.0; |
Sigma884 | 0:4c82133c291f | 188 | amg[1] = (double)data[1] / 100.0; |
Sigma884 | 0:4c82133c291f | 189 | amg[2] = (double)data[2] / 100.0; |
Sigma884 | 0:4c82133c291f | 190 | |
Sigma884 | 0:4c82133c291f | 191 | data[0] = (short)buff[7] << 8 | (short)buff[6]; |
Sigma884 | 0:4c82133c291f | 192 | data[1] = (short)buff[9] << 8 | (short)buff[8]; |
Sigma884 | 0:4c82133c291f | 193 | data[2] = (short)buff[11] << 8 | (short)buff[10]; |
Sigma884 | 0:4c82133c291f | 194 | amg[3] = (double)data[0] / 16.0; |
Sigma884 | 0:4c82133c291f | 195 | amg[4] = (double)data[1] / 16.0; |
Sigma884 | 0:4c82133c291f | 196 | amg[5] = (double)data[2] / 16.0; |
Sigma884 | 0:4c82133c291f | 197 | |
Sigma884 | 0:4c82133c291f | 198 | data[0] = (short)buff[13] << 8 | (short)buff[12]; |
Sigma884 | 0:4c82133c291f | 199 | data[1] = (short)buff[15] << 8 | (short)buff[14]; |
Sigma884 | 0:4c82133c291f | 200 | data[2] = (short)buff[17] << 8 | (short)buff[16]; |
Sigma884 | 0:4c82133c291f | 201 | amg[6] = (double)data[0] / 16.0; |
Sigma884 | 0:4c82133c291f | 202 | amg[7] = (double)data[1] / 16.0; |
Sigma884 | 0:4c82133c291f | 203 | amg[8] = (double)data[2] / 16.0; |
Sigma884 | 0:4c82133c291f | 204 | } |
Sigma884 | 0:4c82133c291f | 205 | |
Sigma884 | 0:4c82133c291f | 206 | void BNO055_lib::getEuler(float *euler){ |
Sigma884 | 0:4c82133c291f | 207 | cmd[0] = BNO055_EUL_DATA; |
Sigma884 | 0:4c82133c291f | 208 | i2c -> write(slave << 1, cmd, 1); |
Sigma884 | 0:4c82133c291f | 209 | i2c -> read(slave << 1 | 1, buff, 6); |
Sigma884 | 0:4c82133c291f | 210 | data[0] = (short)buff[1] << 8 | (short)buff[0]; |
Sigma884 | 0:4c82133c291f | 211 | data[1] = (short)buff[3] << 8 | (short)buff[2]; |
Sigma884 | 0:4c82133c291f | 212 | data[2] = (short)buff[5] << 8 | (short)buff[4]; |
Sigma884 | 0:4c82133c291f | 213 | |
Sigma884 | 0:4c82133c291f | 214 | euler[0] = (float)data[0] / 16.0; |
Sigma884 | 0:4c82133c291f | 215 | euler[1] = (float)data[1] / 16.0; |
Sigma884 | 0:4c82133c291f | 216 | euler[2] = (float)data[2] / 16.0; |
Sigma884 | 0:4c82133c291f | 217 | } |
Sigma884 | 0:4c82133c291f | 218 | |
Sigma884 | 0:4c82133c291f | 219 | void BNO055_lib::getEuler(double *euler){ |
Sigma884 | 0:4c82133c291f | 220 | cmd[0] = BNO055_EUL_DATA; |
Sigma884 | 0:4c82133c291f | 221 | i2c -> write(slave << 1, cmd, 1); |
Sigma884 | 0:4c82133c291f | 222 | i2c -> read(slave << 1 | 1, buff, 6); |
Sigma884 | 0:4c82133c291f | 223 | data[0] = (short)buff[1] << 8 | (short)buff[0]; |
Sigma884 | 0:4c82133c291f | 224 | data[1] = (short)buff[3] << 8 | (short)buff[2]; |
Sigma884 | 0:4c82133c291f | 225 | data[2] = (short)buff[5] << 8 | (short)buff[4]; |
Sigma884 | 0:4c82133c291f | 226 | |
Sigma884 | 0:4c82133c291f | 227 | euler[0] = (double)data[0] / 16.0; |
Sigma884 | 0:4c82133c291f | 228 | euler[1] = (double)data[1] / 16.0; |
Sigma884 | 0:4c82133c291f | 229 | euler[2] = (double)data[2] / 16.0; |
Sigma884 | 0:4c82133c291f | 230 | } |
Sigma884 | 0:4c82133c291f | 231 | |
Sigma884 | 0:4c82133c291f | 232 | void BNO055_lib::getQuart(float *quart){ |
Sigma884 | 0:4c82133c291f | 233 | cmd[0] = BNO055_QUA_DATA; |
Sigma884 | 0:4c82133c291f | 234 | i2c -> write(slave << 1, cmd, 1); |
Sigma884 | 0:4c82133c291f | 235 | i2c -> read(slave << 1 | 1, buff, 8); |
Sigma884 | 0:4c82133c291f | 236 | data[0] = (short)buff[1] << 8 | (short)buff[0]; |
Sigma884 | 0:4c82133c291f | 237 | data[1] = (short)buff[3] << 8 | (short)buff[2]; |
Sigma884 | 0:4c82133c291f | 238 | data[2] = (short)buff[5] << 8 | (short)buff[4]; |
Sigma884 | 0:4c82133c291f | 239 | data[3] = (short)buff[7] << 8 | (short)buff[6]; |
Sigma884 | 0:4c82133c291f | 240 | |
Sigma884 | 0:4c82133c291f | 241 | quart[0] = (float)(data[0] / 16384.0); |
Sigma884 | 0:4c82133c291f | 242 | quart[1] = (float)(data[1] / 16384.0); |
Sigma884 | 0:4c82133c291f | 243 | quart[2] = (float)(data[2] / 16384.0); |
Sigma884 | 0:4c82133c291f | 244 | quart[3] = (float)(data[3] / 16384.0); |
Sigma884 | 0:4c82133c291f | 245 | } |
Sigma884 | 0:4c82133c291f | 246 | |
Sigma884 | 0:4c82133c291f | 247 | void BNO055_lib::getQuart(double *quart){ |
Sigma884 | 0:4c82133c291f | 248 | cmd[0] = BNO055_QUA_DATA; |
Sigma884 | 0:4c82133c291f | 249 | i2c -> write(slave << 1, cmd, 1); |
Sigma884 | 0:4c82133c291f | 250 | i2c -> read(slave << 1 | 1, buff, 8); |
Sigma884 | 0:4c82133c291f | 251 | data[0] = (short)buff[1] << 8 | (short)buff[0]; |
Sigma884 | 0:4c82133c291f | 252 | data[1] = (short)buff[3] << 8 | (short)buff[2]; |
Sigma884 | 0:4c82133c291f | 253 | data[2] = (short)buff[5] << 8 | (short)buff[4]; |
Sigma884 | 0:4c82133c291f | 254 | data[3] = (short)buff[7] << 8 | (short)buff[6]; |
Sigma884 | 0:4c82133c291f | 255 | |
Sigma884 | 0:4c82133c291f | 256 | quart[0] = (double)(data[0] / 16384.0); |
Sigma884 | 0:4c82133c291f | 257 | quart[1] = (double)(data[1] / 16384.0); |
Sigma884 | 0:4c82133c291f | 258 | quart[2] = (double)(data[2] / 16384.0); |
Sigma884 | 0:4c82133c291f | 259 | quart[3] = (double)(data[3] / 16384.0); |
Sigma884 | 0:4c82133c291f | 260 | } |
Sigma884 | 0:4c82133c291f | 261 | |
Sigma884 | 0:4c82133c291f | 262 | void BNO055_lib::getEulerFromQ(float *euler){ |
Sigma884 | 0:4c82133c291f | 263 | getQuart(data_d); |
Sigma884 | 0:4c82133c291f | 264 | /*************** |
Sigma884 | 0:4c82133c291f | 265 | data_d[0] : w |
Sigma884 | 0:4c82133c291f | 266 | data_d[1] : x |
Sigma884 | 0:4c82133c291f | 267 | data_d[2] : y |
Sigma884 | 0:4c82133c291f | 268 | data_d[3] : z |
Sigma884 | 0:4c82133c291f | 269 | ***************/ |
Sigma884 | 0:4c82133c291f | 270 | |
Sigma884 | 0:4c82133c291f | 271 | yy = data_d[2] * data_d[2]; |
Sigma884 | 0:4c82133c291f | 272 | |
Sigma884 | 0:4c82133c291f | 273 | t0 = 2.0 * (data_d[0] * data_d[3] + data_d[1] * data_d[2]); |
Sigma884 | 0:4c82133c291f | 274 | t1 = 1.0 - 2.0 * (yy + data_d[3] * data_d[3]); |
Sigma884 | 0:4c82133c291f | 275 | euler[0] = (float)atan2(t0, t1) * 57.2957795131; |
Sigma884 | 0:4c82133c291f | 276 | |
Sigma884 | 0:4c82133c291f | 277 | t0 = 2.0 * (data_d[0] * data_d[1] + data_d[2] * data_d[3]); |
Sigma884 | 0:4c82133c291f | 278 | t1 = 1.0 - 2.0 * (data_d[1] * data_d[1] + yy); |
Sigma884 | 0:4c82133c291f | 279 | euler[1] = (float)atan2(t0, t1) * 57.2957795131; |
Sigma884 | 0:4c82133c291f | 280 | |
Sigma884 | 0:4c82133c291f | 281 | t0 = 2.0 * (data_d[0] * data_d[2] - data_d[3] * data_d[1]); |
Sigma884 | 0:4c82133c291f | 282 | t0 = (t0 > 1.0) ? 1.0 : t0; |
Sigma884 | 0:4c82133c291f | 283 | t0 = (t0 < -1.0) ? -1.0 : t0; |
Sigma884 | 0:4c82133c291f | 284 | euler[2] = (float)asin(t0) * 57.2957795131; |
Sigma884 | 0:4c82133c291f | 285 | } |
Sigma884 | 0:4c82133c291f | 286 | |
Sigma884 | 0:4c82133c291f | 287 | void BNO055_lib::getEulerFromQ(double *euler){ |
Sigma884 | 0:4c82133c291f | 288 | getQuart(data_d); |
Sigma884 | 0:4c82133c291f | 289 | /*************** |
Sigma884 | 0:4c82133c291f | 290 | data_d[0] : w |
Sigma884 | 0:4c82133c291f | 291 | data_d[1] : x |
Sigma884 | 0:4c82133c291f | 292 | data_d[2] : y |
Sigma884 | 0:4c82133c291f | 293 | data_d[3] : z |
Sigma884 | 0:4c82133c291f | 294 | ***************/ |
Sigma884 | 0:4c82133c291f | 295 | |
Sigma884 | 0:4c82133c291f | 296 | yy = data_d[2] * data_d[2]; |
Sigma884 | 0:4c82133c291f | 297 | |
Sigma884 | 0:4c82133c291f | 298 | t0 = 2.0 * (data_d[0] * data_d[3] + data_d[1] * data_d[2]); |
Sigma884 | 0:4c82133c291f | 299 | t1 = 1.0 - 2.0 * (yy + data_d[3] * data_d[3]); |
Sigma884 | 0:4c82133c291f | 300 | euler[0] = atan2(t0, t1) * 57.2957795131; |
Sigma884 | 0:4c82133c291f | 301 | |
Sigma884 | 0:4c82133c291f | 302 | t0 = 2.0 * (data_d[0] * data_d[1] + data_d[2] * data_d[3]); |
Sigma884 | 0:4c82133c291f | 303 | t1 = 1.0 - 2.0 * (data_d[1] * data_d[1] + yy); |
Sigma884 | 0:4c82133c291f | 304 | euler[1] = atan2(t0, t1) * 57.2957795131; |
Sigma884 | 0:4c82133c291f | 305 | |
Sigma884 | 0:4c82133c291f | 306 | t0 = 2.0 * (data_d[0] * data_d[2] - data_d[3] * data_d[1]); |
Sigma884 | 0:4c82133c291f | 307 | t0 = (t0 > 1.0) ? 1.0 : t0; |
Sigma884 | 0:4c82133c291f | 308 | t0 = (t0 < -1.0) ? -1.0 : t0; |
Sigma884 | 0:4c82133c291f | 309 | euler[2] = asin(t0) * 57.2957795131; |
Sigma884 | 0:4c82133c291f | 310 | } |
Sigma884 | 0:4c82133c291f | 311 | |
Sigma884 | 0:4c82133c291f | 312 | void BNO055_lib::getLinearAcc(float *lia){ |
Sigma884 | 0:4c82133c291f | 313 | cmd[0] = BNO055_LIA_DATA; |
Sigma884 | 0:4c82133c291f | 314 | i2c -> write(slave << 1, cmd, 1); |
Sigma884 | 0:4c82133c291f | 315 | i2c -> read(slave << 1 | 1, buff, 6); |
Sigma884 | 0:4c82133c291f | 316 | data[0] = (short)buff[1] << 8 | (short)buff[0]; |
Sigma884 | 0:4c82133c291f | 317 | data[1] = (short)buff[3] << 8 | (short)buff[2]; |
Sigma884 | 0:4c82133c291f | 318 | data[2] = (short)buff[5] << 8 | (short)buff[4]; |
Sigma884 | 0:4c82133c291f | 319 | |
Sigma884 | 0:4c82133c291f | 320 | lia[0] = (float)data[0] / 100.0; |
Sigma884 | 0:4c82133c291f | 321 | lia[1] = (float)data[1] / 100.0; |
Sigma884 | 0:4c82133c291f | 322 | lia[2] = (float)data[2] / 100.0; |
Sigma884 | 0:4c82133c291f | 323 | } |
Sigma884 | 0:4c82133c291f | 324 | |
Sigma884 | 0:4c82133c291f | 325 | void BNO055_lib::getLinearAcc(double *lia){ |
Sigma884 | 0:4c82133c291f | 326 | cmd[0] = BNO055_LIA_DATA; |
Sigma884 | 0:4c82133c291f | 327 | i2c -> write(slave << 1, cmd, 1); |
Sigma884 | 0:4c82133c291f | 328 | i2c -> read(slave << 1 | 1, buff, 6); |
Sigma884 | 0:4c82133c291f | 329 | data[0] = (short)buff[1] << 8 | (short)buff[0]; |
Sigma884 | 0:4c82133c291f | 330 | data[1] = (short)buff[3] << 8 | (short)buff[2]; |
Sigma884 | 0:4c82133c291f | 331 | data[2] = (short)buff[5] << 8 | (short)buff[4]; |
Sigma884 | 0:4c82133c291f | 332 | |
Sigma884 | 0:4c82133c291f | 333 | lia[0] = (double)data[0] / 100.0; |
Sigma884 | 0:4c82133c291f | 334 | lia[1] = (double)data[1] / 100.0; |
Sigma884 | 0:4c82133c291f | 335 | lia[2] = (double)data[2] / 100.0; |
Sigma884 | 0:4c82133c291f | 336 | } |
Sigma884 | 0:4c82133c291f | 337 | |
Sigma884 | 0:4c82133c291f | 338 | void BNO055_lib::getGravity(float *grv){ |
Sigma884 | 0:4c82133c291f | 339 | cmd[0] = BNO055_GRV_DATA; |
Sigma884 | 0:4c82133c291f | 340 | i2c -> write(slave << 1, cmd, 1); |
Sigma884 | 0:4c82133c291f | 341 | i2c -> read(slave << 1 | 1, buff, 6); |
Sigma884 | 0:4c82133c291f | 342 | data[0] = (short)buff[1] << 8 | (short)buff[0]; |
Sigma884 | 0:4c82133c291f | 343 | data[1] = (short)buff[3] << 8 | (short)buff[2]; |
Sigma884 | 0:4c82133c291f | 344 | data[2] = (short)buff[5] << 8 | (short)buff[4]; |
Sigma884 | 0:4c82133c291f | 345 | |
Sigma884 | 0:4c82133c291f | 346 | grv[0] = (float)data[0] / 100.0; |
Sigma884 | 0:4c82133c291f | 347 | grv[1] = (float)data[1] / 100.0; |
Sigma884 | 0:4c82133c291f | 348 | grv[2] = (float)data[2] / 100.0; |
Sigma884 | 0:4c82133c291f | 349 | } |
Sigma884 | 0:4c82133c291f | 350 | |
Sigma884 | 0:4c82133c291f | 351 | void BNO055_lib::getGravity(double *grv){ |
Sigma884 | 0:4c82133c291f | 352 | cmd[0] = BNO055_GRV_DATA; |
Sigma884 | 0:4c82133c291f | 353 | i2c -> write(slave << 1, cmd, 1); |
Sigma884 | 0:4c82133c291f | 354 | i2c -> read(slave << 1 | 1, buff, 6); |
Sigma884 | 0:4c82133c291f | 355 | data[0] = (short)buff[1] << 8 | (short)buff[0]; |
Sigma884 | 0:4c82133c291f | 356 | data[1] = (short)buff[3] << 8 | (short)buff[2]; |
Sigma884 | 0:4c82133c291f | 357 | data[2] = (short)buff[5] << 8 | (short)buff[4]; |
Sigma884 | 0:4c82133c291f | 358 | |
Sigma884 | 0:4c82133c291f | 359 | grv[0] = (double)data[0] / 100.0; |
Sigma884 | 0:4c82133c291f | 360 | grv[1] = (double)data[1] / 100.0; |
Sigma884 | 0:4c82133c291f | 361 | grv[2] = (double)data[2] / 100.0; |
Sigma884 | 0:4c82133c291f | 362 | } |
Sigma884 | 0:4c82133c291f | 363 |