Diff: BNO055.cpp

Revision:

3:0ad6f85b178f

Parent:

2:0f225b686cd5

Child:

4:9e6fead1e93e

--- a/BNO055.cpp	Wed Apr 08 11:27:57 2015 +0000
+++ b/BNO055.cpp	Fri Apr 10 11:18:12 2015 +0000
@@ -7,7 +7,7 @@
  *  http://www.page.sannet.ne.jp/kenjia/index.html
  *  http://mbed.org/users/kenjiArai/
  *      Created: March     30th, 2015
- *      Revised: April      8th, 2015
+ *      Revised: April     10th, 2015
  *
  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
  * INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE
@@ -204,6 +204,8 @@
 #else
     _i2c.frequency(400000);
 #endif
+    page_flag = 0xff;
+    select_page(0);
     // Check Acc & Mag & Gyro are available of not
     check_id();
     // Set initial data
@@ -224,17 +226,20 @@
 
 uint8_t BNO055::select_page(uint8_t page)
 {
-    dt[0] = BNO055_PAGE_ID;
-    if (page == 1) {
-        dt[1] = 1;  // select page 1
-    } else {
-        dt[1] = 0;  // select page 0
+    if (page != page_flag){
+        dt[0] = BNO055_PAGE_ID;
+        if (page == 1) {
+            dt[1] = 1;  // select page 1
+        } else {
+            dt[1] = 0;  // select page 0
+        }
+        _i2c.write(chip_addr, dt, 2, false);
+        dt[0] = BNO055_PAGE_ID;
+        _i2c.write(chip_addr, dt, 1, true);
+        _i2c.read(chip_addr, dt, 1, false);
+        page_flag = dt[0];
     }
-    _i2c.write(chip_addr, dt, 2, false);
-    dt[0] = BNO055_PAGE_ID;
-    _i2c.write(chip_addr, dt, 1, true);
-    _i2c.read(chip_addr, dt, 1, false);
-    return dt[0];
+    return page_flag;
 }
 
 uint8_t BNO055::reset(void)
@@ -249,6 +254,8 @@
     _i2c.frequency(400000);
 #endif
     _i2c.stop();
+    page_flag = 0xff;
+    select_page(0);
     check_id();
     if (chip_id != I_AM_BNO055_CHIP){
         return 1;
@@ -275,21 +282,21 @@
     _i2c.read(chip_addr, dt, 7, false);
     chip_id = dt[0];
     if (chip_id == I_AM_BNO055_CHIP) {
-        ready_flg = 1;
+        ready_flag = 1;
     } else {
-        ready_flg = 0;
+        ready_flag = 0;
     }
     acc_id = dt[1];
     if (acc_id == I_AM_BNO055_ACC) {
-        ready_flg |= 2;
+        ready_flag |= 2;
     }
     mag_id = dt[2];
     if (mag_id == I_AM_BNO055_MAG) {
-        ready_flg |= 4;
+        ready_flag |= 4;
     }
     gyr_id = dt[3];
     if (mag_id == I_AM_BNO055_MAG) {
-        ready_flg |= 8;
+        ready_flag |= 8;
     }
     bootldr_rev_id = dt[5]<< 8 | dt[4];
     sw_rev_id = dt[6];
@@ -308,7 +315,7 @@
 /////////////// Check chip ready or not  //////////////////
 uint8_t BNO055::chip_ready(void)
 {
-    if (ready_flg == 0x0f) {
+    if (ready_flag == 0x0f) {
         return 1;
     }
     return 0;