added variable frequency input to Mahony AHRS algortihm

src/IMU.cpp

@@ -102,6 +102,24 @@ void IMU::getAhrsData(float *pitch,float *roll,float *yaw){
   getGyroData(&gyroX,&gyroY,&gyroZ);
   getAccelData(&accX,&accY,&accZ);
 
-  MahonyAHRSupdateIMU(gyroX * DEG_TO_RAD, gyroY * DEG_TO_RAD, gyroZ * DEG_TO_RAD, accX, accY, accZ,pitch,roll,yaw);
+  MahonyAHRSupdateIMU(gyroX * DEG_TO_RAD, gyroY * DEG_TO_RAD, gyroZ * DEG_TO_RAD, accX, accY, accZ,pitch,roll,yaw,0.0f); // 0 frequency internal defined value
 
 }
+
+void IMU::getAhrsData(float *pitch,float *roll,float *yaw, float samplefrequency){
+
+  float accX = 0; 
+  float accY = 0;
+  float accZ = 0;
+
+  float gyroX = 0;
+  float gyroY = 0;
+  float gyroZ = 0;
+
+
+  getGyroData(&gyroX,&gyroY,&gyroZ);
+  getAccelData(&accX,&accY,&accZ);
+
+  MahonyAHRSupdateIMU(gyroX * DEG_TO_RAD, gyroY * DEG_TO_RAD, gyroZ * DEG_TO_RAD, accX, accY, accZ,pitch,roll,yaw,samplefrequency);
+
+}

src/IMU.h

@@ -27,6 +27,7 @@ class IMU {
     void getTempData(float *t);
 
     void getAhrsData(float *pitch,float *roll,float *yaw);
+    void getAhrsData(float *pitch,float *roll,float *yaw, float samplefrequency);
 
     ImuType imuType;
     float aRes, gRes;

src/utility/MPU6886.cpp

@@ -143,7 +143,25 @@ void MPU6886::getAhrsData(float *pitch,float *roll,float *yaw){
   getGyroData(&gyroX,&gyroY,&gyroZ);
   getAccelData(&accX,&accY,&accZ);
 
-  MahonyAHRSupdateIMU(gyroX * DEG_TO_RAD, gyroY * DEG_TO_RAD, gyroZ * DEG_TO_RAD, accX, accY, accZ,pitch,roll,yaw);
+  MahonyAHRSupdateIMU(gyroX * DEG_TO_RAD, gyroY * DEG_TO_RAD, gyroZ * DEG_TO_RAD, accX, accY, accZ,pitch,roll,yaw,0.0f); // 0 frequency internal defined value
+
+}
+
+void MPU6886::getAhrsData(float *pitch,float *roll,float *yaw, float samplefrequency){
+
+  float accX = 0; 
+  float accY = 0;
+  float accZ = 0;
+
+  float gyroX = 0;
+  float gyroY = 0;
+  float gyroZ = 0;
+
+
+  getGyroData(&gyroX,&gyroY,&gyroZ);
+  getAccelData(&accX,&accY,&accZ);
+
+  MahonyAHRSupdateIMU(gyroX * DEG_TO_RAD, gyroY * DEG_TO_RAD, gyroZ * DEG_TO_RAD, accX, accY, accZ,pitch,roll,yaw,samplefrequency);
 
 }
 

src/utility/MPU6886.h

@@ -82,6 +82,7 @@ class MPU6886 {
       void SetAccelFsr(Ascale scale);
 
       void getAhrsData(float *pitch,float *roll,float *yaw);
+      void getAhrsData(float *pitch,float *roll,float *yaw,float samplefrequency);
 
     public:
       float aRes, gRes;

src/utility/MahonyAHRS.cpp

@@ -150,12 +150,13 @@ void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az
 //---------------------------------------------------------------------------------------------------
 // IMU algorithm update
 
-void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az,float *pitch,float *roll,float *yaw) {
+void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az,float *pitch,float *roll,float *yaw, float samplefrequency) {
 	float recipNorm;
 	float halfvx, halfvy, halfvz;
 	float halfex, halfey, halfez;
 	float qa, qb, qc;
 
+	if (samplefrequency == 0.0f) samplefrequency = sampleFreq;
 
 	// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
 	if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
@@ -180,9 +181,9 @@ void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float
 
 		// Compute and apply integral feedback if enabled
 		if(twoKi > 0.0f) {
-			integralFBx += twoKi * halfex * (1.0f / sampleFreq);	// integral error scaled by Ki
-			integralFBy += twoKi * halfey * (1.0f / sampleFreq);
-			integralFBz += twoKi * halfez * (1.0f / sampleFreq);
+			integralFBx += twoKi * halfex * (1.0f / samplefrequency);	// integral error scaled by Ki
+			integralFBy += twoKi * halfey * (1.0f / samplefrequency);
+			integralFBz += twoKi * halfez * (1.0f / samplefrequency);
 			gx += integralFBx;	// apply integral feedback
 			gy += integralFBy;
 			gz += integralFBz;
@@ -200,9 +201,9 @@ void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float
 	}
 
 	// Integrate rate of change of quaternion
-	gx *= (0.5f * (1.0f / sampleFreq));		// pre-multiply common factors
-	gy *= (0.5f * (1.0f / sampleFreq));
-	gz *= (0.5f * (1.0f / sampleFreq));
+	gx *= (0.5f * (1.0f / samplefrequency));		// pre-multiply common factors
+	gy *= (0.5f * (1.0f / samplefrequency));
+	gz *= (0.5f * (1.0f / samplefrequency));
 	qa = q0;
 	qb = q1;
 	qc = q2;

src/utility/MahonyAHRS.h

@@ -25,7 +25,7 @@ extern volatile float twoKi;			// 2 * integral gain (Ki)
 
 void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
 //void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az);
-void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az,float *pitch,float *roll,float *yaw);
+void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az,float *pitch,float *roll,float *yaw, float samplefrequency);
 float invSqrt(float x);
 #endif
 //=====================================================================================================