xiaocheng/Er_sentry
1
1
Fork 1
Code Issues 1 Pull Requests Actions Packages Projects Releases Wiki Activity

xianzai

Browse Source
This commit is contained in:
Xiaocheng 2026-03-20 00:39:21 +08:00
parent 2ad0d97189
commit c4a1a48770
12 changed files with 278 additions and 386 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

93
User/device/ai.c
View File

@ -4,6 +4,11 @@
#include "component/crc16.h" #include "component/crc16.h"
#include "device/referee.h" #include "device/referee.h"
/* ---- 接收缓冲 ---- */
static uint8_t ai_rx_buf[64]; /* DMA 接收缓冲，略大于 PackageAI_t */
static AI_result_t ai_parsed; /* 解析后的结果 */
static volatile uint8_t ai_data_ready = 0;
static int8_t Package_BuildAndVerify(uint8_t *raw, uint16_t len, uint8_t id) { static int8_t Package_BuildAndVerify(uint8_t *raw, uint16_t len, uint8_t id) {
uint16_t *crc = NULL; uint16_t *crc = NULL;
@ -22,53 +27,72 @@ static int8_t Package_BuildAndVerify(uint8_t *raw, uint16_t len, uint8_t id) {
return DEVICE_OK; return DEVICE_OK;
} }
static int8_t Package_Check(const uint8_t *raw, uint16_t len, uint8_t expected_id) { /* IDLE LINE 回调 —— 上位机发完一帧后总线空闲触发 */
if (raw == NULL || len <= sizeof(uint16_t)) { static void AI_IdleLineCallback(void) {
return DEVICE_ERR; UART_HandleTypeDef *huart = BSP_UART_GetHandle(BSP_UART_1);
/* 停止DMA，计算实际收到的字节数 */
HAL_UART_DMAStop(huart);
uint16_t rx_len = sizeof(ai_rx_buf) - __HAL_DMA_GET_COUNTER(huart->hdmarx);
/* 长度匹配才解析 */
if (rx_len == sizeof(PackageAI_t)) {
PackageAI_t *pkt = (PackageAI_t *)ai_rx_buf;
if (pkt->id == ID_AI) {
ai_parsed.mode = pkt->data.mode;
ai_parsed.gimbal_t.setpoint.yaw = pkt->data.yaw;
ai_parsed.gimbal_t.vel.yaw = pkt->data.yaw_vel;
ai_parsed.gimbal_t.accl.yaw = pkt->data.yaw_acc;
ai_parsed.gimbal_t.setpoint.pit = pkt->data.pitch;
ai_parsed.gimbal_t.vel.pit = pkt->data.pitch_vel;
ai_parsed.gimbal_t.accl.pit = pkt->data.pitch_acc;
ai_parsed.chassis_t.Vx = pkt->data.vx;
ai_parsed.chassis_t.Vy = pkt->data.vy;
ai_parsed.chassis_t.Vw = pkt->data.wz;
ai_data_ready = 1;
}
} }
if (raw[0] != expected_id) { /* 重启DMA接收，等待下一帧 */
return DEVICE_ERR; BSP_UART_Receive(BSP_UART_1, ai_rx_buf, sizeof(ai_rx_buf), true);
} }
if (CRC16_Verify(raw, len) != true) { /**
return DEVICE_ERR; * @brief 初始化AI通信，注册IDLE回调并启动DMA接收
} */
int8_t AI_Init(void) {
UART_HandleTypeDef *huart = BSP_UART_GetHandle(BSP_UART_1);
/* 注册IDLE LINE回调 */
BSP_UART_RegisterCallback(BSP_UART_1, BSP_UART_IDLE_LINE_CB,
(void (*)(void))AI_IdleLineCallback);
/* 使能IDLE中断 */
__HAL_UART_ENABLE_IT(huart, UART_IT_IDLE);
/* 启动第一次DMA接收 */
BSP_UART_Receive(BSP_UART_1, ai_rx_buf, sizeof(ai_rx_buf), true);
return DEVICE_OK; return DEVICE_OK;
} }
/**
int8_t AI_StartReceiving(PackageAI_t *ai) { * @brief 获取最新的AI数据（非阻塞，task循环中调用）
if (BSP_UART_Receive(BSP_UART_1,(uint8_t *)ai,sizeof(*ai), true)==HAL_OK) { * @return DEVICE_OK=有新数据已拷贝, DEVICE_ERR=无新数据
return DEVICE_OK;} */
int8_t AI_GetLatest(AI_result_t *result) {
if (result == NULL) {
return DEVICE_ERR; return DEVICE_ERR;
} }
if (!ai_data_ready) {
int8_t AI_Get_NUC(PackageAI_t *ai,AI_result_t* result) {
if (ai == NULL || result == NULL) {
return DEVICE_ERR; return DEVICE_ERR;
} }
*result = ai_parsed;
if (Package_Check((const uint8_t *)ai, sizeof(*ai), ID_AI) != DEVICE_OK) { ai_data_ready = 0;
return DEVICE_ERR;
}
result->mode=ai->data.mode;
result->gimbal_t.setpoint.yaw=ai->data.yaw;
result->gimbal_t.vel.yaw=ai->data.yaw_vel;
result->gimbal_t.accl.yaw=ai->data.yaw_acc;
result->gimbal_t.setpoint.pit=ai->data.pitch;
result->gimbal_t.vel.pit=ai->data.pitch_vel;
result->gimbal_t.accl.pit=ai->data.pitch_acc;
result->chassis_t.Vx=ai->data.vx;
result->chassis_t.Vy=ai->data.vy;
result->chassis_t.Vw=ai->data.wz;
return DEVICE_OK; return DEVICE_OK;
} }
/* ---- 以下发送函数不变 ---- */
int8_t MCU_Send(PackageMCU_t *mcu, Gimbal_feedback_t *motor, AHRS_Quaternion_t *quat) { int8_t MCU_Send(PackageMCU_t *mcu, Gimbal_feedback_t *motor, AHRS_Quaternion_t *quat) {
(void)quat; (void)quat;
@ -113,14 +137,9 @@ int8_t REF_StartSend(PackageReferee_t *referee) {
return DEVICE_ERR; return DEVICE_ERR;
} }
int8_t MCU_StartSend(PackageMCU_t *mcu) { int8_t MCU_StartSend(PackageMCU_t *mcu) {
if (BSP_UART_Transmit(BSP_UART_1, (uint8_t *)mcu, sizeof(*mcu), true) == HAL_OK) if (BSP_UART_Transmit(BSP_UART_1, (uint8_t *)mcu, sizeof(*mcu), true) == HAL_OK)
return DEVICE_OK; return DEVICE_OK;
return DEVICE_ERR; return DEVICE_ERR;
} }

5
User/device/ai.h
View File

@ -162,12 +162,13 @@ typedef struct __attribute__((packed)) {
}AI_result_t; //接收到的所有数据，来自NUC }AI_result_t; //接收到的所有数据，来自NUC
int8_t AI_Init(void);
int8_t AI_GetLatest(AI_result_t *result);
int8_t MCU_Send(PackageMCU_t* mcu,Gimbal_feedback_t* motor,AHRS_Quaternion_t* quat); int8_t MCU_Send(PackageMCU_t* mcu,Gimbal_feedback_t* motor,AHRS_Quaternion_t* quat);
int8_t REF_Send(PackageReferee_t *referee,Referee_RobotStatus_t* robot_status,Referee_GameStatus_t *game_status); int8_t REF_Send(PackageReferee_t *referee,Referee_RobotStatus_t* robot_status,Referee_GameStatus_t *game_status);
int8_t REF_StartSend(PackageReferee_t *referee); int8_t REF_StartSend(PackageReferee_t *referee);
int8_t MCU_StartSend(PackageMCU_t *mcu); int8_t MCU_StartSend(PackageMCU_t *mcu);
int8_t AI_Get_NUC(PackageAI_t *ai,AI_result_t* result);
int8_t AI_StartReceiving(PackageAI_t *ai);
#ifdef __cplusplus #ifdef __cplusplus
} }
#endif #endif

