338 lines
12 KiB
C
338 lines
12 KiB
C
/**
|
||
* @file control_task.c
|
||
* @brief 二轴云台增稳控制任务（速度控制模式）
|
||
* @note 目标：保持 Roll=0, Pitch=0，Yaw 用于方向控制
|
||
*
|
||
* 控制架构（速度控制）：
|
||
* IMU 角度 → PID 控制器 → 角速度指令 → 电机速度闭环 → 抵消基座倾斜
|
||
*
|
||
* 数据来源：
|
||
* 通过 FreeRTOS Message Queue 从 sensor_task 接收 imu_data_t
|
||
*/
|
||
|
||
#include "app.h"
|
||
#include "cmsis_os2.h"
|
||
#include "dead_zone.h"
|
||
#include "mf4010v2.h"
|
||
#include "pid.h"
|
||
#include <stdbool.h>
|
||
#include <stdio.h>
|
||
#include <string.h>
|
||
#include "lowpass.h"
|
||
/* ============================================================================
|
||
* 云台配置
|
||
* ============================================================================ */
|
||
|
||
/* 目标角度：保持水平 */
|
||
static float g_target_roll = 0.0f; /* Roll 目标 (deg) */
|
||
static float g_target_pitch = 0.0f; /* Pitch 目标 (deg) */
|
||
static float g_target_yaw = 0.0f; /* Yaw 目标 (deg) */
|
||
|
||
/* 电机实例 */
|
||
static mf4010v2_t motor_1; /* Yaw 轴电机（外框，补偿 Roll） */
|
||
static mf4010v2_t motor_2; /* Pitch 轴电机（内框，补偿 Pitch） */
|
||
|
||
/* PID 控制器 */
|
||
static pid_t g_roll_pid; /* Roll 轴 PID */
|
||
static pid_t g_pitch_pid; /* Pitch 轴 PID */
|
||
|
||
static lowpass_t g_roll_lowpass;
|
||
static lowpass_t g_pitch_lowpass;
|
||
static dead_zone_t g_yaw_deadzone;
|
||
|
||
/* 电机使能状态 — 由 callback_task 通过 UART 命令控制 */
|
||
static bool g_motor_enabled[2] = {false, false};
|
||
/* ============================================================================
|
||
* 速度控制参数
|
||
* ============================================================================ */
|
||
|
||
/*
|
||
* PID 输出为速度指令 (0.01 DPS/LSB)
|
||
*
|
||
* 电机 MF4010V2 速度范围：±204800 (即 ±2048 DPS)
|
||
* 稳像场景使用较低速度：建议 ±36000 (即 ±360 DPS) 作为输出上限
|
||
*/
|
||
#define SPEED_OUTPUT_MAX 204800.0f /* 最大角速度 360 DPS (0.01 DPS) */
|
||
|
||
/*
|
||
* PID 参数说明（速度控制模式）:
|
||
*
|
||
* Kp: 角度误差 → 速度指令的比例系数 (0.01 DPS / deg)
|
||
* 例如 Kp=200, 则 1° 误差 → 200 DPS 的补偿速度
|
||
*
|
||
* Ki: 消除稳态误差的积分系数
|
||
* 例如 Ki=5, 则 1° 持续误差每秒累积 5 DPS
|
||
*
|
||
* Kd: 抑制振荡的微分系数
|
||
* 例如 Kd=50, 则角速度变化率反馈阻尼
|
||
*/
|
||
#define ROLL_KP 1050.0f
|
||
#define ROLL_KI 0.0f
|
||
#define ROLL_KD 50.0f
|
||
#define ROLL_ALPHA 0.7f
|
||
|
||
#define PITCH_KP 1050.0f
|
||
#define PITCH_KI 0.0f
|
||
#define PITCH_KD 50.0f
|
||
|
||
#define PITCH_ALPHA 0.7f
|
||
/* ============================================================================
|
||
* 控制任务（速度控制模式）
|
||
* ============================================================================ */
|
||
|
||
// void control_task(void) {
|
||
// log_printf("[control_task] starting stabilization control (speed mode)\n");
|
||
// // mf4010v2_init(&motor_1, 0x141, 1); /* Yaw 电机 ID: 0x141, CAN2 */
|
||
// mf4010v2_init(&motor_2, 0x142, 1); /* Pitch 电机 ID: 0x142, CAN2 */
|
||
// // mf4010v2_close(&motor_1);
|
||
// mf4010v2_close(&motor_2);
|
||
// osDelay(pdMS_TO_TICKS(5000));
|
||
// // mf4010v2_run(&motor_1);
|
||
// mf4010v2_run(&motor_2);
|
||
// // mf4010v2_set_speed(&motor_1, 0);
|
||
// mf4010v2_set_speed(&motor_2, 0);
|
||
// // int8_t tempture;
|
||
// int16_t speed;
|
||
// int ret;
|
||
// int aim_speed;
|
||
// while(true) {
|
||
// aim_speed = 500;
|
||
// ret = mf4010v2_get_motor_state(&motor_2, NULL, NULL, &speed, NULL);
|
||
// log_printf("[control_task] speed: %d, ret = %d\r\n", speed, ret);
|
||
// ret = mf4010v2_set_speed(&motor_2, aim_speed);
|
||
// log_printf("[control_task] aim_speed: %d, ret = %d\r\n", aim_speed, ret);
|
||
// osDelay(pdMS_TO_TICKS(2000));
|
||
|
||
// aim_speed = 1000;
|
||
// ret = mf4010v2_get_motor_state(&motor_2, NULL, NULL, &speed, NULL);
|
||
// log_printf("[control_task] speed: %d, ret = %d\r\n", speed, ret);
|
||
// ret = mf4010v2_set_speed(&motor_2, aim_speed);
|
||
// log_printf("[control_task] aim_speed: %d, ret = %d\r\n", aim_speed, ret);
|
||
// osDelay(pdMS_TO_TICKS(2000));
|
||
|
||
// aim_speed = -1000;
|
||
// ret = mf4010v2_get_motor_state(&motor_2, NULL, NULL, &speed, NULL);
|
||
// log_printf("[control_task] speed: %d, ret = %d\r\n", speed, ret);
|
||
// ret = mf4010v2_set_speed(&motor_2, aim_speed);
|
||
// log_printf("[control_task] aim_speed: %d, ret = %d\r\n", aim_speed, ret);
|
||
// osDelay(pdMS_TO_TICKS(2000));
|
||
|
||
// aim_speed = -500;
|
||
// ret = mf4010v2_get_motor_state(&motor_2, NULL, NULL, &speed, NULL);
|
||
// log_printf("[control_task] speed: %d, ret = %d\r\n", speed, ret);
|
||
// ret = mf4010v2_set_speed(&motor_2, aim_speed);
|
||
// log_printf("[control_task] aim_speed: %d, ret = %d\r\n", aim_speed, ret);
|
||
// osDelay(pdMS_TO_TICKS(2000));
|
||
|
||
// }
|
||
// }
|
||
void control_task(void)
|
||
{
|
||
imu_data_t imu;
|
||
osStatus_t qstat;
|
||
uint32_t last_tick = osKernelGetTickCount();
|
||
|
||
log_printf("[control_task] starting stabilization control (speed mode)\n");
|
||
|
||
/* 1. 初始化 PID 控制器 */
|
||
pid_init(&g_roll_pid);
|
||
g_roll_pid.Kp = ROLL_KP;
|
||
g_roll_pid.Ki = ROLL_KI;
|
||
g_roll_pid.Kd = ROLL_KD;
|
||
g_roll_pid.output_max = SPEED_OUTPUT_MAX;
|
||
g_roll_pid.integral_max = SPEED_OUTPUT_MAX;
|
||
|
||
pid_init(&g_pitch_pid);
|
||
g_pitch_pid.Kp = PITCH_KP;
|
||
g_pitch_pid.Ki = PITCH_KI;
|
||
g_pitch_pid.Kd = PITCH_KD;
|
||
g_pitch_pid.output_max = SPEED_OUTPUT_MAX;
|
||
g_pitch_pid.integral_max = SPEED_OUTPUT_MAX;
|
||
|
||
lowpass_init(&g_roll_lowpass, ROLL_ALPHA);
|
||
lowpass_init(&g_pitch_lowpass, PITCH_ALPHA);
|
||
dead_zone_init(&g_yaw_deadzone, 0.2f);
|
||
|
||
log_printf("[control_task] PID: Roll(Kp=%.1f Ki=%.1f Kd=%.1f) Pitch(Kp=%.1f Ki=%.1f Kd=%.1f)\n",
|
||
ROLL_KP, ROLL_KI, ROLL_KD, PITCH_KP, PITCH_KI, PITCH_KD);
|
||
|
||
/* 2. 初始化电机 */
|
||
mf4010v2_init(&motor_1, 0x141, 1); /* Yaw 电机 ID: 0x141, CAN2 */
|
||
mf4010v2_init(&motor_2, 0x142, 1); /* Pitch 电机 ID: 0x142, CAN2 */
|
||
|
||
/* 3. 电机关机状态启动 */
|
||
mf4010v2_close(&motor_1);
|
||
mf4010v2_close(&motor_2);
|
||
|
||
log_printf("[control_task] motor yaw is fault: %d\n", mf4010v2_is_fault(&motor_1));
|
||
log_printf("[control_task] motor pitch is fault: %d\n", mf4010v2_is_fault(&motor_2));
|
||
|
||
/* 等待 IMU 稳定 */
|
||
osDelay(pdMS_TO_TICKS(5000));
|
||
|
||
/* 4. 使能电机（零速启动） */
|
||
mf4010v2_set_speed(&motor_1, 0);
|
||
mf4010v2_set_speed(&motor_2, 0);
|
||
mf4010v2_run(&motor_1);
|
||
mf4010v2_run(&motor_2);
|
||
|
||
log_printf("[control_task] stabilization enabled (speed mode)\n");
|
||
|
||
/* ========================================================================
|
||
* 主控制循环 (100Hz, osDelayUntil 精确周期性调度)
|
||
*
|
||
* 速度控制策略：
|
||
* 1. 从队列读取最新 IMU 角度 (Roll, Pitch)
|
||
* 2. PID 计算角速度补偿量
|
||
* 3. 速度指令发送到电机（电机内部完成速度闭环）
|
||
* ======================================================================== */
|
||
while (1) {
|
||
last_tick += pdMS_TO_TICKS(20); /* 50Hz 周期 */
|
||
|
||
/* 从 sensor_task 获取最新 IMU 数据（不等待，由定时保证节奏） */
|
||
qstat = osMessageQueueGet(g_imu_queue, &imu, NULL, 0);
|
||
if (qstat == osOK) {
|
||
imu.angle[0] = lowpass_update(&g_roll_lowpass, imu.angle[0]);
|
||
imu.angle[1] = lowpass_update(&g_pitch_lowpass, imu.angle[1]);
|
||
imu.angle[0] = dead_zone_apply(&g_yaw_deadzone, imu.angle[0]);
|
||
/* ---- Roll 轴速度控制 ---- */
|
||
float roll_speed = pid_compute(&g_roll_pid,
|
||
g_target_roll,
|
||
imu.angle[0]); /* Roll */
|
||
int32_t motor_1_speed_cmd = (int32_t)roll_speed;
|
||
|
||
/* ---- Pitch 轴速度控制 ---- */
|
||
float pitch_speed = pid_compute(&g_pitch_pid,
|
||
g_target_pitch,
|
||
imu.angle[1]); /* Pitch */
|
||
int32_t motor_2_speed_cmd = (int32_t)pitch_speed;
|
||
log_printf("[control_task:motor] %d, %d", (int)(imu.angle[0]*10), (int)(imu.angle[1]*10));
|
||
if (g_motor_enabled[GIMBAL_MOTOR_YAW]) {
|
||
if (imu.angle[0] < 1e-6f && imu.angle[0] > -1e-6f) {
|
||
mf4010v2_set_speed_nb(&motor_1, 0);
|
||
log_printf(" 1: %d,", 0);
|
||
pid_reset(&g_roll_pid);
|
||
} else {
|
||
mf4010v2_set_speed_nb(&motor_1, motor_1_speed_cmd);
|
||
log_printf(" 1: %ld,", motor_1_speed_cmd);
|
||
}
|
||
} else {
|
||
log_printf(" 1: %d,", 0);
|
||
mf4010v2_close_nb(&motor_1);
|
||
}
|
||
if (g_motor_enabled[GIMBAL_MOTOR_PITCH]) {
|
||
mf4010v2_set_speed_nb(&motor_2, motor_2_speed_cmd);
|
||
log_printf(" 2: %ld\r\n", motor_2_speed_cmd);
|
||
} else {
|
||
mf4010v2_close_nb(&motor_2);
|
||
log_printf(" 2: %d\r\n", 0);
|
||
}
|
||
}
|
||
|
||
if (osDelayUntil(last_tick) != osOK) {
|
||
last_tick = osKernelGetTickCount();
|
||
}
|
||
}
|
||
}
|
||
|
||
/* ============================================================================
|
||
* 外部 API
|
||
* ============================================================================ */
|
||
|
||
/* 设置 Yaw 目标方向 */
|
||
void gimbal_set_yaw_target(float yaw_deg)
|
||
{
|
||
g_target_yaw = yaw_deg;
|
||
}
|
||
|
||
/* 重置 PID 积分（用于模式切换或异常恢复） */
|
||
void gimbal_reset_pid(void)
|
||
{
|
||
pid_reset(&g_roll_pid);
|
||
pid_reset(&g_pitch_pid);
|
||
}
|
||
|
||
/* 使能电机 — 零速启动，重置 PID */
|
||
void gimbal_motor_enable(void)
|
||
{
|
||
gimbal_motor_enable_id(GIMBAL_MOTOR_YAW);
|
||
gimbal_motor_enable_id(GIMBAL_MOTOR_PITCH);
|
||
}
|
||
|
||
/* 关闭电机 — 进入惰行（coast）状态 */
|
||
void gimbal_motor_disable(void)
|
||
{
|
||
gimbal_motor_disable_id(GIMBAL_MOTOR_YAW);
|
||
gimbal_motor_disable_id(GIMBAL_MOTOR_PITCH);
|
||
}
|
||
|
||
/* 使能单个电机 */
|
||
void gimbal_motor_enable_id(int id)
|
||
{
|
||
g_motor_enabled[id] = true;
|
||
mf4010v2_t *motor = (id == GIMBAL_MOTOR_YAW) ? &motor_1 : &motor_2;
|
||
pid_t *pid = (id == GIMBAL_MOTOR_YAW) ? &g_roll_pid : &g_pitch_pid;
|
||
mf4010v2_set_speed(motor, 0);
|
||
mf4010v2_run(motor);
|
||
pid_reset(pid);
|
||
}
|
||
|
||
/* 关闭单个电机 */
|
||
void gimbal_motor_disable_id(int id)
|
||
{
|
||
g_motor_enabled[id] = false;
|
||
mf4010v2_t *motor = (id == GIMBAL_MOTOR_YAW) ? &motor_1 : &motor_2;
|
||
mf4010v2_close(motor);
|
||
}
|
||
|
||
/* 查询电机是否使能（任意一个使能即返回真） */
|
||
int gimbal_is_motor_enabled(void)
|
||
{
|
||
return (int)(g_motor_enabled[GIMBAL_MOTOR_YAW] || g_motor_enabled[GIMBAL_MOTOR_PITCH]);
|
||
}
|
||
|
||
/* 调参接口 — 仅可在电机 disabled 后调用 */
|
||
int gimbal_set_pid(const char *axis, const char *param, float value)
|
||
{
|
||
pid_t *pid;
|
||
|
||
if (strcmp(axis, "roll") == 0) {
|
||
pid = &g_roll_pid;
|
||
} else if (strcmp(axis, "pitch") == 0) {
|
||
pid = &g_pitch_pid;
|
||
} else {
|
||
return -1; /* 未知轴 */
|
||
}
|
||
|
||
if (strcmp(param, "kp") == 0) {
|
||
pid->Kp = value;
|
||
} else if (strcmp(param, "ki") == 0) {
|
||
pid->Ki = value;
|
||
} else if (strcmp(param, "kd") == 0) {
|
||
pid->Kd = value;
|
||
} else {
|
||
return -1; /* 未知参数 */
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
/* 获取 PID 系数（忽略 axis 大小写） */
|
||
int gimbal_get_pid(const char *axis, float *kp, float *ki, float *kd)
|
||
{
|
||
pid_t *pid;
|
||
|
||
if (strcmp(axis, "roll") == 0) {
|
||
pid = &g_roll_pid;
|
||
} else if (strcmp(axis, "pitch") == 0) {
|
||
pid = &g_pitch_pid;
|
||
} else {
|
||
return -1;
|
||
}
|
||
|
||
*kp = pid->Kp;
|
||
*ki = pid->Ki;
|
||
*kd = pid->Kd;
|
||
return 0;
|
||
}
|