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 13:53:32 +08:00
parent ead6c6e64e
commit 9a2ff3e150
5 changed files with 39 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
User/device/ai.c
View File

@ -76,7 +76,7 @@ int8_t MCU_Send(PackageMCU_t* mcu,Gimbal_feedback_t* motor,AHRS_Quaternion_t* qu
return DEVICE_ERR;
}
mcu->data.mode=0;
mcu->data.mode=1;
mcu->data.q[0]=motor->imu.quat.q0;
mcu->data.q[1]=motor->imu.quat.q1;
mcu->data.q[2]=motor->imu.quat.q2;

23
User/module/chassis.c
View File

@ -64,6 +64,8 @@ fp32 vofa_send[8]; //vofa输出数据
#define CHASSIS_ROTOR_OMEGA 0.001
#define CHASSIS_ROTOR_WZ_MIN 0.6f /* 小陀螺旋转位移下界 */
#define CHASSIS_ROTOR_WZ_MAX 0.8f /* 小陀螺旋转位移上界 */
#define CHASSIS_FOLLOW_LOCK_DEADBAND_RAD 0.02f /* 跟随锁定死区，约1.1度 */
#define CHASSIS_FOLLOW_VW_DEADBAND 0.03f /* 跟随角速度输出死区 */
static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode ,uint32_t now)
{
@ -405,7 +407,26 @@ 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.Vy =-c_cmd->Vx;
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);
{
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;
}
}
// c->move_vec.Vw = -PID_Calc(&c->pid.chassis_follow_gimbal_pid, c->motorfeedback.gimbal_yaw_encoder-2.07694483f ,c->motorfeedback.gimbal_yaw_encoder, 0.0f, c->dt);
break;

2
User/module/config.c
View File

@ -268,7 +268,7 @@ Config_Param_t config = {
.mech_zero = {
.yaw_6020 = 1.31f,/*1.31*/
.yaw_4310 = 2.06f, /*大yaw零点*/
.yaw_4310 = 4.517f, /*大yaw零点*/
.pitch_4310 = 0.93f,
},
.travel = {

21
User/module/gimbal.c
View File

@ -56,7 +56,7 @@ static float g_scan_pitch_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_SMALL_YAW_SPEED (0.20f)
#define GIMBAL_SCAN_SMALL_YAW_SPEED (0.40f)
#define GIMBAL_SCAN_BIG_YAW_PERIOD_SEC (5.0f)
#define GIMBAL_SCAN_PITCH_SPEED (0.16f)
#define GIMBAL_SCAN_PITCH_AMPLITUDE (0.18f)
@ -66,6 +66,7 @@ static int8_t g_scan_yaw_edge_latch = 0; /* -1: min边界锁存, 1: max边界锁
#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)
@ -218,7 +219,9 @@ int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu){
}
gimbal->feedback.imu.gyro = imu->gyro;
gimbal->feedback.imu.eulr.yaw = imu->eulr.yaw;
/* 先同步完整欧拉角，保证rol轴可用于90度装配下的pitch控制 */
gimbal->feedback.imu.eulr = imu->eulr;
/* 保留pit映射，兼容现有使用pit坐标的逻辑 */
gimbal->feedback.imu.eulr.pit = -imu->eulr.rol; // 坐标系转换
gimbal->feedback.imu.quat = imu->quat;
@ -290,7 +293,7 @@ int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu){
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.pit;
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;
@ -350,9 +353,9 @@ int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu){
}
}
if (g->feedback.imu.eulr.pit >= g_scan_pitch_center + GIMBAL_SCAN_PITCH_AMPLITUDE) {
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.pit <= g_scan_pitch_center - GIMBAL_SCAN_PITCH_AMPLITUDE) {
} else if (g->feedback.imu.eulr.rol <= g_scan_pitch_center - GIMBAL_SCAN_PITCH_AMPLITUDE) {
g_scan_pitch_dir = 1.0f;
}
@ -376,7 +379,11 @@ int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu){
}
/*处理小yaw轴控制命令*/
float delta_yaw_6020 = 10.0f * g_cmd->delta_yaw_6020 * g->dt;
float yaw_6020_gain = 10.0f;
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;
@ -517,7 +524,7 @@ switch (g->mode)
// g->out.yaw_4310 = yaw_omega_set_point; // 直接输出速度环目标值作为电机输出
/*控制云台4310电机 也是单环，但是加了重力补偿函数，可以根据不一样的情况去拟合函数*/
pitch_omega_set_point =PID_Calc(&g->pid.pitch_4310_angle,g->setpoint.eulr.pit,
g->feedback.imu.eulr.rol,-g->feedback.imu.gyro.y,g->dt);
g->feedback.imu.eulr.rol,-g->feedback.imu.gyro.y,g->dt);
g->out.pitch_4310 = pitch_omega_set_point+g->param->K_forward_pid.K_vel*g->setpoint.pit_vel+g->param->K_forward_pid.K_accl*g->setpoint.pit_accl+poly(g->feedback.motor.pitch_4310_motor_feedback.rotor_abs_angle);
// g->out.pitch_4310 = PID_Calc(&g->pid.pitch_4310_omega,8*pitch_omega_set_point,
// -g->feedback.imu.gyro.y,0.0f,g->dt)+poly(g->feedback.motor.pitch_4310_motor_feedback.rotor_abs_angle);

1
User/task/gimbal.c
View File

@ -83,6 +83,7 @@ if (cmd_gimbal.ctrl_mode == GIMBAL_MODE_AI) {
Gimbal_UpdateFeedback(&gimbal);
osMessageQueueReset(task_runtime.msgq.gimbal.yaw6020);
/* 底盘跟随统一使用大YAW反馈，避免跟随锁到小YAW */
osMessageQueuePut(task_runtime.msgq.gimbal.yaw6020, &gimbal.feedback.motor.yaw_4310_motor_feedback, 0, 0);
osMessageQueueReset(task_runtime.msgq.gimbal.ai.feedback);