334 lines
14 KiB
C
334 lines
14 KiB
C
/* =============================================================================
|
|
* gimbal.c - 云台双轴控制(串级 PID)
|
|
* Yaw(2026-09 重写):双陀螺环,见 gimbal.h 与 robot_param.h 说明
|
|
* 位置环:err(°) = 目标角 - SIGN*att_deg_yaw -> 目标角速度(°/s,限幅)
|
|
* 速度环:err(°/s) = 目标角速度 - SIGN*gyro_z -> 电流(限幅)
|
|
* 稳定性条件:SIGN*G>0,G 为"电流->陀螺 z 角速度"的装配增益,
|
|
* 由 gimbal_detect_yaw_sign() 上电实测,彻底消除双传感器符号打架
|
|
* Pitch:位置环用 QEKF Roll(安装方向),速度环转子转速(legacy 案,无符号史问题)
|
|
* 发送:CAN2 0x1FE 槽1=Yaw 电流;CAN1 0x1FE 槽2=Pitch 电流
|
|
* ========================================================================== */
|
|
#include "gimbal.h"
|
|
#include "shoot.h"
|
|
#include "pid.h"
|
|
#include "attitude.h"
|
|
#include "bmi088.h"
|
|
#include "can.h"
|
|
#include "robot_param.h"
|
|
#include "filter.h"
|
|
#include "robot_typedef.h"
|
|
#include "dr16.h"
|
|
#include <math.h>
|
|
|
|
#ifndef RAD_TO_ANGLE
|
|
#define RAD_TO_ANGLE 57.295779513082320876798154814105f
|
|
#endif
|
|
|
|
#ifndef ANGLE_TO_RAD
|
|
#define ANGLE_TO_RAD 0.01745329251994329576923690768489f
|
|
#endif
|
|
|
|
dji_motor_t motor_yaw_gimbal;
|
|
dji_motor_t motor_pitch_gimbal;
|
|
dji_motor_t motor_friction_l;
|
|
dji_motor_t motor_friction_r;
|
|
dji_motor_t motor_rammer;
|
|
|
|
static PID_PositionTypeDef yaw_angle_pid, yaw_speed_pid;
|
|
static PID_PositionTypeDef pitch_angle_pid, pitch_speed_pid;
|
|
|
|
static float yaw_target_deg; // 世界系目标角(°)
|
|
static float pitch_target_deg; // 目标角(°)
|
|
static float yaw_current;
|
|
float pitch_pid = 0.0f;
|
|
float pitch_g = 0.0f;
|
|
float pitch_current = 0.0f;
|
|
|
|
/* ===== 2026-10-15 台架调参临时量(已停用,保留备查) ===== */
|
|
#if 0
|
|
/* 台架调参用 Pitch 目标角附加偏置(°)。由 gimbal_task 的方波发生器写入,
|
|
* 只叠加到位置环设定值上,不累加进 pitch_target_deg,默认 0 无副作用 */
|
|
float gimbal_bench_pitch_offset = 0.0f;
|
|
|
|
/* 实际施加的偏置(经限位安全门控后),仅用于遥测显示真实设定值 */
|
|
static float pitch_applied_offset = 0.0f;
|
|
#endif
|
|
|
|
/* 装配符号:0=未检测,±1=已测定;未检测前 gimbal_control 不应被调用 */
|
|
static float yaw_sign_f = 0.0f;
|
|
static uint8_t yaw_sign_valid = 0;
|
|
|
|
void gimbal_motor_init(void)
|
|
{
|
|
dji_motor_init(&motor_yaw_gimbal, &hcan2, 0x205, DJI_MOTOR_6020);
|
|
dji_motor_init(&motor_pitch_gimbal, &hcan1, 0x206, DJI_MOTOR_6020);
|
|
dji_motor_init(&motor_friction_l, &hcan1, 0x201, DJI_MOTOR_3508);
|
|
dji_motor_init(&motor_friction_r, &hcan1, 0x202, DJI_MOTOR_3508);
|
|
dji_motor_init(&motor_rammer, &hcan2, 0x201, DJI_MOTOR_2006);
|
|
|
|
/* Yaw 位置环:° -> °/s */
|
|
PID_PositionStructureInit(&yaw_angle_pid, 0);
|
|
PID_PositionSetParameter(&yaw_angle_pid, GIMBAL_YAW_ANGLE_KP, GIMBAL_YAW_ANGLE_KI, GIMBAL_YAW_ANGLE_KD);
|
|
PID_PositionSetEkRange(&yaw_angle_pid, -GIMBAL_YAW_ANGLE_EK_DEADBAND, GIMBAL_YAW_ANGLE_EK_DEADBAND);
|
|
PID_PositionSetOUTRange(&yaw_angle_pid, -GIMBAL_YAW_ANGLE_OUT_LIMIT, GIMBAL_YAW_ANGLE_OUT_LIMIT);
|
|
|
|
/* Yaw 速度环:°/s -> 电流 */
|
|
PID_PositionStructureInit(&yaw_speed_pid, 0);
|
|
PID_PositionSetParameter(&yaw_speed_pid, GIMBAL_YAW_SPEED_KP, GIMBAL_YAW_SPEED_KI, GIMBAL_YAW_SPEED_KD);
|
|
PID_PositionSetOUTRange(&yaw_speed_pid, -GIMBAL_YAW_SPEED_OUT_LIMIT, GIMBAL_YAW_SPEED_OUT_LIMIT);
|
|
|
|
/* Pitch:与 legacy 相同的两环(位置环反馈为 Roll) */
|
|
PID_PositionStructureInit(&pitch_angle_pid, 0);
|
|
PID_PositionSetParameter(&pitch_angle_pid, GIMBAL_PITCH_ANGLE_KP, GIMBAL_PITCH_ANGLE_KI, GIMBAL_PITCH_ANGLE_KD);
|
|
PID_PositionSetEkRange(&pitch_angle_pid, -GIMBAL_PITCH_ANGLE_EK_DEADBAND, GIMBAL_PITCH_ANGLE_EK_DEADBAND);
|
|
PID_PositionSetOUTRange(&pitch_angle_pid, -200, 200);
|
|
|
|
PID_PositionStructureInit(&pitch_speed_pid, 300);
|
|
PID_PositionSetParameter(&pitch_speed_pid, GIMBAL_PITCH_SPEED_KP, GIMBAL_PITCH_SPEED_KI, GIMBAL_PITCH_SPEED_KD);
|
|
PID_PositionSetEkRange(&pitch_speed_pid, 0, 0);
|
|
PID_PositionSetOUTRange(&pitch_speed_pid, -16384, 16384);
|
|
}
|
|
|
|
uint8_t gimbal_detect_yaw_sign(void)
|
|
{
|
|
if (gimbal_motor_offline())
|
|
return 0;
|
|
|
|
/* 静默期:输出清零等待机械/陀螺稳定 */
|
|
dji_motor_send_gm6020_current_low(&hcan2, 0, 0, 0, 0);
|
|
HAL_Delay(50);
|
|
|
|
/* 正向电流脉冲 60ms,期间平均陀螺 z 角速度 */
|
|
float sum = 0.0f;
|
|
dji_motor_send_gm6020_current_low(&hcan2, GIMBAL_YAW_DETECT_CURRENT, 0, 0, 0);
|
|
for (uint16_t i = 0; i < 60; i++)
|
|
{
|
|
sum += bmi088_gyro[2]; // rad/s(z 轴向上为正)
|
|
HAL_Delay(1);
|
|
}
|
|
dji_motor_send_gm6020_current_low(&hcan2, 0, 0, 0, 0);
|
|
HAL_Delay(80); // 回稳,随后 gimbal_task 的首次控制会同步目标角
|
|
|
|
const float rate_dps = sum / 60.0f * RAD_TO_ANGLE;
|
|
if (rate_dps > GIMBAL_YAW_DETECT_RATE_MIN)
|
|
{
|
|
yaw_sign_f = 1.0f; // 正电流->陀螺正转,装配增益 G>0
|
|
yaw_sign_valid = 1;
|
|
}
|
|
else if (rate_dps < -GIMBAL_YAW_DETECT_RATE_MIN)
|
|
{
|
|
yaw_sign_f = -1.0f; // 皮带交叉等反向装配
|
|
yaw_sign_valid = 1;
|
|
}
|
|
else
|
|
return 0; // 被按住/没转:不给结论,由调用方决定重试或回退
|
|
|
|
return 1;
|
|
}
|
|
|
|
float gimbal_yaw_sign(void)
|
|
{
|
|
return yaw_sign_valid ? yaw_sign_f : GIMBAL_YAW_SIGN_FALLBACK;
|
|
}
|
|
|
|
int8_t gimbal_encoder_ccw(void)
|
|
{
|
|
/* 自检:电流正脉冲 -> 陀螺 z 正转(云台 CCW),而电流正方向即编码器
|
|
* tick 增大方向,故 tick 增方向 = sign(gimbal 旋转) */
|
|
if (!yaw_sign_valid)
|
|
return 0;
|
|
return (yaw_sign_f > 0.0f) ? 1 : -1;
|
|
}
|
|
|
|
void gimbal_control(float yaw_delta, float pitch_delta, uint8_t reset_flag)
|
|
{
|
|
/* 双环共用装配符号 */
|
|
const float sign = gimbal_yaw_sign();
|
|
const float yaw_meas = sign * att_deg_yaw; // °
|
|
const float yaw_rate_meas = sign * (bmi088_gyro[2] * RAD_TO_ANGLE); // °/s
|
|
|
|
/* 首次进入控制或复位:将目标角设定为设定的编码器零位朝向(类似 Pitch 上电归位) */
|
|
static uint8_t first_run = 1;
|
|
if (first_run || reset_flag)
|
|
{
|
|
first_run = 0;
|
|
|
|
/* 计算当前编码器距离设定零位的最短差值(-4096 ~ +4096) */
|
|
int16_t ecd_diff = (int16_t)motor_yaw_gimbal.fdb.raw_angle - (int16_t)YAW_GM6020_ENCODER_ZERO;
|
|
if (ecd_diff > 4096)
|
|
ecd_diff -= 8192;
|
|
else if (ecd_diff < -4096)
|
|
ecd_diff += 8192;
|
|
|
|
/* 将编码器 tick 差折算为机械角度偏差(°)，补偿给世界系目标角 */
|
|
const float ecd_err_deg = (float)ecd_diff * (360.0f / 8192.0f);
|
|
yaw_target_deg = yaw_meas - ecd_err_deg;
|
|
|
|
/* Pitch: 计算当前编码器距离机械零位的差值，折算为角度偏差补偿给目标角(上电自动归位到零位) */
|
|
int16_t pitch_ecd_diff = (int16_t)motor_pitch_gimbal.fdb.raw_angle - (int16_t)PITCH_GM6020_ENCODER_ZERO;
|
|
const float pitch_ecd_err_deg = (float)pitch_ecd_diff * (360.0f / 8192.0f);
|
|
pitch_target_deg = -att_deg_roll - pitch_ecd_err_deg;
|
|
|
|
if (reset_flag)
|
|
{
|
|
PID_PositionClean(&yaw_angle_pid);
|
|
PID_PositionClean(&yaw_speed_pid);
|
|
PID_PositionClean(&pitch_angle_pid);
|
|
PID_PositionClean(&pitch_speed_pid);
|
|
}
|
|
}
|
|
|
|
yaw_target_deg += yaw_delta;
|
|
pitch_target_deg += pitch_delta;
|
|
|
|
/* Yaw 位置环:° -> °/s(反馈为带符号姿态角) */
|
|
yaw_angle_pid.Need_Value = yaw_target_deg;
|
|
PID_PositionCalc(&yaw_angle_pid, yaw_meas);
|
|
|
|
/* Yaw 速度环:°/s -> 电流(反馈为带符号陀螺角速度,双环符号自洽) */
|
|
yaw_speed_pid.Need_Value = yaw_angle_pid.OUT;
|
|
PID_PositionCalc(&yaw_speed_pid, yaw_rate_meas);
|
|
|
|
/* 小陀螺底盘自旋解耦前馈:根据底盘相对转速补偿动摩擦与反电动势拖拽，消除同向/反向手感不对称 */
|
|
float spin_feedforward = 0.0f;
|
|
if (cmd_robot.spin_flag || dr16.s2 == 1 || dr16.s2 == 2)
|
|
{
|
|
spin_feedforward = GIMBAL_YAW_SPIN_FEEDFORWARD_K * (float)motor_yaw_gimbal.fdb.rotor_speed;
|
|
}
|
|
yaw_current = yaw_speed_pid.OUT + spin_feedforward;
|
|
if (yaw_current > GIMBAL_YAW_SPEED_OUT_LIMIT)
|
|
yaw_current = GIMBAL_YAW_SPEED_OUT_LIMIT;
|
|
else if (yaw_current < -GIMBAL_YAW_SPEED_OUT_LIMIT)
|
|
yaw_current = -GIMBAL_YAW_SPEED_OUT_LIMIT;
|
|
|
|
/* Pitch 串级闭环,位置环反馈为 AHRS Roll */
|
|
#if 0
|
|
/* ===== 2026-10-15 台架调参临时改动(已停用,保留备查) =====
|
|
* 启用时:先算编码器角与上下软限位,做台架偏置的限位安全门控(轴距限位
|
|
* 不足 GIMBAL_BENCH_PITCH_SAFE_MARGIN_DEG 时不施加阶跃),并把下面一行
|
|
* 改为 pitch_angle_pid.Need_Value = pitch_target_deg + pitch_applied_offset; */
|
|
const float pitch_ecd_deg = ((float)motor_pitch_gimbal.fdb.raw_angle -
|
|
(float)PITCH_GM6020_ENCODER_ZERO) * (360.0f / 8192.0f);
|
|
const float pitch_lim_up_deg = ((float)PITCH_GM6020_POSITION_UPPER_LIMIT -
|
|
(float)PITCH_GM6020_ENCODER_ZERO) * (360.0f / 8192.0f);
|
|
const float pitch_lim_dn_deg = ((float)PITCH_GM6020_POSITION_LOWER_LIMIT -
|
|
(float)PITCH_GM6020_ENCODER_ZERO) * (360.0f / 8192.0f);
|
|
pitch_applied_offset = gimbal_bench_pitch_offset;
|
|
if (pitch_ecd_deg < pitch_lim_up_deg + GIMBAL_BENCH_PITCH_SAFE_MARGIN_DEG ||
|
|
pitch_ecd_deg > pitch_lim_dn_deg - GIMBAL_BENCH_PITCH_SAFE_MARGIN_DEG)
|
|
pitch_applied_offset = 0.0f;
|
|
#endif
|
|
pitch_angle_pid.Need_Value = pitch_target_deg;
|
|
PID_PositionCalc(&pitch_angle_pid, -att_deg_roll);
|
|
pitch_speed_pid.Need_Value = pitch_angle_pid.OUT;
|
|
PID_PositionCalc(&pitch_speed_pid, motor_pitch_gimbal.fdb.rotor_speed);
|
|
pitch_pid = pitch_speed_pid.OUT;
|
|
|
|
/* Pitch 重力拟合前馈补偿:
|
|
* a 为当前物理编码值减去 3200*360/8192 的数值(折算为弧度送入三角函数)
|
|
* I = 3105.88 * cos(a) + 3565.50 * sin(a) + 64.31
|
|
* 最终电流输出 pitch_out = pitch_pid + pitch_g (速度环不代入 pitch_g) */
|
|
const float a_deg = ((float)motor_pitch_gimbal.fdb.raw_angle - GIMBAL_PITCH_GRAVITY_ZERO_ECD) * (360.0f / 8192.0f);
|
|
const float a_rad = a_deg * ANGLE_TO_RAD;
|
|
pitch_g = GIMBAL_PITCH_GRAVITY_K_COS * cosf(a_rad) +
|
|
GIMBAL_PITCH_GRAVITY_K_SIN * sinf(a_rad) +
|
|
GIMBAL_PITCH_GRAVITY_K_CONST;
|
|
|
|
pitch_current = pitch_pid + pitch_g;
|
|
if (pitch_current > 16384.0f)
|
|
pitch_current = 16384.0f;
|
|
else if (pitch_current < -16384.0f)
|
|
pitch_current = -16384.0f;
|
|
|
|
#if 0
|
|
/* ===== 2026-10-15 台架调参临时改动:Pitch 软限位/堵转保护(已停用,保留备查) =====
|
|
* 背景:原设计的上下限位只作用于遥控输入路径(robot_cmd_task),控制环自身的
|
|
* 目标角/前馈没有任何限位约束。台架烧录会失力矩十几秒~几分钟,轴会掉到限位,
|
|
* 若不加保护,原逻辑会持续输出大电流顶着限位(GM6020 直驱堵转 = 持续过载发热)。
|
|
* 做法:越限位时只允许"离开限位"方向的电流(正电流 = 抬头/编码器减小,故下限位
|
|
* 只留正向、上限位只留负向),反方向清零;限位外连续 GIMBAL_PITCH_LIMIT_STALL_MS
|
|
* 编码器不动则判堵转,电流降到涓流上限。轴回到软限位内后闭环自动恢复正常。 */
|
|
if (pitch_ecd_deg >= pitch_lim_dn_deg || pitch_ecd_deg <= pitch_lim_up_deg)
|
|
{
|
|
/* 堵转计时:相对基准编码值移动超过 10 个刻度(0.44°)才算"在动"。
|
|
* 阈值取 10 而不是 2:陀螺/结构微振会让编码器抖 2~3 个刻度,阈值太小会把
|
|
* 静止误判成在动,保护永远不生效(实测踩过);慢速抬升 5°/s 在 300ms 内
|
|
* 也能走 34 个刻度,不会被误判。 */
|
|
static uint16_t lim_stall = 0;
|
|
static uint16_t lim_ref_ecd = 0;
|
|
const int16_t d_ecd = (int16_t)((int16_t)motor_pitch_gimbal.fdb.raw_angle -
|
|
(int16_t)lim_ref_ecd);
|
|
if (d_ecd > 10 || d_ecd < -10)
|
|
{
|
|
lim_ref_ecd = motor_pitch_gimbal.fdb.raw_angle;
|
|
lim_stall = 0;
|
|
}
|
|
else if (lim_stall < 60000u)
|
|
lim_stall++;
|
|
|
|
const float cap = (lim_stall * 2u >= (uint16_t)GIMBAL_PITCH_LIMIT_STALL_MS)
|
|
? GIMBAL_PITCH_LIMIT_CURRENT
|
|
: GIMBAL_PITCH_LIMIT_CURRENT_RECOVER;
|
|
|
|
if (pitch_ecd_deg >= pitch_lim_dn_deg) /* 轴过低:只许抬升 */
|
|
{
|
|
if (pitch_current < 0.0f)
|
|
pitch_current = 0.0f;
|
|
else if (pitch_current > cap)
|
|
pitch_current = cap;
|
|
}
|
|
else /* 轴过高:只许下压 */
|
|
{
|
|
if (pitch_current > 0.0f)
|
|
pitch_current = 0.0f;
|
|
else if (pitch_current < -cap)
|
|
pitch_current = -cap;
|
|
}
|
|
PID_PositionClean(&pitch_speed_pid);
|
|
}
|
|
#endif
|
|
/* ===== 停用块结束 ===== */
|
|
|
|
/* 发送:CAN2 0x1FE 槽1 = Yaw 电流;CAN1 0x1FE 槽2 = Pitch 电流 */
|
|
dji_motor_send_gm6020_current_low(&hcan2, (int16_t)yaw_current, 0, 0, 0);
|
|
dji_motor_send_gm6020_current_low(&hcan1, 0, (int16_t)pitch_current, 0, 0);
|
|
}
|
|
|
|
#if 0
|
|
/* ===== 2026-10-15 台架调参临时接口(已停用,保留备查) ===== */
|
|
/* Pitch 位置环实际设定值(°) = 真实目标角 + 经限位门控后的台架偏置 */
|
|
float gimbal_pitch_setpoint_deg(void)
|
|
{
|
|
return pitch_target_deg + pitch_applied_offset;
|
|
}
|
|
|
|
/* Pitch 位置环输出 = 速度环目标(°/s),用于判断内环是否被限幅 */
|
|
float gimbal_pitch_speed_ref(void)
|
|
{
|
|
return pitch_angle_pid.OUT;
|
|
}
|
|
#endif
|
|
|
|
uint8_t gimbal_motor_offline(void)
|
|
{
|
|
uint32_t now = HAL_GetTick();
|
|
uint8_t rammer_off = 0;
|
|
#if GIMBAL_RAMMER_INSTALLED
|
|
rammer_off = (motor_rammer.fdb.last_online_tick == 0 ||
|
|
now - motor_rammer.fdb.last_online_tick > DEVICE_OFFLINE_TIME_MS);
|
|
#endif
|
|
return (motor_yaw_gimbal.fdb.last_online_tick == 0 || now - motor_yaw_gimbal.fdb.last_online_tick > DEVICE_OFFLINE_TIME_MS ||
|
|
motor_pitch_gimbal.fdb.last_online_tick == 0 || now - motor_pitch_gimbal.fdb.last_online_tick > DEVICE_OFFLINE_TIME_MS ||
|
|
motor_friction_l.fdb.last_online_tick == 0 || now - motor_friction_l.fdb.last_online_tick > DEVICE_OFFLINE_TIME_MS ||
|
|
motor_friction_r.fdb.last_online_tick == 0 || now - motor_friction_r.fdb.last_online_tick > DEVICE_OFFLINE_TIME_MS ||
|
|
rammer_off);
|
|
}
|
|
|
|
void gimbal_output_zero(void)
|
|
{
|
|
dji_motor_send_gm6020_current_low(&hcan2, 0, 0, 0, 0);
|
|
dji_motor_send_gm6020_current_low(&hcan1, 0, 0, 0, 0);
|
|
dji_motor_send_current_low(&hcan1, 0, 0, 0, 0);
|
|
dji_motor_send_current_low(&hcan2, 0, 0, 0, 0);
|
|
}
|