same
Fork of BNO055_fusion by
Diff: BNO055.cpp
- Revision:
- 3:0ad6f85b178f
- Parent:
- 2:0f225b686cd5
- Child:
- 4:9e6fead1e93e
diff -r 0f225b686cd5 -r 0ad6f85b178f BNO055.cpp --- 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;