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

encapsulation and optimization

Browse Source
This commit is contained in:
Sam Hos
2024-05-17 15:53:30 +02:00
parent df9b045936
commit f3307eb82b
3 changed files with 65 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -15,12 +15,12 @@ void SensorManager::sensorSetup() {
//start sensorfunction and start autocalibration
//once calibration is enabled it attempts to every 5 min
Wire.setClock(400000); //Increase I2C data rate to 400kHz
myIMU.calibrateAll(); //Turn on cal for Accel, Gyro, and Mag
myIMU.enableGyroIntegratedRotationVector(100); //send data every 100ms
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
Wire.setClock(400000); //Increase I2C data rate to 400kHz
myIMU.calibrateAll(); //Turn on cal for Accel, Gyro, and Mag
myIMU.enableGyroIntegratedRotationVector(100); //send data every 100ms
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");
@@ -29,23 +29,31 @@ void SensorManager::sensorSetup() {
Serial.println(F("magnetometer rotation enabled"));
}
SensorManager::Rotation SensorManager::readLoop() {
SensorManager::RotationQuintillions SensorManager::getQuintillions() {
if (myIMU.dataAvailable() == true) {
float i = myIMU.getQuatI();
float j = myIMU.getQuatJ();
float k = myIMU.getQuatK();
float w = myIMU.getQuatReal();
Rotation rotation = { i, j, k, w };
RotationQuintillions rotation = { i, j, k, w };
return rotation;
}
else {
} else {
float i = myIMU.getQuatI();
float j = myIMU.getQuatJ();
float k = myIMU.getQuatK();
float w = myIMU.getQuatReal();
Rotation rotation = { i, j, k, w };
RotationQuintillions rotation = { i, j, k, w };
return rotation;
}
}
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;
}