将视觉通信转移到INStask之后，频率改为1kHz，cmd仅保留颜色和模式设置

Src/freertos.c

@@ -29,6 +29,7 @@
 #include "motor_task.h"
 #include "led_task.h"
 #include "referee_task.h"
+#include "master_process.h"
 #include "daemon.h"
 #include "robot.h"
 /* USER CODE END Includes */
@@ -169,6 +170,7 @@ void StartINSTASK(void const *argument)
   {
     // 1kHz
     INS_Task();
+    VisionSend(); // 解算完成后发送视觉数据
     osDelay(1);
   }
 }

application/cmd/robot_cmd.c

@@ -316,12 +316,7 @@ void RobotCMDTask()
     EmergencyHandler(); // 处理模块离线和遥控器急停等紧急情况
 
     // 设置视觉发送数据,还需增加加速度和角速度数据
-    vision_send_data.bullet_speed = 15;
-    vision_send_data.enemy_color = 0;
-    vision_send_data.pitch = gimbal_fetch_data.gimbal_imu_data.Pitch;
-    vision_send_data.yaw = gimbal_fetch_data.gimbal_imu_data.Yaw;
-    vision_send_data.roll = gimbal_fetch_data.gimbal_imu_data.Roll;
-    ;
+    // VisionSetFlag(chassis_fetch_data.enemy_color,,chassis_fetch_data.bullet_speed)
 
     // 推送消息,双板通信,视觉通信等
     // 其他应用所需的控制数据在remotecontrolsetmode和mousekeysetmode中完成设置

application/gimbal/gimbal.c

@@ -15,45 +15,8 @@ static Subscriber_t *gimbal_sub;                  // cmd控制消息订阅者
 static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给cmd的云台状态信息
 static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv;         // 来自cmd的控制信息
 
