99 lines
1.8 KiB
C
99 lines
1.8 KiB
C
/*
|
||
开源的AHRS算法。
|
||
MadgwickAHRS
|
||
*/
|
||
|
||
#pragma once
|
||
|
||
#ifdef __cplusplus
|
||
extern "C" {
|
||
#endif
|
||
|
||
#include "user_math.h"
|
||
|
||
/* 欧拉角（Euler angle） */
|
||
typedef struct {
|
||
float yaw; /* 偏航角（Yaw angle） */
|
||
float pit; /* 俯仰角（Pitch angle） */
|
||
float rol; /* 翻滚角（Roll angle） */
|
||
} AHRS_Eulr_t;
|
||
|
||
/* 加速度计 Accelerometer */
|
||
typedef struct {
|
||
float x;
|
||
float y;
|
||
float z;
|
||
} AHRS_Accl_t;
|
||
|
||
/* 陀螺仪 Gyroscope */
|
||
typedef struct {
|
||
float x;
|
||
float y;
|
||
float z;
|
||
} AHRS_Gyro_t;
|
||
|
||
/* 磁力计 Magnetometer */
|
||
typedef struct {
|
||
float x;
|
||
float y;
|
||
float z;
|
||
} AHRS_Magn_t;
|
||
|
||
/* 四元数 */
|
||
typedef struct {
|
||
float q0;
|
||
float q1;
|
||
float q2;
|
||
float q3;
|
||
} AHRS_Quaternion_t;
|
||
|
||
/* 姿态解算算法主结构体 */
|
||
typedef struct {
|
||
/* 四元数 */
|
||
AHRS_Quaternion_t quat;
|
||
|
||
float inv_sample_freq; /* 采样频率的的倒数 */
|
||
} AHRS_t;
|
||
|
||
/**
|
||
* @brief 初始化姿态解算
|
||
*
|
||
* @param ahrs 姿态解算主结构体
|
||
* @param magn 磁力计数据
|
||
* @param sample_freq 采样频率
|
||
* @return int8_t 0对应没有错误
|
||
*/
|
||
int8_t AHRS_Init(AHRS_t *ahrs, const AHRS_Magn_t *magn, float sample_freq);
|
||
|
||
/**
|
||
* @brief 姿态运算更新一次
|
||
*
|
||
* @param ahrs 姿态解算主结构体
|
||
* @param accl 加速度计数据
|
||
* @param gyro 陀螺仪数据
|
||
* @param magn 磁力计数据
|
||
* @return int8_t 0对应没有错误
|
||
*/
|
||
int8_t AHRS_Update(AHRS_t *ahrs, const AHRS_Accl_t *accl,
|
||
const AHRS_Gyro_t *gyro, const AHRS_Magn_t *magn);
|
||
|
||
/**
|
||
* @brief 通过姿态解算主结构体中的四元数计算欧拉角
|
||
*
|
||
* @param eulr 欧拉角
|
||
* @param ahrs 姿态解算主结构体
|
||
* @return int8_t 0对应没有错误
|
||
*/
|
||
int8_t AHRS_GetEulr(AHRS_Eulr_t *eulr, const AHRS_t *ahrs);
|
||
|
||
/**
|
||
* \brief 将对应数据置零
|
||
*
|
||
* \param eulr 被操作的数据
|
||
*/
|
||
void AHRS_ResetEulr(AHRS_Eulr_t *eulr);
|
||
|
||
#ifdef __cplusplus
|
||
}
|
||
#endif
|