PQ_Hybrid_Electrical_Equipment_Team / BNO055_lib

クオータニオンを出力してくれる9軸センサ，BNO055のライブラリ

Dependents:   SWAN_IZU2019_v1

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?

UserRevisionLine numberNew 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