main.cpp

00001  #include    "mbed.h"
00002  #include    "math.h"
00003  #include    "BNO055.h"
00004  
00005  #define PI 3.141593
00006  
00007  Serial pc(USBTX,USBRX);
00008  BNO055 imu(PTE25, PTE24);         // SDA, SCL 
00009  
00010  double y;
00011  double x;
00012  double z;
00013  double angleToNorth;
00014  double result;
00015 // BNO055_ID_INF_TypeDef bno055_id_inf;
00016 // BNO055_EULER_TypeDef  euler_angles;
00017  
00018  int main() {
00019 //     setup();
00020      imu.reset();
00021      pc.printf("Bosch Sensortec BNO055 test program on \r\n");//" __DATE__ "/" __TIME__ "\r\n");
00022      while (imu.check() == 0){
00023          pc.printf("Bosch BNO055 is NOT available!!\r\n");
00024          wait(.5);
00025      }
00026  //   pc.printf("Bosch Sensortec BNO055 available \r\n");//" __DATE__ "/" __TIME__ "\r\n");
00027     pc.printf("BNO055 found\r\n\r\n");
00028     pc.printf("Chip          ID: %0z\r\n",imu.ID.id);
00029     pc.printf("Accelerometer ID: %0z\r\n",imu.ID.accel);
00030     pc.printf("Gyroscope     ID: %0z\r\n",imu.ID.gyro);
00031     pc.printf("Magnetometer  ID: %0z\r\n\r\n",imu.ID.mag);
00032     pc.printf("Firmware version v%d.%0d\r\n",imu.ID.sw[0],imu.ID.sw[1]);
00033     pc.printf("Bootloader version v%d\r\n\r\n",imu.ID.bootload);
00034     imu.set_accel_units(MPERSPERS);
00035     imu.setmode(OPERATION_MODE_AMG);
00036     imu.read_calibration_data();
00037     imu.write_calibration_data();
00038     imu.set_angle_units(DEGREES);
00039     while(1)
00040     {
00041         imu.setmode(OPERATION_MODE_AMG);
00042         imu.get_accel();
00043         pc.printf("acceleration: x %f\ty %f\tz %f \r\n", imu.accel.x, imu.accel.y+.25, imu.accel.z);
00044         imu.get_gyro();
00045         pc.printf("gyroscope: x %f\ty %f\tz %f \r\n", imu.gyro.x, imu.gyro.y, imu.gyro.z);
00046         imu.get_mag();
00047         pc.printf("magnometer: x %f\ty %f\t %f  \r\n", imu.mag.x, imu.mag.y, imu.mag.z);
00048         x = imu.mag.x;
00049         y = imu.mag.y;
00050         
00051         result = x/y;
00052         
00053         if(imu.mag.y>0)
00054             angleToNorth = 90.0 - atan(result)*180/PI;        
00055         else if(imu.mag.y<0)
00056             angleToNorth = 270.0 - atan(result)*180/PI;
00057         else if(y == 0 && x <= 0)
00058             angleToNorth = 180;
00059         else if(y == 0 && x > 0)
00060             angleToNorth = 0;        
00061         pc.printf("it is pointing %f angle from north \r\n", angleToNorth); 
00062         wait(1);
00063     }
00064 }