Diff: BNO055.cpp

Revision:

2:e8bc1ae2c20c

Parent:

0:3807ce385b2a

--- a/BNO055.cpp	Tue Nov 29 02:57:08 2016 +0000
+++ b/BNO055.cpp	Sun Jun 25 05:02:39 2017 +0000
@@ -45,7 +45,7 @@
  */
 void BNO055_CTRL::init(){}
 char BNO055_CTRL::rr(bool isPage1, char regAddr){return 0;}
-char BNO055_CTRL::rrc(bool isPage1, char startRegAddr, char *receiveBytes, char length){return 0;}
+char BNO055_CTRL::rrc(bool isPage1, char startRegAddr, unsigned char *receiveBytes, char length){return 0;}
 char BNO055_CTRL::wr(bool isPage1, char regAddr, char wBytes){return 0;}
 char BNO055_CTRL::wrc(bool isPage1, char startRegAddr, char *Bytes, char length){return 0;}
 
@@ -172,7 +172,7 @@
  * -4           レスポンスエラー
  * それ以外     成功した際の戻り値バイト数
  */
-char BNO055_UART_CTRL::rrc(bool isPage1, char startRegAddr, char *receiveBytes, char length){
+char BNO055_UART_CTRL::rrc(bool isPage1, char startRegAddr, unsigned char *receiveBytes, char length){
     //読み取りバイト数が1未満またはBNO055_UART_BUF_MAXLEN以上はバッファが足りないので読み取れない
     if(length < 1 || length > BNO055_UART_BUF_MAXLEN) return -1;
 
@@ -497,7 +497,7 @@
  * <I2C>
  * レジスタの内容を読み取り(複数可)
  */
-char BNO055_I2C_CTRL::rrc(bool isPage1, char startRegAddr, char *receiveBytes, char length){
+char BNO055_I2C_CTRL::rrc(bool isPage1, char startRegAddr, unsigned char *receiveBytes, char length){
     //読み取りバイト数が1未満
     if(length < 1) return -1;
 
@@ -511,7 +511,7 @@
 
     ary[0] = startRegAddr;
     iface->write(i2c_writeAddr, ary, 1, true);
-    iface->read(i2c_readAddr, receiveBytes, length, false);
+    iface->read(i2c_readAddr, (char *)receiveBytes, length, false);
 
     return receiveBytes[0];
 }
@@ -727,7 +727,7 @@
  * ユーザー定義読み取り(連続)
  * レジスタを指定して値を直接読み取る
  */
-char BOARDC_BNO055::customReadC(bool isPage1, char startRegAddr, char *receiveBytes, unsigned char length){
+char BOARDC_BNO055::customReadC(bool isPage1, char startRegAddr, unsigned char *receiveBytes, unsigned char length){
     return ctrl->rrc(isPage1, startRegAddr, receiveBytes, length);
 }
 
@@ -798,7 +798,7 @@
  */
 short BOARDC_BNO055::getRevision(){
     //連続2byte読み取り
-    char rsv[2];
+    unsigned char rsv[2];
     ctrl->rrc(0, BNO055P0_SW_REV_ID_LSB, rsv, 2);
 
     return (rsv[1] << 8) | rsv[0];
@@ -877,7 +877,7 @@
  */
 void BOARDC_BNO055::getAccDataAll(short &accX, short &accY, short &accZ){
     //連続6byte読み取り
-    char rsv[6];
+    unsigned char rsv[6];
     ctrl->rrc(0, BNO055P0_ACC_DATA_X_LSB, rsv, 6);
 
     accX = (rsv[1] << 8) | rsv[0];
@@ -890,7 +890,7 @@
  */
 short BOARDC_BNO055::getAccDataX(){
     //連続2byte読み取り
-    char rsv[2];
+    unsigned char rsv[2];
     ctrl->rrc(0, BNO055P0_ACC_DATA_X_LSB, rsv, 2);
 
     return (rsv[1] << 8) | rsv[0];
@@ -901,7 +901,7 @@
  */
 short BOARDC_BNO055::getAccDataY(){
     //連続2byte読み取り
-    char rsv[2];
+    unsigned char rsv[2];
     ctrl->rrc(0, BNO055P0_ACC_DATA_Y_LSB, rsv, 2);
 
     return (rsv[1] << 8) | rsv[0];
@@ -912,7 +912,7 @@
  */
 short BOARDC_BNO055::getAccDataZ(){
     //連続2byte読み取り
-    char rsv[2];
+    unsigned char rsv[2];
     ctrl->rrc(0, BNO055P0_ACC_DATA_Z_LSB, rsv, 2);
 
     return (rsv[1] << 8) | rsv[0];
@@ -927,7 +927,7 @@
  */
 void BOARDC_BNO055::getMagDataAll(short &magX, short &magY, short &magZ){
     //連続6byte読み取り
-    char rsv[6];
+    unsigned char rsv[6];
     ctrl->rrc(0, BNO055P0_MAG_DATA_X_LSB, rsv, 6);
 
     magX = (rsv[1] << 8) | rsv[0];
@@ -940,7 +940,7 @@
  */
 short BOARDC_BNO055::getMagDataX(){
     //連続2byte読み取り
-    char rsv[2];
+    unsigned char rsv[2];
     ctrl->rrc(0, BNO055P0_MAG_DATA_X_LSB, rsv, 2);
 
     return (rsv[1] << 8) | rsv[0];
@@ -951,7 +951,7 @@
  */
 short BOARDC_BNO055::getMagDataY(){
     //連続2byte読み取り
-    char rsv[2];
+    unsigned char rsv[2];
     ctrl->rrc(0, BNO055P0_MAG_DATA_Y_LSB, rsv, 2);
 
     return (rsv[1] << 8) | rsv[0];
@@ -962,7 +962,7 @@
  */
 short BOARDC_BNO055::getMagDataZ(){
     //連続2byte読み取り
-    char rsv[2];
+    unsigned char rsv[2];
     ctrl->rrc(0, BNO055P0_MAG_DATA_Z_LSB, rsv, 2);
 
     return (rsv[1] << 8) | rsv[0];
@@ -977,7 +977,7 @@
  */
 void BOARDC_BNO055::getGyroDataAll(short &gyroX, short &gyroY, short &gyroZ){
     //連続6byte読み取り
-    char rsv[6];
+    unsigned char rsv[6];
     ctrl->rrc(0, BNO055P0_GYR_DATA_X_LSB, rsv, 6);
 
     gyroX = (rsv[1] << 8) | rsv[0];
@@ -990,7 +990,7 @@
  */
 short BOARDC_BNO055::getGyroDataX(){
     //連続2byte読み取り
-    char rsv[2];
+    unsigned char rsv[2];
     ctrl->rrc(0, BNO055P0_GYR_DATA_X_LSB, rsv, 2);
 
     return (rsv[1] << 8) | rsv[0];
@@ -1001,7 +1001,7 @@
  */
 short BOARDC_BNO055::getGyroDataY(){
     //連続2byte読み取り
-    char rsv[2];
+    unsigned char rsv[2];
     ctrl->rrc(0, BNO055P0_GYR_DATA_Y_LSB, rsv, 2);
 
     return (rsv[1] << 8) | rsv[0];
@@ -1012,7 +1012,7 @@
  */
 short BOARDC_BNO055::getGyroDataZ(){
     //連続2byte読み取り
-    char rsv[2];
+    unsigned char rsv[2];
     ctrl->rrc(0, BNO055P0_GYR_DATA_Z_LSB, rsv, 2);
 
     return (rsv[1] << 8) | rsv[0];
@@ -1021,6 +1021,8 @@
 /* ==================================================================
  * FusionSensing:オイラー角の値を取得する
  * FusionSensing機能(内部演算機能)を使用していない場合は不定の値を返す
+ * Roll軸およびPitch軸が+-45degの範囲を超える場合、Yaw軸の値は不安定となる
+ * (その場合はgetEulerFromQを使用することで四元数からオイラー角を導出可能)
  * ------------------------------------------------------------------
  * &E_heading: アドレス参照引数:関数実行後、この変数にYaw軸(heading)の値が格納される
  * &E_roll: アドレス参照引数:関数実行後、この変数にroll軸の値が格納される
@@ -1028,7 +1030,7 @@
  */
 void BOARDC_BNO055::getEulerDataAll(short &E_heading, short &E_roll, short &E_pitch){
     //連続6byte読み取り
-    char rsv[6];
+    unsigned char rsv[6];
     ctrl->rrc(0, BNO055P0_EUL_HEADING_LSB, rsv, 6);
 
     E_heading = (rsv[1] << 8) | rsv[0];
@@ -1042,7 +1044,7 @@
  */
 short BOARDC_BNO055::getEulerDataHeading(){
     //連続2byte読み取り
-    char rsv[2];
+    unsigned char rsv[2];
     ctrl->rrc(0, BNO055P0_EUL_HEADING_LSB, rsv, 2);
 
     return (rsv[1] << 8) | rsv[0];
@@ -1062,7 +1064,7 @@
  */
 short BOARDC_BNO055::getEulerDataRoll(){
     //連続2byte読み取り
-    char rsv[2];
+    unsigned char rsv[2];
     ctrl->rrc(0, BNO055P0_EUL_ROLL_LSB, rsv, 2);
 
     return (rsv[1] << 8) | rsv[0];
@@ -1074,7 +1076,7 @@
  */
 short BOARDC_BNO055::getEulerDataPitch(){
     //連続2byte読み取り
-    char rsv[2];
+    unsigned char rsv[2];
     ctrl->rrc(0, BNO055P0_EUL_PITCH_LSB, rsv, 2);
 
     return (rsv[1] << 8) | rsv[0];
@@ -1084,7 +1086,7 @@
  * FusionSensing:9軸センサーのすべての値の値を取得する
  */
 void BOARDC_BNO055::get9Axis(short *box){
-    char rsv[18];
+    unsigned char rsv[18];
     ctrl->rrc(0, BNO055P0_ACC_DATA_X_LSB, rsv, 18);
 
     box[0] = (rsv[1] << 8) | rsv[0];
@@ -1103,7 +1105,7 @@
  * FusionSensing機能(内部演算機能)を使用していない場合は不定の値を返す
  */
 void BOARDC_BNO055::get9AxisAndEUL(short *box){
-    char rsv[24];
+    unsigned char rsv[24];
     ctrl->rrc(0, BNO055P0_ACC_DATA_X_LSB, rsv, 24);
 
     box[0] = (rsv[1] << 8) | rsv[0];
@@ -1128,7 +1130,7 @@
  */
 void BOARDC_BNO055::getQuaternion(short &q1, short &q2, short &q3, short &q4){
     //連続8byte読み取り
-    char rsv[8];
+    unsigned char rsv[8];
     ctrl->rrc(0, BNO055P0_QUA_DATA_W_LSB, rsv, 8);
 
     q1 = (rsv[1] << 8) | rsv[0];
@@ -1138,6 +1140,44 @@
 }
 
 /* ==================================================================
+ * FusionSensing:四元数(Quaternion)を取得し、オイラー角を算出して返す
+ * FusionSensing機能(内部演算機能)を使用していない場合は不定の値を返す
+ * 出力数値範囲 -180.0 to 180.0[deg] CW(時計回り)- CCW(反時計回り)+
+ * ------------------------------------------------------------------
+ * &E_heading, &E_roll, &E_pitch:アドレス参照引数:関数実行後、この変数にオイラー角が格納される
+ */
+void BOARDC_BNO055::getEulerFromQ(double &E_heading, double &E_roll, double &E_pitch){
+    //連続8byte読み取り
+    unsigned char rsv[8];
+    ctrl->rrc(0, BNO055P0_QUA_DATA_W_LSB, rsv, 8);
+
+    //四元数を実際の数値に変換(1 / 2^14を掛けるより2^14で割ったほうが早い・・・)
+    double q1, q2, q3, q4;
+    q1 = (double)((short)((rsv[1] << 8) | rsv[0]) / 16384.0);
+    q2 = (double)((short)((rsv[3] << 8) | rsv[2]) / 16384.0);
+    q3 = (double)((short)((rsv[5] << 8) | rsv[4]) / 16384.0);
+    q4 = (double)((short)((rsv[7] << 8) | rsv[6]) / 16384.0);
+    
+    //四元数からオイラー角に変換
+    //ref: Wikipedia
+    //Conversion between quaternions and Euler angles(Quaternion to Euler Angles Conversion)
+    double q3q3 = q3 * q3;
+    
+    double m1 = +2.0 * (q1 * q2 + q3 * q4);
+    double m2 = +1.0 - 2.0 * (q2 * q2 + q3q3);
+    E_roll = atan2(m1, m2) * 57.2957795131;
+    
+    m1 = +2.0 * (q1 * q3 - q4 * q2);
+    m1 = (m1 > 1.0)? 1.0 : m1;
+    m1 = (m1 < -1.0)? -1.0 : m1;
+    E_pitch = asin(m1) * 57.2957795131;
+    
+    m1 = +2.0 * (q1 * q4 + q2 * q3);
+    m2 = +1.0 - 2.0 * (q3q3 + q4 * q4);
+    E_heading = atan2(m1, m2) * 57.2957795131;
+}
+
+/* ==================================================================
  * 線形加速度(LinearAcceleration)での加速度センサの値を取得する
  * ------------------------------------------------------------------
  * &L_accX: アドレス参照引数:関数実行後、この変数にX軸の値が格納される
@@ -1146,7 +1186,7 @@
  */
 void BOARDC_BNO055::getLinearAccDataAll(short &L_accX, short &L_accY, short &L_accZ){
     //連続6byte読み取り
-    char rsv[6];
+    unsigned char rsv[6];
     ctrl->rrc(0, BNO055P0_LIA_DATA_X_LSB, rsv, 6);
 
     L_accX = (rsv[1] << 8) | rsv[0];
@@ -1159,7 +1199,7 @@
  */
 short BOARDC_BNO055::getLinearAccDataX(){
     //連続2byte読み取り
-    char rsv[2];
+    unsigned char rsv[2];
     ctrl->rrc(0, BNO055P0_LIA_DATA_X_LSB, rsv, 2);
 
     return (rsv[1] << 8) | rsv[0];
@@ -1170,7 +1210,7 @@
  */
 short BOARDC_BNO055::getLinearAccDataY(){
     //連続2byte読み取り
-    char rsv[2];
+    unsigned char rsv[2];
     ctrl->rrc(0, BNO055P0_LIA_DATA_Y_LSB, rsv, 2);
 
     return (rsv[1] << 8) | rsv[0];
@@ -1181,7 +1221,7 @@
  */
 short BOARDC_BNO055::getLinearAccDataZ(){
     //連続2byte読み取り
-    char rsv[2];
+    unsigned char rsv[2];
     ctrl->rrc(0, BNO055P0_LIA_DATA_Z_LSB, rsv, 2);
 
     return (rsv[1] << 8) | rsv[0];
@@ -1196,7 +1236,7 @@
  */
 void BOARDC_BNO055::getGVectorDataAll(short &gvX, short &gvY, short &gvZ){
     //連続6byte読み取り
-    char rsv[6];
+    unsigned char rsv[6];
     ctrl->rrc(0, BNO055P0_GRV_DATA_X_LSB, rsv, 6);
 
     gvX = (rsv[1] << 8) | rsv[0];
@@ -1209,7 +1249,7 @@
  */
 short BOARDC_BNO055::getGVectorDataX(){
     //連続2byte読み取り
-    char rsv[2];
+    unsigned char rsv[2];
     ctrl->rrc(0, BNO055P0_GRV_DATA_X_LSB, rsv, 2);
 
     return (rsv[1] << 8) | rsv[0];
@@ -1220,7 +1260,7 @@
  */
 short BOARDC_BNO055::getGVectorDataY(){
     //連続2byte読み取り
-    char rsv[2];
+    unsigned char rsv[2];
     ctrl->rrc(0, BNO055P0_GRV_DATA_Y_LSB, rsv, 2);
 
     return (rsv[1] << 8) | rsv[0];
@@ -1231,7 +1271,7 @@
  */
 short BOARDC_BNO055::getGVectorDataZ(){
     //連続2byte読み取り
-    char rsv[2];
+    unsigned char rsv[2];
     ctrl->rrc(0, BNO055P0_GRV_DATA_Z_LSB, rsv, 2);
 
     return (rsv[1] << 8) | rsv[0];
@@ -2244,7 +2284,7 @@
  */
 void BOARDC_BNO055::getAccOffsetAll(float &offsetX, float &offsetY, float &offsetZ){
     //連続6byte読み取り
-    char rsv[6];
+    unsigned char rsv[6];
     ctrl->rrc(0, BNO055P0_ACC_OFFSET_X_LSB, rsv, 6);
 
     short offX = (rsv[1] << 8) | rsv[0];
@@ -2263,7 +2303,7 @@
  */
 float BOARDC_BNO055::getAccOffsetX(){
     //連続2byte読み取り
-    char rsv[2];
+    unsigned char rsv[2];
     ctrl->rrc(0, BNO055P0_ACC_OFFSET_X_LSB, rsv, 2);
 
     short offX = (rsv[1] << 8) | rsv[0];
@@ -2278,7 +2318,7 @@
  */
 float BOARDC_BNO055::getAccOffsetY(){
     //連続2byte読み取り
-    char rsv[2];
+    unsigned char rsv[2];
     ctrl->rrc(0, BNO055P0_ACC_OFFSET_Y_LSB, rsv, 2);
 
     short offX = (rsv[1] << 8) | rsv[0];
@@ -2293,7 +2333,7 @@
  */
 float BOARDC_BNO055::getAccOffsetZ(){
     //連続2byte読み取り
-    char rsv[2];
+    unsigned char rsv[2];
     ctrl->rrc(0, BNO055P0_ACC_OFFSET_Z_LSB, rsv, 2);
 
     short offX = (rsv[1] << 8) | rsv[0];
@@ -2366,7 +2406,7 @@
  */
 void BOARDC_BNO055::getMagOffsetAll(float &offsetX, float &offsetY, float &offsetZ){
     //連続6byte読み取り
-    char rsv[6];
+    unsigned char rsv[6];
     ctrl->rrc(0, BNO055P0_MAG_OFFSET_X_LSB, rsv, 6);
 
     short offX = (rsv[1] << 8) | rsv[0];
@@ -2385,7 +2425,7 @@
  */
 float BOARDC_BNO055::getMagOffsetX(){
     //連続2byte読み取り
-    char rsv[2];
+    unsigned char rsv[2];
     ctrl->rrc(0, BNO055P0_MAG_OFFSET_X_LSB, rsv, 2);
 
     short offX = (rsv[1] << 8) | rsv[0];
@@ -2400,7 +2440,7 @@
  */
 float BOARDC_BNO055::getMagOffsetY(){
     //連続2byte読み取り
-    char rsv[2];
+    unsigned char rsv[2];
     ctrl->rrc(0, BNO055P0_MAG_OFFSET_Y_LSB, rsv, 2);
 
     short offY = (rsv[1] << 8) | rsv[0];
@@ -2415,7 +2455,7 @@
  */
 float BOARDC_BNO055::getMagOffsetZ(){
     //連続2byte読み取り
-    char rsv[2];
+    unsigned char rsv[2];
     ctrl->rrc(0, BNO055P0_MAG_OFFSET_Z_LSB, rsv, 2);
 
     short offZ = (rsv[1] << 8) | rsv[0];
@@ -2515,7 +2555,7 @@
  */
 void BOARDC_BNO055::getGyroOffsetAll(float &offsetX, float &offsetY, float &offsetZ){
     //連続6byte読み取り
-    char rsv[6];
+    unsigned char rsv[6];
     ctrl->rrc(0, BNO055P0_GYR_OFFSET_X_LSB, rsv, 6);
 
     short offX = (rsv[1] << 8) | rsv[0];
@@ -2534,7 +2574,7 @@
  */
 float BOARDC_BNO055::getGyroOffsetX(){
     //連続2byte読み取り
-    char rsv[2];
+    unsigned char rsv[2];
     ctrl->rrc(0, BNO055P0_GYR_OFFSET_X_LSB, rsv, 2);
 
     short offX = (rsv[1] << 8) | rsv[0];
@@ -2549,7 +2589,7 @@
  */
 float BOARDC_BNO055::getGyroOffsetY(){
     //連続2byte読み取り
-    char rsv[2];
+    unsigned char rsv[2];
     ctrl->rrc(0, BNO055P0_GYR_OFFSET_Y_LSB, rsv, 2);
 
     short offY = (rsv[1] << 8) | rsv[0];
@@ -2564,7 +2604,7 @@
  */
 float BOARDC_BNO055::getGyroOffsetZ(){
     //連続2byte読み取り
-    char rsv[2];
+    unsigned char rsv[2];
     ctrl->rrc(0, BNO055P0_GYR_OFFSET_Z_LSB, rsv, 2);
 
     short offZ = (rsv[1] << 8) | rsv[0];
@@ -2663,7 +2703,7 @@
  */
 short BOARDC_BNO055::getAccRadius(){
     //連続2byte読み取り
-    char rsv[2];
+    unsigned char rsv[2];
     ctrl->rrc(0, BNO055P0_ACC_RADIUS_LSB, rsv, 2);
 
     return (rsv[1] << 8) | rsv[0];
@@ -2698,7 +2738,7 @@
  */
 short BOARDC_BNO055::getMagRadius(){
     //連続2byte読み取り
-    char rsv[2];
+    unsigned char rsv[2];
     ctrl->rrc(0, BNO055P0_MAG_RADIUS_LSB, rsv, 2);
 
     return (rsv[1] << 8) | rsv[0];