449 lines
16 KiB
C
449 lines
16 KiB
C
/* Includes ----------------------------------------------------------------- */
|
||
#include "xm_quadctrl.h"
|
||
#include "component/pid.h"
|
||
#include "bsp/pwm.h"
|
||
#include "bsp/time.h"
|
||
#include "module/HeightEstimation.h"
|
||
#include "component/filter.h"
|
||
// 电机布局:
|
||
// [0] 前右 (CW)
|
||
// [1] 前左 (CCW)
|
||
// [2] 后左 (CW)
|
||
// [3] 后右 (CCW)
|
||
|
||
|
||
|
||
//低通滤波初始化和计算
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
//// PID控制器实例
|
||
//KPID_t roll_angle_pid, pitch_angle_pid; // 角度环PID (外环)
|
||
//KPID_t roll_rate_pid, pitch_rate_pid; // 角速度环PID (内环)
|
||
//KPID_t yaw_rate_pid; // 偏航角速度环PID
|
||
//KPID_t altitude_pos_pid; // 高度环PID
|
||
//KPID_t altitude_vel_pid; // 高度环PID
|
||
//// PID参数配置
|
||
//KPID_Params_t angle_pid_params = {
|
||
// .k = 1.0f, // 控制器增益
|
||
// .p = 0.8f, // 比例增益
|
||
// .i = 0.05f, // 积分增益
|
||
// .d = 0.0f, // 微分增益 (角度环通常不需要D)
|
||
// .i_limit = 0.3f, // 积分限幅
|
||
// .out_limit = 1.0f, // 输出限幅
|
||
// .d_cutoff_freq = 0.0f, // 截止频率(不使用)
|
||
// .range = 0.0f // 误差范围(不使用)
|
||
//};
|
||
//KPID_Params_t rate_pid_params = {
|
||
// .k = 1.0f,
|
||
// .p = 0.25f, // 比例增益
|
||
// .i = 0.05f, // 积分增益
|
||
// .d = 0.01f, // 微分增益
|
||
// .i_limit = 0.05f,
|
||
// .out_limit = 0.2f,
|
||
// .d_cutoff_freq = 30.0f, // 微分项低通截止频率(Hz)
|
||
// .range = 0.0f
|
||
//};
|
||
//KPID_Params_t alt_pos_pid_params = {
|
||
// .k = 0.25f,
|
||
// .p = 1.0f, // 比例增益
|
||
// .i = 0.05f, // 积分增益
|
||
// .d = 0.00f, // 微分增益
|
||
// .i_limit = 0.5f,
|
||
// .out_limit = 3.0f, // 输出限幅(油门变化量)
|
||
// .d_cutoff_freq = 0.0f, // 截止频率(Hz)
|
||
// .range = 0.0f
|
||
//};
|
||
//KPID_Params_t alt_vel_pid_params = {
|
||
// .k = 0.08f,
|
||
// .p = 2.2f, // 比例增益
|
||
// .i = 0.05f, // 积分增益
|
||
// .d = 0.01f, // 微分增益
|
||
// .i_limit = 0.05f,
|
||
// .out_limit = 0.2f, // 输出限幅(油门变化量)
|
||
// .d_cutoff_freq = -1.0f, // 截止频率(Hz)
|
||
// .range = 0.0f
|
||
//};
|
||
|
||
/* Private typedef ---------------------------------------------------------- */
|
||
|
||
/* Private define ----------------------------------------------------------- */
|
||
|
||
/* Private macro ------------------------------------------------------------ */
|
||
/* Private variables -------------------------------------------------------- */
|
||
/* Private function -------------------------------------------------------- */
|
||
static inline void Quad_Math_LimitFloat(float* value, float min, float max) {
|
||
if (*value < min) {
|
||
*value = min;
|
||
} else if (*value > max) {
|
||
*value = max;
|
||
}
|
||
}
|
||
|
||
static inline void Quad_Math_ScaleSumTo1(float *a, float *b, float *c, float *d) {
|
||
float sum = *a + *b + *c + *d;
|
||
if (sum > 1.0f) {
|
||
float scale = 1.0f / sum;
|
||
*a *= scale;
|
||
*b *= scale;
|
||
*c *= scale;
|
||
*d *= scale;
|
||
}
|
||
}
|
||
|
||
// 初始化卡尔曼滤波器
|
||
void Kalman_Init(KalmanFilter* kf) {
|
||
kf->Q_angle = 0.001f;
|
||
kf->Q_bias = 0.003f;
|
||
kf->R_measure = 0.03f;
|
||
kf->angle = 0.0f;
|
||
kf->bias = 0.0f;
|
||
kf->P[0][0] = 0.0f;
|
||
kf->P[0][1] = 0.0f;
|
||
kf->P[1][0] = 0.0f;
|
||
kf->P[1][1] = 0.0f;
|
||
}
|
||
// 卡尔曼滤波器更新
|
||
float Kalman_Update(KalmanFilter* kf, float newAngle, float newRate, float dt) {
|
||
// 预测步骤
|
||
kf->angle += dt * (newRate - kf->bias);
|
||
kf->P[0][0] += dt * (dt * kf->P[1][1] - kf->P[0][1] - kf->P[1][0] + kf->Q_angle);
|
||
kf->P[0][1] -= dt * kf->P[1][1];
|
||
kf->P[1][0] -= dt * kf->P[1][1];
|
||
kf->P[1][1] += kf->Q_bias * dt;
|
||
// 更新步骤
|
||
float y = newAngle - kf->angle;
|
||
float S = kf->P[0][0] + kf->R_measure;
|
||
float K[2];
|
||
K[0] = kf->P[0][0] / S;
|
||
K[1] = kf->P[1][0] / S;
|
||
// 更新状态和协方差
|
||
kf->angle += K[0] * y;
|
||
kf->bias += K[1] * y;
|
||
float P00_temp = kf->P[0][0];
|
||
float P01_temp = kf->P[0][1];
|
||
kf->P[0][0] -= K[0] * P00_temp;
|
||
kf->P[0][1] -= K[0] * P01_temp;
|
||
kf->P[1][0] -= K[1] * P00_temp;
|
||
kf->P[1][1] -= K[1] * P01_temp;
|
||
return kf->angle;
|
||
}
|
||
|
||
static bool Quad_UnlockDetection(Quad_t *q, Quad_CMD_t *cmd){
|
||
static bool arm_flag=false;
|
||
float thr = cmd->throttle;
|
||
float yaw = cmd->yaw;
|
||
float pit = cmd->pit;
|
||
float rol = cmd->rol;
|
||
/* 2. 内八判定 */
|
||
bool inside_8 = (thr < 0.1f) &&
|
||
(yaw > 0.9f) &&
|
||
(pit < -0.9f) &&
|
||
(rol < -0.9f);
|
||
|
||
uint32_t now_ms = BSP_TIME_Get_us() / 1000.0f;
|
||
|
||
if (!inside_8) { // 一旦出界，计时清零
|
||
q->timer_ms = 0;
|
||
return arm_flag;
|
||
}
|
||
|
||
if (q->timer_ms == 0) // 第一次进入
|
||
q->timer_ms = now_ms;
|
||
|
||
if ((now_ms - q->timer_ms) >= 1000 && !arm_flag) {
|
||
arm_flag = true;
|
||
q->timer_ms = 0;
|
||
}
|
||
return arm_flag;
|
||
}
|
||
|
||
static bool Quad_OutZeroDetection(Quad_CMD_t *cmd)
|
||
{
|
||
return (cmd->throttle < 0.05f) &&
|
||
(fabsf(cmd->yaw) < 0.05f) &&
|
||
(fabsf(cmd->pit) < 0.05f) &&
|
||
(fabsf(cmd->rol) < 0.05f);
|
||
}
|
||
|
||
int8_t Quad_UpdateStatus(Quad_t *q) {
|
||
// 1. 姿态估计（使用卡尔曼滤波）精度不好
|
||
// q->current_status.eulr.rol = Kalman_Update(&q->kalman.rol_kf,
|
||
// atan2f(q->current_status.accl.y, q->current_status.accl.z),
|
||
// q->current_status.gyro.x,
|
||
// q->dt);
|
||
//
|
||
// q->current_status.eulr.pit = Kalman_Update(&q->kalman.pit_kf,
|
||
// atan2f(-q->current_status.accl.x, q->current_status.accl.z),
|
||
// q->current_status.gyro.y,
|
||
// q->dt);
|
||
LowPassFilter2p_Apply(&q->filter.gyro_x,q->current_status.gyro.x);
|
||
LowPassFilter2p_Apply(&q->filter.gyro_y,q->current_status.gyro.y);
|
||
LowPassFilter2p_Apply(&q->filter.gyro_z,q->current_status.gyro.z);
|
||
LowPassFilter2p_Apply(&q->filter.eulr_rol,q->current_status.eulr.rol);
|
||
LowPassFilter2p_Apply(&q->filter.eulr_pit,q->current_status.eulr.pit);
|
||
LowPassFilter2p_Apply(&q->filter.alt,q->current_status.alt);
|
||
LowPassFilter2p_Apply(&q->filter.vel_z,q->current_status.vel_z);
|
||
return 0;
|
||
}
|
||
|
||
int8_t Quad_SetTargetStates(Quad_t *q, Quad_CMD_t *cmd) {
|
||
|
||
q->expect_status.vel = cmd->throttle;
|
||
q->expect_status.yaw_omg = cmd->yaw;
|
||
q->expect_status.rol_agl = cmd->rol+q->calibration_status.eulr.rol;
|
||
q->expect_status.pit_agl = cmd->pit+q->calibration_status.eulr.pit;
|
||
|
||
Quad_Math_LimitFloat(&q->expect_status.yaw_omg,
|
||
-q->param->expect_status_limit.yaw_omg,
|
||
q->param->expect_status_limit.yaw_omg);
|
||
Quad_Math_LimitFloat(&q->expect_status.rol_agl,
|
||
q->expect_status.rol_agl-q->param->expect_status_limit.rol_agl,
|
||
q->expect_status.rol_agl+q->param->expect_status_limit.rol_agl);
|
||
Quad_Math_LimitFloat(&q->expect_status.pit_agl,
|
||
-q->param->expect_status_limit.pit_agl,
|
||
q->param->expect_status_limit.pit_agl);
|
||
return 0;
|
||
}
|
||
|
||
//位置控制
|
||
int8_t Quad_PosCtrl(Quad_t *q,float dt) {
|
||
|
||
// q->expect_status.vel=PID_Calc(&q->pid.alt_pos,
|
||
// q->expect_status.alt,
|
||
// q->current_status.alt,
|
||
// 0,
|
||
// dt);
|
||
q->expect_status.thrust=PID_Calc(&q->pid.alt_vel,
|
||
q->expect_status.vel,
|
||
q->current_status.vel_z,
|
||
0,
|
||
dt);
|
||
Quad_Math_LimitFloat(&q->expect_status.thrust, -1.0f, 1.0f);
|
||
q->output.throttle=q->param->baseThrottle
|
||
+q->expect_status.thrust;
|
||
return 0;
|
||
}
|
||
//姿态控制
|
||
int8_t Quad_AttCtrl_agl(Quad_t *q, float dt) {
|
||
//角度控制
|
||
q->expect_status.rol_omg = PID_Calc(&q->pid.rol_agl,
|
||
q->expect_status.rol_agl,
|
||
q->current_status.eulr.rol,
|
||
0,
|
||
q->dt);
|
||
q->expect_status.pit_omg = PID_Calc(&q->pid.pit_agl,
|
||
q->expect_status.pit_agl,
|
||
q->current_status.eulr.pit,
|
||
0,
|
||
q->dt);
|
||
return 0;
|
||
}
|
||
|
||
int8_t Quad_AttCtrl_omg(Quad_t *q, float dt) {
|
||
//角速度控制
|
||
q->output.pit = PID_Calc(&q->pid.pit_omg,
|
||
q->expect_status.pit_omg,
|
||
q->current_status.gyro.x,
|
||
0,
|
||
q->dt);
|
||
q->output.yaw = PID_Calc(&q->pid.yaw_omg,
|
||
q->expect_status.yaw_omg,
|
||
-q->current_status.gyro.z,
|
||
0,
|
||
q->dt);
|
||
q->output.rol = PID_Calc(&q->pid.rol_omg,
|
||
q->expect_status.rol_omg,
|
||
-q->current_status.gyro.y,
|
||
0,
|
||
q->dt);
|
||
LowPassFilter2p_Apply(&q->filter.output_pit,q->output.pit);
|
||
return 0;
|
||
}
|
||
|
||
int8_t Quad_Mixer(Quad_t *q) {
|
||
|
||
Quad_Math_ScaleSumTo1(&q->output.throttle, &q->output.rol, &q->output.pit, &q->output.yaw);
|
||
|
||
q->output.mix[0] = q->output.throttle - q->output.rol - q->output.pit - q->output.yaw;// 前右
|
||
q->output.mix[1] = q->output.throttle + q->output.rol - q->output.pit + q->output.yaw;// 前左
|
||
q->output.mix[2] = q->output.throttle + q->output.rol + q->output.pit - q->output.yaw;// 后左
|
||
q->output.mix[3] = q->output.throttle - q->output.rol + q->output.pit + q->output.yaw;// 后右
|
||
|
||
for(int i=0;i<4;i++){
|
||
Quad_Math_LimitFloat(&q->output.mix[i], q->param->motor_limit.min_output, q->param->motor_limit.max_output);
|
||
q->output.motor[i] = 0.275+q->output.mix[i]*0.21;
|
||
LowPassFilter2p_Apply(&q->filter.motor[i],q->output.motor[i]);
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
int8_t Quad_OutPut(Quad_Output_t *output){
|
||
|
||
if(output == NULL){
|
||
return -1;
|
||
}
|
||
BSP_PWM_SetComp(BSP_PWM_MOTOR_OUT1, output->motor[0]);
|
||
BSP_PWM_SetComp(BSP_PWM_MOTOR_OUT2, output->motor[1]);
|
||
BSP_PWM_SetComp(BSP_PWM_MOTOR_OUT3, output->motor[2]);
|
||
BSP_PWM_SetComp(BSP_PWM_MOTOR_OUT4, output->motor[3]);
|
||
return 0;
|
||
}
|
||
|
||
int8_t Quad_ResetStatus(Quad_CurrentStatus_t *currentStatus) {
|
||
|
||
return 0; // 返回0表示成功
|
||
}
|
||
|
||
int8_t Quad_ResetOutput(Quad_Output_t *output) {
|
||
|
||
for (int i = 0; i < 4; i++) {
|
||
output->motor[i] = 0.0f;
|
||
}
|
||
return 0; // 返回0表示成功
|
||
}
|
||
/* Exported functions ------------------------------------------------------- */
|
||
|
||
int8_t Quad_Init(Quad_t *q, Quad_Params_t *param, Quad_CMD_t *cmd, float freq) {
|
||
|
||
if(param == NULL) {
|
||
return -1; // 参数错误
|
||
}
|
||
q->param = param;
|
||
// 初始化卡尔曼滤波器
|
||
Kalman_Init(&q->kalman.rol_kf);
|
||
Kalman_Init(&q->kalman.pit_kf);
|
||
|
||
uint16_t pos_alt=param->ctrl_freq.pos_alt;
|
||
uint16_t pos_vel=param->ctrl_freq.pos_vel;
|
||
uint16_t att_agl=param->ctrl_freq.att_agl;
|
||
uint16_t att_omg=param->ctrl_freq.att_omg;
|
||
|
||
LowPassFilter2p_Init(&q->filter.accl_x, freq, 30.0f);
|
||
LowPassFilter2p_Init(&q->filter.accl_y, freq, 30.0f);
|
||
LowPassFilter2p_Init(&q->filter.accl_z, freq, 30.0f);
|
||
LowPassFilter2p_Init(&q->filter.gyro_x, freq, 20.0f);
|
||
LowPassFilter2p_Init(&q->filter.gyro_y, freq, 20.0f);
|
||
LowPassFilter2p_Init(&q->filter.gyro_z, freq, 20.0f);
|
||
LowPassFilter2p_Init(&q->filter.eulr_yaw, freq, 20.0f);
|
||
LowPassFilter2p_Init(&q->filter.eulr_pit, freq, 10.0f);
|
||
LowPassFilter2p_Init(&q->filter.eulr_rol, freq, 10.0f);
|
||
LowPassFilter2p_Init(&q->filter.alt, freq, 10.0f);
|
||
LowPassFilter2p_Init(&q->filter.vel_z, freq, 15.0f);
|
||
LowPassFilter2p_Init(&q->filter.output_pit, freq, 5.0f);
|
||
for(int i=0;i<4;i++)
|
||
LowPassFilter2p_Init(&q->filter.motor[i],freq,30.0f);
|
||
|
||
|
||
PID_Init(&q->pid.yaw_agl, KPID_MODE_CALC_D, att_agl, ¶m->pid.yaw_agl);
|
||
PID_Init(&q->pid.pit_agl, KPID_MODE_CALC_D, att_agl, ¶m->pid.pit_agl);
|
||
PID_Init(&q->pid.rol_agl, KPID_MODE_CALC_D, att_agl, ¶m->pid.rol_agl);
|
||
PID_Init(&q->pid.yaw_omg, KPID_MODE_CALC_D, att_omg, ¶m->pid.yaw_omg);
|
||
PID_Init(&q->pid.pit_omg, KPID_MODE_CALC_D, att_omg, ¶m->pid.pit_omg);
|
||
PID_Init(&q->pid.rol_omg, KPID_MODE_CALC_D, att_omg, ¶m->pid.rol_omg);
|
||
PID_Init(&q->pid.alt_pos, KPID_MODE_CALC_D, pos_alt, ¶m->pid.alt_pos);
|
||
PID_Init(&q->pid.alt_vel, KPID_MODE_CALC_D, pos_vel, ¶m->pid.alt_vel);
|
||
|
||
q->expect_status.alt = q->current_status.alt;
|
||
q->expect_status.pit_agl = q->current_status.eulr.pit;
|
||
q->expect_status.rol_agl = q->current_status.eulr.rol;
|
||
q->expect_status.yaw_agl = q->current_status.eulr.yaw;
|
||
q->calibration_status.eulr.rol = q->current_status.eulr.rol;
|
||
q->calibration_status.eulr.pit = q->current_status.eulr.pit;
|
||
q->calibration_status.eulr.yaw = q->current_status.eulr.yaw;
|
||
|
||
BSP_PWM_Start(BSP_PWM_MOTOR_OUT1);
|
||
BSP_PWM_Start(BSP_PWM_MOTOR_OUT2);
|
||
BSP_PWM_Start(BSP_PWM_MOTOR_OUT3);
|
||
BSP_PWM_Start(BSP_PWM_MOTOR_OUT4);
|
||
|
||
|
||
return 0;
|
||
}
|
||
uint16_t dt;
|
||
int8_t Quad_Ctrl(Quad_t *q, Quad_CMD_t *cmd) {
|
||
|
||
if(q == NULL) {
|
||
return -1; // 参数错误
|
||
}
|
||
|
||
q->now =BSP_TIME_Get_us() / 1000000.0f;
|
||
q->dt =(BSP_TIME_Get_us() - q->last) / 1000000.0f;
|
||
q->last=BSP_TIME_Get_us();
|
||
|
||
switch(q->fsm_states){
|
||
case JOYSTICKDETECTION:
|
||
BSP_PWM_SetComp(BSP_PWM_MOTOR_OUT1, 0.275+0.21*cmd->throttle);
|
||
BSP_PWM_SetComp(BSP_PWM_MOTOR_OUT2, 0.275+0.21*cmd->throttle);
|
||
BSP_PWM_SetComp(BSP_PWM_MOTOR_OUT3, 0.275+0.21*cmd->throttle);
|
||
BSP_PWM_SetComp(BSP_PWM_MOTOR_OUT4, 0.275+0.21*cmd->throttle);
|
||
if(!Quad_UnlockDetection(q,cmd)){
|
||
Quad_ResetOutput(&q->output);
|
||
return -1;
|
||
}
|
||
q->fsm_states=UNLOCKMOTOR;
|
||
q->timer_ms=q->now;
|
||
case UNLOCKMOTOR:
|
||
BSP_PWM_SetComp(BSP_PWM_MOTOR_OUT1, 0.275);
|
||
BSP_PWM_SetComp(BSP_PWM_MOTOR_OUT2, 0.275);
|
||
BSP_PWM_SetComp(BSP_PWM_MOTOR_OUT3, 0.275);
|
||
BSP_PWM_SetComp(BSP_PWM_MOTOR_OUT4, 0.275);
|
||
dt=1000*q->now-q->timer_ms;
|
||
if(dt>2000){
|
||
q->fsm_states=RUNNING;
|
||
q->timer_ms=1000*q->now;
|
||
}
|
||
break;
|
||
case RUNNING:
|
||
Quad_UpdateStatus(q);
|
||
Quad_SetTargetStates(q, cmd);
|
||
|
||
static uint8_t cnt_1ms = 0;
|
||
cnt_1ms = (cnt_1ms + 1) % 10;
|
||
|
||
/* 位置环 100 Hz */
|
||
if (cnt_1ms == 0) {
|
||
Quad_PosCtrl(q,10*q->dt);
|
||
}
|
||
|
||
/* 角度环 250 Hz */
|
||
if (cnt_1ms % 4 == 0) {
|
||
Quad_AttCtrl_agl(q,4*q->dt);
|
||
}
|
||
|
||
/* 角速度环 1000 Hz */
|
||
Quad_AttCtrl_omg(q,q->dt);
|
||
|
||
/* 混控 & 输出（1000 Hz） */
|
||
Quad_Mixer(q);
|
||
|
||
if(cmd->lockoutput){
|
||
for(int i=0;i<4;i++){
|
||
q->output.motor[i]=0;
|
||
}
|
||
}else if(cmd->stop){
|
||
Quad_stop();
|
||
}
|
||
|
||
Quad_OutPut(&q->output);
|
||
return 0;
|
||
break;
|
||
}
|
||
}
|
||
|
||
void Quad_stop() {
|
||
BSP_PWM_Stop(BSP_PWM_MOTOR_OUT1);
|
||
BSP_PWM_Stop(BSP_PWM_MOTOR_OUT2);
|
||
BSP_PWM_Stop(BSP_PWM_MOTOR_OUT3);
|
||
BSP_PWM_Stop(BSP_PWM_MOTOR_OUT4);
|
||
}
|