fsdf
Dependencies: BNO055_fusion MODSERIAL mbed
Fork of Shared-1BNO055 by
Diff: main.cpp
- Revision:
- 7:d4b5e83c7947
- Parent:
- 6:0590c7ff8c34
- Child:
- 8:41d1f9ab3be0
diff -r 0590c7ff8c34 -r d4b5e83c7947 main.cpp --- a/main.cpp Sun Jan 24 17:04:40 2016 +0000 +++ b/main.cpp Mon Feb 01 00:20:46 2016 +0000 @@ -19,16 +19,27 @@ // Include --------------------------------------------------------------------------------------- #include "mbed.h" #include "BNO055.h" +#include "MODSERIAL.h" // Definition ------------------------------------------------------------------------------------ #define NUM_LOOP 100 // Object ---------------------------------------------------------------------------------------- -Serial pc(USBTX,USBRX); +MODSERIAL pc(USBTX,USBRX); + +volatile bool Capture = true; +volatile bool SaveFile = false; +bool printed = false; +DigitalOut led1(LED1); +DigitalOut led2(LED2); +DigitalOut led3(LED3); +DigitalOut led4(LED4); 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 +LocalFileSystem local("local"); // Create the local filesystem under the name "local" Timer t; @@ -44,24 +55,39 @@ // Function prototypes --------------------------------------------------------------------------- + +//------------------------------------------------------------------------------------------------- +// Serial Interrupt +//------------------------------------------------------------------------------------------------- +void rxCallback(MODSERIAL_IRQ_INFO *q) +{ + MODSERIAL *serial = q->serial; + if ( serial->rxGetLastChar() == 'S') { + Capture = false; + SaveFile = true; + } +} + //------------------------------------------------------------------------------------------------- // Control Program //------------------------------------------------------------------------------------------------- // Calibration // Please refer BNO055 Data sheet 3.10 Calibration & 3.6.4 Sensor calibration data -void bno055_calbration(void){ +void bno055_calbration(void) +{ + uint8_t d; - pc.printf("calibrating:, ------ Enter BNO055 Manual Calibration Mode ------\r\n"); + pc.printf("------ 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 pc.printf("Step1) Please wait few seconds\r\n"); t.start(); - while (t.read() < 10){ + while (t.read() < 10) { d = imu.read_calib_status(); pc.printf("calibrating = 0x%x target = 0x30(at least)\r\n", d); - if ((d & 0x30) == 0x30){ + if ((d & 0x30) == 0x30) { break; } wait(1.0); @@ -74,13 +100,13 @@ // NDOF mode. pc.printf("Step2) random moving (try to change the BNO055 axis)\r\n"); t.start(); - while (t.read() < 30){ + while (t.read() < 30) { d = imu.read_calib_status(); pc.printf("calibrating, = 0x%x target = 0x33(at least)\r\n", d); - if ((d & 0x03) == 0x03){ + if ((d & 0x03) == 0x03) { break; } - wait(1.0); + wait(1.0); } pc.printf("-> Step2) is done\r\n\r\n"); //---------- Magnetometer Caliblation --------------------------------------------------------- @@ -95,35 +121,42 @@ pc.printf("(4)ACC:X-9,Y0,Z0,(5)ACC:X0,Y-9,Z0,(6)ACC:X0,Y9,Z0,\r\n"); pc.printf(" If you will give up, hit any key.\r\n", d); t.stop(); - while (true){ + while (true) { d = imu.read_calib_status(); imu.get_gravity(&gravity); 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;} + d, gravity.x, gravity.y, gravity.z); + if (d == 0xff) { + break; + } + if (pc.readable()) { + break; + } wait(1.0); } - if (imu.read_calib_status() == 0xff){ + if (imu.read_calib_status() == 0xff) { pc.printf("-> All of Calibration steps are done successfully!\r\n\r\n"); } else { pc.printf("-> Calibration steps are suspended!\r\n\r\n"); } t.stop(); } + //-------------------------------------------------------------------------------- -int main(){ +int main() +{ pc.baud(9600); - pc.printf("notstreaming: BNO055 Hello World\r\n\r\n"); + pc.attach(&rxCallback, MODSERIAL::RxIrq); + 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"); + pc.printf("Bosch Sensortec BNO055 test program on " __DATE__ "/" __TIME__ "\r\n"); // Is BNO055 avairable? - if (imu.chip_ready() == 0){ + 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 @@ -132,54 +165,52 @@ } 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_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, ", - bno055_id_inf.chip_id, bno055_id_inf.acc_id, bno055_id_inf.mag_id, bno055_id_inf.gyr_id); + 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", - bno055_id_inf.sw_rev_id, bno055_id_inf.bootldr_rev_id); + 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"); c = pc.getc(); - if (c == 'y'){ + if (c == 'y') { bno055_calbration(); } 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(); - - //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) { + + while(Capture == true) { imu.get_Euler_Angles(&euler_angles); - pc.printf("Streaming:,Orientation:,X,%+6.1f,Y,%+6.1f,Z,%+6.1f,", - euler_angles.h, euler_angles.r, euler_angles.p); + pc.printf("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("Acc:,X,%+6.1f,Y,%+6.1f,Z,%+6.1f,", - linear_acc.x, linear_acc.y, linear_acc.z); - + linear_acc.x, linear_acc.y, linear_acc.z); + imu.get_gravity(&gravity); pc.printf("Gravity,X,%+6.1f,Y,%+6.1f,Z,%+6.1f,", - gravity.x, gravity.y, gravity.z); - - pc.printf("Time:,%d,\n",t.read_ms()); - - - -// c = pc.getc(); -// if (c == 'c'){ -// bno055_calbration(); -// } -// if (c == 'r'){ -// imu.reset(); -// } - } + gravity.x, gravity.y, gravity.z); + + pc.printf("Time:,%d,\r\n",t.read_ms()); + }; + while(SaveFile == true) { + if (printed == false) { + pc.printf("Save File Location\r\n"); + //pc.printf("Enter Filename: \r\n"); + printed = true; + }; + FILE *fp = fopen("/local/out.txt", "w"); // Open "out.txt" on the local file system for writing + fprintf(fp, "MBED SAVE FILESZZZ"); + fclose(fp); + pc.printf("File closed\r\n"); + }; + + } + + // 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); @@ -197,5 +228,5 @@ // 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("Words\r\n"); -// pc.printf("Words\r\n"); \ No newline at end of file