Get all but euler. Absolute North to

Dependencies:   BNO055 mbed

main.cpp

Committer:
jvfausto
Date:
2018-07-16
Revision:
0:c9ec02922858

File content as of revision 0:c9ec02922858:

 #include    "mbed.h"
 #include    "math.h"
 #include    "BNO055.h"
 
 #define PI 3.141593
 
 Serial pc(USBTX,USBRX);
 BNO055 imu(PTE25, PTE24);         // SDA, SCL 
 
 double y;
 double x;
 double z;
 double angleToNorth;
 double result;
// BNO055_ID_INF_TypeDef bno055_id_inf;
// BNO055_EULER_TypeDef  euler_angles;
 
 int main() {
//     setup();
     imu.reset();
     pc.printf("Bosch Sensortec BNO055 test program on \r\n");//" __DATE__ "/" __TIME__ "\r\n");
     while (imu.check() == 0){
         pc.printf("Bosch BNO055 is NOT available!!\r\n");
         wait(.5);
     }
 //   pc.printf("Bosch Sensortec BNO055 available \r\n");//" __DATE__ "/" __TIME__ "\r\n");
    pc.printf("BNO055 found\r\n\r\n");
    pc.printf("Chip          ID: %0z\r\n",imu.ID.id);
    pc.printf("Accelerometer ID: %0z\r\n",imu.ID.accel);
    pc.printf("Gyroscope     ID: %0z\r\n",imu.ID.gyro);
    pc.printf("Magnetometer  ID: %0z\r\n\r\n",imu.ID.mag);
    pc.printf("Firmware version v%d.%0d\r\n",imu.ID.sw[0],imu.ID.sw[1]);
    pc.printf("Bootloader version v%d\r\n\r\n",imu.ID.bootload);
    imu.set_accel_units(MPERSPERS);
    imu.setmode(OPERATION_MODE_AMG);
    imu.read_calibration_data();
    imu.write_calibration_data();
    imu.set_angle_units(DEGREES);
    while(1)
    {
        imu.setmode(OPERATION_MODE_AMG);
        imu.get_accel();
        pc.printf("acceleration: x %f\ty %f\tz %f \r\n", imu.accel.x, imu.accel.y+.25, imu.accel.z);
        imu.get_gyro();
        pc.printf("gyroscope: x %f\ty %f\tz %f \r\n", imu.gyro.x, imu.gyro.y, imu.gyro.z);
        imu.get_mag();
        pc.printf("magnometer: x %f\ty %f\t %f  \r\n", imu.mag.x, imu.mag.y, imu.mag.z);
        x = imu.mag.x;
        y = imu.mag.y;
        
        result = x/y;
        
        if(imu.mag.y>0)
            angleToNorth = 90.0 - atan(result)*180/PI;        
        else if(imu.mag.y<0)
            angleToNorth = 270.0 - atan(result)*180/PI;
        else if(y == 0 && x <= 0)
            angleToNorth = 180;
        else if(y == 0 && x > 0)
            angleToNorth = 0;        
        pc.printf("it is pointing %f angle from north \r\n", angleToNorth); 
        wait(1);
    }
}