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

添加了一个云台搜寻模式

Browse Source
This commit is contained in:
Xiaocheng 2026-03-18 02:04:02 +08:00
parent f121d4057a
commit b78aa02d9b
3 changed files with 92 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

79
User/module/gimbal.c
View File

@ -50,6 +50,18 @@ static double poly(double x) {
return 0.420757*pow(x,3) + -2.27122*pow(x,2) + 4.13016*x + -1.98984; return 0.420757*pow(x,3) + -2.27122*pow(x,2) + 4.13016*x + -1.98984;
} }
static uint8_t g_scan_active = 0;
static float g_scan_pitch_center = 0.0f;
static float g_scan_pitch_dir = 1.0f;
static float g_scan_yaw_dir = 1.0f;
#define GIMBAL_SCAN_YAW_SPEED (0.55f)
#define GIMBAL_SCAN_PITCH_SPEED (0.35f)
#define GIMBAL_SCAN_PITCH_AMPLITUDE (0.18f)
#define GIMBAL_SCAN_YAW_EDGE_MARGIN (0.16f)
#define GIMBAL_SCAN_PITCH_EDGE_MARGIN (0.12f)
#define GIMBAL_SCAN_EDGE_SLOW_BAND (0.26f)
/** /**
@ -250,6 +262,68 @@ int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu){
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 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) {
g_scan_active = 1;
g_scan_pitch_center = g->feedback.imu.eulr.pit;
g_scan_pitch_dir = 1.0f;
g_scan_yaw_dir = 1.0f;
}
if (yaw_abs >= g->limit.yaw_6020.max - GIMBAL_SCAN_YAW_EDGE_MARGIN) {
g_scan_yaw_dir = -1.0f;
} else if (yaw_abs <= g->limit.yaw_6020.min + GIMBAL_SCAN_YAW_EDGE_MARGIN) {
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 = 0.25f + 0.75f * (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.pit >= g_scan_pitch_center + GIMBAL_SCAN_PITCH_AMPLITUDE) {
g_scan_pitch_dir = -1.0f;
} else if (g->feedback.imu.eulr.pit <= 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_YAW_SPEED * yaw_speed_scale;
g_cmd->delta_pitch_4310 = g_scan_pitch_dir * GIMBAL_SCAN_PITCH_SPEED * pitch_speed_scale;
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;
}
/*处理小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;
@ -319,6 +393,11 @@ switch (g_cmd->ctrl_mode) {
g->setpoint.eulr.pit = g->setpoint.eulr.pit; g->setpoint.eulr.pit = g->setpoint.eulr.pit;
break; break;
case GIMBAL_MODE_SCAN:
g->setpoint.eulr.yaw = g->setpoint.eulr.yaw;
g->setpoint.eulr.pit = g->setpoint.eulr.pit;
break;
case GIMBAL_MODE_AI: case GIMBAL_MODE_AI:
g->setpoint.eulr.yaw = g_cmd->set_yaw; g->setpoint.eulr.yaw = g_cmd->set_yaw;
g->setpoint.eulr.pit = g_cmd->set_pitch; g->setpoint.eulr.pit = g_cmd->set_pitch;

1
User/module/gimbal.h
View File

@ -116,6 +116,7 @@ extern "C"
{ {
GIMBAL_MODE_REMOTE, GIMBAL_MODE_REMOTE,
GIMBAL_MODE_AI, GIMBAL_MODE_AI,
GIMBAL_MODE_SCAN,
}GIMBAL_Ctrl_mode_t; }GIMBAL_Ctrl_mode_t;
typedef struct { typedef struct {

36
User/task/gimbal.c
View File

@ -64,31 +64,19 @@ osMessageQueueGet(task_runtime.msgq.gimbal.ai.g_cmd,&ai_gimbal_result_cmd,NULL,
osMessageQueueGet(task_runtime.msgq.gimbal.cmd, &cmd_gimbal, NULL, 0); osMessageQueueGet(task_runtime.msgq.gimbal.cmd, &cmd_gimbal, NULL, 0);
if(ai_gimbal_result_cmd.mode==0){ if (cmd_gimbal.ctrl_mode == GIMBAL_MODE_AI) {
cmd_gimbal.set_pitch =gimbal_imu.eulr.pit; if (ai_gimbal_result_cmd.mode == 0) {
cmd_gimbal.set_yaw=gimbal_imu.eulr.yaw; cmd_gimbal.ctrl_mode = GIMBAL_MODE_SCAN;
} else {
cmd_gimbal.ctrl_mode = GIMBAL_MODE_AI;
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.yaw_vel = ai_gimbal_result_cmd.gimbal_t.vel.yaw;
cmd_gimbal.yaw_accl = ai_gimbal_result_cmd.gimbal_t.accl.yaw;
cmd_gimbal.pit_vel = ai_gimbal_result_cmd.gimbal_t.vel.pit;
cmd_gimbal.pit_accl = ai_gimbal_result_cmd.gimbal_t.accl.pit;
}
} }
if(ai_gimbal_result_cmd.mode==1)
{
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.yaw_vel=ai_gimbal_result_cmd.gimbal_t.vel.yaw;
cmd_gimbal.yaw_accl=ai_gimbal_result_cmd.gimbal_t.accl.yaw;
cmd_gimbal.pit_vel=ai_gimbal_result_cmd.gimbal_t.vel.pit;
cmd_gimbal.pit_accl=ai_gimbal_result_cmd.gimbal_t.accl.pit;
}
if(ai_gimbal_result_cmd.mode==2){
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.yaw_vel=ai_gimbal_result_cmd.gimbal_t.vel.yaw;
cmd_gimbal.yaw_accl=ai_gimbal_result_cmd.gimbal_t.accl.yaw;
cmd_gimbal.pit_vel=ai_gimbal_result_cmd.gimbal_t.vel.pit;
cmd_gimbal.pit_accl=ai_gimbal_result_cmd.gimbal_t.accl.pit;
}
Gimbal_UpdateFeedback(&gimbal); Gimbal_UpdateFeedback(&gimbal);