fsdf
Dependencies: BNO055_fusion MODSERIAL mbed
Fork of Shared-1BNO055 by
Diff: main.cpp
- Revision:
- 6:0590c7ff8c34
- Parent:
- 5:9594519c9462
- Child:
- 7:d4b5e83c7947
diff -r 9594519c9462 -r 0590c7ff8c34 main.cpp --- a/main.cpp Thu Apr 16 11:25:47 2015 +0000 +++ b/main.cpp Sun Jan 24 17:04:40 2016 +0000 @@ -25,25 +25,11 @@ // 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 ------------------------------------------------------------------------------------------- @@ -66,7 +52,7 @@ void bno055_calbration(void){ uint8_t d; - pc.printf("------ Enter BNO055 Manual Calibration Mode ------\r\n"); + pc.printf("calibrating:, ------ Enter BNO055 Manual Calibration Mode ------\r\n"); //---------- Gyroscope Caliblation ------------------------------------------------------------ // (a) Place the device in a single stable position for a period of few seconds to allow the // gyroscope to calibrate @@ -74,7 +60,7 @@ t.start(); while (t.read() < 10){ d = imu.read_calib_status(); - pc.printf("Calb dat = 0x%x target = 0x30(at least)\r\n", d); + pc.printf("calibrating = 0x%x target = 0x30(at least)\r\n", d); if ((d & 0x30) == 0x30){ break; } @@ -90,7 +76,7 @@ t.start(); while (t.read() < 30){ d = imu.read_calib_status(); - pc.printf("Calb dat = 0x%x target = 0x33(at least)\r\n", d); + pc.printf("calibrating, = 0x%x target = 0x33(at least)\r\n", d); if ((d & 0x03) == 0x03){ break; } @@ -112,7 +98,7 @@ while (true){ d = imu.read_calib_status(); imu.get_gravity(&gravity); - pc.printf("Calb dat = 0x%x target = 0xff ACC:X %4.1f, Y %4.1f, Z %4.1f\r\n", + pc.printf("calibrating = 0x%x target = 0xff ACC:X %4.1f, Y %4.1f, Z %4.1f\r\n", d, gravity.x, gravity.y, gravity.z); if (d == 0xff){ break;} if (pc.readable()){ break;} @@ -125,17 +111,19 @@ } t.stop(); } - +//-------------------------------------------------------------------------------- int main(){ + pc.baud(9600); + pc.printf("notstreaming: BNO055 Hello World\r\n\r\n"); imu.set_mounting_position(MT_P6); pwr_onoff = 0; - pc.printf("\r\n\r\nIf pc terminal soft is ready, please hit any key!\r\n"); + 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"); + 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 Reset\r\n"); + pc.printf(",Bosch BNO055 is NOT avirable!!\r\n Reset\r\n"); pwr_onoff = 1; // Power off wait(0.1); pwr_onoff = 0; // Power on @@ -158,88 +146,56 @@ 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) { + + //Set Register into Config Mode + //imu.write_reg0(0x3D, 0x00); + //Set register unit to Radians + imu.write_reg0(0x3B, 0x02); + //Change back into normal operation mode + //imu.write_reg0(0x3D, 0x0C); + + 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); + pc.printf("Streaming:,Orientation:,X,%+6.1f,Y,%+6.1f,Z,%+6.1f,", + euler_angles.h, euler_angles.r, euler_angles.p); 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); + pc.printf("Acc:,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,", + pc.printf("Gravity,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()); + + pc.printf("Time:,%d,\n",t.read_ms()); + + + +// c = pc.getc(); +// if (c == 'c'){ +// bno055_calbration(); +// } +// if (c == 'r'){ +// imu.reset(); +// } } } - -// Diffrent output format as for your reference -#if 0 -int main() { - uint8_t i; +// 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()); - 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); - } - } -} -#endif +// pc.printf("Words\r\n"); \ No newline at end of file