54 lines
1.2 KiB
C++
54 lines
1.2 KiB
C++
#include <Wire.h>
|
|
#include <Adafruit_Sensor.h>
|
|
#include <Adafruit_BNO055.h>
|
|
#include <utility/imumaths.h>
|
|
|
|
Adafruit_BNO055 bno = Adafruit_BNO055(55);
|
|
|
|
void setup(void)
|
|
{
|
|
Serial.begin(115200);
|
|
Serial.println("Orientation Sensor Test"); Serial.println("");
|
|
|
|
/* Initialise the sensor */
|
|
if(!bno.begin())
|
|
{
|
|
/* There was a problem detecting the BNO055 ... check your connections */
|
|
Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
|
|
while(1);
|
|
}
|
|
|
|
delay(1000);
|
|
|
|
bno.setExtCrystalUse(true);
|
|
}
|
|
|
|
void loop(void)
|
|
{
|
|
/* Get a new sensor event */
|
|
imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER);
|
|
imu::Vector<3> mag = bno.getVector(Adafruit_BNO055::VECTOR_MAGNETOMETER);
|
|
|
|
// float heading = 0.f;
|
|
float mag_x = mag.x();
|
|
float mag_y = mag.y();
|
|
float mag_z = mag.z();
|
|
// heading = -atan2(mag_x, mag_y) * 180 / M_PI;
|
|
// if (heading < 0){
|
|
// heading += 360;
|
|
// }
|
|
|
|
/* Display the floating point data */
|
|
Serial.print("heading|");
|
|
Serial.print(euler.x());
|
|
Serial.print("|MAG|");
|
|
Serial.print(mag_x);
|
|
Serial.print(";");
|
|
Serial.print(mag_y);
|
|
Serial.print(";");
|
|
Serial.print(mag_z);
|
|
Serial.println();
|
|
delay(100);
|
|
}
|
|
|