Jesus Fausto
/
LMU
Get all but euler. Absolute North to
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); } }