Diff: BNO055.cpp

Revision:

1:cb7e19c0a702

Parent:

0:86a17116e8be

Child:

2:0f225b686cd5

--- a/BNO055.cpp	Sun Apr 05 04:12:58 2015 +0000
+++ b/BNO055.cpp	Tue Apr 07 12:03:58 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      5th, 2015
+ *      Revised: April      7th, 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
@@ -19,37 +19,33 @@
 #include "mbed.h"
 #include "BNO055.h"
 
-BNO055::BNO055 (PinName p_sda, PinName p_scl, uint8_t addr, uint8_t mode):
-    _i2c(p_sda, p_scl)
+BNO055::BNO055 (PinName p_sda, PinName p_scl, PinName p_reset, uint8_t addr, uint8_t mode):
+    _i2c(p_sda, p_scl), _res(p_reset)
 {
-    _i2c.frequency(400000);
     chip_addr = addr;
     chip_mode = mode;
     initialize ();
 }
 
-BNO055::BNO055 (PinName p_sda, PinName p_scl) :
-    _i2c(p_sda, p_scl)
+BNO055::BNO055 (PinName p_sda, PinName p_scl, PinName p_reset) :
+    _i2c(p_sda, p_scl), _res(p_reset)
 {
-    _i2c.frequency(400000);
     chip_addr = BNO055_G_CHIP_ADDR;
     chip_mode = MODE_NDOF;
     initialize ();
 }
 
-BNO055::BNO055 (I2C& p_i2c,  uint8_t addr, uint8_t mode) :
-    _i2c(p_i2c)
+BNO055::BNO055 (I2C& p_i2c, PinName p_reset, uint8_t addr, uint8_t mode) :
+    _i2c(p_i2c), _res(p_reset)
 {
-    _i2c.frequency(400000);
     chip_addr = addr;
     chip_mode = mode;
     initialize ();
 }
 
-BNO055::BNO055 (I2C& p_i2c) :
-    _i2c(p_i2c)
+BNO055::BNO055 (I2C& p_i2c, PinName p_reset) :
+    _i2c(p_i2c), _res(p_reset)
 {
-    _i2c.frequency(400000);
     chip_addr = BNO055_G_CHIP_ADDR;
     chip_mode = MODE_NDOF;
     initialize ();
@@ -203,6 +199,11 @@
 /////////////// Initialize ////////////////////////////////
 void BNO055::initialize (void)
 {
+#if defined(TARGET_STM32L152RE)
+    _i2c.frequency(100000);
+#else
+    _i2c.frequency(400000);
+#endif
     // Check Acc & Mag & Gyro are available of not
     check_id();
     // Set initial data
@@ -236,6 +237,27 @@
     return dt[0];
 }
 
+uint8_t BNO055::reset(void)
+{
+     _res = 0;
+     wait_ms(1);   // Reset 1mS
+     _res = 1;
+     wait(0.7);  // Need to wait at least 650mS
+#if defined(TARGET_STM32L152RE)
+    _i2c.frequency(400000);
+#else
+    _i2c.frequency(400000);
+#endif
+    _i2c.stop();
+    check_id();
+    if (chip_id != I_AM_BNO055_CHIP){
+        return 1;
+    } else {
+        initialize();
+        return 0;
+    }
+}
+
 ////// Set initialize data to related registers ///////////
 void BNO055::set_initial_dt_to_regs(void)
 {