diff --git a/keywords.txt b/keywords.txt
index 9ad144c..49e7748 100644
--- a/keywords.txt
+++ b/keywords.txt
@@ -17,7 +17,9 @@ updateIMU	KEYWORD2
 getPitch	KEYWORD2
 getYaw	KEYWORD2
 getRoll	KEYWORD2
-
+getYPR	KEYWORD2
+getYPRRadians	KEYWORD2
+getQuaternion	KEYWORD2
 
 #######################################
 # Constants (LITERAL1)
diff --git a/src/MadgwickAHRS.cpp b/src/MadgwickAHRS.cpp
index ef2bbce..1773703 100644
--- a/src/MadgwickAHRS.cpp
+++ b/src/MadgwickAHRS.cpp
@@ -42,10 +42,20 @@ Madgwick::Madgwick() {
 	q2 = 0.0f;
 	q3 = 0.0f;
 	invSampleFreq = 1.0f / sampleFreqDef;
+        lastUpdateMillis = 0;
 	anglesComputed = 0;
 }
 
 void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
+	innerUpdate(gx, gy, gz, ax, ay, az, mx, my, mz, invSampleFreq);
+}
+
+void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, unsigned long currentMillis) {
+	float ellapsedTime = computeEllapsedTime(currentMillis);
+	innerUpdate(gx, gy, gz, ax, ay, az, mx, my, mz, ellapsedTime);
+}
+
+void Madgwick::innerUpdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float ellapsedTime) {
 	float recipNorm;
 	float s0, s1, s2, s3;
 	float qDot1, qDot2, qDot3, qDot4;
@@ -54,7 +64,7 @@ void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az
 
 	// Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
 	if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
-		updateIMU(gx, gy, gz, ax, ay, az);
+		innerUpdateIMU(gx, gy, gz, ax, ay, az, ellapsedTime);
 		return;
 	}
 
@@ -133,10 +143,10 @@ void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az
 	}
 
 	// Integrate rate of change of quaternion to yield quaternion
-	q0 += qDot1 * invSampleFreq;
-	q1 += qDot2 * invSampleFreq;
-	q2 += qDot3 * invSampleFreq;
-	q3 += qDot4 * invSampleFreq;
+	q0 += qDot1 * ellapsedTime;
+	q1 += qDot2 * ellapsedTime;
+	q2 += qDot3 * ellapsedTime;
+	q3 += qDot4 * ellapsedTime;
 
 	// Normalise quaternion
 	recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
@@ -151,6 +161,15 @@ void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az
 // IMU algorithm update
 
 void Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
+	innerUpdateIMU(gx, gy, gz, ax, ay, az, invSampleFreq);
+}
+
+void Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay, float az, unsigned long currentMillis) {
+	float ellapsedTime = computeEllapsedTime(currentMillis);
+	innerUpdateIMU(gx, gy, gz, ax, ay, az, ellapsedTime);
+}
+
+void Madgwick::innerUpdateIMU(float gx, float gy, float gz, float ax, float ay, float az, float ellapsedTime) {
 	float recipNorm;
 	float s0, s1, s2, s3;
 	float qDot1, qDot2, qDot3, qDot4;
@@ -210,10 +229,10 @@ void Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay, float
 	}
 
 	// Integrate rate of change of quaternion to yield quaternion
-	q0 += qDot1 * invSampleFreq;
-	q1 += qDot2 * invSampleFreq;
-	q2 += qDot3 * invSampleFreq;
-	q3 += qDot4 * invSampleFreq;
+	q0 += qDot1 * ellapsedTime;
+	q1 += qDot2 * ellapsedTime;
+	q2 += qDot3 * ellapsedTime;
+	q3 += qDot4 * ellapsedTime;
 
 	// Normalise quaternion
 	recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
@@ -249,3 +268,18 @@ void Madgwick::computeAngles()
 	anglesComputed = 1;
 }
 
+//-------------------------------------------------------------------------------------------
+
+float Madgwick::computeEllapsedTime(unsigned long currentMillis)
+{
+	float ellapsedTime = 0.0f;
+
+	if (lastUpdateMillis != 0) {
+		ellapsedTime = (currentMillis - lastUpdateMillis) / 1000.0f;
+	}
+
+	lastUpdateMillis = currentMillis;
+		
+	return ellapsedTime;		
+}
+
diff --git a/src/MadgwickAHRS.h b/src/MadgwickAHRS.h
index 7689523..8ff9aef 100644
--- a/src/MadgwickAHRS.h
+++ b/src/MadgwickAHRS.h
@@ -29,11 +29,15 @@ class Madgwick{
     float q2;
     float q3;	// quaternion of sensor frame relative to auxiliary frame
     float invSampleFreq;
+    unsigned long lastUpdateMillis;
     float roll;
     float pitch;
     float yaw;
     char anglesComputed;
+    void innerUpdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float ellapsedTime);
+    void innerUpdateIMU(float gx, float gy, float gz, float ax, float ay, float az, float ellapsedTime);
     void computeAngles();
+    float computeEllapsedTime(unsigned long currentMillis);
 
 //-------------------------------------------------------------------------------------------
 // Function declarations
@@ -41,7 +45,9 @@ class Madgwick{
     Madgwick(void);
     void begin(float sampleFrequency) { invSampleFreq = 1.0f / sampleFrequency; }
     void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
+    void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, unsigned long currentMillis);
     void updateIMU(float gx, float gy, float gz, float ax, float ay, float az);
+    void updateIMU(float gx, float gy, float gz, float ax, float ay, float az, unsigned long currentMillis);
     //float getPitch(){return atan2f(2.0f * q2 * q3 - 2.0f * q0 * q1, 2.0f * q0 * q0 + 2.0f * q3 * q3 - 1.0f);};
     //float getRoll(){return -1.0f * asinf(2.0f * q1 * q3 + 2.0f * q0 * q2);};
     //float getYaw(){return atan2f(2.0f * q1 * q2 - 2.0f * q0 * q3, 2.0f * q0 * q0 + 2.0f * q1 * q1 - 1.0f);};
@@ -57,6 +63,12 @@ class Madgwick{
         if (!anglesComputed) computeAngles();
         return yaw * 57.29578f + 180.0f;
     }
+    void getRPY(float * _roll, float * _pitch, float * _yaw) {
+	if (!anglesComputed) computeAngles();
+	* _roll = roll * 57.29578f;
+	* _pitch = pitch * 57.29578f;
+	* _yaw = yaw * 57.29578f + 180.0f;
+    }
     float getRollRadians() {
         if (!anglesComputed) computeAngles();
         return roll;
@@ -69,6 +81,18 @@ class Madgwick{
         if (!anglesComputed) computeAngles();
         return yaw;
     }
+    void getRPYRadians(float * _roll, float * _pitch, float * _yaw) {
+	if (!anglesComputed) computeAngles();
+	* _roll = roll;
+	* _pitch = pitch;
+	* _yaw = yaw;
+    }
+    void getQuaternion(float * _q0, float * _q1, float * _q2, float * _q3) {
+	* _q0 = q0;
+	* _q1 = q1;
+	* _q2 = q2;
+	* _q3 = q3;
+    }
 };
 #endif