User/module/gimbal.c

@@ -45,7 +45,6 @@ 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;
@@ -399,10 +398,10 @@ switch (g->mode)
 
 
     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;  // 直接输出速度环目标值作为电机输出
       /*控制云台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);
@@ -464,7 +463,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+1.5;//1.5是重力补偿
+    output_pitch_4310.torque = g->out.pitch_4310 * 1.0f;
     output_pitch_4310.kd=0.2f;
 
     MOTOR_RM_Ctrl(&g->param->yaw_6020_motor);
@@ -482,14 +481,7 @@ void Gimbal_DumpUI(const Gimbal_t *g, Gimbal_RefereeUI_t *ui) {
   ui->mode = g->mode;
 }
 
-/* @brief 导出云台UI数据
+
- *
- * @param g 云台结构体
- * @param ui UI结构体
- */
-void Gimbal_DumpUI(const Gimbal_t *g, Gimbal_RefereeUI_t *ui) {
-  ui->mode = g->mode;
-}
 
 
 

User/module/gimbal.h

@@ -16,7 +16,7 @@ extern "C"
 #define GIMBAL_ERR_NULL -2
 #define GIMBAL_ERR_MODE (-3) 
 #define GIMBAL_ERR_TYPE (-4) 
-#define AI_GIMBAL_SEARCH (1u << 1)  // bit1: 云台搜索模式
+
 
 
 
@@ -201,10 +201,6 @@ typedef struct {
     } Gimbal_t;
 
 
-typedef struct {
-  Gimbal_Mode_t mode;
-} Gimbal_RefereeUI_t;
-
   /* Exported functions prototypes -------------------------------------------- */
 
 /**

User/task/gimbal.c

@@ -22,7 +22,7 @@ Gimbal_t gimbal;
 AI_result_t ai_gimbal_result_cmd; /*ai发送自瞄数据和 导航数据*/
 
 Gimbal_CMD_t final_gimbal_cmd; //最终命令
-Gimbal_RefereeUI_t gimbal_ui;
+
 /* USER STRUCT END */
 
 /* Private function --------------------------------------------------------- */
@@ -46,6 +46,7 @@ void Task_gimbal(void *argument) {
     /* USER CODE BEGIN */
     osMessageQueueGet(task_runtime.msgq.gimbal.imu, &gimbal_imu,NULL, 0);
     Gimbal_UpdateIMU(&gimbal, &gimbal_imu);
+
 osMessageQueueGet(task_runtime.msgq.gimbal.ai.g_cmd,&ai_gimbal_result_cmd,NULL, 0);
     /* ai指令 */
     // if(osMessageQueueGet(task_runtime.msgq.gimbal.ai.g_cmd, &ai_gimbal_result_cmd, NULL, 0)==osOK){
@@ -58,14 +59,16 @@ void Task_gimbal(void *argument) {
     //   final_gimbal_cmd.set_yaw=ai_gimbal_cmd.gimbal_t.setpoint.yaw;
     //  }   
     // }       
+
+
+
 osMessageQueueGet(task_runtime.msgq.gimbal.cmd, &cmd_gimbal, NULL, 0);
 
 if (cmd_gimbal.ctrl_mode == GIMBAL_MODE_AI) {
       if (ai_gimbal_result_cmd.mode == 0) {
         /* AI无目标 */
         cmd_gimbal.mode = GIMBAL_MODE_ABSOLUTE;
-        /* 烧苗模式标志位判断 */
+        if (cmd_gimbal.scan_enable) {
-        if (ai_gimbal_result_cmd.reserved & AI_GIMBAL_SEARCH ) {
           cmd_gimbal.ctrl_mode = GIMBAL_MODE_SCAN; /* 有扫描：回退到扫描模式 */
         } else {
           cmd_gimbal.ctrl_mode = GIMBAL_MODE_REMOTE; /* 无扫描：保持当前姿态不动 */
@@ -85,8 +88,6 @@ if (cmd_gimbal.ctrl_mode == GIMBAL_MODE_AI) {
 
 Gimbal_UpdateFeedback(&gimbal);
 
-
-
 osMessageQueueReset(task_runtime.msgq.gimbal.yaw4310);
 /* 底盘跟随统一使用大YAW反馈，避免跟随锁到小YAW */
 osMessageQueuePut(task_runtime.msgq.gimbal.yaw4310, &gimbal.feedback.motor.yaw_4310_motor_feedback, 0, 0);
@@ -98,7 +99,7 @@ osMessageQueuePut(task_runtime.msgq.gimbal.ai.feedback, &gimbal.feedback, 0, 0);
 Gimbal_Control(&gimbal, &cmd_gimbal);
 Gimbal_Output(&gimbal);
 
-Gimbal_DumpUI(&gimbal,&gimbal_ui);
+
 
     /* USER CODE END */
     osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */