166 lines
5.3 KiB
C
166 lines
5.3 KiB
C
/**
|
||
* @file control_task.c
|
||
* @brief 二轴云台增稳控制任务
|
||
* @note 目标：保持 Roll=0, Pitch=0，Yaw 用于方向控制
|
||
*
|
||
* 控制架构：
|
||
* IMU 数据 → PID 控制器 → 电机补偿角度 → 抵消基座倾斜
|
||
*/
|
||
|
||
#include "app.h"
|
||
#include "cmsis_os2.h"
|
||
#include "mf4010v2.h"
|
||
#include <stdio.h>
|
||
|
||
/* ============================================================================
|
||
* 外部变量 (来自 sensor_task)
|
||
* ============================================================================ */
|
||
extern float fAcc[3], fGyro[3], fAngle[3]; /* fAngle[0]=Roll, fAngle[1]=Pitch, fAngle[2]=Yaw */
|
||
|
||
/* ============================================================================
|
||
* 云台配置
|
||
* ============================================================================ */
|
||
|
||
/* 目标角度：保持水平 */
|
||
static float g_target_roll = 0.0f;
|
||
static float g_target_pitch = 0.0f;
|
||
static float g_target_yaw = 0.0f; /* Yaw 目标可通过外部修改 */
|
||
|
||
/* 电机实例 */
|
||
static mf4010v2_t motor_yaw; /* Yaw 轴电机（外框） */
|
||
static mf4010v2_t motor_pitch; /* Pitch 轴电机（内框） */
|
||
|
||
/* PID 控制器 */
|
||
typedef struct {
|
||
float Kp, Ki, Kd;
|
||
float integral;
|
||
float prev_error;
|
||
float output_max;
|
||
} PID_t;
|
||
|
||
static PID_t g_roll_pid = {0};
|
||
static PID_t g_pitch_pid = {0};
|
||
|
||
/* ============================================================================
|
||
* PID 控制函数
|
||
* ============================================================================ */
|
||
|
||
void PID_Init(PID_t *pid, float kp, float ki, float kd, float max)
|
||
{
|
||
pid->Kp = kp;
|
||
pid->Ki = ki;
|
||
pid->Kd = kd;
|
||
pid->output_max = max;
|
||
pid->integral = 0;
|
||
pid->prev_error = 0;
|
||
}
|
||
|
||
float PID_Compute(PID_t *pid, float target, float actual)
|
||
{
|
||
float error = target - actual;
|
||
float output;
|
||
|
||
/* 比例项 */
|
||
output = pid->Kp * error;
|
||
|
||
/* 积分项（带抗饱和） */
|
||
pid->integral += error;
|
||
if (pid->integral > pid->output_max) {
|
||
pid->integral = pid->output_max;
|
||
} else if (pid->integral < -pid->output_max) {
|
||
pid->integral = -pid->output_max;
|
||
}
|
||
output += pid->Ki * pid->integral;
|
||
|
||
/* 微分项 */
|
||
float derivative = error - pid->prev_error;
|
||
output += pid->Kd * derivative;
|
||
pid->prev_error = error;
|
||
|
||
/* 输出限幅 */
|
||
if (output > pid->output_max) {
|
||
output = pid->output_max;
|
||
} else if (output < -pid->output_max) {
|
||
output = -pid->output_max;
|
||
}
|
||
|
||
return output;
|
||
}
|
||
|
||
/* ============================================================================
|
||
* 控制任务
|
||
* ============================================================================ */
|
||
|
||
void control_task(void)
|
||
{
|
||
// int32_t yaw_cmd, pitch_cmd;
|
||
// float roll_comp, pitch_comp;
|
||
|
||
printf("[control_task] starting stabilization control\n");
|
||
|
||
/* 1. 初始化 PID 参数 */
|
||
/* Roll 轴 PID：控制 Yaw 电机来补偿 Roll 倾斜 */
|
||
// PID_Init(&g_roll_pid, 0.5f, 0.01f, 0.1f, 18000.0f); /* 最大补偿 180 度 */
|
||
|
||
/* Pitch 轴 PID：控制 Pitch 电机补偿 Pitch 倾斜 */
|
||
// PID_Init(&g_pitch_pid, 0.5f, 0.01f, 0.1f, 9000.0f); /* 最大补偿 90 度 */
|
||
|
||
/* 2. 初始化电机 */
|
||
// osDelay(pdMS_TO_TICKS(1000));
|
||
mf4010v2_init(&motor_yaw, 0x141, 1); /* Yaw 电机 ID: 0x141, CAN2 */
|
||
mf4010v2_init(&motor_pitch, 0x142, 1); /* Pitch 电机 ID: 0x142, CAN2 */
|
||
|
||
/* 3. 电机关机状态启动 */
|
||
mf4010v2_close(&motor_yaw);
|
||
mf4010v2_close(&motor_pitch);
|
||
|
||
printf("[control_task] motor yaw is fault: %d\n", mf4010v2_is_fault(&motor_yaw));
|
||
printf("[control_task] motor pitch is fault: %d\n", mf4010v2_is_fault(&motor_pitch));
|
||
/* 等待 IMU 稳定 */
|
||
osDelay(pdMS_TO_TICKS(5000));
|
||
|
||
/* 4. 使能电机 */
|
||
mf4010v2_run(&motor_yaw);
|
||
mf4010v2_run(&motor_pitch);
|
||
|
||
printf("[control_task] stabilization enabled\n");
|
||
|
||
/* 主控制循环 (100Hz) */
|
||
while (1) {
|
||
/* 读取 IMU 数据 */
|
||
// float current_roll = fAngle[0]; /* Roll 角 */
|
||
// float current_pitch = fAngle[1]; /* Pitch 角 */
|
||
|
||
// /* PID 计算补偿量 */
|
||
// /* 目标：Roll=0, Pitch=0 */
|
||
// roll_comp = PID_Compute(&g_roll_pid, g_target_roll, current_roll);
|
||
// pitch_comp = PID_Compute(&g_pitch_pid, g_target_pitch, current_pitch);
|
||
|
||
// /* 运动学映射：补偿量 -> 电机角度 */
|
||
// /* 对于 Pitch-Yaw 云台：
|
||
// * - Pitch 倾斜 → Pitch 电机反向补偿
|
||
// * - Roll 倾斜 → Yaw 电机 + Pitch 电机联合补偿（简化为 Yaw 电机）
|
||
// */
|
||
// yaw_cmd = (int32_t)(roll_comp * 100.0f); /* 转为 0.01 度 */
|
||
// pitch_cmd = (int32_t)(pitch_comp * 100.0f);
|
||
|
||
// /* 发送位置指令到电机 */
|
||
// /* 使用增量位置控制，直接叠加补偿量 */
|
||
// mf4010v2_set_angle(&motor_yaw, motor_yaw.current_angle + yaw_cmd);
|
||
// mf4010v2_set_angle(&motor_pitch, motor_pitch.current_angle + pitch_cmd);
|
||
|
||
/* 100Hz 控制循环 */
|
||
osDelay(pdMS_TO_TICKS(10));
|
||
}
|
||
}
|
||
|
||
/* ============================================================================
|
||
* 外部 API
|
||
* ============================================================================ */
|
||
|
||
/* 设置 Yaw 目标方向 */
|
||
void gimbal_set_yaw_target(float yaw_deg)
|
||
{
|
||
g_target_yaw = yaw_deg;
|
||
}
|