main.cpp

00001 #include <Adafruit_Sensor.h>
00002 #include <Adafruit_BNO055.h>
00003 #include <imumaths.h>
00004 #include "mbed.h"
00005 
00006 /* Set the delay between fresh samples */
00007 #define BNO055_SAMPLERATE_DELAY_MS (100)
00008 
00009 Serial pc(USBTX, USBRX);
00010 I2C i2c(D7, D6);
00011 Adafruit_BNO055 bno = Adafruit_BNO055(-1, BNO055_ADDRESS_A, &i2c);
00012 
00013 void loop();
00014 
00015 /**************************************************************************/
00016 /*
00017     Arduino setup function (automatically called at startup)
00018 */
00019 /**************************************************************************/
00020 int main()
00021 {
00022   pc.baud(9600);
00023   pc.printf("Orientation Sensor Raw Data Test\r\n");
00024 
00025   /* Initialise the sensor */
00026   if(!bno.begin())
00027   {
00028     /* There was a problem detecting the BNO055 ... check your connections */
00029     pc.printf("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!\r\n");
00030     while(1);
00031   }
00032   else
00033     pc.printf("BNO055 was detected!\r\n");
00034 
00035   wait(1);
00036 
00037   /* Display the current temperature */
00038   int8_t temp = bno.getTemp();
00039   pc.printf("Current Temperature: %d C\r\n", temp);
00040   bno.setExtCrystalUse(true);
00041 
00042   pc.printf("Calibration status values: 0=uncalibrated, 3=fully calibrated\r\n");
00043   
00044   while(true)
00045       loop();
00046 }
00047 
00048 /**************************************************************************/
00049 /*
00050     Arduino loop function, called once 'setup' is complete (your own code
00051     should go here)
00052 */
00053 /**************************************************************************/
00054 void loop(void)
00055 {
00056   // Possible vector values can be:
00057   // - VECTOR_ACCELEROMETER - m/s^2
00058   // - VECTOR_MAGNETOMETER  - uT
00059   // - VECTOR_GYROSCOPE     - rad/s
00060   // - VECTOR_EULER         - degrees
00061   // - VECTOR_LINEARACCEL   - m/s^2
00062   // - VECTOR_GRAVITY       - m/s^2
00063   imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER);
00064 
00065   /* Display the floating point data */
00066   pc.printf("X: %f Y: %f Z: %f\t\t", euler.x(), euler.y(), euler.z());
00067 
00068   /*
00069   // Quaternion data
00070   imu::Quaternion quat = bno.getQuat();
00071   Serial.print("qW: ");
00072   Serial.print(quat.w(), 4);
00073   Serial.print(" qX: ");
00074   Serial.print(quat.y(), 4);
00075   Serial.print(" qY: ");
00076   Serial.print(quat.x(), 4);
00077   Serial.print(" qZ: ");
00078   Serial.print(quat.z(), 4);
00079   Serial.print("\t\t");
00080   */
00081 
00082   /* Display calibration status for each sensor. */
00083   uint8_t system, gyro, accel, mag = 0;
00084   bno.getCalibration(&system, &gyro, &accel, &mag);
00085   pc.printf("CALIBRATION: Sys=%d, Gyro=%d, Accel=%d, Mag=%d\r\n", (int)(system), (int)(gyro), (int)(accel), (int)(mag));
00086   wait_ms(BNO055_SAMPLERATE_DELAY_MS);
00087 }