68 lines
2.1 KiB
C++
68 lines
2.1 KiB
C++
#include "SensorManager.h"
|
|
#include <Wire.h>
|
|
|
|
|
|
SensorManager::SensorManager() {}
|
|
|
|
void SensorManager::sensorSetup() {
|
|
Wire.begin();
|
|
//wait for the sensor to start before continue
|
|
if (myIMU.begin() == false) {
|
|
delay(1000);
|
|
}
|
|
//start sensorfunction and start autocalibration
|
|
//once calibration is enabled it attempts to every 5 min
|
|
myIMU.enableGyroIntegratedRotationVector(100); //send data every 100ms
|
|
// myIMU.enableAccelerometer(100); //Send data update every 100ms
|
|
// myIMU.enableStepCounter(500); //Send data update every 500ms
|
|
}
|
|
//get sensordata
|
|
SensorManager::RotationQuintillions SensorManager::getQuintillions() {
|
|
if (myIMU.dataAvailable() == true) {
|
|
float i = myIMU.getQuatI();
|
|
float j = myIMU.getQuatJ();
|
|
float k = myIMU.getQuatK();
|
|
float w = myIMU.getQuatReal();
|
|
|
|
RotationQuintillions rotation = { i, j, k, w };
|
|
return rotation;
|
|
} else {
|
|
float i = myIMU.getQuatI();
|
|
float j = myIMU.getQuatJ();
|
|
float k = myIMU.getQuatK();
|
|
float w = myIMU.getQuatReal();
|
|
|
|
RotationQuintillions rotation = { i, j, k, w };
|
|
return rotation;
|
|
}
|
|
}
|
|
//calculate Quintillions to Euler angles from -1π to +1π
|
|
SensorManager::eulerAngles SensorManager::getEulerAngles() {
|
|
SensorManager::RotationQuintillions rotation = getQuintillions();
|
|
float roll = atan2(2.0f * (rotation.w * rotation.i + rotation.j * rotation.k), 1.0f - 2.0f * (rotation.i * rotation.i + rotation.j * rotation.j));
|
|
float pitch = asin(2.0f * (rotation.w * rotation.j - rotation.k * rotation.i));
|
|
float yaw = atan2(2.0f * (rotation.w * rotation.k + rotation.i * rotation.j), 1.0f - 2.0f * (rotation.j * rotation.j + rotation.k * rotation.k));
|
|
eulerAngles EulerAngles = { roll, pitch, yaw };
|
|
return EulerAngles;
|
|
}
|
|
SensorManager::acceleration SensorManager::getAcelleration() {
|
|
float x = myIMU.getAccelX();
|
|
float y = myIMU.getAccelY();
|
|
float z = myIMU.getAccelZ();
|
|
acceleration Acceleration = { x, y, z };
|
|
|
|
return Acceleration;
|
|
}
|
|
|
|
bool SensorManager::sensorTap() {
|
|
int taps = 0;
|
|
if (myIMU.dataAvailable() == true) {
|
|
int taps = myIMU.getStepCount();
|
|
}
|
|
if (taps) {
|
|
return true;
|
|
}
|
|
else {
|
|
return false;
|
|
}
|
|
} |