From 8a86d525df646136249585666456843e05c725ad Mon Sep 17 00:00:00 2001 From: yunhai Date: Fri, 20 Mar 2026 05:20:15 +0800 Subject: [PATCH] =?UTF-8?q?=E6=9B=B4=E6=96=B0=20User/module/gimbal.c?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 大yaw逻辑修正 --- User/module/gimbal.c | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/User/module/gimbal.c b/User/module/gimbal.c index bcb7a87..a444e09 100644 --- a/User/module/gimbal.c +++ b/User/module/gimbal.c @@ -45,6 +45,7 @@ Gimbal_Output(&gimbal); /* *拟合函数，用于对pitch的重力补偿 + */ static double poly(double x) { return 0.420757*pow(x,3) + -2.27122*pow(x,2) + 4.13016*x + -1.98984; @@ -398,10 +399,10 @@ switch (g->mode) yaw_omega_set_point =PID_Calc(&g->pid.yaw_4310_angle,g->setpoint.yaw_4310, - 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->feedback.motor.yaw_4310_motor_feedback.rotor_speed,0.0f,g->dt); - // g->out.yaw_4310 = yaw_omega_set_point; // 直接输出速度环目标值作为电机输出 + g->feedback.motor.yaw_6020_motor_feedback.rotor_abs_angle,g->feedback.motor.yaw_6020_motor_feedback.rotor_speed,g->dt); +// 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->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); @@ -463,7 +464,7 @@ void Gimbal_Output(Gimbal_t *g) // output_yaw_4310.kd=0.1f; MOTOR_MIT_Output_t output_pitch_4310 = {0}; - output_pitch_4310.torque = g->out.pitch_4310 * 1.0f; + output_pitch_4310.torque = g->out.pitch_4310 * 1.0f+1.5;//1.5是重力补偿 output_pitch_4310.kd=0.2f; MOTOR_RM_Ctrl(&g->param->yaw_6020_motor);