Accomodations for OpenVR's weird coordinate system and a Madgwick fil…

…ter for the test app

driver/c2vr/resources/settings/default.vrsettings

@@ -14,7 +14,7 @@
       "ipd": 0.0685,
       "edid_vid": 6991, // 1b4f
       "edid_pid": 37382, // 9206
-      "vidReverseW": true,
-      "vidReverseH": true
+      "vidReverseW": false,
+      "vidReverseH": false 
    }
 }

driver/src/driver.cpp

@@ -1,4 +1,6 @@
 #include <openvr/openvr_driver.h>
+#include <glm/glm.hpp>
+#include <glm/gtc/type_ptr.hpp>
 #include <cstring>
 #include <memory>
 #include <string>
@@ -194,10 +196,23 @@ class CHMDDriver : public vr::ITrackedDeviceServerDriver, public vr::IVRDisplayC
             pose.deviceIsConnected = false;
         }
 
-        pose.qWorldFromDriverRotation = HmdQuaternion_Init( 1, 0, 0, 0 );
-        pose.qDriverFromHeadRotation = HmdQuaternion_Init( 1, 0, 0, 0 );
+        pose.qWorldFromDriverRotation = HmdQuaternion_Init( 1, 0, 0, 0 ); 
+        pose.qDriverFromHeadRotation = HmdQuaternion_Init( 1, 0, 0, 0 ); 
+        //pose.qDriverFromHeadRotation = HmdQuaternion_Init( -0.5, 0.5, 0.5, 0.5 ); 
 
-        pose.qRotation = hidHandler.getHMDQuat();
+
+        vr::HmdQuaternion_t inquat = hidHandler.getHMDQuat();
+        //pose.qRotation = hidHandler.getHMDQuat();
+
+        // Using GLM because I'm too lazy to implement a quaternion inverse function
+        // Also, OpenVR's coordinate system is weird, so components Z and Y have to get swapped
+        glm::quat gquat = glm::quat(inquat.w, inquat.x, inquat.z, inquat.y);
+        gquat = glm::inverse(gquat);
+
+        pose.qRotation.w = gquat.w;
+        pose.qRotation.x = gquat.x;
+        pose.qRotation.y = gquat.y;
+        pose.qRotation.z = gquat.z;
 
         return pose;
     }

driver/src/hid.cpp

@@ -31,10 +31,10 @@ void CHIDHandler::thread_loop() {
                 tq[6], tq[7], tq[8]);
 
         vr::HmdQuaternion_t curq;
-        curq.x = madg.q0;
-        curq.y = madg.q1;
-        curq.z = madg.q2;
-        curq.w = madg.q3;
+        curq.w = madg.q0;
+        curq.x = madg.q1;
+        curq.y = madg.q2;
+        curq.z = madg.q3;
 
         // Just for debugging
         //float rot = fmod((float)clock() / (float)CLOCKS_PER_SEC, 3.14159f);

testing/hid_gl/src/ahrs.cpp

@@ -16,3 +16,155 @@ void StupidAHRS::update(IMUDat rawdat) {
 glm::quat StupidAHRS::getQuat() {
     return this->estimate;
 }
+
+// Madgwick
+
+void MadgwickAHRS::init(unsigned int sampleFreq) {
+    this->sampleFreq = sampleFreq;
+    this->beta = 0.1f;
+    this->estimate = glm::quat(1.0f, 0.f, 0.f, 0.f);
+}
+
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Warray-bounds"
+#pragma GCC diagnostic ignored "-Wstrict-aliasing"
+#pragma GCC diagnostic ignored "-Wuninitialized"
+static float invSqrt(float x) {
+	float halfx = 0.5f * x;
+	float y = x;
+	long i = *(long*)&y;
+	i = 0x5f3759df - (i>>1);
+	y = *(float*)&i;
+	y = y * (1.5f - (halfx * y * y));
+	y = y * (1.5f - (halfx * y * y));
+	return y;
+}
+#pragma GCC diagnostic pop
+
+// Most lines here are derived from Madgwick's original code
+void MadgwickAHRS::update(IMUDat rawdat) {
+    float q0 = estimate.w;
+    float q1 = estimate.x;
+    float q2 = estimate.y;
+    float q3 = estimate.z;
+
+    float gx = rawdat.gyro.x;
+    float gy = rawdat.gyro.y;
+    float gz = rawdat.gyro.z;
+
+    float ax = rawdat.acc.x;
+    float ay = rawdat.acc.y;
+    float az = rawdat.acc.z;
+
+    float mx = rawdat.mag.x;
+    float my = rawdat.mag.y;
+    float mz = rawdat.mag.z;
+
+    float recipNorm;
+	float s0, s1, s2, s3;
+	float qDot1, qDot2, qDot3, qDot4;
+	float hx, hy;
+	float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
+
+    float invSampleFreq = 1.f/(float)sampleFreq;
+
+
+	// Rate of change of quaternion from gyroscope
+	qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
+	qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
+	qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
+    qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
+
+    // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
+	if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
+
+		// Normalise accelerometer measurement
+		recipNorm = invSqrt(ax * ax + ay * ay + az * az);
+		ax *= recipNorm;
+		ay *= recipNorm;
+		az *= recipNorm;
+
+		// Normalise magnetometer measurement
+		recipNorm = invSqrt(mx * mx + my * my + mz * mz);
+		mx *= recipNorm;
+		my *= recipNorm;
+		mz *= recipNorm;
+
+		// Auxiliary variables to avoid repeated arithmetic
+		_2q0mx = 2.0f * q0 * mx;
+		_2q0my = 2.0f * q0 * my;
+		_2q0mz = 2.0f * q0 * mz;
+		_2q1mx = 2.0f * q1 * mx;
+		_2q0 = 2.0f * q0;
+		_2q1 = 2.0f * q1;
+		_2q2 = 2.0f * q2;
+		_2q3 = 2.0f * q3;
+		_2q0q2 = 2.0f * q0 * q2;
+		_2q2q3 = 2.0f * q2 * q3;
+		q0q0 = q0 * q0;
+		q0q1 = q0 * q1;
+		q0q2 = q0 * q2;
+		q0q3 = q0 * q3;
+		q1q1 = q1 * q1;
+		q1q2 = q1 * q2;
+		q1q3 = q1 * q3;
+		q2q2 = q2 * q2;
+		q2q3 = q2 * q3;
+		q3q3 = q3 * q3;
+
+		// Reference direction of Earth's magnetic field
+		hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
+		hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
+		_2bx = sqrtf(hx * hx + hy * hy);
+		_2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
+		_4bx = 2.0f * _2bx;
+		_4bz = 2.0f * _2bz;
+
+		// Gradient decent algorithm corrective step
+		s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+		s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+		s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+		s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+		recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
+		s0 *= recipNorm;
+		s1 *= recipNorm;
+		s2 *= recipNorm;
+		s3 *= recipNorm;
+
+		// Apply feedback step
+		qDot1 -= beta * s0;
+		qDot2 -= beta * s1;
+		qDot3 -= beta * s2;
+		qDot4 -= beta * s3;
+	}
+
+	// Integrate rate of change of quaternion to yield quaternion
+	q0 += qDot1 * invSampleFreq;
+	q1 += qDot2 * invSampleFreq;
+	q2 += qDot3 * invSampleFreq;
+	q3 += qDot4 * invSampleFreq;
+
+	// Normalise quaternion
+	recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+	q0 *= recipNorm;
+	q1 *= recipNorm;
+	q2 *= recipNorm;
+	q3 *= recipNorm;
+
+    this->estimate.w = q0;
+    this->estimate.x = q1;
+    this->estimate.y = q2;
+    this->estimate.z = q3;
+
+
+    //glm::quat curRot = glm::quat(rawdat.gyro / ((float)sampleFreq));
+    //this->estimate = curRot * this->estimate; 
+}
+
+void MadgwickAHRS::setBetaVal(float beta) {
+    this->beta = beta;
+}
+
+glm::quat MadgwickAHRS::getQuat() {
+    return this->estimate;
+}

