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

扫描模式小YAW不动

Browse Source
This commit is contained in:
Xiaocheng 2026-03-18 10:38:00 +08:00
parent b78aa02d9b
commit ead6c6e64e
4 changed files with 159 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
User/module/cmd.c
View File

@ -61,6 +61,7 @@ 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_right.x * 2.0f;
ctx->output.gimbal.cmd.delta_yaw_4310 = -ctx->input.rc.joy_right.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_right.y * 1.5f;
} }
@ -71,19 +72,19 @@ static void CMD_RC_BuildShootCmd(CMD_t *ctx) {
} else { } else {
ctx->output.shoot.cmd.mode = SHOOT_MODE_SAFE; ctx->output.shoot.cmd.mode = SHOOT_MODE_SAFE;
} }
/* 根据c拨杆控制射击 */ /* C拨杆选择发弹模式：上单发，中多发，下连发 */
switch (ctx->input.rc.sw[2]) { switch (ctx->input.rc.sw[2]) {
case CMD_SW_DOWN: case CMD_SW_DOWN:
ctx->output.shoot.cmd.mode = SHOOT_MODE_BURST; ctx->output.shoot.cmd.mode = SHOOT_MODE_CONTINUE;
break; break;
case CMD_SW_MID: case CMD_SW_MID:
ctx->output.shoot.cmd.mode = SHOOT_MODE_SINGLE; ctx->output.shoot.cmd.mode = SHOOT_MODE_BURST;
break; break;
case CMD_SW_UP: case CMD_SW_UP:
ctx->output.shoot.cmd.ready = SHOOT_MODE_SAFE; ctx->output.shoot.cmd.mode = SHOOT_MODE_SINGLE;
break;
default: default:
ctx->output.shoot.cmd.ready = false; ctx->output.shoot.cmd.mode = SHOOT_MODE_SAFE;
ctx->output.shoot.cmd.firecmd = false;
break; break;
} }
/* 根据D拨杆控制射击 */ /* 根据D拨杆控制射击 */
@ -108,18 +109,22 @@ static void CMD_RC_BuildShootCmd(CMD_t *ctx) {
case CMD_SW_UP: case CMD_SW_UP:
ctx->output.chassis.cmd.ctrl_mode = CHASSIS_MODE_RC; ctx->output.chassis.cmd.ctrl_mode = CHASSIS_MODE_RC;
ctx->output.gimbal.cmd.ctrl_mode = GIMBAL_MODE_REMOTE; ctx->output.gimbal.cmd.ctrl_mode = GIMBAL_MODE_REMOTE;
ctx->output.shoot.cmd.control_mode = SHOOT_REMOTE;
break; break;
case CMD_SW_MID: case CMD_SW_MID:
ctx->output.chassis.cmd.ctrl_mode = CHASSIS_MODE_DAOHANG; ctx->output.chassis.cmd.ctrl_mode = CHASSIS_MODE_DAOHANG;
ctx->output.gimbal.cmd.mode = GIMBAL_MODE_DAOHANG; ctx->output.gimbal.cmd.mode = GIMBAL_MODE_DAOHANG;
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_RELAX;
ctx->output.gimbal.cmd.ctrl_mode = GIMBAL_MODE_AI; ctx->output.gimbal.cmd.ctrl_mode = GIMBAL_MODE_AI;
ctx->output.shoot.cmd.control_mode = SHOOT_AI;
break; break;
default: default:
ctx->output.chassis.cmd.mode = CHASSIS_MODE_RELAX; ctx->output.chassis.cmd.mode = CHASSIS_MODE_RELAX;
ctx->output.gimbal.cmd.mode = GIMBAL_MODE_RELAX; ctx->output.gimbal.cmd.mode = GIMBAL_MODE_RELAX;
ctx->output.shoot.cmd.control_mode = SHOOT_REMOTE;
break; break;
} }

138
User/module/gimbal.c
View File

