hugo/feature/Add IMUKit

libs/CMakeLists.txt

@@ -13,6 +13,7 @@ add_subdirectory(${LIBS_DIR}/ContainerKit)
 add_subdirectory(${LIBS_DIR}/EventLoopKit)
 add_subdirectory(${LIBS_DIR}/FileManagerKit)
 add_subdirectory(${LIBS_DIR}/FirmwareKit)
+add_subdirectory(${LIBS_DIR}/IMUKit)
 add_subdirectory(${LIBS_DIR}/IOKit)
 add_subdirectory(${LIBS_DIR}/LedKit)
 add_subdirectory(${LIBS_DIR}/ReinforcerKit)

libs/IMUKit/CMakeLists.txt

@@ -0,0 +1,27 @@
+# Leka - LekaOS
+# Copyright 2022 APF France handicap
+# SPDX-License-Identifier: Apache-2.0
+
+add_library(IMUKit STATIC)
+
+target_include_directories(IMUKit
+	PUBLIC
+	include
+)
+
+target_sources(IMUKit
+	PRIVATE
+	source/IMUKit.cpp
+	include/internal/Mahony.cpp
+)
+
+target_link_libraries(IMUKit
+	CoreIMU
+	EventLoopKit
+)
+
+if(${CMAKE_PROJECT_NAME} STREQUAL "LekaOSUnitTests")
+	leka_unit_tests_sources(
+		tests/IMUKit_test.cpp
+	)
+endif()

libs/IMUKit/include/IMUKit.h

@@ -0,0 +1,51 @@
+// Leka - LekaOS
+// Copyright 2022 APF France handicap
+// SPDX-License-Identifier: Apache-2.0
+
+#pragma once
+
+#include <chrono>
+
+#include "interface/Accelerometer.h"
+#include "interface/Gyroscope.h"
+#include "interface/libs/EventLoop.h"
+#include "internal/Mahony.h"
+
+namespace leka {
+
+class IMUKit
+{
+  public:
+	IMUKit(interface::EventLoop &event_loop, interface::Accelerometer &accel, interface::Gyroscope &gyro)
+		: _event_loop(event_loop),
+		  _accel(accel),
+		  _gyro(gyro) {
+			  // nothing to do
+		  };
+
+	void init();
+	void start();
+	void run();
+	void stop();
+
+	auto getAngles() -> std::array<float, 3>;
+
+	void reset();
+
+	void computeAngles();
+
+  private:
+	interface::EventLoop &_event_loop;
+	interface::Accelerometer &_accel;
+	interface::Gyroscope &_gyro;
+
+	ahrs::Mahony _mahony {};
+	struct SamplingConfig {
+		const std::chrono::milliseconds delay {};
+		const float frequency {};
+	};
+	const SamplingConfig kDefaultSamplingConfig {.delay = std::chrono::milliseconds {70}, .frequency = 12.5F};
+	bool _is_running {false};
+};
+
+}	// namespace leka

libs/IMUKit/include/internal/Mahony.cpp

