07/02/15
Dependencies: BNO055_fusion MODSERIAL dsp mbed
Fork of BNO055-ELEC3810 by
main.cpp
- Committer:
- kenjiArai
- Date:
- 2015-04-08
- Revision:
- 3:f5b5c4d795ce
- Parent:
- 2:cf77282aea7b
- Child:
- 4:6d1118089a36
File content as of revision 3:f5b5c4d795ce:
/* * mbed Application program for the mbed Nucleo F401 * BNO055 Intelligent 9-axis absolute orientation sensor * by Bosch Sensortec * * Copyright (c) 2015 Kenji Arai / JH1PJL * http://www.page.sannet.ne.jp/kenjia/index.html * http://mbed.org/users/kenjiArai/ * Created: March 30th, 2015 * Revised: April 8th, 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 * AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ // Include --------------------------------------------------------------------------------------- #include "mbed.h" #include "BNO055.h" // Definition ------------------------------------------------------------------------------------ #define NUM_LOOP 100 #define FORMAT_0 0 #define FORMAT_1 1 // Object ---------------------------------------------------------------------------------------- Serial pc(USBTX,USBRX); #if defined(TARGET_LPC1114) DigitalOut pwr_onoff(dp17); I2C i2c(dp5, dp27); // SDA, SCL BNO055 imu(i2c, dp18); // Reset =D7, addr = BNO055_G_CHIP_ADDR, mode = MODE_NDOF <- as default #elif defined(TARGET_LPC1768) DigitalOut pwr_onoff(p30); I2C i2c(p28, p27); // SDA, SCL BNO055 imu(i2c, p29); // Reset =D7, addr = BNO055_G_CHIP_ADDR, mode = MODE_NDOF <- as default #elif defined(TARGET_STM32L152RE) || defined(TARGET_STM32F401RE) || defined(TARGET_STM32F411RE) DigitalOut pwr_onoff(PB_10); I2C i2c(PB_9, PB_8); // SDA, SCL BNO055 imu(i2c, PA_8); // Reset =D7, addr = BNO055_G_CHIP_ADDR, mode = MODE_NDOF <- as default #elif defined(TARGET_RZ_A1H) DigitalOut pwr_onoff(P8_11); I2C i2c(P1_3, P1_2); // SDA, SCL BNO055 imu(i2c, P8_13); // Reset =D7, addr = BNO055_G_CHIP_ADDR, mode = MODE_NDOF <- as default #else #error "Not cheched yet" #endif Timer t; // RAM ------------------------------------------------------------------------------------------- BNO055_ID_INF_TypeDef bno055_id_inf; BNO055_EULER_TypeDef euler_angles; BNO055_QUATERNION_TypeDef quaternion; BNO055_LIN_ACC_TypeDef linear_acc; BNO055_GRAVITY_TypeDef gravity; BNO055_TEMPERATURE_TypeDef chip_temp; // ROM / Constant data --------------------------------------------------------------------------- // Function prototypes --------------------------------------------------------------------------- //------------------------------------------------------------------------------------------------- // Control Program //------------------------------------------------------------------------------------------------- #if FORMAT_0 int main() { uint8_t i; pwr_onoff = 0; pc.printf("Bosch Sensortec BNO055 test program on " __DATE__ "/" __TIME__ "\r\n"); // Is BNO055 avairable? if (imu.chip_ready() == 0){ do { pc.printf("Bosch BNO055 is NOT avirable!!\r\n"); pwr_onoff = 1; // Power off wait(0.1); pwr_onoff = 0; // Power on wait(0.02); } while(imu.reset()); } imu.set_mounting_position(MT_P6); pc.printf("AXIS_REMAP_CONFIG:0x%02x, AXIS_REMAP_SIGN:0x%02x\r\n", imu.read_reg0(BNO055_AXIS_MAP_CONFIG), imu.read_reg0(BNO055_AXIS_MAP_SIGN)); imu.read_id_inf(&bno055_id_inf); pc.printf("CHIP:0x%02x, ACC:0x%02x, MAG:0x%02x, GYR:0x%02x, , SW:0x%04x, , BL:0x%02x\r\n", bno055_id_inf.chip_id, bno055_id_inf.acc_id, bno055_id_inf.mag_id, bno055_id_inf.gyr_id, bno055_id_inf.sw_rev_id, bno055_id_inf.bootldr_rev_id); while(1) { pc.printf("Euler Angles data\r\n"); for (i = 0; i < NUM_LOOP; i++){ imu.get_Euler_Angles(&euler_angles); pc.printf("Heading:%+6.1f [deg], Roll:%+6.1f [deg], Pich:%+6.1f [deg], #%02d\r\n", euler_angles.h, euler_angles.r, euler_angles.p, i); wait(0.5); } pc.printf("Quaternion data\r\n"); for (i = 0; i < NUM_LOOP; i++){ imu.get_quaternion(&quaternion); pc.printf("W:%d, X:%d, Y:%d, Z:%d, #%02d\r\n", quaternion.w, quaternion.x, quaternion.y, quaternion.z, i); wait(0.5); } pc.printf("Linear accel data\r\n"); for (i = 0; i < NUM_LOOP; i++){ imu.get_linear_accel(&linear_acc); pc.printf("X:%+6.1f [m/s*s], Y:%+6.1f [m/s*s], Z:%+6.1f [m/s*s], #%02d\r\n", linear_acc.x, linear_acc.y, linear_acc.z, i); wait(0.5); } pc.printf("Gravity vector data\r\n"); for (i = 0; i < NUM_LOOP; i++){ imu.get_gravity(&gravity); pc.printf("X:%+6.1f [m/s*s], Y:%+6.1f [m/s*s], Z:%+6.1f [m/s*s], #%02d\r\n", gravity.x, gravity.y, gravity.z, i); wait(0.5); } pc.printf("Chip temperature data\r\n"); for (i = 0; i < (NUM_LOOP / 4); i++){ imu.get_chip_temperature(&chip_temp); pc.printf("Acc chip:%+d [degC], Gyr chip:%+d [degC], #%02d\r\n", chip_temp.acc_chip, chip_temp.gyr_chip, i); wait(0.5); } } } #elif FORMAT_1 int main() { pwr_onoff = 0; pc.printf("\r\n\r\nIf pc terminal soft is ready, please hit any key!\r\n"); char c = pc.getc(); pc.printf("Bosch Sensortec BNO055 test program on " __DATE__ "/" __TIME__ "\r\n"); // Is BNO055 avairable? if (imu.chip_ready() == 0){ do { pc.printf("Bosch BNO055 is NOT avirable!!\r\n"); pwr_onoff = 1; // Power off wait(0.1); pwr_onoff = 0; // Power on wait(0.02); } while(imu.reset()); } imu.set_mounting_position(MT_P6); pc.printf("AXIS_REMAP_CONFIG:0x%02x, AXIS_REMAP_SIGN:0x%02x\r\n", imu.read_reg0(BNO055_AXIS_MAP_CONFIG), imu.read_reg0(BNO055_AXIS_MAP_SIGN)); imu.read_id_inf(&bno055_id_inf); pc.printf("CHIP:0x%02x, ACC:0x%02x, MAG:0x%02x, GYR:0x%02x, , SW:0x%04x, , BL:0x%02x\r\n", bno055_id_inf.chip_id, bno055_id_inf.acc_id, bno055_id_inf.mag_id, bno055_id_inf.gyr_id, bno055_id_inf.sw_rev_id, bno055_id_inf.bootldr_rev_id); pc.printf("[E]:Euler Angles[deg],[Q]:Quaternion[],[L]:Linear accel[m/s*s],"); pc.printf("[G]:Gravity vector[m/s*s],[T]:Chip temperature,Acc,Gyr[degC],[S]:Status,[M]:time[mS]\r\n"); t.start(); while(1) { imu.get_Euler_Angles(&euler_angles); pc.printf("[E],Y,%+6.1f,R,%+6.1f,P,%+6.1f,", euler_angles.h, euler_angles.r, euler_angles.p); imu.get_quaternion(&quaternion); pc.printf("[Q],W:%d,X:%d,Y:%d,Z:%d,", quaternion.w, quaternion.x, quaternion.y, quaternion.z); imu.get_linear_accel(&linear_acc); pc.printf("[L],X,%+6.1f,Y,%+6.1f,Z,%+6.1f,", linear_acc.x, linear_acc.y, linear_acc.z); imu.get_gravity(&gravity); pc.printf("[G],X,%+6.1f,Y,%+6.1f,Z,%+6.1f,", gravity.x, gravity.y, gravity.z); imu.get_chip_temperature(&chip_temp); pc.printf("[T],%+d,%+d,", chip_temp.acc_chip, chip_temp.gyr_chip); pc.printf("[S],0x%x,[M],%d\r\n", imu.read_calib_status(), t.read_ms()); } } #endif