Smikkelbakje/J1B4-Fitbot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Refactoring

Browse Source
This commit is contained in:
Sam Hos
2024-06-03 16:17:27 +02:00
parent 7264811444
commit ffb90c001d
4 changed files with 10 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
code/arduino/Movement-sensor-code/SensorManager.cpp
View File

@@ -17,14 +17,14 @@ void SensorManager::sensorSetup() {
// myIMU.enableStepCounter(500); //Send data update every 500ms
}
//get sensordata
SensorManager::RotationQuintillions SensorManager::getQuintillions() {
SensorManager::RotationQuaternions SensorManager::getQuaternions() {
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 };
RotationQuaternions rotation = { i, j, k, w };
return rotation;
} else {
float i = myIMU.getQuatI();
@@ -32,13 +32,13 @@ SensorManager::RotationQuintillions SensorManager::getQuintillions() {
float k = myIMU.getQuatK();
float w = myIMU.getQuatReal();
RotationQuintillions rotation = { i, j, k, w };
RotationQuaternions rotation = { i, j, k, w };
return rotation;
}
}
//calculate Quintillions to Euler angles from -1π to +1π
//calculate Quaternions to Euler angles from -1π to +1π
SensorManager::eulerAngles SensorManager::getEulerAngles() {
SensorManager::RotationQuintillions rotation = getQuintillions();
SensorManager::RotationQuaternions rotation = getQuaternions();
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));