Diff: BNO055.cpp

Revision:

7:b48d96169302

Parent:

6:07d01bf36ad0

--- a/BNO055.cpp	Wed Aug 23 09:44:43 2017 +0000
+++ b/BNO055.cpp	Wed Aug 05 05:28:13 2020 +0000
@@ -3,26 +3,22 @@
  *  BNO055 Intelligent 9-axis absolute orientation sensor
  *  by Bosch Sensortec
  *
- * Copyright (c) 2015,'17 Kenji Arai / JH1PJL
- *  http://www.page.sannet.ne.jp/kenjia/index.html
- *  http://mbed.org/users/kenjiArai/
+ * Copyright (c) 2015,'17,'20 Kenji Arai / JH1PJL
+ *  http://www7b.biglobe.ne.jp/~kenjia/
+ *  https://os.mbed.com/users/kenjiArai/
  *      Created: March     30th, 2015
- *      Revised: August    23rd, 2017
+ *      Revised: August     5th, 2020
  */
 
 #include "mbed.h"
 #include "BNO055.h"
 
-
-#if MBED_MAJOR_VERSION == 2
-#define WAIT_MS(x)       wait_ms(x)
-#elif  MBED_MAJOR_VERSION == 5
-#define WAIT_MS(x)       Thread::wait(x)
-#else
-#error "Running on Unknown OS"
-#endif
-
-BNO055::BNO055 (PinName p_sda, PinName p_scl, PinName p_reset, uint8_t addr, uint8_t mode):
+BNO055::BNO055 (PinName p_sda,
+                PinName p_scl,
+                PinName p_reset,
+                uint8_t addr,
+                uint8_t mode
+               ):
     _i2c_p(new I2C(p_sda, p_scl)), _i2c(*_i2c_p), _res(p_reset)
 {
     chip_addr = addr;
@@ -54,7 +50,7 @@
     initialize ();
 }
 
-/////////////// Read data & normalize /////////////////////
+/////////////// Read data & normalize //////////////////////////////////////////
 void BNO055::get_Euler_Angles(BNO055_EULER_TypeDef *el)
 {
     uint8_t deg_or_rad;
@@ -176,7 +172,7 @@
     dt[0] = BNO055_TEMP_SOURCE;
     dt[1] = 0;
     _i2c.write(chip_addr, dt, 2, false);
-    WAIT_MS(1); // Do I need to wait?
+    ThisThread::sleep_for(1ms); // Do I need to wait?
     dt[0] = BNO055_TEMP;
     _i2c.write(chip_addr, dt, 1, true);
     _i2c.read(chip_addr, dt, 1, false);
@@ -188,7 +184,7 @@
     dt[0] = BNO055_TEMP_SOURCE;
     dt[1] = 1;
     _i2c.write(chip_addr, dt, 2, false);
-    WAIT_MS(1); // Do I need to wait?
+    ThisThread::sleep_for(1ms); // Do I need to wait?
     dt[0] = BNO055_TEMP;
     _i2c.write(chip_addr, dt, 1, true);
     _i2c.read(chip_addr, dt, 1, false);
@@ -199,10 +195,10 @@
     }
 }
 