25
User/module/chassis.c
View File

@ -448,28 +448,8 @@ int8_t Chassis_Control(Chassis_t *c, Chassis_CMD_t *c_cmd,uint32_t now)
// 跟随云台模式 // 跟随云台模式
c->move_vec.Vx =-c_cmd->Vy; c->move_vec.Vx =-c_cmd->Vy;
c->move_vec.Vy =-c_cmd->Vx; c->move_vec.Vy =-c_cmd->Vx;
if (fabsf(c->move_vec.Vx) < CHASSIS_FOLLOW_TRANS_DEADBAND) c->move_vec.Vx = 0.0f; c->move_vec.Vw =PID_Calc(&c->pid.chassis_follow_gimbal_pid, 4.51735544f ,c->motorfeedback.gimbal_yaw_encoder.rotor_abs_angle, 0.0f, c->dt);
if (fabsf(c->move_vec.Vy) < CHASSIS_FOLLOW_TRANS_DEADBAND) c->move_vec.Vy = 0.0f;
{
const float follow_target = c->mech_zero_4310;
const float follow_err = CircleError(follow_target,
c->motorfeedback.gimbal_yaw_encoder.rotor_abs_angle,
M_2PI);
if (fabsf(follow_err) < CHASSIS_FOLLOW_LOCK_DEADBAND_RAD) {
c->move_vec.Vw = 0.0f;
PID_ResetIntegral(&c->pid.chassis_follow_gimbal_pid);
} else {
float follow_vw = PID_Calc(&c->pid.chassis_follow_gimbal_pid,
follow_target,
c->motorfeedback.gimbal_yaw_encoder.rotor_abs_angle,
0.0f,
c->dt);
if (fabsf(follow_vw) < CHASSIS_FOLLOW_VW_DEADBAND) {
follow_vw = 0.0f;
}
c->move_vec.Vw = follow_vw;
}
}
break; break;
@ -561,6 +541,7 @@ void Chassis_Setoutput(Chassis_t *c)
} }
// MOTOR_RM_SetOutput(&(c->param->motor_3508_param[0]), 1.0f); // MOTOR_RM_SetOutput(&(c->param->motor_3508_param[0]), 1.0f);
MOTOR_RM_Ctrl(&(c->param->motor_3508_param[0])); MOTOR_RM_Ctrl(&(c->param->motor_3508_param[0]));
MOTOR_RM_Ctrl(&(c->param->motor_3508_param[1]));
MOTOR_RM_Ctrl(&(c->param->motor_6020_param[0])); MOTOR_RM_Ctrl(&(c->param->motor_6020_param[0]));
// MOTOR_RM_Ctrl(&(c->param->motor_6020_param[3])); // MOTOR_RM_Ctrl(&(c->param->motor_6020_param[3]));

21
User/module/cmd.c
View File

