Addition of the ability to get accel in earth frame

Linux/python/PyRTIMU_RTIMU.cpp

@@ -288,6 +288,14 @@ static PyMethodDef RTIMU_RTIMU_methods[] = {
     METH_NOARGS,
     "Return the accel residual readings" },
 
+    //////// getAccelGlobalFrame
+    {"getAccelGlobalFrame", (PyCFunction)([] (PyObject *self, PyObject* args) -> PyObject* {
+        const RTVector3& r = ((RTIMU_RTIMU*)self)->val->getAccelGlobalFrame();
+        return Py_BuildValue("(d,d,d)", r.x(), r.y(), r.z());
+        }),
+    METH_NOARGS,
+    "Return the accel rotated into the Earth Frame" },
+
     //////// setExtIMUData
     {"setExtIMUData", (PyCFunction)([] (PyObject *self, PyObject* args) -> PyObject* {
         double gx, gy, gz, ax, ay, az, mx, my, mz;

RTIMULib/IMUDrivers/RTIMU.h

@@ -159,6 +159,7 @@ class RTIMU
     const RTVector3& getCompass() { return m_imuData.compass; } // gets compass data in uT
 
     RTVector3 getAccelResiduals() { return m_fusion->getAccelResiduals(CalibratedAccel()); }
+    RTVector3 getAccelGlobalFrame() { return m_fusion->getAccelGlobalFrame(CalibratedAccel()); }
 
 protected:
     void gyroBiasInit();                                    // sets up gyro bias calculation

RTIMULib/RTFusion.cpp

@@ -132,3 +132,29 @@ RTVector3 RTFusion::getAccelResiduals(RTVector3 accel)
     residuals.setZ(-(accel.z() - rotatedGravity.z()));
     return residuals;
 }
+
+RTVector3 RTFusion::getAccelGlobalFrame(RTVector3 accel)
+{
+    RTQuaternion rotatedAccel;
+    RTQuaternion fusedConjugate;
+    RTQuaternion qTemp;
+    RTVector3 accelGCS;
+
+    // simply rotate measured accel with the fusion pose quaternion
+
+    // create the conjugate of the pose
+
+    fusedConjugate = m_fusionQPose.conjugate();
+
+    // now do the rotation - takes two steps with qTemp as the intermediate variable
+    RTQuaternion measuredAccel(0, accel.x(), accel.y(), accel.z());
+    qTemp = measuredAccel * fusedConjugate;
+    rotatedAccel = m_fusionQPose * qTemp;
+
+    // now set the accel in Global Coordinate System as the rotated accel
+
+    accelGCS.setX(rotatedAccel.x());
+    accelGCS.setY(rotatedAccel.y());
+    accelGCS.setZ(rotatedAccel.z());
+    return accelGCS;
+}

RTIMULib/RTFusion.h

@@ -73,6 +73,12 @@ class RTFusion
 
     RTVector3 getAccelResiduals(RTVector3 accel);
 
+    //  getAccelGlobalFrame() returns the rotated acceleration
+    //  from the sensor Local Coordinate System to the Earth/Global Coordinate System
+    //  useful for inertial navigation systems (like indoor positioning systems)
+
+    RTVector3 getAccelGlobalFrame(RTVector3 accel);
+
     void setDebugEnable(bool enable) { m_debug = enable; }
     void calculatePose(const RTVector3& accel, const RTVector3& mag, float magDeclination); // generates pose from accels and mag