@@ -0,0 +1,157 @@
+// Leka - LekaOS
+// Copyright 2020 Adafruit Industries (MIT License)
+// Copyright 2022 APF France handicap
+// SPDX-License-Identifier: Apache-2.0
+
+#include "Mahony.h"
+#include <cmath>
+#include <numbers>
+
+namespace leka::ahrs {
+
+void Mahony::update(std::tuple<float, float, float> accel, std::tuple<float, float, float> gyro,
+					std::tuple<float, float, float> mag)
+{
+	auto [ax, ay, az] = accel;
+	auto [gx, gy, gz] = gyro;
+	auto [mx, my, mz] = mag;
+
+	float dt = _invSampleFreq;
+	float recipNorm;
+	float q0q0;
+	float q0q1;
+	float q0q2;
+	float q0q3;
+	float q1q1;
+	float q1q2;
+	float q1q3;
+	float q2q2;
+	float q2q3;
+	float q3q3;
+	float hx;
+	float hy;
+	float bx;
+	float bz;
+	float halfvx;
+	float halfvy;
+	float halfvz;
+	float halfwx;
+	float halfwy;
+	float halfwz;
+	float halfex;
+	float halfey;
+	float halfez;
+	float qa;
+	float qb;
+	float qc;
+
+	// LCOV_EXCL_START - Exclude while magnetometer is not used
+	if ((mx == !0.0F) && (my == !0.0F) && (mz == !0.0F)) {
+		recipNorm = utils::math::invSqrt(mx * mx + my * my + mz * mz);
+		mx *= recipNorm;
+		my *= recipNorm;
+		mz *= recipNorm;
+	}
+	// LCOV_EXCL_STOP
+
+	gx *= std::numbers::pi_v<float> / 180.F;
+	gy *= std::numbers::pi_v<float> / 180.F;
+	gz *= std::numbers::pi_v<float> / 180.F;
+
+	if ((ax != 0.0F) || (ay != 0.0F) || (az != 0.0F)) {
+		recipNorm = utils::math::invSqrt(ax * ax + ay * ay + az * az);
+		ax *= recipNorm;
+		ay *= recipNorm;
+		az *= recipNorm;
+
+		q0q0 = _q0 * _q0;
+		q0q1 = _q0 * _q1;
+		q0q2 = _q0 * _q2;
+		q0q3 = _q0 * _q3;
+		q1q1 = _q1 * _q1;
+		q1q2 = _q1 * _q2;
+		q1q3 = _q1 * _q3;
+		q2q2 = _q2 * _q2;
+		q2q3 = _q2 * _q3;
+		q3q3 = _q3 * _q3;
+
+		hx = 2.0F * (mx * (0.5F - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
+		hy = 2.0F * (mx * (q1q2 + q0q3) + my * (0.5F - q1q1 - q3q3) + mz * (q2q3 - q0q1));
+		bx = sqrtf(hx * hx + hy * hy);
+		bz = 2.0F * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5F - q1q1 - q2q2));
+
+		halfvx = q1q3 - q0q2;
+		halfvy = q0q1 + q2q3;
+		halfvz = q0q0 - 0.5F + q3q3;
+		halfwx = bx * (0.5F - q2q2 - q3q3) + bz * (q1q3 - q0q2);
+		halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
+		halfwz = bx * (q0q2 + q1q3) + bz * (0.5F - q1q1 - q2q2);
+
+		halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy);
+		halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz);
+		halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx);
+
+		gx += kTwoKp * halfex;
+		gy += kTwoKp * halfey;
+		gz += kTwoKp * halfez;
+	}
+
+	gx *= (0.5F * dt);
+	gy *= (0.5F * dt);
+	gz *= (0.5F * dt);
+	qa = _q0;
+	qb = _q1;
+	qc = _q2;
+	_q0 += (-qb * gx - qc * gy - _q3 * gz);
+	_q1 += (qa * gx + qc * gz - _q3 * gy);
+	_q2 += (qa * gy - qb * gz + _q3 * gx);
+	_q3 += (qa * gz + qb * gy - qc * gx);
+
+	recipNorm = utils::math::invSqrt(_q0 * _q0 + _q1 * _q1 + _q2 * _q2 + _q3 * _q3);
+	_q0 *= recipNorm;
+	_q1 *= recipNorm;
+	_q2 *= recipNorm;
+	_q3 *= recipNorm;
+	anglesComputed = false;
+}
+
+auto Mahony::getRoll() -> float
+{
+	if (!anglesComputed) {
+		computeAngles();
+	}
+	return _roll * 180.F * std::numbers::inv_pi_v<float>;
+}
+auto Mahony::getPitch() -> float
+{
+	if (!anglesComputed) {
+		computeAngles();
+	}
+	return _pitch * 180.F * std::numbers::inv_pi_v<float>;
+}
+auto Mahony::getYaw() -> float
+{
+	if (!anglesComputed) {
+		computeAngles();
+	}
+	return _yaw * 180.F * std::numbers::inv_pi_v<float> + 180.0F;
+}
+
+void Mahony::setOrigin()
+{
+	_q0 = 1.F;
+	_q1 = _q2 = _q3 = 0.0F;
+}
+
+void Mahony::computeAngles()
+{
+	_roll			  = atan2f(_q0 * _q1 + _q2 * _q3, 0.5F - _q1 * _q1 - _q2 * _q2);
+	_pitch			  = asinf(-2.0F * (_q1 * _q3 - _q0 * _q2));
+	_yaw			  = atan2f(_q1 * _q2 + _q0 * _q3, 0.5F - _q2 * _q2 - _q3 * _q3);
+	gravity_vector[0] = 2.0F * (_q1 * _q3 - _q0 * _q2);
+	gravity_vector[1] = 2.0F * (_q0 * _q1 + _q2 * _q3);
+	gravity_vector[2] = 2.0F * (_q1 * _q0 - 0.5F + _q3 * _q3);
+	anglesComputed	  = true;
+}
+
+}	// namespace leka::ahrs