@ -3,6 +3,7 @@
*/ */
#include "module/cmd.h" #include "module/cmd.h"
#include "bsp/time.h" #include "bsp/time.h"
#include "chassis.h"
#include <stdint.h> #include <stdint.h>
#include <string.h> #include <string.h>
@ -35,8 +36,8 @@ static void CMD_RC_BuildChassisCmd(CMD_t *ctx) {
} }
/* 左摇杆控制移动 */ /* 左摇杆控制移动 */
ctx->output.chassis.cmd.Vx = ctx->input.rc.joy_left.x; ctx->output.chassis.cmd.Vx = ctx->input.rc.joy_right.x;
ctx->output.chassis.cmd.Vy = ctx->input.rc.joy_left.y; ctx->output.chassis.cmd.Vy = ctx->input.rc.joy_right.y;
} }
/* 从RC输入生成云台命令 */ /* 从RC输入生成云台命令 */
@ -60,9 +61,9 @@ static void CMD_RC_BuildGimbalCmd(CMD_t *ctx) {
} }
/* 右摇杆控制云台 */ /* 右摇杆控制云台 */
ctx->output.gimbal.cmd.delta_yaw_6020 = -ctx->input.rc.joy_right.x * 2.0f; ctx->output.gimbal.cmd.delta_yaw_6020 = -ctx->input.rc.joy_left.x * 2.0f;
ctx->output.gimbal.cmd.delta_yaw_4310 = -ctx->input.rc.joy_right.x * 1.2f; ctx->output.gimbal.cmd.delta_yaw_4310 = -ctx->input.rc.joy_left.x * 1.2f;
ctx->output.gimbal.cmd.delta_pitch_4310 = -ctx->input.rc.joy_right.y * 1.5f; ctx->output.gimbal.cmd.delta_pitch_4310 = -ctx->input.rc.joy_left.y * 1.5f;
} }
/* 从RC输入生成射击命令 */ /* 从RC输入生成射击命令 */
@ -117,8 +118,8 @@ static void CMD_RC_BuildShootCmd(CMD_t *ctx) {
ctx->output.shoot.cmd.control_mode = SHOOT_REMOTE; ctx->output.shoot.cmd.control_mode = SHOOT_REMOTE;
break; break;
case CMD_SW_DOWN: case CMD_SW_DOWN:
ctx->output.chassis.cmd.ctrl_mode = CHASSIS_MODE_RELAX; ctx->output.chassis.cmd.ctrl_mode = CHASSIS_MODE_FOLLOW_GIMBAL;
ctx->output.gimbal.cmd.ctrl_mode = GIMBAL_MODE_AI; ctx->output.gimbal.cmd.ctrl_mode = GIMBAL_MODE_SCAN;
ctx->output.shoot.cmd.control_mode = SHOOT_AI; ctx->output.shoot.cmd.control_mode = SHOOT_AI;
break; break;
default: default:
@ -128,6 +129,12 @@ static void CMD_RC_BuildShootCmd(CMD_t *ctx) {
break; break;
} }
/* sw[5] (F拨杆): 下=纯AI自瞄(无扫描), 上/中=保持B拨杆的设定 */
if (ctx->input.rc.sw[5] == CMD_SW_DOWN) {
ctx->output.gimbal.cmd.ctrl_mode = GIMBAL_MODE_AI;
ctx->output.shoot.cmd.control_mode = SHOOT_AI;
}
} }
/* 从PC输入生成底盘命令 */ /* 从PC输入生成底盘命令 */

6
User/module/config.c
View File

