Bugfixes

README.md

@@ -4,12 +4,14 @@ A simple library that applies a complementary filter to accelerometer and
 gyroscope readings (6-dof IMU) in order to achieve an accurate estimation 
 of the pitch and roll of a gyroscope.
 
+### Be Aware:
+The library expects accelerometer readings in m/second^2 and gyroscope readings in radians/second. It can output an angle in radians or degrees.
+
 ### Benefits
 
-Complementary filters attempt to do something similar to a Kalman filter, 
-however are much faster and easier to understand. Kalman filters, however,
-are typically more accurate (but they can be pretty similar) and can be 
-applied to a greater range of problems.
+Complementary filters remove the drift from gyroscopes by considering accelerometer readings, which don't drift (but are very noisy and subject to inaccuracy).
+
+
 
 Sooner or later, I'll publish this as an official Arduino library... once I've 
 done more testing and ironed out the bugs 😉.

src/simpleFusion.cpp

@@ -2,7 +2,7 @@
 
 SimpleFusion::SimpleFusion(void) {};
 
-bool SimpleFusion::init(int16_t filterUpdateRate, int16_t pitchGyroFavoring, int16_t rollGyroFavoring) {
+bool SimpleFusion::init(int16_t filterUpdateRate, float pitchGyroFavoring, float rollGyroFavoring) {
 	_filterUpdateRate = filterUpdateRate;
 	_pitchGyroFavoring = pitchGyroFavoring;
 	_rollGyroFavoring = rollGyroFavoring;
@@ -41,13 +41,13 @@ void SimpleFusion::getFilteredAngles(ThreeAxis &accelerometer, ThreeAxis &gyrosc
 	float pitchFromAccel = 0;
 	float rollFromAccel  = 0;
 
-	pitchFromAccel = atan(accelerometer.x / sqrt(pow(accelerometer.y, 2) + pow(accelerometer.z, 2))) * (float)(180 / PI);	
-// 	rollFromAccel = atan(accelerometer.y / sqrt(pow(accelerometer.x, 2) + pow(accelerometer.z, 2))) * (180 / PI);
-	rollFromAccel = atan2(-accelerometer.y , accelerometer.z) * (float)(180 / PI);
+	pitchFromAccel = atan(-accelerometer.x / sqrt(pow(accelerometer.y, 2) + pow(accelerometer.z, 2))) * (float)(180 / PI);	
+	rollFromAccel = atan(accelerometer.y / sqrt(pow(accelerometer.x, 2) + pow(accelerometer.z, 2))) * (float)(180 / PI);
 
 	// Complimentary Filter
-	_pitch = (_pitchGyroFavoring) * (_pitch + (gyroscope.y * (1.00 / _filterUpdateRate))) + (1 - _pitchGyroFavoring) * (pitchFromAccel);
-	_roll = (_rollGyroFavoring) * (_roll + (gyroscope.x * (1.00 / _filterUpdateRate))) + (1 - _rollGyroFavoring) * (rollFromAccel);
+	_pitch = (_pitchGyroFavoring) * (_pitch + (gyroscope.y * (180 / PI) * (1.00 / _filterUpdateRate))) + (1.00 - _pitchGyroFavoring) * (pitchFromAccel);
+	_roll = (_rollGyroFavoring) * (_roll + (gyroscope.x * (180 / PI) * (1.00 / _filterUpdateRate))) + (1.00 - _rollGyroFavoring) * (rollFromAccel);
+
 
 	switch (angleUnit) {
 		case UNIT_DEGREES:
@@ -59,5 +59,6 @@ void SimpleFusion::getFilteredAngles(ThreeAxis &accelerometer, ThreeAxis &gyrosc
 			angleOutputs->roll 	= _roll  * (PI / 180);
 			break;
 	}
+
 };
 

src/simpleFusion.h

@@ -24,7 +24,7 @@ class SimpleFusion {
 
 	public:
 		SimpleFusion();
-		bool init(int16_t filterUpdateRate, int16_t pitchGyroFavoring, int16_t rollGyroFavoring);
+		bool init(int16_t filterUpdateRate, float pitchGyroFavoring, float rollGyroFavoring);
 		void getFilteredAngles(ThreeAxis &accelerometer, ThreeAxis &gyroscope, FusedAngles *angleOutputs, AngleUnit angleUnit);
 
 		bool shouldUpdateData();