libs/IMUKit/include/internal/Mahony.h

@@ -0,0 +1,52 @@
+// Leka - LekaOS
+// Copyright 2020 Adafruit Industries (MIT License)
+// Copyright 2022 APF France handicap
+// SPDX-License-Identifier: Apache-2.0
+
+// ? Heavily inspired by (taken from) Adafruit' AHRS library
+// ? https://github.com/adafruit/Adafruit_AHRS
+
+// ? Original paper by Robert Mahony
+// ? https://ieeexplore.ieee.org/document/4608934
+
+#pragma once
+
+#include <array>
+#include <tuple>
+
+#include "MathUtils.h"
+
+namespace leka::ahrs {
+
+class Mahony
+{
+  public:
+	Mahony() = default;
+
+	void begin(float sampleFrequency) { _invSampleFreq = 1.0F / sampleFrequency; }
+	void update(std::tuple<float, float, float>, std::tuple<float, float, float>, std::tuple<float, float, float>);
+
+	auto getRoll() -> float;
+	auto getPitch() -> float;
+	auto getYaw() -> float;
+
+	void setOrigin();
+
+  private:
+	void computeAngles();
+
+	const float kTwoKp = float {2.0F * 2.F};
+
+	float _q0 {1.0F};
+	float _q1 {0.0F};
+	float _q2 {0.0F};
+	float _q3 {0.0F};
+	float _invSampleFreq {};
+	float _roll {};
+	float _pitch {};
+	float _yaw {};
+	std::array<float, 3> gravity_vector {};
+	bool anglesComputed = false;
+};
+
+}	// namespace leka::ahrs

libs/IMUKit/source/IMUKit.cpp

@@ -0,0 +1,53 @@
+// Leka - LekaOS
+// Copyright 2022 APF France handicap
+// SPDX-License-Identifier: Apache-2.0
+
+#include "IMUKit.h"
+
+#include "rtos/ThisThread.h"
+
+using namespace leka;
+
+void IMUKit::init()
+{
+	_mahony.begin(kDefaultSamplingConfig.frequency);
+	_event_loop.registerCallback([this] { run(); });
+}
+
+void IMUKit::start()
+{
+	_is_running = true;
+	_event_loop.start();
+}
+
+void IMUKit::run()
+{
+	while (_is_running) {
+		computeAngles();
+		rtos::ThisThread::sleep_for(kDefaultSamplingConfig.delay);
+	}
+}
+
+void IMUKit::stop()
+{
+	_is_running = false;
+	_event_loop.stop();
+}
+
+auto IMUKit::getAngles() -> std::array<float, 3>
+{
+	return {_mahony.getPitch(), _mahony.getRoll(), _mahony.getYaw()};
+}
+
+void IMUKit::reset()
+{
+	_mahony.setOrigin();
+}
+
+void IMUKit::computeAngles()
+{
+	auto accel = _accel.getXYZ();
+	auto gyro  = _gyro.getXYZ();
+	auto mag   = std::make_tuple(0.0F, 0.0F, 0.0F);
+	_mahony.update(accel, gyro, mag);
+}