PQ_Hybrid_Electrical_Equipment_Team
クオータニオンを出力してくれる9軸センサ，BNO055のライブラリ
Diff: BNO055_lib.cpp
- Revision:
- 0:4c82133c291f
- Child:
- 1:6de1be86a23f
--- /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