-/////////////// Initialize ////////////////////////////////
+/////////////// Initialize /////////////////////////////////////////////////////
 void BNO055::initialize (void)
 {
-#if defined(TARGET_STM32L152RE)
+#if defined(TARGET_NUCLEO_L152RE)
     _i2c.frequency(100000);
 #else
     _i2c.frequency(400000);
@@ -223,13 +219,14 @@
 {
     select_page(0);
     dt[0] = BNO055_UNIT_SEL;
-    dt[1] = UNIT_ORI_WIN + UNIT_ACC_MSS + UNIT_GYR_DPS + UNIT_EULER_DEG + UNIT_TEMP_C;
+    dt[1] = UNIT_ORI_WIN + UNIT_ACC_MSS +
+            UNIT_GYR_DPS + UNIT_EULER_DEG + UNIT_TEMP_C;
     _i2c.write(chip_addr, dt, 2, false);
 }
 
 uint8_t BNO055::select_page(uint8_t page)
 {
-    if (page != page_flag){
+    if (page != page_flag) {
         dt[0] = BNO055_PAGE_ID;
         if (page == 1) {
             dt[1] = 1;  // select page 1
@@ -247,11 +244,11 @@
 
 uint8_t BNO055::reset(void)
 {
-     _res = 0;
-     WAIT_MS(1);   // Reset 1mS
-     _res = 1;
-     WAIT_MS(700); // Need to wait at least 650mS
-#if defined(TARGET_STM32L152RE)
+    _res = 0;
+    ThisThread::sleep_for(1ms);   // Reset 1mS
+    _res = 1;
+    ThisThread::sleep_for(700ms); // Need to wait at least 650mS
+#if defined(TARGET_NUCLEO_L152RE)
     _i2c.frequency(400000);
 #else
     _i2c.frequency(400000);
@@ -260,7 +257,7 @@
     page_flag = 0xff;
     select_page(0);
     check_id();
-    if (chip_id != I_AM_BNO055_CHIP){
+    if (chip_id != I_AM_BNO055_CHIP) {
         return 1;
     } else {
         initialize();
@@ -268,14 +265,14 @@
     }
 }
 
-////// Set initialize data to related registers ///////////
+////// Set initialize data to related registers ////////////////////////////////
 void BNO055::set_initial_dt_to_regs(void)
 {
     // select_page(0);
     // current setting is only used default values
 }
 
-/////////////// Check Who am I? ///////////////////////////
+/////////////// Check Who am I? ////////////////////////////////////////////////
 void BNO055::check_id(void)
 {
     select_page(0);
@@ -315,7 +312,7 @@
     id->sw_rev_id = sw_rev_id;
 }
 
-/////////////// Check chip ready or not  //////////////////
+/////////////// Check chip ready or not  ///////////////////////////////////////
 uint8_t BNO055::chip_ready(void)
 {
     if (ready_flag == 0x0f) {
@@ -324,7 +321,7 @@
     return 0;
 }
 
-/////////////// Read Calibration status  //////////////////
+/////////////// Read Calibration status  ///////////////////////////////////////
 uint8_t BNO055::read_calib_status(void)
 {
     select_page(0);
@@ -334,7 +331,7 @@
     return dt[0];
 }
 
-/////////////// Change Fusion mode  ///////////////////////
+/////////////// Change Fusion mode  ////////////////////////////////////////////
 void BNO055::change_fusion_mode(uint8_t mode)
 {
     uint8_t current_mode;
@@ -346,23 +343,24 @@
             dt[0] = BNO055_OPR_MODE;
             dt[1] = mode;
             _i2c.write(chip_addr, dt, 2, false);
-            WAIT_MS(19);    // wait 19mS
+            ThisThread::sleep_for(19ms);    // wait 19mS
             break;
         case MODE_IMU:
         case MODE_COMPASS:
         case MODE_M4G:
         case MODE_NDOF_FMC_OFF:
         case MODE_NDOF:
-            if (current_mode != CONFIGMODE) {   // Can we change the mode directry?
+            // Can we change the mode directry?
+            if (current_mode != CONFIGMODE) {
                 dt[0] = BNO055_OPR_MODE;
                 dt[1] = CONFIGMODE;
                 _i2c.write(chip_addr, dt, 2, false);
-                WAIT_MS(19);    // wait 19mS
+                ThisThread::sleep_for(19ms);    // wait 19mS
             }
             dt[0] = BNO055_OPR_MODE;
             dt[1] = mode;
             _i2c.write(chip_addr, dt, 2, false);
-            WAIT_MS(7);    // wait 7mS
+            ThisThread::sleep_for(7ms);    // wait 7mS
             break;
         default:
             break;
@@ -378,7 +376,7 @@
     return dt[0];
 }
 
-/////////////// Set Mouting position  /////////////////////
+/////////////// Set Mouting position  //////////////////////////////////////////
 void BNO055::set_mounting_position(uint8_t position)
 {
     uint8_t remap_config;
@@ -429,13 +427,13 @@
     change_fusion_mode(current_mode);
 }
 
-/////////////// I2C Freq. /////////////////////////////////
+/////////////// I2C Freq. //////////////////////////////////////////////////////
 void BNO055::frequency(int hz)
 {
     _i2c.frequency(hz);
 }
 
-/////////////// Read/Write specific register //////////////
+/////////////// Read/Write specific register ///////////////////////////////////
 uint8_t BNO055::read_reg0(uint8_t addr)
 {
     select_page(0);
@@ -484,5 +482,3 @@
     change_fusion_mode(current_mode);
     return d;
 }
-
-