fsdf
Dependencies: BNO055_fusion MODSERIAL mbed
Fork of Shared-1BNO055 by
Diff: main.cpp
- Revision:
- 13:f5cccb8beac0
- Parent:
- 12:c8019aca78d0
- Child:
- 14:418d1703806c
diff -r c8019aca78d0 -r f5cccb8beac0 main.cpp --- a/main.cpp Wed Feb 17 18:03:04 2016 +0000 +++ b/main.cpp Thu Feb 18 00:41:21 2016 +0000 @@ -32,11 +32,12 @@ volatile bool FUS = false; bool printed = false; int Time; +int TimeStart; int TimeStamp = 0; -int TimeStamp2=0; -int TimeOfStart; +int TimeStamp2 = 0; +int TimeOfStart = 0; int TimeNow; -int sync = 0; +int sync = 1; bool direction = false; //string sdata = ""; @@ -89,6 +90,7 @@ if ( serial->rxGetLastChar() == '*') { TimeOfStart = t.read_ms(); TimeStamp = 0; + pc.printf("RESETTING TIME"); } } @@ -175,38 +177,46 @@ //------------------------------------------------------------------------------------------------- int main() { - pc.baud(115200); + pc.baud(921600); pc.attach(&rxCallback, MODSERIAL::RxIrq); - pc.printf("BNO055 Hello World\r\n\r\n"); + pc.printf("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"); // 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 wait(0.02); } while(imu.reset()); } - pc.printf("Bosch BNO055 is available now!!\r\n"); - pc.printf("AXIS_REMAP_CONFIG:0x%02x, AXIS_REMAP_SIGN:0x%02x\r\n", + pc.printf("Bosch BNO055 is available now!!/\r\n"); + 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 ID:0x%02x, ACC ID:0x%02x, MAG ID:0x%02x, GYR ID:0x%02x, ", + pc.printf("CHIP ID:0x%02x, ACC ID:0x%02x, MAG ID:0x%02x, GYR ID:0x%02x,/", bno055_id_inf.chip_id, bno055_id_inf.acc_id, bno055_id_inf.mag_id, bno055_id_inf.gyr_id); - pc.printf("SW REV:0x%04x, BL REV:0x%02x\r\n", + pc.printf("SW REV:0x%04x, BL REV:0x%02x/\r\n", bno055_id_inf.sw_rev_id, bno055_id_inf.bootldr_rev_id); - pc.printf("If you would like to calibrate the BNO055, please hit 'y' (No: any other key)\r\n"); + pc.printf("If you would like to calibrate the BNO055, please hit 'y' (No: any other key)/\r\n"); c = pc.getc(); if (c == 'y') { bno055_calbration(); } + pc.printf("If you would like to Synchronize the System, please hit 'y' (No: any other key)\r\n/"); + c = pc.getc(); + if (c == 'y') { + sync=0; + pc.printf("Synchronise/"); + wait(0.5); + } + while (sync == 0) { pc.printf("HELLO/"); if (pc.readable()) { @@ -238,36 +248,26 @@ } } while (RAW == false) { - Time = t.read_ms(); - TimeNow = + Time - TimeOfStart; - if (TimeNow > TimeStamp + 5) { - TimeStamp = TimeNow; - //imu.get_linear_accel(&linear_acc); -// pc.printf("FUS,T:,%d,x:,%+6.1f,y:,%+6.1f,z:,%+6.1f, , , , ," -// ,TimeNow,linear_acc.x ,linear_acc.y ,linear_acc.z); -// -// imu.get_Euler_Angles(&euler_angles); -// pc.printf("Y,%+6.1f,R,%+6.1f,P,%+6.1f,", -// euler_angles.h, euler_angles.r, euler_angles.p); -// -// imu.get_quaternion(&quaternion); -// pc.printf("W,%d,X,%d,Y,%d,Z,%d,", -// quaternion.w, quaternion.x, quaternion.y, quaternion.z); -// -// imu.get_gravity(&gravity); -// pc.printf("X,%+6.1f,Y,%+6.1f,Z,%+6.1f,/", -// gravity.x, gravity.y, gravity.z); + //Time = t.read_ms(); + //TimeNow = + t.read_ms() - TimeOfStart; + if (t.read_ms() - TimeOfStart > TimeStamp + 5) { + + //TimeStart = t.read_ms() - TimeOfStart; imu.get_linear_accel(&linear_acc); imu.get_Euler_Angles(&euler_angles); imu.get_quaternion(&quaternion); imu.get_gravity(&gravity); pc.printf("FUS,T:,%d,x:,%+6.1f,y:,%+6.1f,z:,%+6.1f, , ,Y,%+6.1f,R,%+6.1f,P,%+6.1f,W,%d,X,%d,Y,%d,Z,%d,X,%+6.1f,Y,%+6.1f,Z,%+6.1f,/" - ,TimeNow,linear_acc.x ,linear_acc.y ,linear_acc.z, + ,t.read_ms() - TimeOfStart,linear_acc.x ,linear_acc.y ,linear_acc.z, euler_angles.h, euler_angles.r, euler_angles.p, quaternion.w, quaternion.x, quaternion.y, quaternion.z, - gravity.x, gravity.y, gravity.z); + gravity.x, gravity.y, gravity.z); + + //pc.printf("TimeStart:,%d,/",TimeStart); + //pc.printf("Timend:,%d,/",+ t.read_ms() - TimeOfStart); + TimeStamp = t.read_ms()- TimeOfStart; } /*if (Time > TimeStamp2 + 999) {