129 lines
3.8 KiB
C
129 lines
3.8 KiB
C
// 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"
|
|
|
|
#define MADGWICK_BETA 0.2f
|
|
#define MADGWICK_SAMPLE_RATE 250.0f
|
|
#define DEG2RAD 3.14f/180.0f
|
|
|
|
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_ */
|