diff --git a/code/arduino/Movement-sensor-code/Movement-sensor-code.ino b/code/arduino/Movement-sensor-code/Movement-sensor-code.ino index ac12140..d20f77e 100644 --- a/code/arduino/Movement-sensor-code/Movement-sensor-code.ino +++ b/code/arduino/Movement-sensor-code/Movement-sensor-code.ino @@ -14,11 +14,21 @@ void setup() { void loop() { SensorManager::Rotation rotation = sensorManager.readLoop(); - Serial.print(rotation.x); + // Convert quaternion to Euler angles in radians + 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)); + + // Convert to degrees + float rollDegrees = roll * 180.0f / PI; + float pitchDegrees = pitch * 180.0f / PI; + float yawDegrees = yaw * 180.0f / PI; + + Serial.print(roll); Serial.print(" "); - Serial.print(rotation.y); + Serial.print(pitch); Serial.print(" "); - Serial.print(rotation.z); + Serial.print(yaw); Serial.println(); } \ No newline at end of file diff --git a/code/arduino/Movement-sensor-code/SensorManager.cpp b/code/arduino/Movement-sensor-code/SensorManager.cpp index 260ce1c..a43c923 100644 --- a/code/arduino/Movement-sensor-code/SensorManager.cpp +++ b/code/arduino/Movement-sensor-code/SensorManager.cpp @@ -18,7 +18,7 @@ void SensorManager::sensorSetup() { Wire.setClock(400000); //Increase I2C data rate to 400kHz myIMU.calibrateAll(); //Turn on cal for Accel, Gyro, and Mag - myIMU.enableGameRotationVector(100); + myIMU.enableGyroIntegratedRotationVector(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 @@ -32,19 +32,21 @@ void SensorManager::sensorSetup() { SensorManager::Rotation SensorManager::readLoop() { if (myIMU.dataAvailable() == true) { - float x = myIMU.getMagX(); - float y = myIMU.getMagY(); - float z = myIMU.getMagZ(); + float i = myIMU.getQuatI(); + float j = myIMU.getQuatJ(); + float k = myIMU.getQuatK(); + float w = myIMU.getQuatReal(); - Rotation rotation = { x, y, z }; + Rotation rotation = { i, j, k, w }; return rotation; } else { - float x = myIMU.getMagX(); - float y = myIMU.getMagY(); - float z = myIMU.getMagZ(); + float i = myIMU.getQuatI(); + float j = myIMU.getQuatJ(); + float k = myIMU.getQuatK(); + float w = myIMU.getQuatReal(); - Rotation rotation = { x, y, z }; + Rotation rotation = { i, j, k, w }; return rotation; } } \ No newline at end of file diff --git a/code/arduino/Movement-sensor-code/SensorManager.h b/code/arduino/Movement-sensor-code/SensorManager.h index 3eaf75e..3c4b231 100644 --- a/code/arduino/Movement-sensor-code/SensorManager.h +++ b/code/arduino/Movement-sensor-code/SensorManager.h @@ -9,9 +9,10 @@ class SensorManager { SensorManager(); void sensorSetup(); struct Rotation { - float x; - float y; - float z; + float i; + float j; + float k; + float w; }; Rotation readLoop(); private: