50 lines
1.3 KiB
C++
50 lines
1.3 KiB
C++
#include "SensorManager.h"
|
|
#include <Wire.h>
|
|
|
|
SensorManager::SensorManager() {}
|
|
|
|
void SensorManager::sensorSetup() {
|
|
Serial.println();
|
|
Serial.println("BNO080 Read Example");
|
|
|
|
delay(1000); // Wait for BNO to boot
|
|
|
|
Wire.begin();
|
|
|
|
if (myIMU.begin() == false) {
|
|
delay(1000);
|
|
Serial.println(".");
|
|
}
|
|
|
|
Wire.setClock(400000); //Increase I2C data rate to 400kHz
|
|
myIMU.calibrateAll(); //Turn on cal for Accel, Gyro, and Mag
|
|
myIMU.enableGameRotationVector(100);
|
|
myIMU.enableMagnetometer(100); //Send data update every 100ms
|
|
myIMU.saveCalibration(); //Saves the current dynamic calibration data (DCD) to memory
|
|
myIMU.requestCalibrationStatus(); //Sends command to get the latest calibration status
|
|
|
|
if (myIMU.calibrationComplete() == true) {
|
|
Serial.println("Calibration data successfully stored");
|
|
}
|
|
|
|
Serial.println(F("magnetometer rotation enabled"));
|
|
}
|
|
|
|
SensorManager::Rotation SensorManager::readLoop() {
|
|
if (myIMU.dataAvailable() == true) {
|
|
float x = myIMU.getMagX();
|
|
float y = myIMU.getMagY();
|
|
float z = myIMU.getMagZ();
|
|
|
|
Rotation rotation = { x, y, z };
|
|
return rotation;
|
|
}
|
|
else {
|
|
float x = myIMU.getMagX();
|
|
float y = myIMU.getMagY();
|
|
float z = myIMU.getMagZ();
|
|
|
|
Rotation rotation = { x, y, z };
|
|
return rotation;
|
|
}
|
|
} |