WIP calibration, euler angles

master
Jurij Podgoršek 2022-03-05 19:07:01 +01:00
parent 0b5016a278
commit c7c5e4fa96
1 changed files with 16 additions and 6 deletions

View File

@ -120,6 +120,7 @@ Matrix<3> position; // [x,y,z] tracks position of device
Matrix<3> speed; // [x,y,z] tracks speed of device Matrix<3> speed; // [x,y,z] tracks speed of device
Matrix<3> eulerVector; Matrix<3> eulerVector;
Matrix<3> eulerDiffVector; Matrix<3> eulerDiffVector;
bool reset; // For quaternion calibration
// Sem dobimo vrednosti pospeskomerja in ziroskopa // Sem dobimo vrednosti pospeskomerja in ziroskopa
@ -285,15 +286,15 @@ void loop() {
bundle.add("/quaternion").add(q.w).add(q.y * -1).add(q.z).add(q.x * -1); // W X Y Z bundle.add("/quaternion").add(q.w).add(q.y * -1).add(q.z).add(q.x * -1); // W X Y Z
// Euler - rotacija // Euler - rotacija
eulerVector = eulerFromQuaternion(q); //eulerVector = eulerFromQuaternion(q);
bundle.add("/euler").add(eulerVector(0)).add(eulerVector(1)).add(eulerVector(2)); // X Y Z //bundle.add("/euler").add(eulerVector(0)).add(eulerVector(1)).add(eulerVector(2)); // X Y Z
// Quaterion difference - rotacijska razlika (prejsnji reading - trenutni reading) // Quaterion difference - rotacijska razlika (prejsnji reading - trenutni reading)
bundle.add("/quaternionDiff").add(diff.w).add(diff.y * -1).add(diff.z).add(diff.x * -1); // W X Y Z bundle.add("/quaternionDiff").add(diff.w).add(diff.y * -1).add(diff.z).add(diff.x * -1); // W X Y Z
// Rotation diff value in euler angle // Rotation diff value in euler angle
eulerDiffVector = eulerFromQuaternion(diff); //eulerDiffVector = eulerFromQuaternion(diff);
bundle.add("/eulerDiff").add(eulerDiffVector(0)).add(eulerDiffVector(1)).add(eulerDiffVector(2)); // X Y Z //bundle.add("/eulerDiff").add(eulerDiffVector(0)).add(eulerDiffVector(1)).add(eulerDiffVector(2)); // X Y Z
#ifdef OUTPUT_READABLE_REALACCEL #ifdef OUTPUT_READABLE_REALACCEL
// display real acceleration, adjusted to remove gravity // display real acceleration, adjusted to remove gravity
@ -318,6 +319,7 @@ void loop() {
#endif #endif
// Calculate speed and position from accelerometer data // Calculate speed and position from accelerometer data
/*
int prevTime = timeOn; int prevTime = timeOn;
timeOn = millis(); timeOn = millis();
int elapsedTime = timeOn - prevTime; int elapsedTime = timeOn - prevTime;
@ -329,7 +331,7 @@ void loop() {
bundle.add("/position/").add(position(0)).add(position(1)).add(position(2)); bundle.add("/position/").add(position(0)).add(position(1)).add(position(2));
bundle.add("/speed/").add(speed(0)).add(speed(1)).add(speed(2)); bundle.add("/speed/").add(speed(0)).add(speed(1)).add(speed(2));
*/
// Accelerometer // Accelerometer
bundle.add("/accel").add(AcX).add(AcY).add(AcZ); ; // X Y Z bundle.add("/accel").add(AcX).add(AcY).add(AcZ); ; // X Y Z
@ -344,7 +346,15 @@ void loop() {
// Reset calibration euler? // Reset calibration euler?
if (pressed[0] && pressed[1] && pressed[2] && pressed[3]) { if (pressed[0] && pressed[1] && pressed[2] && pressed[3]) {
cq = q.getConjugate(); if (!reset) {
cq = q.getConjugate();
reset = true;
Serial.println("Quaternion calibrate");
}
} else {
if (reset) {
reset = false;
}
} }
#ifdef SERIAL_OSC #ifdef SERIAL_OSC