Merge branch 'xm'

111

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->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->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);
@@ -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);
@@ -481,6 +482,15 @@ 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;
+}
+
 
 
 
@@ -491,5 +501,3 @@ void Gimbal_DumpUI(const Gimbal_t *g, Gimbal_RefereeUI_t *ui) {
 
 
 
-
-

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,6 +201,10 @@ 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,7 +46,6 @@ 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){
@@ -59,16 +58,14 @@ osMessageQueueGet(task_runtime.msgq.gimbal.ai.g_cmd,&ai_gimbal_result_cmd,NULL,
     //   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; /* 无扫描：保持当前姿态不动 */
@@ -88,6 +85,8 @@ 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);
@@ -99,7 +98,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); /* 运行结束，等待下一次唤醒 */