117 lines
3.4 KiB
C
117 lines
3.4 KiB
C
#ifndef ATTITUDE_ESTIMATION_H
|
|
#define ATTITUDE_ESTIMATION_H
|
|
#include <stdio.h>
|
|
#include <math.h>
|
|
#include "QMI8658A.h"
|
|
#include "freertos/FreeRTOS.h"
|
|
#include "freertos/task.h"
|
|
#include "freertos/queue.h"
|
|
#include "LED.h"
|
|
|
|
#define PI 3.14159265358979323846
|
|
|
|
// 初始比例参数，用于姿态融合时的比例控制
|
|
#define KP_INITIAL 1.5f
|
|
// 初始积分参数，用于姿态融合时的积分控制
|
|
#define KI_INITIAL 0.005f
|
|
// 传感器数据的采样频率，单位为 Hz
|
|
#define SAMPLE_FREQ SAMPLERATE
|
|
// 积分误差的上限值，防止积分饱和
|
|
#define INTTT_MAX 1.0f
|
|
// 积分误差的下限值，防止积分饱和
|
|
#define INTTT_MIN -1.0f
|
|
|
|
|
|
// 四元数结构体
|
|
typedef struct {
|
|
float w;
|
|
float x;
|
|
float y;
|
|
float z;
|
|
} Quaternion;
|
|
|
|
|
|
|
|
// 定义积分误差结构体
|
|
typedef struct {
|
|
float exInt;
|
|
float eyInt;
|
|
float ezInt;
|
|
} IntegralError;
|
|
|
|
// 定义传感器数据结构体
|
|
typedef struct {
|
|
float gx; // 陀螺仪 x 轴角速度
|
|
float gy; // 陀螺仪 y 轴角速度
|
|
float gz; // 陀螺仪 z 轴角速度
|
|
float ax; // 加速度计 x 轴测量值
|
|
float ay; // 加速度计 y 轴测量值
|
|
float az; // 加速度计 z 轴测量值
|
|
} SensorData;
|
|
|
|
// 定义 Mahony AHRS 状态结构体
|
|
typedef struct {
|
|
Quaternion quaternion;
|
|
IntegralError integralError;
|
|
} MahonyAHRSState;
|
|
|
|
|
|
|
|
|
|
// 初始化 Mahony AHRS 状态
|
|
void MahonyAHRSInit(MahonyAHRSState *state);
|
|
|
|
/**
|
|
* @brief 对积分误差进行限幅处理，防止积分饱和
|
|
* @param value 待限幅的积分误差值
|
|
* @return 限幅后的积分误差值
|
|
*/
|
|
float limitIntegral(float value);
|
|
|
|
/**
|
|
* @brief 计算四元数在当前角速度下的导数
|
|
* @param gx 陀螺仪在 x 轴上的角速度，单位为弧度/秒
|
|
* @param gy 陀螺仪在 y 轴上的角速度，单位为弧度/秒
|
|
* @param gz 陀螺仪在 z 轴上的角速度，单位为弧度/秒
|
|
* @param q 输入的四元数
|
|
* @param dq 输出的四元数导数
|
|
*/
|
|
void computeQuaternionDerivative(float gx, float gy, float gz, Quaternion *q, Quaternion *dq);
|
|
|
|
/**
|
|
* @brief 使用四阶龙格 - 库塔法更新四元数
|
|
* @param gx 陀螺仪在 x 轴上的角速度，单位为弧度/秒
|
|
* @param gy 陀螺仪在 y 轴上的角速度，单位为弧度/秒
|
|
* @param gz 陀螺仪在 z 轴上的角速度，单位为弧度/秒
|
|
* @param dt 时间步长，等于 1 / 采样频率
|
|
* @param q 输入并输出的四元数
|
|
*/
|
|
void rk4QuaternionUpdate(float gx, float gy, float gz, float dt, Quaternion *q);
|
|
|
|
/**
|
|
* @brief 根据加速度计数据自适应调整比例和积分参数，动态调整范围
|
|
* @param ax 加速度计在 x 轴上的测量值，单位为 m/s²
|
|
* @param ay 加速度计在 y 轴上的测量值，单位为 m/s²
|
|
* @param az 加速度计在 z 轴上的测量值，单位为 m/s²
|
|
* @param Kp 输出的比例参数，用于姿态融合的比例控制
|
|
* @param Ki 输出的积分参数，用于姿态融合的积分控制
|
|
*/
|
|
void adaptiveParameterAdjustment(float ax, float ay, float az, float *Kp, float *Ki);
|
|
|
|
/**
|
|
* @brief Mahony 姿态更新函数，融合加速度计和陀螺仪数据更新姿态
|
|
* @param state Mahony AHRS 状态结构体
|
|
* @param data 传感器数据结构体
|
|
*/
|
|
void MahonyAHRSupdate(MahonyAHRSState *state, SensorData *data);
|
|
|
|
|
|
// 从四元数中提取欧拉角
|
|
void quaternion_to_euler(Quaternion q, float *roll, float *pitch, float *yaw);
|
|
|
|
// 使用加速度计初始化初始姿态
|
|
int Initialize_Attitude_With_Accelerometer(Quaternion *q);
|
|
|
|
void app_QMI8658A() ;
|
|
|
|
#endif |