162 lines
3.5 KiB
C
162 lines
3.5 KiB
C
#pragma once
|
|
|
|
#ifdef __cplusplus
|
|
extern "C" {
|
|
#endif
|
|
#include "device/at9s_pro.h"
|
|
#include "component/pid.h"
|
|
// 卡尔曼滤波器结构体
|
|
typedef struct {
|
|
float Q_angle; // 过程噪声协方差（角度）
|
|
float Q_bias; // 过程噪声协方差（陀螺仪偏置）
|
|
float R_measure; // 测量噪声协方差
|
|
float angle; // 估计角度
|
|
float bias; // 估计陀螺仪偏置
|
|
float P[2][2]; // 误差协方差矩阵
|
|
} KalmanFilter;
|
|
typedef struct{
|
|
float x;
|
|
float y;
|
|
float z;
|
|
}Quad_Vectory3f_t;
|
|
|
|
typedef struct{
|
|
float yaw;
|
|
float pit;
|
|
float rol;
|
|
}Quad_EulrVectory3f_t;
|
|
|
|
typedef struct {
|
|
uint16_t pos_alt;
|
|
uint16_t pos_vel;
|
|
uint16_t att_agl;
|
|
uint16_t att_omg;
|
|
}Quad_CtrlFreqParams_t;
|
|
|
|
typedef struct {
|
|
LowPassFilter2p_t accl_x;
|
|
LowPassFilter2p_t accl_y;
|
|
LowPassFilter2p_t accl_z;
|
|
LowPassFilter2p_t gyro_x;
|
|
LowPassFilter2p_t gyro_y;
|
|
LowPassFilter2p_t gyro_z;
|
|
LowPassFilter2p_t eulr_yaw;
|
|
LowPassFilter2p_t eulr_pit;
|
|
LowPassFilter2p_t eulr_rol;
|
|
LowPassFilter2p_t alt;
|
|
LowPassFilter2p_t vel_z;
|
|
}Quad_LowPassFilter2p_t;
|
|
|
|
typedef struct {
|
|
KPID_Params_t pit_agl;
|
|
KPID_Params_t rol_agl;
|
|
KPID_Params_t yaw_agl;
|
|
KPID_Params_t pit_omg;
|
|
KPID_Params_t rol_omg;
|
|
KPID_Params_t yaw_omg;
|
|
KPID_Params_t alt_pos;
|
|
KPID_Params_t alt_vel;
|
|
}Quad_PIDParams_t;
|
|
|
|
typedef struct {
|
|
KPID_t rol_omg, rol_agl; // rol轴PID
|
|
KPID_t pit_omg, pit_agl; // rol轴PID
|
|
KPID_t yaw_omg, yaw_agl; // rol轴PID
|
|
KPID_t alt_pos, alt_vel; // 高度环PID
|
|
}Quad_PID_t;
|
|
|
|
typedef struct {
|
|
KalmanFilter rol_kf, pit_kf;
|
|
}Quad_Kalman_t;
|
|
|
|
// 电机配置结构体
|
|
typedef struct {
|
|
float min_output; // 最小输出值 (0.0-1.0)
|
|
float max_output; // 最大输出值 (0.0-1.0)
|
|
}Quad_MotorConfig_t;
|
|
typedef struct {
|
|
bool online;
|
|
bool stop;
|
|
|
|
float throttle;
|
|
float yaw;
|
|
float rol;
|
|
float pit;
|
|
|
|
}Quad_CMD_t;
|
|
|
|
typedef struct{
|
|
Quad_Vectory3f_t accl; // m/s²
|
|
Quad_Vectory3f_t gyro;
|
|
Quad_EulrVectory3f_t eulr;
|
|
float alt; // 气压计加速度计融合后相对高度 (m)
|
|
float vel_z; //z轴速度
|
|
}Quad_CurrentStatus_t;
|
|
|
|
typedef struct {
|
|
float yaw_agl;
|
|
float rol_agl;
|
|
float pit_agl;
|
|
|
|
float yaw_omg;
|
|
float rol_omg;
|
|
float pit_omg;
|
|
|
|
float vel;
|
|
float thrust;
|
|
|
|
float alt;
|
|
}Quad_ExpectStatus_t;
|
|
|
|
typedef struct{
|
|
float yaw_omg;
|
|
float rol_agl;
|
|
float pit_agl;
|
|
}Quad_ExpectStatusLimit_t;
|
|
|
|
typedef struct {
|
|
|
|
float throttle;
|
|
float rol;
|
|
float pit;
|
|
float yaw;
|
|
|
|
float mix[4]; // 混控计算中间变量
|
|
float motor[4]; // 电机输出值 (0.0-1.0)
|
|
}Quad_Output_t;
|
|
typedef struct {
|
|
Quad_CtrlFreqParams_t ctrl_freq; // 控制频率参数
|
|
|
|
float baseThrottle; // 基础油门值
|
|
|
|
Quad_PIDParams_t pid; // PID参数
|
|
// 低通滤波器参数
|
|
|
|
Quad_ExpectStatusLimit_t expect_status_limit;
|
|
|
|
Quad_MotorConfig_t motor_limit; // 四个电机的配置
|
|
}Quad_Params_t;
|
|
|
|
typedef struct{
|
|
int64_t now;
|
|
float dt;
|
|
int64_t last;
|
|
|
|
Quad_Params_t *param;
|
|
|
|
Quad_PID_t pid;
|
|
Quad_LowPassFilter2p_t filter;
|
|
Quad_Kalman_t kalman;
|
|
|
|
Quad_CurrentStatus_t current_status;
|
|
Quad_ExpectStatus_t expect_status;
|
|
|
|
Quad_Output_t output;
|
|
}Quad_t;
|
|
|
|
int8_t Quad_Init(Quad_t *q, Quad_Params_t *param, float freq);
|
|
int8_t Quad_Ctrl(Quad_t *q, Quad_CMD_t cmd);
|
|
void Quad_stop();
|
|
#ifdef __cplusplus
|
|
}
|
|
#endif |