-BMI088Instance *imu;
 void GimbalInit()
 {
-    BMI088_Init_Config_s imu_config = {
-        .spi_acc_config = {
-            .GPIOx = CS1_ACCEL_GPIO_Port,
-            .cs_pin = CS1_ACCEL_Pin,
-            .spi_handle = &hspi1,
-        },
-        .spi_gyro_config = {
-            .GPIOx = CS1_GYRO_GPIO_Port,
-            .cs_pin = CS1_GYRO_Pin,
-            .spi_handle = &hspi1,
-        },
-        .acc_int_config = {
-            .exti_mode = EXTI_TRIGGER_FALLING,
-            .GPIO_Pin = INT_ACC_Pin,
-            .GPIOx = INT_ACC_GPIO_Port,
-        },
-        .gyro_int_config = {
-            .exti_mode = EXTI_TRIGGER_FALLING,
-            .GPIO_Pin = INT_GYRO_Pin,
-            .GPIOx = INT_GYRO_GPIO_Port,
-        },
-        .heat_pid_config = {
-            .Kp = 0.0f,
-            .Kd = 0.0f,
-            .Ki = 0.0f,
-            .MaxOut = 0.0f,
-            .DeadBand = 0.0f,
-        },
-        .heat_pwm_config = {
-            .channel = TIM_CHANNEL_1,
-            .htim = &htim1,
-        },
-        .cali_mode = BMI088_CALIBRATE_ONLINE_MODE,
-        .work_mode = BMI088_BLOCK_PERIODIC_MODE,
-    };
-    // imu = BMI088Register(&imu_config);
     gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源
     // YAW
     Motor_Init_Config_s yaw_config = {

modules/imu/ins_task.c

@@ -18,6 +18,8 @@
 #include "spi.h"
 #include "user_lib.h"
 #include "general_def.h"
+#include "master_process.h"
+
 static INS_t INS;
 static IMU_Param_t IMU_Param;
 static PIDInstance TempCtrl = {0};
@@ -152,6 +154,8 @@ void INS_Task(void)
         INS.Pitch = QEKF_INS.Pitch;
         INS.Roll = QEKF_INS.Roll;
         INS.YawTotalAngle = QEKF_INS.YawTotalAngle;
+        
+        VisionSetAltitude(INS.Yaw,INS.Pitch);
     }
 
     // temperature control

modules/master_machine/master_process.c

@@ -16,9 +16,7 @@
 #include "bsp_log.h"
 
 static Vision_Recv_s recv_data;
-// @todo:由于后续需要进行IMU-Cam的硬件触发采集控制,因此可能需要将发送设置为定时任务,或由IMU采集完成产生的中断唤醒的任务,
-// 后者显然更nice,使得时间戳对齐. 因此,在send_data中设定其他的标志位数据,让ins_task填充姿态值.
-// static Vision_Send_s send_data;
+static Vision_Send_s send_data;
 static USARTInstance *vision_usart_instance;
 static DaemonInstance *vision_daemon_instance;
 
@@ -69,26 +67,36 @@ Vision_Recv_s *VisionInit(UART_HandleTypeDef *_handle)
 
 /**
  * @brief 发送函数
- * @todo 1.提高可读性,将get_protocol_send_data的第6个参数增加一个float buffer
- *       2.添加标志位解码
- *       3.由于后续需要进行IMU-Cam的硬件触发采集控制,因此可能需要将发送设置为定时任务
- *         或由IMU采集完成产生的中断唤醒的任务,使得时间戳对齐.
- *
+ * 
  * @param send 待发送数据
+ * 
  */
-void VisionSend(Vision_Send_s *send)
+void VisionSend()
 {
     // buff和txlen必须为static,才能保证在函数退出后不被释放,使得DMA正确完成发送
     // 析构后的陷阱需要特别注意!
     static uint16_t flag_register;
-    static uint8_t send_buff[VISION_SEND_SIZE]; 
+    static uint8_t send_buff[VISION_SEND_SIZE];
     static uint16_t tx_len;
     // TODO: code to set flag_register
     flag_register = 30 << 8 | 0b00000001;
     // 将数据转化为seasky协议的数据包
-    get_protocol_send_data(0x02, flag_register, &send->yaw, 3, send_buff, &tx_len);
+    get_protocol_send_data(0x02, flag_register, &send_data.yaw, 3, send_buff, &tx_len);
     USARTSend(vision_usart_instance, send_buff, tx_len, USART_TRANSFER_DMA); // 和视觉通信使用IT,防止和接收使用的DMA冲突
     // 此处为HAL设计的缺陷,DMASTOP会停止发送和接收,导致再也无法进入接收中断.
     // 也可在发送完成中断中重新启动DMA接收,但较为复杂.因此,此处使用IT发送.
     // 若使用了daemon,则也可以使用DMA发送.
-}
+}
+
+void VisionSetFlag(Enemy_Color_e enemy_color, Work_Mode_e work_mode, Bullet_Speed_e bullet_speed)
+{
+    send_data.enemy_color = enemy_color;
+    send_data.work_mode = work_mode;
+    send_data.bullet_speed = bullet_speed;
+}
+
+void VisionSetAltitude(float yaw, float pitch)
+{
+    send_data.yaw = yaw;
+    send_data.pitch = pitch;
+}

modules/master_machine/master_process.h

@@ -47,8 +47,9 @@ typedef struct
 
 typedef enum
 {
-	BLUE = 0,
-	RED = 1
+	COLOR_NONE = 0,
+	COLOR_BLUE = 1,
+	COLOR_RED = 2,
 } Enemy_Color_e;
 
 typedef enum
@@ -76,9 +77,6 @@ typedef struct
 
 	float yaw;
 	float pitch;
-	float roll;
-
-	// uint32_t time_stamp; // @todo 用于和相机的时间戳对齐
 } Vision_Send_s;
 #pragma pack()
 
@@ -90,10 +88,26 @@ typedef struct
 Vision_Recv_s *VisionInit(UART_HandleTypeDef *_handle);
 
 /**
- * @brief 发送视觉视觉
+ * @brief 发送视觉数据
  *
- * @param send 视觉需要的数据
  */
-void VisionSend(Vision_Send_s *send);
+void VisionSend();
+
+/**
+ * @brief 设置视觉发送标志位
+ *
+ * @param enemy_color
+ * @param work_mode
+ * @param bullet_speed
+ */
+void VisionSetFlag(Enemy_Color_e enemy_color, Work_Mode_e work_mode, Bullet_Speed_e bullet_speed);
+
+/**
+ * @brief 设置发送数据的姿态部分
+ *
+ * @param yaw
+ * @param pitch
+ */
+void VisionSetAltitude(float yaw, float pitch);
 
 #endif // !MASTER_PROCESS_H