07/02/15
Dependencies: BNO055_fusion MODSERIAL dsp mbed
Fork of BNO055-ELEC3810 by
main.cpp
- Committer:
- tommyallen
- Date:
- 2016-02-04
- Revision:
- 9:7be40e42e578
- Parent:
- 8:41d1f9ab3be0
- Child:
- 10:acbb851070c2
File content as of revision 9:7be40e42e578:
/* * 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 16th, 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" #include "MODSERIAL.h" #include "arm_math.h" // Definition ------------------------------------------------------------------------------------ #define NUM_LOOP 100 // Object ---------------------------------------------------------------------------------------- MODSERIAL pc(USBTX,USBRX); volatile bool RAW = true; volatile bool FUS = false; bool printed = false; int Time; int TimeStamp; //string sdata = ""; 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; // 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 --------------------------------------------------------------------------- //------------------------------------------------------------------------------------------------- // Serial Interrupt //------------------------------------------------------------------------------------------------- void rxCallback(MODSERIAL_IRQ_INFO *q) { MODSERIAL *serial = q->serial; if ( serial->rxGetLastChar() == 'f') { imu.write_reg0(0x3d, 0x0c); RAW = false; FUS = true; pc.printf("\r\nSWITCHING TO FUSION DATA!\r\n"); } if ( serial->rxGetLastChar() == 'r') { imu.write_reg0(0x3d, 0x07); RAW = true; FUS = false; pc.printf("\r\nSWITCHING TO RAW DATA!\r\n"); } } //------------------------------------------------------------------------------------------------- // Control Program //------------------------------------------------------------------------------------------------- // Calibration // Please refer BNO055 Data sheet 3.10 Calibration & 3.6.4 Sensor calibration data void bno055_calbration(void) { uint8_t d; 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) { d = imu.read_calib_status(); pc.printf("calibrating = 0x%x target = 0x30(at least)\r\n", d); if ((d & 0x30) == 0x30) { break; } wait(1.0); } pc.printf("-> Step1) is done\r\n\r\n"); //---------- Magnetometer Caliblation --------------------------------------------------------- // (a) Make some random movements (for example: writing the number ‘8’ on air) until the // CALIB_STAT register indicates fully calibrated. // (b) It takes more calibration movements to get the magnetometer calibrated than in the // NDOF mode. pc.printf("Step2) random moving (try to change the BNO055 axis)\r\n"); t.start(); 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) { break; } wait(1.0); } pc.printf("-> Step2) is done\r\n\r\n"); //---------- Magnetometer Caliblation --------------------------------------------------------- // a) Place the device in 6 different stable positions for a period of few seconds // to allow the accelerometer to calibrate. // b) Make sure that there is slow movement between 2 stable positions // The 6 stable positions could be in any direction, but make sure that the device is // lying at least once perpendicular to the x, y and z axis. pc.printf("Step3) Change rotation each X,Y,Z axis KEEP SLOWLY!!"); pc.printf(" Each 90deg stay a 5 sec and set at least 6 position.\r\n"); pc.printf(" e.g. (1)ACC:X0,Y0,Z-9,(2)ACC:X9,Y0,Z0,(3)ACC:X0,Y0,Z9,"); 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) { 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; } wait(1.0); } 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(); } //------------------------------------------------------------------------------------------------- //------------------------------------------------------------------------------------------------- //------------------------------------------------------------------------------------------------- //------------------------------------------------------------------------------------------------- // Main Program //------------------------------------------------------------------------------------------------- //------------------------------------------------------------------------------------------------- //------------------------------------------------------------------------------------------------- //------------------------------------------------------------------------------------------------- int main() { pc.baud(115200); 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"); 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"); 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", 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); 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"); c = pc.getc(); if (c == 'y') { bno055_calbration(); } t.start(); imu.write_reg0(0x3d, 0x07); // Default FUS while (1) { while (RAW == true) { Time = t.read_ms(); if (Time > TimeStamp + 9) { pc.printf("RAW,xLSB:,%d,xMSB:,%d,yLSB:%d,yMSB:,%d,zLSB:,%d,zMSB:,%d," ,imu.read_reg0(0x08),imu.read_reg0(0x09) ,imu.read_reg0(0x0A),imu.read_reg0(0x0B) ,imu.read_reg0(0x0C),imu.read_reg0(0x0D)); pc.printf("Time:,%d,/",t.read_ms()); TimeStamp = Time; } } while (RAW == false) { Time = t.read_ms(); if (Time > TimeStamp + 9) { imu.get_linear_accel(&linear_acc); pc.printf("FUS,x:,%+6.1f,y:,%+6.1f,z:,%+6.1f,Total:,%+6.1f," ,linear_acc.x ,linear_acc.y ,linear_acc.z ,t.read_ms()); pc.printf("Time:,%d,/",t.read_ms()); TimeStamp = Time; } } } } // imu.get_Euler_Angles(&euler_angles); // 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); // // 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,\r\n",t.read_ms()); // 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()); // pc.printf("Words\r\n");