@ -54,13 +54,21 @@ static uint8_t g_scan_active = 0;
static float g_scan_pitch_center = 0.0f; static float g_scan_pitch_center = 0.0f;
static float g_scan_pitch_dir = 1.0f; 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边界锁存 */
#define GIMBAL_SCAN_YAW_SPEED (0.55f) #define GIMBAL_SCAN_SMALL_YAW_SPEED (0.20f)
#define GIMBAL_SCAN_PITCH_SPEED (0.35f) #define GIMBAL_SCAN_BIG_YAW_PERIOD_SEC (5.0f)
#define GIMBAL_SCAN_PITCH_SPEED (0.16f)
#define GIMBAL_SCAN_PITCH_AMPLITUDE (0.18f) #define GIMBAL_SCAN_PITCH_AMPLITUDE (0.18f)
#define GIMBAL_SCAN_YAW_EDGE_MARGIN (0.16f) #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_PITCH_EDGE_MARGIN (0.12f)
#define GIMBAL_SCAN_EDGE_SLOW_BAND (0.26f) #define GIMBAL_SCAN_EDGE_SLOW_BAND (0.26f)
#define GIMBAL_REMOTE_BIG_YAW_SPEED_GAIN (1.6f)
#define GIMBAL_REMOTE_EDGE_ASSIST_BAND (0.20f)
#define GIMBAL_YAW6020_INPUT_DEADBAND (0.0005f)
#define GIMBAL_CONTROL_DT_MAX (0.02f)
@ -98,6 +106,7 @@ static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) {
g->setpoint.eulr.pit = g->feedback.imu.eulr.rol; g->setpoint.eulr.pit = g->feedback.imu.eulr.rol;
g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw; g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw;
g->setpoint.yaw_4310 = g->feedback.motor.yaw_4310_motor_feedback.rotor_abs_angle;
g->mode = mode; g->mode = mode;
return GIMBAL_OK; return GIMBAL_OK;
@ -124,6 +133,8 @@ int8_t Gimbal_Init(Gimbal_t *g, Gimbal_Param_t *param,float target_freq)
} }
g->param = param; g->param = param;
g->mode = GIMBAL_MODE_RELAX; /* 设置默认模式 不输出*/ g->mode = GIMBAL_MODE_RELAX; /* 设置默认模式 不输出*/
g->last_wakeup = BSP_TIME_Get_us();
g->dt = 0.0f;
/*6020小yaw pid初始化*/ /*6020小yaw pid初始化*/
PID_Init(&g->pid.yaw_6020_angle,KPID_MODE_NO_D, target_freq,&g->param->pid.yaw_6020_motor_angle ); PID_Init(&g->pid.yaw_6020_angle,KPID_MODE_NO_D, target_freq,&g->param->pid.yaw_6020_motor_angle );
PID_Init(&g->pid.yaw_6020_omega,KPID_MODE_CALC_D, target_freq,&g->param->pid.yaw_6020_motor_omega ); PID_Init(&g->pid.yaw_6020_omega,KPID_MODE_CALC_D, target_freq,&g->param->pid.yaw_6020_motor_omega );
@ -144,6 +155,8 @@ int8_t Gimbal_Init(Gimbal_t *g, Gimbal_Param_t *param,float target_freq)
/*机械限位*/ /*机械限位*/
g->limit.yaw_6020.max = g->param->mech_zero.yaw_6020 + g->param->travel.yaw_6020; g->limit.yaw_6020.max = g->param->mech_zero.yaw_6020 + g->param->travel.yaw_6020;
g->limit.yaw_6020.min = g->param->mech_zero.yaw_6020 ; g->limit.yaw_6020.min = g->param->mech_zero.yaw_6020 ;
g->limit.yaw_4310.max = g->param->mech_zero.yaw_4310 + g->param->travel.yaw_4310;
g->limit.yaw_4310.min = g->param->mech_zero.yaw_4310 ;
g->limit.pitch_4310.max = g->param->mech_zero.pitch_4310 + g->param->travel.pitch_4310; g->limit.pitch_4310.max = g->param->mech_zero.pitch_4310 + g->param->travel.pitch_4310;
g->limit.pitch_4310.min = g->param->mech_zero.pitch_4310 ; g->limit.pitch_4310.min = g->param->mech_zero.pitch_4310 ;
@ -164,7 +177,6 @@ int8_t Gimbal_Init(Gimbal_t *g, Gimbal_Param_t *param,float target_freq)
* *
* \return 函数运行结果 * \return 函数运行结果
*/ */
int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal) int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal)
{ {
if (gimbal == NULL) if (gimbal == NULL)
@ -257,28 +269,59 @@ int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu){
return GIMBAL_ERR; // 参数错误 return GIMBAL_ERR; // 参数错误
} }
g->dt = (BSP_TIME_Get_us() - g->last_wakeup) / 1000000.0f; {
g->last_wakeup = BSP_TIME_Get_us(); uint64_t now_us = BSP_TIME_Get_us();
g->dt = (now_us - g->last_wakeup) / 1000000.0f;
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) { if (g_cmd->ctrl_mode == GIMBAL_MODE_SCAN) {
float yaw_speed_scale = 1.0f; float yaw_speed_scale = 1.0f;
float big_yaw_speed = 0.0f;
float pitch_speed_scale = 1.0f; float pitch_speed_scale = 1.0f;
float yaw_abs = g->feedback.motor.yaw_6020_motor_feedback.rotor_abs_angle; 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; float pitch_abs = g->feedback.motor.pitch_4310_motor_feedback.rotor_abs_angle;
if (!g_scan_active) { 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_active = 1;
g_scan_pitch_center = g->feedback.imu.eulr.pit; g_scan_pitch_center = g->feedback.imu.eulr.pit;
g_scan_pitch_dir = 1.0f; g_scan_pitch_dir = 1.0f;
g_scan_yaw_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 (yaw_abs >= g->limit.yaw_6020.max - GIMBAL_SCAN_YAW_EDGE_MARGIN) { 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; 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) { } else if (yaw_abs <= g->limit.yaw_6020.min + GIMBAL_SCAN_YAW_EDGE_MARGIN) {
g_scan_yaw_dir = 1.0f; 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) { if (pitch_abs >= g->limit.pitch_4310.max - GIMBAL_SCAN_PITCH_EDGE_MARGIN) {
@ -293,7 +336,7 @@ int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu){
float yaw_min_dist = (yaw_dist_to_edge_max < yaw_dist_to_edge_min) ? yaw_dist_to_edge_max : yaw_dist_to_edge_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 < GIMBAL_SCAN_EDGE_SLOW_BAND) {
if (yaw_min_dist < 0.0f) yaw_min_dist = 0.0f; if (yaw_min_dist < 0.0f) yaw_min_dist = 0.0f;
yaw_speed_scale = 0.25f + 0.75f * (yaw_min_dist / GIMBAL_SCAN_EDGE_SLOW_BAND); yaw_speed_scale = (yaw_min_dist / GIMBAL_SCAN_EDGE_SLOW_BAND);
} }
} }
@ -313,8 +356,15 @@ int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu){
g_scan_pitch_dir = 1.0f; g_scan_pitch_dir = 1.0f;
} }
g_cmd->delta_yaw_6020 = g_scan_yaw_dir * GIMBAL_SCAN_YAW_SPEED * yaw_speed_scale; 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; 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_vel = 0.0f;
g_cmd->yaw_accl = 0.0f; g_cmd->yaw_accl = 0.0f;
g_cmd->pit_vel = 0.0f; g_cmd->pit_vel = 0.0f;
@ -322,30 +372,37 @@ int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu){
} else { } else {
g_scan_active = 0; g_scan_active = 0;
g_scan_yaw_dir = 1.0f; g_scan_yaw_dir = 1.0f;
g_scan_yaw_edge_latch = 0;
} }
/*处理小yaw轴控制命令*/ /*处理小yaw轴控制命令*/
float delta_yaw_6020 = 10.0f * g_cmd->delta_yaw_6020 * g->dt; float delta_yaw_6020 = 10.0f * 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;
if(g->param->travel.yaw_6020 > 0) // 有限位才处理 /* 摇杆无输入时抑制微小噪声，避免setpoint慢漂 */
{ if (fabsf(delta_yaw_6020) < GIMBAL_YAW6020_INPUT_DEADBAND) {
/* 计算当前电机角度与IMU角度的偏差 */ delta_yaw_6020 = 0.0f;
/*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(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 (g->param->travel.yaw_6020 > 0.0f) {
/* 边界方向判定：到边界时只允许朝边界内侧运动 */
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;
@ -353,9 +410,30 @@ const float delta_min_6020 = CircleError(g->limit.yaw_6020.min,
/*获得小YAW的中点位置，如果小YAW大于中点的一定范围，大YAW开始运动， /*获得小YAW的中点位置，如果小YAW大于中点的一定范围，大YAW开始运动，
跟随小YAW向相同方向运动，将遥控数据进行分配*/ 跟随小YAW向相同方向运动，将遥控数据进行分配*/
/*小YAW中点*/ if (g_cmd->ctrl_mode == GIMBAL_MODE_REMOTE) {
float small_yaw_center_offset = g->param->mech_zero.yaw_6020 + g->param->travel.yaw_6020* 0.5f ; float delta_yaw_4310 = -GIMBAL_REMOTE_BIG_YAW_SPEED_GAIN * g_cmd->delta_yaw_4310 * g->dt;
g->setpoint.yaw_4310=small_yaw_center_offset; if (g->param->travel.yaw_6020 > 0.0f) {
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;
@ -433,7 +511,7 @@ switch (g->mode)
yaw_omega_set_point =PID_Calc(&g->pid.yaw_4310_angle,g->setpoint.yaw_4310, yaw_omega_set_point =PID_Calc(&g->pid.yaw_4310_angle,g->setpoint.yaw_4310,
g->feedback.motor.yaw_6020_motor_feedback.rotor_abs_angle,g->feedback.motor.yaw_6020_motor_feedback.rotor_speed,g->dt); g->feedback.motor.yaw_4310_motor_feedback.rotor_abs_angle,g->feedback.motor.yaw_4310_motor_feedback.rotor_speed,g->dt);
g->out.yaw_4310 = PID_Calc(&g->pid.yaw_4310_omega,yaw_omega_set_point, g->out.yaw_4310 = PID_Calc(&g->pid.yaw_4310_omega,yaw_omega_set_point,
g->feedback.motor.yaw_4310_motor_feedback.rotor_speed,0.0f,g->dt); g->feedback.motor.yaw_4310_motor_feedback.rotor_speed,0.0f,g->dt);
// g->out.yaw_4310 = yaw_omega_set_point; // 直接输出速度环目标值作为电机输出 // g->out.yaw_4310 = yaw_omega_set_point; // 直接输出速度环目标值作为电机输出

2
User/task/gimbal.c
View File

@ -66,8 +66,10 @@ 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) {
cmd_gimbal.mode = GIMBAL_MODE_ABSOLUTE;
cmd_gimbal.ctrl_mode = GIMBAL_MODE_SCAN; cmd_gimbal.ctrl_mode = GIMBAL_MODE_SCAN;
} else { } else {
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;
cmd_gimbal.set_yaw = ai_gimbal_result_cmd.gimbal_t.setpoint.yaw; cmd_gimbal.set_yaw = ai_gimbal_result_cmd.gimbal_t.setpoint.yaw;

47
User/task/shoot.c
View File

@ -41,6 +41,10 @@ Shoot_SetMode(&shoot,SHOOT_MODE_SINGLE);
shoot_ai_mcu_package.data.bullet_count = 0; shoot_ai_mcu_package.data.bullet_count = 0;
static bool last_fire_state = false; static bool last_fire_state = false;
bool current_fire_state = false; // 当前是否需要发射 bool current_fire_state = false; // 当前是否需要发射
static uint32_t ai_last_fire_tick = 0;
static const uint32_t ai_single_interval_ms = 220;
static const uint32_t ai_burst_interval_ms = 110;
static const uint32_t ai_continue_interval_ms = 0;
/* USER CODE INIT END */ /* USER CODE INIT END */
while (1) { while (1) {
@ -56,19 +60,38 @@ if(shoot_cmd.control_mode==SHOOT_REMOTE)
} }
else if(shoot_cmd.control_mode==SHOOT_AI) else if(shoot_cmd.control_mode==SHOOT_AI)
{ {
// shoot_cmd.mode = SHOOT_MODE_SINGLE; uint32_t now_ms = osKernelGetTickCount();
// if(shoot_ai_result_cmd.mode==0 || shoot_ai_result_cmd.mode==1) uint32_t interval_ms = ai_single_interval_ms;
// {
// shoot_cmd.firecmd=false; shoot_cmd.ready = true;
// shoot_cmd.ready=true;
if (shoot_ai_result_cmd.mode == 2) {
switch (shoot_cmd.mode) {
case SHOOT_MODE_CONTINUE:
interval_ms = ai_continue_interval_ms;
break;
case SHOOT_MODE_BURST:
interval_ms = ai_burst_interval_ms;
break;
case SHOOT_MODE_SINGLE:
default:
interval_ms = ai_single_interval_ms;
break;
}
if (interval_ms == 0) {
shoot_cmd.firecmd = true;
} else if ((now_ms - ai_last_fire_tick) >= interval_ms) {
shoot_cmd.firecmd = true;
ai_last_fire_tick = now_ms;
} else {
shoot_cmd.firecmd = false;
}
} else {
shoot_cmd.firecmd = false;
ai_last_fire_tick = now_ms;
}
// }
// else if(shoot_ai_result_cmd.mode==2)
// {
// shoot_cmd.firecmd=true;
// shoot_cmd.ready=true;
// current_fire_state = true;
// }
current_fire_state = shoot_cmd.firecmd; current_fire_state = shoot_cmd.firecmd;
} }