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