Madgwick
Dependents: IMU_ethernet IMU_serial_test IMU_serial
Diff: MadgwickAHRS.cpp
- Revision:
- 3:689b64501b7d
- Parent:
- 2:11ffe983c3f8
--- a/MadgwickAHRS.cpp Thu Oct 06 17:34:56 2016 +0000 +++ b/MadgwickAHRS.cpp Thu Oct 06 23:35:07 2016 +0000 @@ -41,7 +41,7 @@ //--------------------------------------------------------------------------------------------------- // AHRS algorithm update -void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) +void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float &q0, float &q1, float &q2, float &q3) { float recipNorm; float s0, s1, s2, s3; @@ -51,7 +51,7 @@ // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation) if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) { - MadgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az); + MadgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az, q0, q1, q2, q3); return; } @@ -141,7 +141,7 @@ //--------------------------------------------------------------------------------------------------- // IMU algorithm update -void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) +void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az, float &q0, float &q1, float &q2, float &q3) { float recipNorm; float s0, s1, s2, s3;