Update readme, WIP euler angles, calibration quaternion

master
Jurij Podgoršek 2022-03-05 17:14:13 +01:00
parent 0bedee7d4b
commit 0b5016a278
2 changed files with 54 additions and 4 deletions

View File

@ -42,6 +42,7 @@
** dependencies (libraries) ** dependencies (libraries)
1. OSC (filter by "open sound control") 1. OSC (filter by "open sound control")
2. MPU6050 2. MPU6050
3. BasicLinearAlgebra
** calibration ** calibration
- IMU_Zero - IMU_Zero

View File

@ -14,7 +14,7 @@ using namespace BLA;
#define SERIAL_OSC #define SERIAL_OSC
//#define WIFI_OSC //#define WIFI_OSC
//#define BT_OSC #define BT_OSC
#define OUTPUT_READABLE_WORLDACCEL #define OUTPUT_READABLE_WORLDACCEL
@ -107,6 +107,7 @@ uint8_t fifoBuffer[64]; // FIFO storage buffer
Quaternion q; // [w, x, y, z] quaternion container Quaternion q; // [w, x, y, z] quaternion container
Quaternion pq; // [w, x, y, z] previous quaternion container Quaternion pq; // [w, x, y, z] previous quaternion container
Quaternion diff; // [w, x, y, z] quaternion derivate container Quaternion diff; // [w, x, y, z] quaternion derivate container
Quaternion cq; // [w, x, y, z] calibration quaternion
VectorInt16 aa; // [x, y, z] accel sensor measurements VectorInt16 aa; // [x, y, z] accel sensor measurements
VectorInt16 gy; // [x, y, z] gyro sensor measurements VectorInt16 gy; // [x, y, z] gyro sensor measurements
VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
@ -117,6 +118,9 @@ float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gra
uint32_t timeOn = 0; // Uptime counter for movement calculation uint32_t timeOn = 0; // Uptime counter for movement calculation
Matrix<3> position; // [x,y,z] tracks position of device 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> eulerDiffVector;
// Sem dobimo vrednosti pospeskomerja in ziroskopa // Sem dobimo vrednosti pospeskomerja in ziroskopa
int16_t AcX,AcY,AcZ; int16_t AcX,AcY,AcZ;
@ -127,6 +131,37 @@ byte keys[] = {16, 17, 5, 18};
byte pressed[] = {0, 0, 0, 0}; byte pressed[] = {0, 0, 0, 0};
byte KEYLEN = 4; byte KEYLEN = 4;
BLA::Matrix<3> eulerFromQuaternion(Quaternion q) {
float x2 = q.x + q.x; float y2 = q.y + q.y; float z2 = q.z + q.z;
float xx = q.x * x2; float xy = q.x * y2; float xz = q.x * z2;
float yy = q.y * y2; float yz = q.y * z2; float zz = q.z * z2;
float wx = q.w * x2; float wy = q.w * y2; float wz = q.w * z2;
BLA::Matrix<4,4> rotationMatrix = {
1 - (yy + zz), xy + wz, xz - wy, 0,
xy - wz, 1 - ( xx + zz ), yz + wx, 0,
xz + wy, yz - wx, 1 - ( xx + yy ), 0,
0, 0, 0, 1
};
//TODO: test whether BLA library uses column-major matrix notation in code
BLA::Matrix<3> eulerVector;
eulerVector.Fill(0);
eulerVector(1) = asin(clamp(rotationMatrix(1,3),-1,1));
if (fabsf(rotationMatrix(1,3)) < 0.9999999) {
eulerVector(0) = atan2f(-rotationMatrix(2,3), rotationMatrix(3,3));
eulerVector(2) = atan2f( -rotationMatrix(1,2), rotationMatrix(1,1));
} else {
eulerVector(0) = atan2f(rotationMatrix(3,2), rotationMatrix(2,2));
eulerVector(2) = 0;
}
return eulerVector;
}
float clamp(float value,float min,float max) {
return fmaxf( min, fminf(max, value));
}
/* OSC MSG channels */ /* OSC MSG channels */
OSCBundle bundle; OSCBundle bundle;
@ -241,16 +276,25 @@ void loop() {
// get quaternion values in easy matrix form: w x y z // get quaternion values in easy matrix form: w x y z
mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetQuaternion(&q, fifoBuffer);
q = q.getProduct(cq);
//compute the differential rotation between the previous and new orientation //compute the differential rotation between the previous and new orientation
diff = q.getProduct(pq.getConjugate()); diff = q.getProduct(pq.getConjugate());
// Quaternion - rotacija // Quaternion - rotacija
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
eulerVector = eulerFromQuaternion(q);
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
eulerDiffVector = eulerFromQuaternion(diff);
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
mpu.dmpGetAccel(&aa, fifoBuffer); mpu.dmpGetAccel(&aa, fifoBuffer);
@ -297,6 +341,11 @@ void loop() {
pressed[i] = !digitalRead(keys[i]); pressed[i] = !digitalRead(keys[i]);
bundle.getOSCMessage("/keys")->add(pressed[i]); bundle.getOSCMessage("/keys")->add(pressed[i]);
} }
// Reset calibration euler?
if (pressed[0] && pressed[1] && pressed[2] && pressed[3]) {
cq = q.getConjugate();
}
#ifdef SERIAL_OSC #ifdef SERIAL_OSC
SLIPSerial.beginPacket(); SLIPSerial.beginPacket();