Diff: BMX055.cpp

Revision:

2:1bea0ef16d84

Parent:

1:0d05e7c9ff4c

--- a/BMX055.cpp	Wed Jan 16 01:36:49 2019 +0000
+++ b/BMX055.cpp	Mon Mar 04 04:10:59 2019 +0000
@@ -7,12 +7,13 @@
  *  http://www.page.sannet.ne.jp/kenjia/index.html
  *  http://mbed.org/users/kenjiArai/
  *      Started:    October   24th, 2018
- *      Revised:    January    8th, 2019
+ *      Revised:    March      3rd, 2019
  */
 
 #include "mbed.h"
 #include "BMX055.h"
 
+#define DEBUG 0
 
 #if MBED_MAJOR_VERSION == 2
 #define WAIT_MS(x)       wait_ms(x)
@@ -53,7 +54,7 @@
     dt[0] = 0x02;
     _i2c.write(chip_addr, dt, 1, true);
     _i2c.read(chip_addr, dt, 6, false);
-#if 0
+#if DEBUG
     printf("Read ACC data-> ");
     for (uint32_t i = 0; i < 6; i++){
         printf("i=%d,dt=0x%02x, ", i, dt[i]);
@@ -94,7 +95,7 @@
     dt[0] = 0x02;
     _i2c.write(chip_addr, dt, 1, true);
     _i2c.read(chip_addr, dt, 6, false);
-#if 0
+#if DEBUG
     printf("Read MAG data-> ");
     for (uint32_t i = 0; i < 6; i++){
         printf("i=%d,dt=0x%02x, ", i, dt[i]);
@@ -160,22 +161,22 @@
     int16_t x,y,z;
 
     chip_addr = inf_addr.gyr_addr;
-    dt[0] = 0x02;
+    dt[0] = 0x42;
     _i2c.write(chip_addr, dt, 1, true);
     _i2c.read(chip_addr, dt, 6, false);
-#if 0
+#if DEBUG
     printf("Read GYR data-> ");
     for (uint32_t i = 0; i < 6; i++){
         printf("i=%d,dt=0x%02x, ", i, dt[i]);
     }
     printf(", all\r\n");
 #endif
-    x = dt[1] << 8 | (dt[0] & 0xf8);    // 13bit
-    y = dt[3] << 8 | (dt[2] & 0xf8);    // 13bit
-    z = dt[5] << 8 | (dt[4] & 0xfe);    // 15bit
-    mag->x = (double)x / 8.0f;
-    mag->y = (double)y / 8.0f;
-    mag->z = (double)z / 2.0f;
+    x = (dt[1] << 5 | (dt[0] & 0x1f)) << 3;    // 13bit
+    y = (dt[3] << 5 | (dt[2] & 0x1f)) << 3;    // 13bit
+    z = (dt[5] << 7 | (dt[4] & 0x7f)) << 1;    // 15bit
+    mag->x = (double)x;
+    mag->y = (double)y;
+    mag->z = (double)z;
 }
 
 float BMX055::get_chip_temperature()
@@ -195,7 +196,7 @@
     // Check Acc & Mag & Gyro are available of not
     check_id();
     if (ready_flag == 0x07){
-#if 0
+#if DEBUG
         printf("ACC+GYR+MAG are ready!\r\n");
 #endif
     }
@@ -413,7 +414,7 @@
             }
         }
     }
-#if 0
+#if DEBUG
     printf("ACC addr=0x%02x, id=0x%02x\r\n", chip_addr, inf_id.acc_id); 
     printf("GYR addr=0x%02x, id=0x%02x\r\n", chip_addr, inf_id.gyr_id); 
     printf("MAG addr=0x%02x, id=0x%02x\r\n", chip_addr, inf_id.mag_id);