226 lines
9.0 KiB
C
226 lines
9.0 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 0x1FF 槽1=Yaw 电压;CAN1 0x1FF 槽2=Pitch 电压,槽3=拨弹电流
|
|
* ========================================================================== */
|
|
#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"
|
|
|
|
#ifndef RAD_TO_ANGLE
|
|
#define RAD_TO_ANGLE 57.295779513082320876798154814105f
|
|
#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_voltage;
|
|
static float pitch_voltage;
|
|
|
|
/* 装配符号:0=未检测,±1=已测定;未检测前 gimbal_control 不应被调用 */
|
|
static float yaw_sign_f = 0.0f;
|
|
static uint8_t yaw_sign_valid = 0;
|
|
|
|
/* 拨弹电流由 shoot 模块写入,云台与拨弹共帧(0x1FF CAN1)发送 */
|
|
static int16_t rammer_current_shared = 0;
|
|
|
|
void shoot_share_rammer_current(int16_t current);
|
|
|
|
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, &hcan1, 0x207, 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, -5, 5);
|
|
PID_PositionSetOUTRange(&pitch_speed_pid, -30000, 30000);
|
|
}
|
|
|
|
uint8_t gimbal_detect_yaw_sign(void)
|
|
{
|
|
if (gimbal_motor_offline())
|
|
return 0;
|
|
|
|
/* 静默期:输出清零等待机械/陀螺稳定 */
|
|
dji_motor_send_current_high(&hcan2, 0, 0, 0, 0);
|
|
HAL_Delay(50);
|
|
|
|
/* 正向电压脉冲 60ms,期间平均陀螺 z 角速度 */
|
|
float sum = 0.0f;
|
|
dji_motor_send_current_high(&hcan2, GIMBAL_YAW_DETECT_VOLTAGE, 0, 0, 0);
|
|
for (uint16_t i = 0; i < 60; i++)
|
|
{
|
|
sum += bmi088_gyro[2]; // rad/s(z 轴向上为正)
|
|
HAL_Delay(1);
|
|
}
|
|
dji_motor_send_current_high(&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_target_deg = 0;
|
|
|
|
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_voltage = yaw_speed_pid.OUT + spin_feedforward;
|
|
if (yaw_voltage > GIMBAL_YAW_SPEED_OUT_LIMIT)
|
|
yaw_voltage = GIMBAL_YAW_SPEED_OUT_LIMIT;
|
|
else if (yaw_voltage < -GIMBAL_YAW_SPEED_OUT_LIMIT)
|
|
yaw_voltage = -GIMBAL_YAW_SPEED_OUT_LIMIT;
|
|
|
|
/* Pitch 串级闭环,位置环反馈为 AHRS Roll(legacy 语义) */
|
|
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_voltage = pitch_speed_pid.OUT;
|
|
|
|
/* 发送:CAN2 0x1FF 槽1 = Yaw 电压;CAN1 0x1FF 槽2 = Pitch 电压,槽3 = 拨弹电流 */
|
|
dji_motor_send_current_high(&hcan2, (int16_t)yaw_voltage, 0, 0, 0);
|
|
dji_motor_send_current_high(&hcan1, 0, (int16_t)pitch_voltage, rammer_current_shared, 0);
|
|
}
|
|
|
|
void shoot_share_rammer_current(int16_t current)
|
|
{
|
|
rammer_current_shared = current;
|
|
}
|
|
|
|
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_current_high(&hcan2, 0, 0, 0, 0);
|
|
dji_motor_send_current_high(&hcan1, 0, 0, 0, 0);
|
|
dji_motor_send_current_low(&hcan1, 0, 0, 0, 0);
|
|
}
|