Get IMU interface to produce reasonable values for roll/pitch

Seine2017 · Mar 8, 2017 · 61578d4 · 61578d4
1 parent 01413d8
commit 61578d4
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 4 deletions.
diff --git 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
@@ -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