クオータニオンを出力してくれる9軸センサ,BNO055のライブラリ
BNO055_lib.cpp
- Committer:
- Sigma884
- Date:
- 2019-01-22
- Revision:
- 0:4c82133c291f
- Child:
- 1:6de1be86a23f
File content as of revision 0:4c82133c291f:
#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; }