Diff: Mahony.cpp

Revision:

3:6811c0ce95f6

Parent:

1:36bbe04e1f6f

--- a/Mahony.cpp	Fri Oct 26 05:56:25 2018 +0000
+++ b/Mahony.cpp	Tue Dec 04 15:49:48 2018 +0000
@@ -27,7 +27,6 @@
 //-------------------------------------------------------------------------------------------
 // Definitions
 
-#define DEFAULT_SAMPLE_FREQ 100.0f  // sample frequency in Hz
 #define twoKpDef    (2.0f * 0.5f)   // 2 * proportional gain
 #define twoKiDef    (2.0f * 0.0f)   // 2 * integral gain
 
@@ -38,7 +37,7 @@
 //-------------------------------------------------------------------------------------------
 // AHRS algorithm update
 
-Mahony::Mahony()
+Mahony::Mahony(float Ts)
 {
     twoKp = twoKpDef;   // 2 * proportional gain (Kp)
     twoKi = twoKiDef;   // 2 * integral gain (Ki)
@@ -50,7 +49,7 @@
     integralFBy = 0.0f;
     integralFBz = 0.0f;
     anglesComputed = 0;
-    invSampleFreq = 1.0f / DEFAULT_SAMPLE_FREQ;
+    invSampleFreq = Ts;
 }
 
 void Mahony::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)