Initial commit

LICENSE

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2021 Sean Boerhout
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

README.md

@@ -1 +1,15 @@
 # SimpleFusion
+
+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.
+
+### 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.
+
+Sooner or later, I'll publish this as an official Arduino library; once I've 
+done more testing and ironed out the bugs 😉.

library.properties

@@ -0,0 +1,9 @@
+name=SimpleFusion
+version=1.0.0
+author=Sean Boerhout
+maintainer=Sean Boerhout
+sentence=Simple IMU fusion with a complementary filter.
+paragraph=Get Pitch and Roll estimations easily!
+category=Quadruped project (V1)
+url=https://github.com/seanboe/SimpleFusion
+architectures=*

src/simpleFusion.cpp

@@ -0,0 +1,58 @@
+#include "simpleFusion.h"
+
+sensorFusion::sensorFusion(void) {};
+
+bool sensorFusion::init(int16_t filterUpdateRate, int16_t gyroFavoring, RotationType rotationType) {
+	_filterUpdateRate = filterUpdateRate;
+	_gyroFavoring = _gyroFavoring;
+	if (_gyroFavoring > 1 || _gyroFavoring < 0)
+		return false;
+	_rotationType = rotationType;
+
+	_previousTime = millis();
+	_justUpdatedData = false;
+
+	return true;
+};
+
+
+bool sensorFusion::shouldUpdateData() {
+	int16_t dt = (millis() - _previousTime);
+	if (dt % (1000 / _filterUpdateRate) < DATA_UPDATE_POLL_TOLERANCE && _justUpdatedData == false) {
+		_justUpdatedData = true;
+		return true;
+	}
+	else if (!(dt % (1000 / _filterUpdateRate) < DATA_UPDATE_POLL_TOLERANCE))
+		_justUpdatedData = false;
+	return false;
+};
+
+int16_t sensorFusion::getFilteredAngle(ThreeAxis &gyroscope, ThreeAxis &accelerometer) { 
+	int16_t relevantAccelerometer = 0;			// The roll calculation requires the x acclerometer, the pitch requires the y
+	int16_t relevantGyroscope = 0;
+
+	switch (_rotationType) {
+		case PITCH:
+			relevantAccelerometer = accelerometer.y;
+			relevantGyroscope = gyroscope.x;
+			break;
+		case ROLL:
+			relevantAccelerometer = accelerometer.x;
+			relevantGyroscope = gyroscope.y;
+			break;
+	}	
+
+	// Get the angle based on the accelerometer readings
+	int16_t accelerometerAngle = tan(relevantAccelerometer / accelerometer.z);
+
+	// Integrate the gyroscope angle to get the angle based on gyroscope readings
+	_angle += relevantGyroscope * _filterUpdateRate;
+
+	// Apply the complementary filter
+	_angle = _gyroFavoring * _angle + accelerometerAngle * (1 - _gyroFavoring);
+
+	return _angle;
+
+};
+
+

src/simpleFusion.h

@@ -0,0 +1,37 @@
+#ifndef SIMPLE_FUSION
+#define SIMPLE_FUSION
+
+#include <Arduino.h>
+
+#define DATA_UPDATE_POLL_TOLERANCE	5			//milliseconds
+
+typedef struct {
+	int16_t x;
+	int16_t y;
+	int16_t z;
+} ThreeAxis;
+
+typedef enum {
+	PITCH, ROLL
+}	RotationType;
+
+class sensorFusion {
+
+	public:
+		sensorFusion();
+		bool init(int16_t filterUpdateRate, int16_t gyroFavoring, RotationType rotationType);
+		bool shouldUpdateData();
+		int16_t getFilteredAngle(ThreeAxis &gyroscope, ThreeAxis &accelerometer);
+
+	private:
+		RotationType _rotationType;
+		int16_t	_filterUpdateRate;		// Hertz, less than 1000
+		int16_t _gyroFavoring; 				// The amount that the gyro is favored
+
+		int16_t _angle;
+
+		long long _previousTime;
+		bool _justUpdatedData;
+};
+
+#endif