From 61578d4d6899c4f1bc45fa2dce50450cd55294ba Mon Sep 17 00:00:00 2001 From: Kier Davis Date: Wed, 8 Mar 2017 20:11:16 +0000 Subject: [PATCH] Get IMU interface to produce reasonable values for roll/pitch --- src/MadgwickAHRS.c | 8 ++++---- src/imu.c | 5 +++++ 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/src/MadgwickAHRS.c b/src/MadgwickAHRS.c index c1ba176..ca02de7 100644 --- a/src/MadgwickAHRS.c +++ b/src/MadgwickAHRS.c @@ -212,10 +212,10 @@ void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, flo } // Integrate rate of change of quaternion to yield quaternion - q0 += qDot1 * (1.0f / sampleFreq); - q1 += qDot2 * (1.0f / sampleFreq); - q2 += qDot3 * (1.0f / sampleFreq); - q3 += qDot4 * (1.0f / sampleFreq); + q0 += qDot1 * (0.02); + q1 += qDot2 * (0.02); + q2 += qDot3 * (0.02); + q3 += qDot4 * (0.02); // Normalise quaternion recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); diff --git a/src/imu.c b/src/imu.c index 2c6d560..9274737 100644 --- a/src/imu.c +++ b/src/imu.c @@ -384,6 +384,11 @@ void imu_read(measured_state_t *destination){ float accel_y = (int)accel_raw[1]/(float)ACCEL_SENSITIVITY;//-accelBias[1]; float accel_z = (int)accel_raw[2]/(float)ACCEL_SENSITIVITY;//-accelBias[2]; + // Degrees to radians + gyro_x *= M_PI/180.0; + gyro_y *= M_PI/180.0; + gyro_z *= M_PI/180.0; + /* // Integration of the roll and pitch // divide by 250Hz, equivalent to multiplying by 4ms