From f24948bc0983a811aafbea0d928360886fa17c3c Mon Sep 17 00:00:00 2001 From: angoosh Date: Thu, 7 Aug 2025 10:08:25 +0200 Subject: [PATCH] added madgwick --- madgwick.c | 306 +++++++++++++++++++++++++++++++++++++++++++++++++++++ madgwick.h | 124 ++++++++++++++++++++++ main.c | 34 ++++++ 3 files changed, 464 insertions(+) create mode 100644 madgwick.c create mode 100644 madgwick.h create mode 100644 main.c diff --git a/madgwick.c b/madgwick.c new file mode 100644 index 0000000..8003c37 --- /dev/null +++ b/madgwick.c @@ -0,0 +1,306 @@ +#include "madgwick.h" +#include +#include + +#define MADGWICK_INIT_ERR_STR "Madgwick AHRS init error" +#define MADGWICK_SET_BETA_ERR_STR "Madgwick set beta error" +#define MADGWICK_SET_SAMP_FREQ_ERR_STR "Madgwick set sample frequency error" +#define MADGWICK_GET_QUAT_ERR_STR "Madgwick get quaternion error" +#define MADGWICK_UPDATE_6DOF_ERR_STR "Madgwick update 6DOF error" +#define MADGWICK_UPDATE_9DOF_ERR_STR "Madgwick update 9DOF error" + +static const char* MADGWICK_TAG = "MADGWICK AHRS"; +#define MADGWICK_CHECK(a, str, ret) if(!(a)) { \ + return (ret); \ + } + +#define STM_ERR_INVALID_ARG 1 + +typedef struct madgwick { + float beta; + float sample_freq; + float q0; + float q1; + float q2; + float q3; + uint8_t lock; +} madgwick_t; + +static float invSqrt(float x) +{ + float halfx = 0.5f * x; + float y = x; + long i = *(long*)&y; + i = 0x5f3759df - (i >> 1); + y = *(float*)&i; + y = y * (1.5f - (halfx * y * y)); + return y; +} + +madgwick_handle_t madgwick_init(madgwick_cfg_t *config) +{ + /* Check input conditions */ + MADGWICK_CHECK(config, MADGWICK_INIT_ERR_STR, NULL); + + /* Allocate memory for handle structure */ + madgwick_handle_t handle = calloc(1, sizeof(madgwick_t)); + MADGWICK_CHECK(handle, MADGWICK_INIT_ERR_STR, NULL); + + /* Update handle structure */ + handle->beta = config->beta; + handle->sample_freq = config->sample_freq; + handle->q0 = 1.0f; + handle->q1 = 0.0f; + handle->q2 = 0.0f; + handle->q3 = 0.0f; + handle->lock = 0; + + return handle; +} + +uint8_t madgwick_set_beta(madgwick_handle_t handle, float beta) +{ + /* Check input conditions */ + MADGWICK_CHECK(handle, MADGWICK_SET_BETA_ERR_STR, STM_ERR_INVALID_ARG); + + handle->lock = 1; + handle->beta = beta; + handle->lock = 0; + + return 0; +} + +uint8_t madgwick_set_sample_frequency(madgwick_handle_t handle, float sample_freq) +{ + /* Check input conditions */ + MADGWICK_CHECK(handle, MADGWICK_SET_SAMP_FREQ_ERR_STR, STM_ERR_INVALID_ARG); + + handle->lock = 1; + handle->sample_freq = sample_freq; + handle->lock = 0; + + return 0; +} + +uint8_t madgwick_get_quaternion(madgwick_handle_t handle, madgwick_quat_data_t *quat_data) +{ + /* Check input conditions */ + MADGWICK_CHECK(handle, MADGWICK_GET_QUAT_ERR_STR, STM_ERR_INVALID_ARG); + + handle->lock = 1; + quat_data->q0 = handle->q0; + quat_data->q1 = handle->q1; + quat_data->q2 = handle->q2; + quat_data->q3 = handle->q3; + handle->lock = 0; + + return 0; +} + +uint8_t madgwick_update_6dof(madgwick_handle_t handle, float gx, float gy, float gz, float ax, float ay, float az) +{ + /* Check input conditions */ + MADGWICK_CHECK(handle, MADGWICK_UPDATE_6DOF_ERR_STR, STM_ERR_INVALID_ARG); + + handle->lock = 1; + + float q0 = handle->q0; + float q1 = handle->q1; + float q2 = handle->q2; + float q3 = handle->q3; + float beta = handle->beta; + float sampleFreq = handle->sample_freq; + float recipNorm; + float s0, s1, s2, s3; + float qDot1, qDot2, qDot3, qDot4; + float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 , _8q1, _8q2, q0q0, q1q1, q2q2, q3q3; + + // Rate of change of quaternion from gyroscope + qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); + qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy); + qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx); + qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx); + + // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) + if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { + + // Normalise accelerometer measurement + recipNorm = invSqrt(ax * ax + ay * ay + az * az); + ax *= recipNorm; + ay *= recipNorm; + az *= recipNorm; + + // Auxiliary variables to avoid repeated arithmetic + _2q0 = 2.0f * q0; + _2q1 = 2.0f * q1; + _2q2 = 2.0f * q2; + _2q3 = 2.0f * q3; + _4q0 = 4.0f * q0; + _4q1 = 4.0f * q1; + _4q2 = 4.0f * q2; + _8q1 = 8.0f * q1; + _8q2 = 8.0f * q2; + q0q0 = q0 * q0; + q1q1 = q1 * q1; + q2q2 = q2 * q2; + q3q3 = q3 * q3; + + // Gradient decent algorithm corrective step + s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay; + s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az; + s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az; + s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay; + recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude + s0 *= recipNorm; + s1 *= recipNorm; + s2 *= recipNorm; + s3 *= recipNorm; + + // Apply feedback step + qDot1 -= beta * s0; + qDot2 -= beta * s1; + qDot3 -= beta * s2; + qDot4 -= beta * s3; + } + + // 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); + + // Normalise quaternion + recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); + q0 *= recipNorm; + q1 *= recipNorm; + q2 *= recipNorm; + q3 *= recipNorm; + + handle->q0 = q0; + handle->q1 = q1; + handle->q2 = q2; + handle->q3 = q3; + + handle->lock = 0; + + return 0; +} + +uint8_t madgwick_update_9dof(madgwick_handle_t handle, float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) +{ + /* Check input conditions */ + MADGWICK_CHECK(handle, MADGWICK_UPDATE_9DOF_ERR_STR, STM_ERR_INVALID_ARG); + + // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation) + if ((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) { + madgwick_update_6dof(handle, gx, gy, gz, ax, ay, az); + return 0; + } + + handle->lock = 1; + + float q0 = handle->q0; + float q1 = handle->q1; + float q2 = handle->q2; + float q3 = handle->q3; + float beta = handle->beta; + float sampleFreq = handle->sample_freq; + float recipNorm; + float s0, s1, s2, s3; + float qDot1, qDot2, qDot3, qDot4; + float hx, hy; + float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; + + + + // Rate of change of quaternion from gyroscope + qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); + qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy); + qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx); + qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx); + + // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) + if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { + + // Normalise accelerometer measurement + recipNorm = invSqrt(ax * ax + ay * ay + az * az); + ax *= recipNorm; + ay *= recipNorm; + az *= recipNorm; + + // Normalise magnetometer measurement + recipNorm = invSqrt(mx * mx + my * my + mz * mz); + mx *= recipNorm; + my *= recipNorm; + mz *= recipNorm; + + // Auxiliary variables to avoid repeated arithmetic + _2q0mx = 2.0f * q0 * mx; + _2q0my = 2.0f * q0 * my; + _2q0mz = 2.0f * q0 * mz; + _2q1mx = 2.0f * q1 * mx; + _2q0 = 2.0f * q0; + _2q1 = 2.0f * q1; + _2q2 = 2.0f * q2; + _2q3 = 2.0f * q3; + _2q0q2 = 2.0f * q0 * q2; + _2q2q3 = 2.0f * q2 * q3; + 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; + + // Reference direction of Earth's magnetic field + hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3; + hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3; + _2bx = sqrt(hx * hx + hy * hy); + _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3; + _4bx = 2.0f * _2bx; + _4bz = 2.0f * _2bz; + + // Gradient decent algorithm corrective step + s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude + s0 *= recipNorm; + s1 *= recipNorm; + s2 *= recipNorm; + s3 *= recipNorm; + + // Apply feedback step + qDot1 -= beta * s0; + qDot2 -= beta * s1; + qDot3 -= beta * s2; + qDot4 -= beta * s3; + } + + // 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); + + // Normalise quaternion + recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); + q0 *= recipNorm; + q1 *= recipNorm; + q2 *= recipNorm; + q3 *= recipNorm; + + handle->q0 = q0; + handle->q1 = q1; + handle->q2 = q2; + handle->q3 = q3; + + handle->lock = 0; + + return 0; +} diff --git a/madgwick.h b/madgwick.h new file mode 100644 index 0000000..f376986 --- /dev/null +++ b/madgwick.h @@ -0,0 +1,124 @@ +// MIT License + +// Copyright (c) 2020 phonght32, 2025 edited by angoosh for general use + +// 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. + +#ifndef _IMU_MADGWICK_H_ +#define _IMU_MADGWICK_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "stdint.h" +#include "math.h" + +typedef struct madgwick *madgwick_handle_t; + +typedef struct { + float q0; + float q1; + float q2; + float q3; +} madgwick_quat_data_t; + +typedef struct { + float beta; + float sample_freq; +} madgwick_cfg_t; + +/* + * @brief Configure Madgwick AHRS parameters. + * @param config Struct pointer. + * @return + * - Madgwick handle structure: Success. + * - 0: Fail. + */ +madgwick_handle_t madgwick_init(madgwick_cfg_t *config); + +/* + * @brief Set beta value. + * @param handle Handle structure. + * @param beta Beta. + * @return + * - STM_OK: Success. + * - Others: Fail. + */ +uint8_t madgwick_set_beta(madgwick_handle_t handle, float beta); + +/* + * @brief Set sample frequency value. + * @param handle Handle structure. + * @param sample_freq Sample frequency. + * @return + * - STM_OK: Success. + * - Others: Fail. + */ +uint8_t madgwick_set_sample_frequency(madgwick_handle_t handle, float sample_freq); + +/* + * @brief Get quaternion. + * @param handle Handle structure. + * @param quat_data Quaternion. + * @return + * - STM_OK: Success. + * - Others: Fail. + */ +uint8_t madgwick_get_quaternion(madgwick_handle_t handle, madgwick_quat_data_t *quat_data); + +/* + * @brief Update Madgwick AHRS quaternion with 6 motions. + * @param handle Handle structure. + * @param gx Gyroscope along x axis. + * @param gy Gyroscope along y axis. + * @param gz Gyroscope along z axis. + * @param ax Accelerometer along x axis. + * @param ay Accelerometer along y axis. + * @param az Accelerometer along z axis. + * @return + * - STM_OK: Success. + * - Others: Fail. + */ +uint8_t madgwick_update_6dof(madgwick_handle_t handle, float gx, float gy, float gz, float ax, float ay, float az); + +/* + * @brief Update Madgwick AHRS quaternion with 9 motions. + * @param handle Handle structure. + * @param gx Gyroscope along x axis. + * @param gy Gyroscope along y axis. + * @param gz Gyroscope along z axis. + * @param ax Accelerometer along x axis. + * @param ay Accelerometer along y axis. + * @param az Accelerometer along z axis. + * @param mx Magnetometer along x axis. + * @param my Magnetometer along y axis. + * @param mz Magnetometer along z axis. + * @return + * - STM_OK: Success. + * - Others: Fail. + */ +uint8_t madgwick_update_9dof(madgwick_handle_t handle, float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz); + + +#ifdef __cplusplus +} +#endif + +#endif /* _IMU_MADGWICK_H_ */ diff --git a/main.c b/main.c new file mode 100644 index 0000000..f99209e --- /dev/null +++ b/main.c @@ -0,0 +1,34 @@ +#include "madgwick.h" +#include "stdint.h" +#include "stdio.h" + +#define MADGWICK_BETA 0.1f +#define MADGWICK_SAMPLE_RATE 100.0f +#define DEG2RAD 3.14f/180.0f + +madgwick_handle_t madgwick_handle; +madgwick_quat_data_t quat_data; + +int main(){ + madgwick_cfg_t madgwick_cfg; + madgwick_cfg.beta = MADGWICK_BETA; + madgwick_cfg.sample_freq = MADGWICK_SAMPLE_RATE; + madgwick_handle = madgwick_init(&madgwick_cfg); + + madgwick_update_6dof(madgwick_handle, + 0.6 * DEG2RAD, + 1 * DEG2RAD, + 3 * DEG2RAD, + 0.43, + 0.56, + 1.3); + + madgwick_get_quaternion(madgwick_handle, &quat_data); + float roll = 180.0 / 3.14 * atan2(2 * (quat_data.q0 * quat_data.q1 + quat_data.q2 * quat_data.q3), 1 - 2 * (quat_data.q1 * quat_data.q1 + quat_data.q2 * quat_data.q2)); + float pitch = 180.0 / 3.14 * asin(2 * (quat_data.q0 * quat_data.q2 - quat_data.q3 * quat_data.q1)); + float yaw = 180.0 / 3.14 * atan2f(quat_data.q0 * quat_data.q3 + quat_data.q1 * quat_data.q2, 0.5f - quat_data.q2 * quat_data.q2 - quat_data.q3 * quat_data.q3); + + printf("Roll: %f\nPitch: %f\nYaw: %f\n", roll, pitch, yaw); + + return 0; +}