testing/hid_gl/src/ahrs.hpp

@@ -20,8 +20,22 @@ class StupidAHRS : public AHRS {
     glm::quat estimate;
 
 public:
-    void init(unsigned int samplFreq);
+    void init(unsigned int sampleFreq);
     void update(IMUDat rawIn);
     glm::quat getQuat();
 };
 
+class MadgwickAHRS : public AHRS {
+    unsigned int sampleFreq;
+    float beta;
+
+    glm::quat estimate;
+
+public:
+    void init(unsigned int sampleFreq);
+    void update(IMUDat rawIn);
+
+    void setBetaVal(float beta);
+    glm::quat getQuat();
+};
+

testing/hid_gl/src/imu.cpp

@@ -73,7 +73,7 @@ bool IMUThread::start() {
     hidbuf[1] = 0x81;
     hid_write(dev_hndl, (unsigned char*)hidbuf, 17);
 
-    ahrs.reset(new StupidAHRS());
+    ahrs.reset(new MadgwickAHRS());
     imu.reset(new IMU(dev_hndl, 1000, ahrs.get()));
 
     imu->readBiasFile("bias.csv");

testing/hid_gl/src/main.cpp

@@ -76,10 +76,17 @@ int main()
 
     glm::mat4 matProjection, matView, matModel, matLocal;
     glm::vec3 CameraPos = glm::vec3(0,0,-3.f);
+    //glm::quat baseQuat = glm::quat(1.0f, 0.0f, 0.0f, 0.0f);
+    glm::quat baseQuat = glm::quat(-0.5f, 0.5f, 0.5f, 0.5f);
 
     while(!glfwWindowShouldClose(window)) {
         if(glfwGetKey(window, GLFW_KEY_ESCAPE) == GLFW_PRESS)
             glfwSetWindowShouldClose(window, true);
+
+        if(glfwGetKey(window, GLFW_KEY_R) == GLFW_PRESS) {
+            baseQuat = imuthread.getQuat();
+            baseQuat.w *= -1;
+        }
 
         int winw, winh;
         glfwGetWindowSize(window, &winw, &winh);
@@ -97,9 +104,14 @@ int main()
         matLocal = glm::mat4(1.0f);
         //matLocal = glm::scale(matLocal, glm::vec3(0.5, 0.5, 0.5));
 
+        glm::quat hmdQuat = imuthread.getQuat();
         matModel = glm::mat4(1.0f);
         matModel = glm::translate(matModel, glm::vec3(0.f, 0.f, -3.f));
-        matModel = matModel * glm::mat4_cast(imuthread.getQuat()); 
+        matModel = matModel * glm::mat4_cast(baseQuat); 
+        matModel = matModel * glm::mat4_cast(hmdQuat); 
+
+        glm::quat testq = imuthread.getQuat();
+        std::cout << testq.w << " " << testq.x << " " << testq.y << " " << testq.z << std::endl;
 
         defaultShader.setMatrix4("transform", matProjection * matView * matModel * matLocal);
         model.draw(defaultShader);