クオータニオンを出力してくれる9軸センサ,BNO055のライブラリ
Diff: BNO055_lib.cpp
- Revision:
- 1:6de1be86a23f
- Parent:
- 0:4c82133c291f
diff -r 4c82133c291f -r 6de1be86a23f BNO055_lib.cpp --- a/BNO055_lib.cpp Tue Jan 22 11:47:28 2019 +0000 +++ b/BNO055_lib.cpp Tue Feb 19 14:57:28 2019 +0000 @@ -25,18 +25,52 @@ } void BNO055_lib::setAccRange(ACC_RANGE range){ + cmd[0] = BNO055_PAGE_ID; + cmd[1] = 0x01; + i2c -> write(slave << 1, cmd, 2); + cmd[0] = BNO055_ACC_CONFIG; - cmd[1] = 0x0F; + cmd[1] = (char)range | 0x0C; + i2c -> write(slave << 1, cmd, 2); + i2c -> write(slave << 1, cmd, 1); + i2c -> read(slave << 1 | 1, buff, 1); + + cmd[0] = BNO055_PAGE_ID; + cmd[1] = 0x00; i2c -> write(slave << 1, cmd, 2); } void BNO055_lib::setOperationMode(OPERATION_MODE mode){ + wait(0.05f); cmd[0] = BNO055_OPR_MODE; - //cmd[1] = 0x07; //AMG - cmd[1] = 0x0C; //NDOF + cmd[1] = (char)mode; + i2c -> write(slave << 1, cmd, 2); + wait(0.05f); +} + +void BNO055_lib::setAxis(AXIS x, AXIS y, AXIS z){ + if((x != y) && (y != z) && (z != x)){ + cmd[0] = BNO055_AXIS_MAP_CONFIG; + cmd[1] = ((char)z << 4) | ((char)y << 2) | (char)x; + i2c -> write(slave << 1, cmd, 2); + } +} + +void BNO055_lib::setAxisPM(int x_pm, int y_pm, int z_pm){ + cmd[0] = BNO055_AXIS_MAP_SIGN; + cmd[1] = 0x00; + if(x_pm < 0){ + cmd[1] = cmd[1] | 0x04; + } + if(y_pm < 0){ + cmd[1] = cmd[1] | 0x02; + } + if(z_pm < 0){ + cmd[1] = cmd[1] | 0x01; + } i2c -> write(slave << 1, cmd, 2); } - + void BNO055_lib::getAcc(float *acc){ cmd[0] = BNO055_ACC_DATA; i2c -> write(slave << 1, cmd, 1);