AHRS Library
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)