クオータニオンを出力してくれる9軸センサ,BNO055のライブラリ
Diff: BNO055_lib.cpp
- Revision:
- 0:4c82133c291f
- Child:
- 1:6de1be86a23f
diff -r 000000000000 -r 4c82133c291f BNO055_lib.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/BNO055_lib.cpp Tue Jan 22 11:47:28 2019 +0000 @@ -0,0 +1,329 @@ +#include "mbed.h" +#include "BNO055_lib.h" + +BNO055_lib::BNO055_lib(I2C &user_i2c, AD0 ad0){ + slave = ad0; + i2c = &user_i2c; + + i2c -> frequency(400000); + + setAccRange(_16G); + setOperationMode(NDOF); +} + +int BNO055_lib::connectCheck(){ + cmd[0] = BNO055_CHIP_ID; + i2c -> write(slave << 1, cmd, 1); + i2c -> read(slave << 1 | 1, buff, 1); + + if(buff[0] == 0xA0){ + return 1; + } + else{ + return 0; + } +} + +void BNO055_lib::setAccRange(ACC_RANGE range){ + cmd[0] = BNO055_ACC_CONFIG; + cmd[1] = 0x0F; + i2c -> write(slave << 1, cmd, 2); +} + +void BNO055_lib::setOperationMode(OPERATION_MODE mode){ + cmd[0] = BNO055_OPR_MODE; + //cmd[1] = 0x07; //AMG + cmd[1] = 0x0C; //NDOF + i2c -> write(slave << 1, cmd, 2); +} + +void BNO055_lib::getAcc(float *acc){ + cmd[0] = BNO055_ACC_DATA; + i2c -> write(slave << 1, cmd, 1); + i2c -> read(slave << 1 | 1, buff, 6); + data[0] = (short)buff[1] << 8 | (short)buff[0]; + data[1] = (short)buff[3] << 8 | (short)buff[2]; + data[2] = (short)buff[5] << 8 | (short)buff[4]; + + acc[0] = (float)data[0] / 100.0; + acc[1] = (float)data[1] / 100.0; + acc[2] = (float)data[2] / 100.0; +} + +void BNO055_lib::getAcc(double *acc){ + cmd[0] = BNO055_ACC_DATA; + i2c -> write(slave << 1, cmd, 1); + i2c -> read(slave << 1 | 1, buff, 6); + data[0] = (short)buff[1] << 8 | (short)buff[0]; + data[1] = (short)buff[3] << 8 | (short)buff[2]; + data[2] = (short)buff[5] << 8 | (short)buff[4]; + + acc[0] = (double)data[0] / 100.0; + acc[1] = (double)data[1] / 100.0; + acc[2] = (double)data[2] / 100.0; +} + +void BNO055_lib::getMag(float *mag){ + cmd[0] = BNO055_MAG_DATA; + i2c -> write(slave << 1, cmd, 1); + i2c -> read(slave << 1 | 1, buff, 6); + data[0] = (short)buff[1] << 8 | (short)buff[0]; + data[1] = (short)buff[3] << 8 | (short)buff[2]; + data[2] = (short)buff[5] << 8 | (short)buff[4]; + + mag[0] = (float)data[0] / 16.0; + mag[1] = (float)data[1] / 16.0; + mag[2] = (float)data[2] / 16.0; +} + +void BNO055_lib::getMag(double *mag){ + cmd[0] = BNO055_MAG_DATA; + i2c -> write(slave << 1, cmd, 1); + i2c -> read(slave << 1 | 1, buff, 6); + data[0] = (short)buff[1] << 8 | (short)buff[0]; + data[1] = (short)buff[3] << 8 | (short)buff[2]; + data[2] = (short)buff[5] << 8 | (short)buff[4]; + + mag[0] = (double)data[0] / 16.0; + mag[1] = (double)data[1] / 16.0; + mag[2] = (double)data[2] / 16.0; +} + +void BNO055_lib::getGyro(float *gyro){ + cmd[0] = BNO055_GYR_DATA; + i2c -> write(slave << 1, cmd, 1); + i2c -> read(slave << 1 | 1, buff, 6); + data[0] = (short)buff[1] << 8 | (short)buff[0]; + data[1] = (short)buff[3] << 8 | (short)buff[2]; + data[2] = (short)buff[5] << 8 | (short)buff[4]; + + gyro[0] = (float)data[0] / 16.0; + gyro[1] = (float)data[1] / 16.0; + gyro[2] = (float)data[2] / 16.0; +} + +void BNO055_lib::getGyro(double *gyro){ + cmd[0] = BNO055_GYR_DATA; + i2c -> write(slave << 1, cmd, 1); + i2c -> read(slave << 1 | 1, buff, 6); + data[0] = (short)buff[1] << 8 | (short)buff[0]; + data[1] = (short)buff[3] << 8 | (short)buff[2]; + data[2] = (short)buff[5] << 8 | (short)buff[4]; + + gyro[0] = (double)data[0] / 16.0; + gyro[1] = (double)data[1] / 16.0; + gyro[2] = (double)data[2] / 16.0; +} + +void BNO055_lib::getAMG(float *amg){ + cmd[0] = BNO055_ACC_DATA; + i2c -> write(slave << 1, cmd, 1); + i2c -> read(slave << 1 | 1, buff, 18); + + data[0] = (short)buff[1] << 8 | (short)buff[0]; + data[1] = (short)buff[3] << 8 | (short)buff[2]; + data[2] = (short)buff[5] << 8 | (short)buff[4]; + amg[0] = (float)data[0] / 100.0; + amg[1] = (float)data[1] / 100.0; + amg[2] = (float)data[2] / 100.0; + + data[0] = (short)buff[7] << 8 | (short)buff[6]; + data[1] = (short)buff[9] << 8 | (short)buff[8]; + data[2] = (short)buff[11] << 8 | (short)buff[10]; + amg[3] = (float)data[0] / 16.0; + amg[4] = (float)data[1] / 16.0; + amg[5] = (float)data[2] / 16.0; + + data[0] = (short)buff[13] << 8 | (short)buff[12]; + data[1] = (short)buff[15] << 8 | (short)buff[14]; + data[2] = (short)buff[17] << 8 | (short)buff[16]; + amg[6] = (float)data[0] / 16.0; + amg[7] = (float)data[1] / 16.0; + amg[8] = (float)data[2] / 16.0; +} + +void BNO055_lib::getAMG(double *amg){ + cmd[0] = BNO055_ACC_DATA; + i2c -> write(slave << 1, cmd, 1); + i2c -> read(slave << 1 | 1, buff, 18); + + data[0] = (short)buff[1] << 8 | (short)buff[0]; + data[1] = (short)buff[3] << 8 | (short)buff[2]; + data[2] = (short)buff[5] << 8 | (short)buff[4]; + amg[0] = (double)data[0] / 100.0; + amg[1] = (double)data[1] / 100.0; + amg[2] = (double)data[2] / 100.0; + + data[0] = (short)buff[7] << 8 | (short)buff[6]; + data[1] = (short)buff[9] << 8 | (short)buff[8]; + data[2] = (short)buff[11] << 8 | (short)buff[10]; + amg[3] = (double)data[0] / 16.0; + amg[4] = (double)data[1] / 16.0; + amg[5] = (double)data[2] / 16.0; + + data[0] = (short)buff[13] << 8 | (short)buff[12]; + data[1] = (short)buff[15] << 8 | (short)buff[14]; + data[2] = (short)buff[17] << 8 | (short)buff[16]; + amg[6] = (double)data[0] / 16.0; + amg[7] = (double)data[1] / 16.0; + amg[8] = (double)data[2] / 16.0; +} + +void BNO055_lib::getEuler(float *euler){ + cmd[0] = BNO055_EUL_DATA; + i2c -> write(slave << 1, cmd, 1); + i2c -> read(slave << 1 | 1, buff, 6); + data[0] = (short)buff[1] << 8 | (short)buff[0]; + data[1] = (short)buff[3] << 8 | (short)buff[2]; + data[2] = (short)buff[5] << 8 | (short)buff[4]; + + euler[0] = (float)data[0] / 16.0; + euler[1] = (float)data[1] / 16.0; + euler[2] = (float)data[2] / 16.0; +} + +void BNO055_lib::getEuler(double *euler){ + cmd[0] = BNO055_EUL_DATA; + i2c -> write(slave << 1, cmd, 1); + i2c -> read(slave << 1 | 1, buff, 6); + data[0] = (short)buff[1] << 8 | (short)buff[0]; + data[1] = (short)buff[3] << 8 | (short)buff[2]; + data[2] = (short)buff[5] << 8 | (short)buff[4]; + + euler[0] = (double)data[0] / 16.0; + euler[1] = (double)data[1] / 16.0; + euler[2] = (double)data[2] / 16.0; +} + +void BNO055_lib::getQuart(float *quart){ + cmd[0] = BNO055_QUA_DATA; + i2c -> write(slave << 1, cmd, 1); + i2c -> read(slave << 1 | 1, buff, 8); + data[0] = (short)buff[1] << 8 | (short)buff[0]; + data[1] = (short)buff[3] << 8 | (short)buff[2]; + data[2] = (short)buff[5] << 8 | (short)buff[4]; + data[3] = (short)buff[7] << 8 | (short)buff[6]; + + quart[0] = (float)(data[0] / 16384.0); + quart[1] = (float)(data[1] / 16384.0); + quart[2] = (float)(data[2] / 16384.0); + quart[3] = (float)(data[3] / 16384.0); +} + +void BNO055_lib::getQuart(double *quart){ + cmd[0] = BNO055_QUA_DATA; + i2c -> write(slave << 1, cmd, 1); + i2c -> read(slave << 1 | 1, buff, 8); + data[0] = (short)buff[1] << 8 | (short)buff[0]; + data[1] = (short)buff[3] << 8 | (short)buff[2]; + data[2] = (short)buff[5] << 8 | (short)buff[4]; + data[3] = (short)buff[7] << 8 | (short)buff[6]; + + quart[0] = (double)(data[0] / 16384.0); + quart[1] = (double)(data[1] / 16384.0); + quart[2] = (double)(data[2] / 16384.0); + quart[3] = (double)(data[3] / 16384.0); +} + +void BNO055_lib::getEulerFromQ(float *euler){ + getQuart(data_d); + /*************** + data_d[0] : w + data_d[1] : x + data_d[2] : y + data_d[3] : z + ***************/ + + yy = data_d[2] * data_d[2]; + + t0 = 2.0 * (data_d[0] * data_d[3] + data_d[1] * data_d[2]); + t1 = 1.0 - 2.0 * (yy + data_d[3] * data_d[3]); + euler[0] = (float)atan2(t0, t1) * 57.2957795131; + + t0 = 2.0 * (data_d[0] * data_d[1] + data_d[2] * data_d[3]); + t1 = 1.0 - 2.0 * (data_d[1] * data_d[1] + yy); + euler[1] = (float)atan2(t0, t1) * 57.2957795131; + + t0 = 2.0 * (data_d[0] * data_d[2] - data_d[3] * data_d[1]); + t0 = (t0 > 1.0) ? 1.0 : t0; + t0 = (t0 < -1.0) ? -1.0 : t0; + euler[2] = (float)asin(t0) * 57.2957795131; +} + +void BNO055_lib::getEulerFromQ(double *euler){ + getQuart(data_d); + /*************** + data_d[0] : w + data_d[1] : x + data_d[2] : y + data_d[3] : z + ***************/ + + yy = data_d[2] * data_d[2]; + + t0 = 2.0 * (data_d[0] * data_d[3] + data_d[1] * data_d[2]); + t1 = 1.0 - 2.0 * (yy + data_d[3] * data_d[3]); + euler[0] = atan2(t0, t1) * 57.2957795131; + + t0 = 2.0 * (data_d[0] * data_d[1] + data_d[2] * data_d[3]); + t1 = 1.0 - 2.0 * (data_d[1] * data_d[1] + yy); + euler[1] = atan2(t0, t1) * 57.2957795131; + + t0 = 2.0 * (data_d[0] * data_d[2] - data_d[3] * data_d[1]); + t0 = (t0 > 1.0) ? 1.0 : t0; + t0 = (t0 < -1.0) ? -1.0 : t0; + euler[2] = asin(t0) * 57.2957795131; +} + +void BNO055_lib::getLinearAcc(float *lia){ + cmd[0] = BNO055_LIA_DATA; + i2c -> write(slave << 1, cmd, 1); + i2c -> read(slave << 1 | 1, buff, 6); + data[0] = (short)buff[1] << 8 | (short)buff[0]; + data[1] = (short)buff[3] << 8 | (short)buff[2]; + data[2] = (short)buff[5] << 8 | (short)buff[4]; + + lia[0] = (float)data[0] / 100.0; + lia[1] = (float)data[1] / 100.0; + lia[2] = (float)data[2] / 100.0; +} + +void BNO055_lib::getLinearAcc(double *lia){ + cmd[0] = BNO055_LIA_DATA; + i2c -> write(slave << 1, cmd, 1); + i2c -> read(slave << 1 | 1, buff, 6); + data[0] = (short)buff[1] << 8 | (short)buff[0]; + data[1] = (short)buff[3] << 8 | (short)buff[2]; + data[2] = (short)buff[5] << 8 | (short)buff[4]; + + lia[0] = (double)data[0] / 100.0; + lia[1] = (double)data[1] / 100.0; + lia[2] = (double)data[2] / 100.0; +} + +void BNO055_lib::getGravity(float *grv){ + cmd[0] = BNO055_GRV_DATA; + i2c -> write(slave << 1, cmd, 1); + i2c -> read(slave << 1 | 1, buff, 6); + data[0] = (short)buff[1] << 8 | (short)buff[0]; + data[1] = (short)buff[3] << 8 | (short)buff[2]; + data[2] = (short)buff[5] << 8 | (short)buff[4]; + + grv[0] = (float)data[0] / 100.0; + grv[1] = (float)data[1] / 100.0; + grv[2] = (float)data[2] / 100.0; +} + +void BNO055_lib::getGravity(double *grv){ + cmd[0] = BNO055_GRV_DATA; + i2c -> write(slave << 1, cmd, 1); + i2c -> read(slave << 1 | 1, buff, 6); + data[0] = (short)buff[1] << 8 | (short)buff[0]; + data[1] = (short)buff[3] << 8 | (short)buff[2]; + data[2] = (short)buff[5] << 8 | (short)buff[4]; + + grv[0] = (double)data[0] / 100.0; + grv[1] = (double)data[1] / 100.0; + grv[2] = (double)data[2] / 100.0; +} + \ No newline at end of file