altb_pmic / AHRS

AHRS

Dependencies:   Eigen

Dependents:   IndNav_QK3_T265

Diff: MadgwickAHRS.cpp

Revision:
3:6811c0ce95f6
Parent:
1:36bbe04e1f6f
--- a/MadgwickAHRS.cpp	Fri Oct 26 05:56:25 2018 +0000
+++ b/MadgwickAHRS.cpp	Tue Dec 04 15:49:48 2018 +0000
@@ -26,7 +26,6 @@
 //-------------------------------------------------------------------------------------------
 // Definitions
 
-#define sampleFreqDef   100.0f          // sample frequency in Hz
 #define betaDef         0.25f            // 2 * proportional gain 0.1 - 0.5 - 5; 8
 
 
@@ -36,13 +35,13 @@
 //-------------------------------------------------------------------------------------------
 // AHRS algorithm update
 
-Madgwick::Madgwick() {
+Madgwick::Madgwick(float Ts) {
     beta = betaDef;
     q0 = 1.0f;
     q1 = 0.0f;
     q2 = 0.0f;
     q3 = 0.0f;
-    invSampleFreq = 1.0f / sampleFreqDef;
+    invSampleFreq = Ts;
     anglesComputed = 0;
 }