@ -17,7 +17,7 @@ Config_Param_t config = {
.chassis = { .chassis = {
.motor_3508_param[0] = {BSP_CAN_1, 0x201, MOTOR_M3508, false, false}, .motor_3508_param[0] = {BSP_CAN_1, 0x201, MOTOR_M3508, false, false},
.motor_3508_param[1] = {BSP_CAN_1, 0x202, MOTOR_M3508, false, false}, .motor_3508_param[1] = {BSP_CAN_2, 0x204, MOTOR_M3508, false, false},
.motor_3508_param[2] = {BSP_CAN_1, 0x203, MOTOR_M3508, false, false}, .motor_3508_param[2] = {BSP_CAN_1, 0x203, MOTOR_M3508, false, false},
.motor_3508_param[3] = {BSP_CAN_1, 0x204, MOTOR_M3508, false, false}, .motor_3508_param[3] = {BSP_CAN_1, 0x204, MOTOR_M3508, false, false},
@ -247,7 +247,7 @@ Config_Param_t config = {
.pid.daohang_6020_motor_angle={ .pid.daohang_6020_motor_angle={
.k = 1.5f, .k = 1.5f,
.p = 1.7f, .p = 1.2f,
.i = 0.0f, .i = 0.0f,
.d = 0.0f, .d = 0.0f,
.i_limit = 1.0f, .i_limit = 1.0f,
@ -257,7 +257,7 @@ Config_Param_t config = {
}, },
.pid.daohang_4310_motor_angle={ .pid.daohang_4310_motor_angle={
.k = 2.0f, .k = 2.0f,
.p = 5.0f, .p = 2.0f,
.i = 1.0f, .i = 1.0f,
.d = 0.2f, .d = 0.2f,
.i_limit = 1.0f, .i_limit = 1.0f,

213
User/module/gimbal.c
View File

@ -56,20 +56,11 @@ static float g_scan_pitch_dir = 1.0f;
static float g_scan_yaw_dir = 1.0f; static float g_scan_yaw_dir = 1.0f;
static int8_t g_scan_yaw_edge_latch = 0; /* -1: min边界锁存, 1: max边界锁存 */ static int8_t g_scan_yaw_edge_latch = 0; /* -1: min边界锁存, 1: max边界锁存 */
#define GIMBAL_SCAN_SMALL_YAW_SPEED (0.40f) #define GIMBAL_SCAN_SMALL_YAW_SPEED (4.0f) /* 扫描模式小YAW旋转速度 (rad/s) */
#define GIMBAL_SCAN_BIG_YAW_PERIOD_SEC (5.0f) #define GIMBAL_SCAN_PITCH_SPEED (3.0f) /* 扫描模式pitch上下扫描速度 (rad/s) */
#define GIMBAL_SCAN_PITCH_SPEED (0.16f) #define GIMBAL_SCAN_PITCH_EDGE_MARGIN (0.03f) /* pitch接近限位多远开始反转 (rad) */
#define GIMBAL_SCAN_PITCH_AMPLITUDE (0.18f)
#define GIMBAL_SCAN_YAW_EDGE_MARGIN (0.16f)
#define GIMBAL_SCAN_YAW_EDGE_HYST (0.22f)
#define GIMBAL_SCAN_YAW_EDGE_RELEASE (0.28f)
#define GIMBAL_SCAN_PITCH_EDGE_MARGIN (0.12f)
#define GIMBAL_SCAN_EDGE_SLOW_BAND (0.26f)
#define GIMBAL_REMOTE_BIG_YAW_SPEED_GAIN (1.6f)
#define GIMBAL_REMOTE_SMALL_YAW_GAIN (16.0f)
#define GIMBAL_REMOTE_EDGE_ASSIST_BAND (0.20f)
#define GIMBAL_YAW6020_INPUT_DEADBAND (0.0005f)
#define GIMBAL_CONTROL_DT_MAX (0.02f)
@ -276,171 +267,40 @@ int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu){
uint64_t now_us = BSP_TIME_Get_us(); uint64_t now_us = BSP_TIME_Get_us();
g->dt = (now_us - g->last_wakeup) / 1000000.0f; g->dt = (now_us - g->last_wakeup) / 1000000.0f;
g->last_wakeup = now_us; g->last_wakeup = now_us;
if (g->dt < 0.0f || g->dt > GIMBAL_CONTROL_DT_MAX) {
g->dt = 0.002f;
}
} }
Gimbal_SetMode(g, g_cmd->mode); Gimbal_SetMode(g, g_cmd->mode);
if (g_cmd->ctrl_mode == GIMBAL_MODE_SCAN) {
float yaw_speed_scale = 1.0f;
float big_yaw_speed = 0.0f;
float pitch_speed_scale = 1.0f;
float yaw_abs = g->feedback.motor.yaw_6020_motor_feedback.rotor_abs_angle;
float pitch_abs = g->feedback.motor.pitch_4310_motor_feedback.rotor_abs_angle;
if (!g_scan_active) {
float yaw_mid = 0.5f * (g->limit.yaw_6020.max + g->limit.yaw_6020.min);
g_scan_active = 1;
g_scan_pitch_center = g->feedback.imu.eulr.rol;
g_scan_pitch_dir = 1.0f;
g_scan_yaw_dir = (yaw_abs >= yaw_mid) ? -1.0f : 1.0f;
g_scan_yaw_edge_latch = 0;
g->setpoint.yaw_4310 = g->feedback.motor.yaw_4310_motor_feedback.rotor_abs_angle;
}
if (GIMBAL_SCAN_BIG_YAW_PERIOD_SEC > 0.0f) {
/* 连续旋转：按周期计算角速度，5秒转一圈即 2PI/5 */
big_yaw_speed = M_2PI / GIMBAL_SCAN_BIG_YAW_PERIOD_SEC;
}
if (g_scan_yaw_edge_latch == 1) {
g_scan_yaw_dir = -1.0f;
if (yaw_abs < g->limit.yaw_6020.max - GIMBAL_SCAN_YAW_EDGE_RELEASE) {
g_scan_yaw_edge_latch = 0;
}
} else if (g_scan_yaw_edge_latch == -1) {
g_scan_yaw_dir = 1.0f;
if (yaw_abs > g->limit.yaw_6020.min + GIMBAL_SCAN_YAW_EDGE_RELEASE) {
g_scan_yaw_edge_latch = 0;
}
} else if (yaw_abs >= g->limit.yaw_6020.max - GIMBAL_SCAN_YAW_EDGE_MARGIN) {
g_scan_yaw_dir = -1.0f;
g_scan_yaw_edge_latch = 1;
} else if (yaw_abs <= g->limit.yaw_6020.min + GIMBAL_SCAN_YAW_EDGE_MARGIN) {
g_scan_yaw_dir = 1.0f;
g_scan_yaw_edge_latch = -1;
} else if (g_scan_yaw_dir > 0.0f && yaw_abs > g->limit.yaw_6020.max - GIMBAL_SCAN_YAW_EDGE_HYST) {
g_scan_yaw_dir = -1.0f;
} else if (g_scan_yaw_dir < 0.0f && yaw_abs < g->limit.yaw_6020.min + GIMBAL_SCAN_YAW_EDGE_HYST) {
g_scan_yaw_dir = 1.0f;
}
if (pitch_abs >= g->limit.pitch_4310.max - GIMBAL_SCAN_PITCH_EDGE_MARGIN) {
g_scan_pitch_dir = -1.0f;
} else if (pitch_abs <= g->limit.pitch_4310.min + GIMBAL_SCAN_PITCH_EDGE_MARGIN) {
g_scan_pitch_dir = 1.0f;
}
{
float yaw_dist_to_edge_max = g->limit.yaw_6020.max - yaw_abs;
float yaw_dist_to_edge_min = yaw_abs - g->limit.yaw_6020.min;
float yaw_min_dist = (yaw_dist_to_edge_max < yaw_dist_to_edge_min) ? yaw_dist_to_edge_max : yaw_dist_to_edge_min;
if (yaw_min_dist < GIMBAL_SCAN_EDGE_SLOW_BAND) {
if (yaw_min_dist < 0.0f) yaw_min_dist = 0.0f;
yaw_speed_scale = (yaw_min_dist / GIMBAL_SCAN_EDGE_SLOW_BAND);
}
}
{
float pitch_dist_to_edge_max = g->limit.pitch_4310.max - pitch_abs;
float pitch_dist_to_edge_min = pitch_abs - g->limit.pitch_4310.min;
float pitch_min_dist = (pitch_dist_to_edge_max < pitch_dist_to_edge_min) ? pitch_dist_to_edge_max : pitch_dist_to_edge_min;
if (pitch_min_dist < GIMBAL_SCAN_EDGE_SLOW_BAND) {
if (pitch_min_dist < 0.0f) pitch_min_dist = 0.0f;
pitch_speed_scale = 0.25f + 0.75f * (pitch_min_dist / GIMBAL_SCAN_EDGE_SLOW_BAND);
}
}
if (g->feedback.imu.eulr.rol >= g_scan_pitch_center + GIMBAL_SCAN_PITCH_AMPLITUDE) {
g_scan_pitch_dir = -1.0f;
} else if (g->feedback.imu.eulr.rol <= g_scan_pitch_center - GIMBAL_SCAN_PITCH_AMPLITUDE) {
g_scan_pitch_dir = 1.0f;
}
g_cmd->delta_yaw_6020 = g_scan_yaw_dir * GIMBAL_SCAN_SMALL_YAW_SPEED * yaw_speed_scale;
g_cmd->delta_pitch_4310 = g_scan_pitch_dir * GIMBAL_SCAN_PITCH_SPEED * pitch_speed_scale;
if (g->param->travel.yaw_4310 > 0.0f) {
g->setpoint.yaw_4310 += big_yaw_speed * g->dt;
if (g->setpoint.yaw_4310 > g->limit.yaw_4310.max) g->setpoint.yaw_4310 = g->limit.yaw_4310.max;
if (g->setpoint.yaw_4310 < g->limit.yaw_4310.min) g->setpoint.yaw_4310 = g->limit.yaw_4310.min;
} else {
CircleAdd(&(g->setpoint.yaw_4310), big_yaw_speed * g->dt, M_2PI);
}
g_cmd->yaw_vel = 0.0f;
g_cmd->yaw_accl = 0.0f;
g_cmd->pit_vel = 0.0f;
g_cmd->pit_accl = 0.0f;
} else {
g_scan_active = 0;
g_scan_yaw_dir = 1.0f;
g_scan_yaw_edge_latch = 0;
}
/*处理小yaw轴控制命令*/ /*处理小yaw轴控制命令*/
float yaw_6020_gain = 10.0f; float delta_yaw_6020 = 5.0f*g_cmd->delta_yaw_6020 * g->dt;
if (g_cmd->ctrl_mode == GIMBAL_MODE_REMOTE) {
yaw_6020_gain = GIMBAL_REMOTE_SMALL_YAW_GAIN;
}
float delta_yaw_6020 = yaw_6020_gain * g_cmd->delta_yaw_6020 * g->dt;
float yaw_abs_now = g->feedback.motor.yaw_6020_motor_feedback.rotor_abs_angle;
float yaw_abs_target = yaw_abs_now;
/* 摇杆无输入时抑制微小噪声，避免setpoint慢漂 */ if(g->param->travel.yaw_6020 > 0) // 有限位才处理
if (fabsf(delta_yaw_6020) < GIMBAL_YAW6020_INPUT_DEADBAND) { {
delta_yaw_6020 = 0.0f; /* 计算当前电机角度与IMU角度的偏差 */
} float motor_imu_offset_6020 = g->feedback.motor.yaw_6020_motor_feedback.rotor_abs_angle - g->feedback.imu.eulr.yaw;
/* 处理跨越±π的情况 */
if (motor_imu_offset_6020 > M_PI) motor_imu_offset_6020 -= M_2PI;
if (motor_imu_offset_6020 < -M_PI) motor_imu_offset_6020 += M_2PI;
/*计算限位距离*/
const float delta_max_6020 = CircleError(g->limit.yaw_6020.max,
(g->setpoint.eulr.yaw + motor_imu_offset_6020 + delta_yaw_6020), M_2PI);
const float delta_min_6020 = CircleError(g->limit.yaw_6020.min,
(g->setpoint.eulr.yaw + motor_imu_offset_6020 + delta_yaw_6020), M_2PI);
if (g->param->travel.yaw_6020 > 0.0f) { if(delta_yaw_6020 > delta_max_6020) delta_yaw_6020 = delta_max_6020;
/* 边界方向判定：到边界时只允许朝边界内侧运动 */ if(delta_yaw_6020 < delta_min_6020) delta_yaw_6020 = delta_min_6020;
if (yaw_abs_now >= g->limit.yaw_6020.max - GIMBAL_SCAN_YAW_EDGE_MARGIN && delta_yaw_6020 > 0.0f) {
delta_yaw_6020 = 0.0f;
} }
if (yaw_abs_now <= g->limit.yaw_6020.min + GIMBAL_SCAN_YAW_EDGE_MARGIN && delta_yaw_6020 < 0.0f) {
delta_yaw_6020 = 0.0f;
}
yaw_abs_target = yaw_abs_now + delta_yaw_6020;
if (yaw_abs_target > g->limit.yaw_6020.max) yaw_abs_target = g->limit.yaw_6020.max;
if (yaw_abs_target < g->limit.yaw_6020.min) yaw_abs_target = g->limit.yaw_6020.min;
/* 直接用允许的电机角增量映射到欧拉角目标，避免CircleAdd累积导致回弹 */
g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw + (yaw_abs_target - yaw_abs_now);
} else {
CircleAdd(&(g->setpoint.eulr.yaw), delta_yaw_6020, M_2PI); CircleAdd(&(g->setpoint.eulr.yaw), delta_yaw_6020, M_2PI);
}
// g->setpoint.eulr.yaw = g->setpoint.NUC_Yaw; // g->setpoint.eulr.yaw = g->setpoint.NUC_Yaw;
/*处理大yaw控制命令，软件限位 - 使用电机绝对角度*/ /*处理大yaw控制命令，软件限位 - 使用电机绝对角度*/
/*获得小YAW的中点位置，如果小YAW大于中点的一定范围，大YAW开始运动， /*获得小YAW的中点位置，如果小YAW大于中点的一定范围，大YAW开始运动，
跟随小YAW向相同方向运动，将遥控数据进行分配*/ 跟随小YAW向相同方向运动，将遥控数据进行分配*/
if (g_cmd->ctrl_mode == GIMBAL_MODE_REMOTE) { /*小YAW中点*/
float delta_yaw_4310 = -GIMBAL_REMOTE_BIG_YAW_SPEED_GAIN * g_cmd->delta_yaw_4310 * g->dt; float small_yaw_center_offset = g->param->mech_zero.yaw_6020 + g->param->travel.yaw_6020* 0.5f ;
if (g->param->travel.yaw_6020 > 0.0f) { g->setpoint.yaw_4310=small_yaw_center_offset;
float yaw_abs_now = g->feedback.motor.yaw_6020_motor_feedback.rotor_abs_angle;
float dist_to_max = g->limit.yaw_6020.max - yaw_abs_now;
float dist_to_min = yaw_abs_now - g->limit.yaw_6020.min;
float edge_assist = 1.0f;
if (g_cmd->delta_yaw_6020 > 0.0f && dist_to_max < GIMBAL_REMOTE_EDGE_ASSIST_BAND) {
edge_assist += (GIMBAL_REMOTE_EDGE_ASSIST_BAND - dist_to_max) / GIMBAL_REMOTE_EDGE_ASSIST_BAND;
} else if (g_cmd->delta_yaw_6020 < 0.0f && dist_to_min < GIMBAL_REMOTE_EDGE_ASSIST_BAND) {
edge_assist += (GIMBAL_REMOTE_EDGE_ASSIST_BAND - dist_to_min) / GIMBAL_REMOTE_EDGE_ASSIST_BAND;
}
if (edge_assist < 1.0f) edge_assist = 1.0f;
if (edge_assist > 2.0f) edge_assist = 2.0f;
delta_yaw_4310 *= edge_assist;
}
if (g->param->travel.yaw_4310 > 0.0f) {
g->setpoint.yaw_4310 += delta_yaw_4310;
if (g->setpoint.yaw_4310 > g->limit.yaw_4310.max) g->setpoint.yaw_4310 = g->limit.yaw_4310.max;
if (g->setpoint.yaw_4310 < g->limit.yaw_4310.min) g->setpoint.yaw_4310 = g->limit.yaw_4310.min;
} else {
CircleAdd(&(g->setpoint.yaw_4310), delta_yaw_4310, M_2PI);
}
}
/*处理大pitch控制命令*/ /*处理大pitch控制命令*/
float delta_pitch_4310 = 8.0f*g_cmd->delta_pitch_4310*g->dt; float delta_pitch_4310 = 8.0f*g_cmd->delta_pitch_4310*g->dt;
@ -479,8 +339,28 @@ switch (g_cmd->ctrl_mode) {
break; break;
case GIMBAL_MODE_SCAN: case GIMBAL_MODE_SCAN:
g->setpoint.eulr.yaw = g->setpoint.eulr.yaw; {
g->setpoint.eulr.pit = g->setpoint.eulr.pit; /* 扫描模式：小YAW持续旋转带动大YAW转动，pitch在限位间来回扫描 */
/* --- 小YAW持续旋转 --- */
float scan_yaw_delta = GIMBAL_SCAN_SMALL_YAW_SPEED * g->dt;
CircleAdd(&(g->setpoint.eulr.yaw), scan_yaw_delta, M_2PI);
/* --- Pitch增量式来回扫描 --- */
float pitch_pos = g->feedback.motor.pitch_4310_motor_feedback.rotor_abs_angle;
/* 到达下限位(1.83)附近，反转向上 */
if (pitch_pos >= (g->limit.pitch_4310.max - GIMBAL_SCAN_PITCH_EDGE_MARGIN)) {
g_scan_pitch_dir = -1.0f;
}
/* 到达上限位(0.97)附近，反转向下 */
if (pitch_pos <= 1.20f) {
g_scan_pitch_dir = 1.0f;
}
float scan_pitch_delta = GIMBAL_SCAN_PITCH_SPEED * g_scan_pitch_dir * g->dt;
CircleAdd(&(g->setpoint.eulr.pit), scan_pitch_delta, M_2PI);
}
break; break;
case GIMBAL_MODE_AI: case GIMBAL_MODE_AI:
@ -603,3 +483,4 @@ void Gimbal_Output(Gimbal_t *g)

1
User/module/gimbal.h
View File

@ -125,6 +125,7 @@ typedef struct {
float delta_pitch_4310; float delta_pitch_4310;
float delta_yaw_6020; float delta_yaw_6020;
GIMBAL_Ctrl_mode_t ctrl_mode; GIMBAL_Ctrl_mode_t ctrl_mode;
uint8_t scan_enable; /* 1=AI无目标时扫描, 0=AI无目标时保持当前姿态 */
float set_yaw; /*自瞄YAW目标值*/ float set_yaw; /*自瞄YAW目标值*/
float set_pitch;/*自瞄PITCH目标值*/ float set_pitch;/*自瞄PITCH目标值*/
float yaw_vel; /*自瞄YAW角速度*/ float yaw_vel; /*自瞄YAW角速度*/

48
User/task/ai.c
View File

@ -1,6 +1,5 @@
/* /*
ai Task ai Task
*/ */
/* Includes ----------------------------------------------------------------- */ /* Includes ----------------------------------------------------------------- */
@ -10,60 +9,55 @@
#include "module/gimbal.h" #include "module/gimbal.h"
/* USER INCLUDE END */ /* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */ /* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */
PackageAI_t ai;
PackageMCU_t mcu; PackageMCU_t mcu;
PackageReferee_t ref_pkg; PackageReferee_t ref_pkg;
AI_result_t ai_result; AI_result_t ai_result;
Referee_ForAI_t ref_for_ai; Referee_ForAI_t ref_for_ai;
AHRS_Quaternion_t quat; AHRS_Quaternion_t quat;
Gimbal_feedback_t gimbal_motor; Gimbal_feedback_t gimbal_motor;
PackageMCU_t shoot_mcu_package; /* 新增的 ai 数据包 主要是给自瞄发送射击数量*/ PackageMCU_t shoot_mcu_package;
/* USER STRUCT END */
/* Private function --------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */ /* Exported functions ------------------------------------------------------- */
void Task_ai(void *argument) { void Task_ai(void *argument) {
(void)argument; /* 未使用argument，消除警告 */ (void)argument;
/* 计算任务运行到指定频率需要等待的tick数 */
const uint32_t delay_tick = osKernelGetTickFreq() / AI_FREQ; const uint32_t delay_tick = osKernelGetTickFreq() / AI_FREQ;
osDelay(AI_INIT_DELAY);
osDelay(AI_INIT_DELAY); /* 延时一段时间再开启任务 */ /* 初始化AI通信：注册IDLE回调 + 启动DMA接收 */
AI_Init();
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ uint32_t tick = osKernelGetTickCount();
/* USER CODE INIT BEGIN */
/* USER CODE INIT END */
while (1) { while (1) {
tick += delay_tick; /* 计算下一个唤醒时刻 */ tick += delay_tick;
/* USER CODE BEGIN */
/* 获取其他任务传来的数据 */
osMessageQueueGet(task_runtime.msgq.gimbal.ai.quat, &quat, NULL, 0); osMessageQueueGet(task_runtime.msgq.gimbal.ai.quat, &quat, NULL, 0);
osMessageQueueGet(task_runtime.msgq.gimbal.ai.feedback, &gimbal_motor, NULL, 0); osMessageQueueGet(task_runtime.msgq.gimbal.ai.feedback, &gimbal_motor, NULL, 0);
osMessageQueueGet(task_runtime.msgq.shoot.ai.s_cmd_ai_bool_count, &shoot_mcu_package, NULL, 0); osMessageQueueGet(task_runtime.msgq.shoot.ai.s_cmd_ai_bool_count, &shoot_mcu_package, NULL, 0);
osMessageQueueGet(task_runtime.msgq.referee.ai, &ref_for_ai, NULL, 0); osMessageQueueGet(task_runtime.msgq.referee.ai, &ref_for_ai, NULL, 0);
/* 发送MCU数据给上位机 */
MCU_Send(&mcu, &gimbal_motor, &quat); MCU_Send(&mcu, &gimbal_motor, &quat);
mcu.data.bullet_count=shoot_mcu_package.data.bullet_count; /* 从 shoot 任务获取射击数量并放入 mcu 数据包中 */ mcu.data.bullet_count = shoot_mcu_package.data.bullet_count;
MCU_StartSend(&mcu); MCU_StartSend(&mcu);
if (REF_Send(&ref_pkg, &ref_for_ai.robot_status, &ref_for_ai.game_status) == DEVICE_OK) { /* 发送裁判系统数据给上位机 */
if (REF_Send(&ref_pkg, &ref_for_ai.robot_status, &ref_for_ai.game_status) == 0) {
REF_StartSend(&ref_pkg); REF_StartSend(&ref_pkg);
} }
AI_StartReceiving(&ai); /* 非阻塞获取上位机最新数据（IDLE回调已在中断中解析） */
AI_Get_NUC(&ai,&ai_result); AI_GetLatest(&ai_result);
/* 分发AI结果给各任务 */
osMessageQueueReset(task_runtime.msgq.gimbal.ai.g_cmd); osMessageQueueReset(task_runtime.msgq.gimbal.ai.g_cmd);
osMessageQueuePut(task_runtime.msgq.gimbal.ai.g_cmd, &ai_result, 0, 0); osMessageQueuePut(task_runtime.msgq.gimbal.ai.g_cmd, &ai_result, 0, 0);
osMessageQueuePut(task_runtime.msgq.shoot.ai.s_cmd, &ai_result, 0, 0); osMessageQueuePut(task_runtime.msgq.shoot.ai.s_cmd, &ai_result, 0, 0);
osMessageQueuePut(task_runtime.msgq.navi.c_cmd, &ai_result, 0, 0); /* 将 ai_result 中的射击命令布尔值发送给 shoot 任务 */ osMessageQueuePut(task_runtime.msgq.navi.c_cmd, &ai_result, 0, 0);
/* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
}
osDelayUntil(tick);
}
} }

2
User/task/chassis.c
View File

@ -49,7 +49,7 @@ chassis_init(&chassis,&Config_GetRobotParam()->chassis,CHASSIS_FREQ);
osMessageQueueGet(task_runtime.msgq.navi.c_cmd, &c_cmd_ai_result, NULL, 0); osMessageQueueGet(task_runtime.msgq.navi.c_cmd, &c_cmd_ai_result, NULL, 0);
osMessageQueueGet(task_runtime.msgq.gimbal.yaw6020,&chassis.motorfeedback.gimbal_yaw_encoder,NULL,0); osMessageQueueGet(task_runtime.msgq.gimbal.yaw4310,&chassis.motorfeedback.gimbal_yaw_encoder,NULL,0);
/*接受cmd任务数据*/ /*接受cmd任务数据*/
if(osMessageQueueGet(task_runtime.msgq.chassis.cmd, &cmd_chassis, NULL, 0)==osOK) if(osMessageQueueGet(task_runtime.msgq.chassis.cmd, &cmd_chassis, NULL, 0)==osOK)
{ {

12
User/task/gimbal.c
View File

@ -66,9 +66,15 @@ osMessageQueueGet(task_runtime.msgq.gimbal.cmd, &cmd_gimbal, NULL, 0);
if (cmd_gimbal.ctrl_mode == GIMBAL_MODE_AI) { if (cmd_gimbal.ctrl_mode == GIMBAL_MODE_AI) {
if (ai_gimbal_result_cmd.mode == 0) { if (ai_gimbal_result_cmd.mode == 0) {
/* AI无目标 */
cmd_gimbal.mode = GIMBAL_MODE_ABSOLUTE; cmd_gimbal.mode = GIMBAL_MODE_ABSOLUTE;
cmd_gimbal.ctrl_mode = GIMBAL_MODE_SCAN; if (cmd_gimbal.scan_enable) {
cmd_gimbal.ctrl_mode = GIMBAL_MODE_SCAN; /* 有扫描：回退到扫描模式 */
} else { } else {
cmd_gimbal.ctrl_mode = GIMBAL_MODE_REMOTE; /* 无扫描：保持当前姿态不动 */
}
} else {
/* AI有目标，填充自瞄数据 */
cmd_gimbal.mode = GIMBAL_MODE_ABSOLUTE; cmd_gimbal.mode = GIMBAL_MODE_ABSOLUTE;
cmd_gimbal.ctrl_mode = GIMBAL_MODE_AI; cmd_gimbal.ctrl_mode = GIMBAL_MODE_AI;
cmd_gimbal.set_pitch = ai_gimbal_result_cmd.gimbal_t.setpoint.pit; cmd_gimbal.set_pitch = ai_gimbal_result_cmd.gimbal_t.setpoint.pit;
@ -82,9 +88,9 @@ if (cmd_gimbal.ctrl_mode == GIMBAL_MODE_AI) {
Gimbal_UpdateFeedback(&gimbal); Gimbal_UpdateFeedback(&gimbal);
osMessageQueueReset(task_runtime.msgq.gimbal.yaw6020); osMessageQueueReset(task_runtime.msgq.gimbal.yaw4310);
/* 底盘跟随统一使用大YAW反馈，避免跟随锁到小YAW */ /* 底盘跟随统一使用大YAW反馈，避免跟随锁到小YAW */
osMessageQueuePut(task_runtime.msgq.gimbal.yaw6020, &gimbal.feedback.motor.yaw_4310_motor_feedback, 0, 0); osMessageQueuePut(task_runtime.msgq.gimbal.yaw4310, &gimbal.feedback.motor.yaw_4310_motor_feedback, 0, 0);
osMessageQueueReset(task_runtime.msgq.gimbal.ai.feedback); osMessageQueueReset(task_runtime.msgq.gimbal.ai.feedback);
osMessageQueuePut(task_runtime.msgq.gimbal.ai.feedback, &gimbal.feedback, 0, 0); osMessageQueuePut(task_runtime.msgq.gimbal.ai.feedback, &gimbal.feedback, 0, 0);

1
User/task/init.c
View File

@ -60,6 +60,7 @@ void Task_Init(void *argument) {
task_runtime.msgq.gimbal.imu = osMessageQueueNew(2u, sizeof(Gimbal_IMU_t), NULL); task_runtime.msgq.gimbal.imu = osMessageQueueNew(2u, sizeof(Gimbal_IMU_t), NULL);
task_runtime.msgq.gimbal.cmd = osMessageQueueNew(2u, sizeof(Gimbal_CMD_t), NULL); task_runtime.msgq.gimbal.cmd = osMessageQueueNew(2u, sizeof(Gimbal_CMD_t), NULL);
task_runtime.msgq.gimbal.yaw6020 = osMessageQueueNew(2u, sizeof(MOTOR_Feedback_t), NULL); task_runtime.msgq.gimbal.yaw6020 = osMessageQueueNew(2u, sizeof(MOTOR_Feedback_t), NULL);
task_runtime.msgq.gimbal.yaw4310 = osMessageQueueNew(2u, sizeof(MOTOR_Feedback_t), NULL);
task_runtime.msgq.shoot.cmd = osMessageQueueNew(2u, sizeof(Shoot_CMD_t),NULL); task_runtime.msgq.shoot.cmd = osMessageQueueNew(2u, sizeof(Shoot_CMD_t),NULL);
task_runtime.msgq.gimbal.ai.feedback = osMessageQueueNew(2u, sizeof(Gimbal_feedback_t),NULL); task_runtime.msgq.gimbal.ai.feedback = osMessageQueueNew(2u, sizeof(Gimbal_feedback_t),NULL);
task_runtime.msgq.gimbal.ai.g_cmd = osMessageQueueNew(2u, sizeof(AI_result_t),NULL); task_runtime.msgq.gimbal.ai.g_cmd = osMessageQueueNew(2u, sizeof(AI_result_t),NULL);

1
User/task/user_task.h
View File

@ -87,6 +87,7 @@ typedef struct {
osMessageQueueId_t imu; osMessageQueueId_t imu;
osMessageQueueId_t cmd; osMessageQueueId_t cmd;
osMessageQueueId_t yaw6020; /* 新增的 yaw_6020 消息队列 主要是给底盘传6020位置的反馈*/ osMessageQueueId_t yaw6020; /* 新增的 yaw_6020 消息队列 主要是给底盘传6020位置的反馈*/
osMessageQueueId_t yaw4310; /* 大YAW 4310反馈，给底盘跟随用 */
struct{ struct{
osMessageQueueId_t quat; osMessageQueueId_t quat;
osMessageQueueId_t feedback; osMessageQueueId_t feedback;