将默认视觉通信换为usb虚拟串口，修复视觉发送roll错误；增加了attach调试支持

.vscode/launch.json

@@ -4,7 +4,7 @@
     "configurations": [
         // 使用dap-link(如无线调试器时的参考配置)
         {
-            "name": "Debug-DAP",
+            "name": "DAPlink",
             "cwd": "${workspaceRoot}",
             "executable": "${workspaceRoot}\\build\\basic_framework.elf", // 要下载到调试器的文件,花括号中的是vscode两个预定义的参数
             "request": "launch",
@@ -20,13 +20,14 @@
                 "openocd_dap.cfg", // 配置文件已经在根目录提供,若要修改以此类推,openocd的路径下的share/scripts中有各种写好的配置文件
             ],
             "runToEntryPoint": "main", // 调试时在main函数入口停下
+            "rtos": "FreeRTOS",
             //"preLaunchTask": "build task",//先运行Build任务编译项目,取消注释即可使用
             //"preLaunchTask": "log", // 调试时同时开启RTT viewer窗口
             // 若想要在调试前编译并且打开log,可只使用log的prelaunch task并为log任务添加depends on依赖
         },
         // 使用j-link进行调试时的参考配置
         {
-            "name": "Debug-Jlink",
+            "name": "Jlink",
             "cwd": "${workspaceFolder}",
             "executable": "${workspaceRoot}\\build\\basic_framework.elf",
             "request": "launch",
@@ -42,5 +43,31 @@
             //"preLaunchTask": "log", // 调试时同时开启RTT viewer窗口
             // 若想要在调试前编译并且打开log,可只使用log的prelaunch task并为log任务添加depends on依赖
         },
+        {
+            "name": "DAP-attach",
+            "cwd": "${workspaceRoot}",
+            "executable": "${workspaceRoot}\\build\\basic_framework.elf", // 要下载到调试器的文件,花括号中的是vscode两个预定义的参数
+            "request": "attach",
+            "type": "cortex-debug",
+            "device": "STM32F407IG",
+            "svdFile": "STM32F407.svd",
+            "servertype": "openocd",
+            "configFiles": [
+                "openocd_dap.cfg",
+            ],
+        },
+        {
+            "name": "Jlink-attach",
+            "cwd": "${workspaceFolder}",
+            "executable": "${workspaceRoot}\\build\\basic_framework.elf",
+            "request": "attach",
+            "type": "cortex-debug",
+            "device": "STM32F407IG",
+            "showDevDebugOutput": "none",
+            "servertype": "jlink",
+            "interface": "swd",
+            "svdFile": "STM32F407.svd",
+            "rtos": "FreeRTOS",
+        },
     ],
 }

Inc/usbd_cdc_if.h

@@ -64,7 +64,7 @@
   */
 
 /* USER CODE BEGIN EXPORTED_TYPES */
-
+typedef void (*USBCallback)(uint16_t); // USB事件回调函数类型
 /* USER CODE END EXPORTED_TYPES */
 
 /**
@@ -108,7 +108,7 @@ extern USBD_CDC_ItfTypeDef USBD_Interface_fops_FS;
 uint8_t CDC_Transmit_FS(uint8_t* Buf, uint16_t Len);
 
 /* USER CODE BEGIN EXPORTED_FUNCTIONS */
-
+uint8_t* CDCInitRxbufferNcallback(USBCallback transmit_cbk,USBCallback recv_cbk);
 /* USER CODE END EXPORTED_FUNCTIONS */
 
 /**

STM32F407IGHx_FLASH.ld

@@ -55,8 +55,8 @@ ENTRY(Reset_Handler)
 /* Highest address of the user mode stack */
 _estack = ORIGIN(RAM) + LENGTH(RAM);    /* end of RAM */
 /* Generate a link error if heap and stack don't fit into RAM */
-_Min_Heap_Size = 0x4000;      /* required amount of heap  */
+_Min_Heap_Size = 0x6000;      /* required amount of heap  */
-_Min_Stack_Size = 0x4000; /* required amount of stack */
+_Min_Stack_Size = 0x6000; /* required amount of stack */
 
 /* Specify the memory areas */
 MEMORY

Src/usbd_cdc_if.c

@@ -31,7 +31,8 @@
 
 /* USER CODE BEGIN PV */
 /* Private variables ---------------------------------------------------------*/
-
+static USBCallback tx_cbk = NULL;
+static USBCallback rx_cbk = NULL;
 /* USER CODE END PV */
 
 /** @addtogroup STM32_USB_OTG_DEVICE_LIBRARY
@@ -263,6 +264,10 @@ static int8_t CDC_Receive_FS(uint8_t* Buf, uint32_t *Len)
   /* USER CODE BEGIN 6 */
   USBD_CDC_SetRxBuffer(&hUsbDeviceFS, &Buf[0]);
   USBD_CDC_ReceivePacket(&hUsbDeviceFS);
+
+  if(rx_cbk)
+    rx_cbk(*Len);
+
   return (USBD_OK);
   /* USER CODE END 6 */
 }
@@ -311,12 +316,19 @@ static int8_t CDC_TransmitCplt_FS(uint8_t *Buf, uint32_t *Len, uint8_t epnum)
   UNUSED(Buf);
   UNUSED(Len);
   UNUSED(epnum);
+  if(tx_cbk)
+    tx_cbk(*Len);
   /* USER CODE END 13 */
   return result;
 }
 
 /* USER CODE BEGIN PRIVATE_FUNCTIONS_IMPLEMENTATION */
-
+uint8_t* CDCInitRxbufferNcallback(USBCallback transmit_cbk,USBCallback recv_cbk)
+{
+  tx_cbk = transmit_cbk;
+  rx_cbk = recv_cbk;
+  return UserRxBufferFS;
+}
 /* USER CODE END PRIVATE_FUNCTIONS_IMPLEMENTATION */
 
 /**

Src/usbd_conf.c

@@ -79,8 +79,9 @@ void HAL_PCD_MspInit(PCD_HandleTypeDef* pcdHandle)
     /**USB_OTG_FS GPIO Configuration
     PA12     ------> USB_OTG_FS_DP
     PA11     ------> USB_OTG_FS_DM
+    PA10     ------> USB_OTG_FS_ID
     */
-    GPIO_InitStruct.Pin = GPIO_PIN_12|GPIO_PIN_11;
+    GPIO_InitStruct.Pin = GPIO_PIN_12|GPIO_PIN_11|GPIO_PIN_10;
     GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
     GPIO_InitStruct.Pull = GPIO_NOPULL;
     GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
@@ -112,8 +113,9 @@ void HAL_PCD_MspDeInit(PCD_HandleTypeDef* pcdHandle)
     /**USB_OTG_FS GPIO Configuration
     PA12     ------> USB_OTG_FS_DP
     PA11     ------> USB_OTG_FS_DM
+    PA10     ------> USB_OTG_FS_ID
     */
-    HAL_GPIO_DeInit(GPIOA, GPIO_PIN_12|GPIO_PIN_11);
+    HAL_GPIO_DeInit(GPIOA, GPIO_PIN_12|GPIO_PIN_11|GPIO_PIN_10);
 
     /* Peripheral interrupt Deinit*/
     HAL_NVIC_DisableIRQ(OTG_FS_IRQn);

Src/usbd_desc.c

@@ -64,9 +64,9 @@
 
 #define USBD_VID     1155
 #define USBD_LANGID_STRING     1033
-#define USBD_MANUFACTURER_STRING     "STMicroelectronics"
+#define USBD_MANUFACTURER_STRING     "YueLuEmbedded"
-#define USBD_PID_FS     22336
+#define USBD_PID_FS     202
-#define USBD_PRODUCT_STRING_FS     "STM32 Virtual ComPort"
+#define USBD_PRODUCT_STRING_FS     "Vision Comm port"
 #define USBD_CONFIGURATION_STRING_FS     "CDC Config"
 #define USBD_INTERFACE_STRING_FS     "CDC Interface"
 

application/robot_def.h

@@ -22,6 +22,9 @@
 // #define GIMBAL_BOARD  //云台板
 // #define BALANCE_BOARD //启用平衡底盘,则默认双板且当前板位底盘,目前不支持!请勿使用!
 
+#define VISION_USE_VCP  // 使用虚拟串口发送视觉数据
+// #define VISION_USE_UART // 使用串口发送视觉数据
+
 // @todo: 增加机器人类型定义,后续是否要兼容所有机器人?(只兼容步兵英雄哨兵似乎就够了)
 // 通过该宏,你可以直接将所有机器人的参数保存在一处,然后每次只需要修改这个宏就可以替换所有参数
 /* 机器人类型定义 */
@@ -36,13 +39,13 @@
 // 云台参数
 #define YAW_CHASSIS_ALIGN_ECD 2711  // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改
 #define YAW_ECD_GREATER_THAN_4096 0 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度
-#define PITCH_HORIZON_ECD 3412       // 云台处于水平位置时编码器值,若对云台有机械改动需要修改
+#define PITCH_HORIZON_ECD 3412      // 云台处于水平位置时编码器值,若对云台有机械改动需要修改
-#define PITCH_MAX_ANGLE 0 //云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度) 
+#define PITCH_MAX_ANGLE 0           // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
-#define PITCH_MIN_ANGLE 0 //云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度) 
+#define PITCH_MIN_ANGLE 0           // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
 // 发射参数
-#define ONE_BULLET_DELTA_ANGLE 36   // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出
+#define ONE_BULLET_DELTA_ANGLE 36    // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出
 #define REDUCTION_RATIO_LOADER 49.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f
-#define NUM_PER_CIRCLE 10             // 拨盘一圈的装载量
+#define NUM_PER_CIRCLE 10            // 拨盘一圈的装载量
 // 机器人底盘修改的参数,单位为mm(毫米)
 #define WHEEL_BASE 350              // 纵向轴距(前进后退方向)
 #define TRACK_WIDTH 300             // 横向轮距(左右平移方向)
@@ -51,9 +54,9 @@
 #define RADIUS_WHEEL 60             // 轮子半径
 #define REDUCTION_RATIO_WHEEL 19.0f // 电机减速比,因为编码器量测的是转子的速度而不是输出轴的速度故需进行转换
 
-#define GYRO2GIMBAL_DIR_YAW 1 //陀螺仪数据相较于云台的yaw的方向,1为相同,-1为相反
+#define GYRO2GIMBAL_DIR_YAW 1   // 陀螺仪数据相较于云台的yaw的方向,1为相同,-1为相反
-#define GYRO2GIMBAL_DIR_PITCH 1 //陀螺仪数据相较于云台的pitch的方向,1为相同,-1为相反
+#define GYRO2GIMBAL_DIR_PITCH 1 // 陀螺仪数据相较于云台的pitch的方向,1为相同,-1为相反
-#define GYRO2GIMBAL_DIR_ROLL 1 //陀螺仪数据相较于云台的roll的方向,1为相同,-1为相反
+#define GYRO2GIMBAL_DIR_ROLL 1  // 陀螺仪数据相较于云台的roll的方向,1为相同,-1为相反
 
 // 检查是否出现主控板定义冲突,只允许一个开发板定义存在,否则编译会自动报错
 #if (defined(ONE_BOARD) && defined(CHASSIS_BOARD)) || \

basic_framework.ioc

@@ -227,58 +227,59 @@ Mcu.Pin13=PI6
 Mcu.Pin14=PG9
 Mcu.Pin15=PD1
 Mcu.Pin16=PA11
-Mcu.Pin17=PF0
+Mcu.Pin17=PA10
-Mcu.Pin18=PA9
+Mcu.Pin18=PF0
-Mcu.Pin19=PC9
+Mcu.Pin19=PA9
 Mcu.Pin2=PB4
-Mcu.Pin20=PA8
+Mcu.Pin20=PC9
-Mcu.Pin21=PH0-OSC_IN
+Mcu.Pin21=PA8
-Mcu.Pin22=PH1-OSC_OUT
+Mcu.Pin22=PH0-OSC_IN
-Mcu.Pin23=PF1
+Mcu.Pin23=PH1-OSC_OUT
-Mcu.Pin24=PC6
+Mcu.Pin24=PF1
-Mcu.Pin25=PG6
+Mcu.Pin25=PC6
-Mcu.Pin26=PF6
+Mcu.Pin26=PG6
-Mcu.Pin27=PH12
+Mcu.Pin27=PF6
-Mcu.Pin28=PG3
+Mcu.Pin28=PH12
-Mcu.Pin29=PH11
+Mcu.Pin29=PG3
 Mcu.Pin3=PB3
-Mcu.Pin30=PH10
+Mcu.Pin30=PH11
-Mcu.Pin31=PD14
+Mcu.Pin31=PH10
-Mcu.Pin32=PA0-WKUP
+Mcu.Pin32=PD14
-Mcu.Pin33=PA4
+Mcu.Pin33=PA0-WKUP
-Mcu.Pin34=PC4
+Mcu.Pin34=PA4
-Mcu.Pin35=PE13
+Mcu.Pin35=PC4
-Mcu.Pin36=PA5
+Mcu.Pin36=PE13
-Mcu.Pin37=PC5
+Mcu.Pin37=PA5
-Mcu.Pin38=PE9
+Mcu.Pin38=PC5
-Mcu.Pin39=PE11
+Mcu.Pin39=PE9
 Mcu.Pin4=PA14
-Mcu.Pin40=PE14
+Mcu.Pin40=PE11
-Mcu.Pin41=PB13
+Mcu.Pin41=PE14
-Mcu.Pin42=PA7
+Mcu.Pin42=PB13
-Mcu.Pin43=PB0
+Mcu.Pin43=PA7
-Mcu.Pin44=PB14
+Mcu.Pin44=PB0
-Mcu.Pin45=PB15
+Mcu.Pin45=PB14
-Mcu.Pin46=VP_ADC1_TempSens_Input
+Mcu.Pin46=PB15
-Mcu.Pin47=VP_ADC1_Vref_Input
+Mcu.Pin47=VP_ADC1_TempSens_Input
-Mcu.Pin48=VP_CRC_VS_CRC
+Mcu.Pin48=VP_ADC1_Vref_Input
-Mcu.Pin49=VP_FREERTOS_VS_CMSIS_V1
+Mcu.Pin49=VP_CRC_VS_CRC
 Mcu.Pin5=PA13
-Mcu.Pin50=VP_RNG_VS_RNG
+Mcu.Pin50=VP_FREERTOS_VS_CMSIS_V1
-Mcu.Pin51=VP_RTC_VS_RTC_Activate
+Mcu.Pin51=VP_RNG_VS_RNG
-Mcu.Pin52=VP_SYS_VS_tim14
+Mcu.Pin52=VP_RTC_VS_RTC_Activate
-Mcu.Pin53=VP_TIM1_VS_ClockSourceINT
+Mcu.Pin53=VP_SYS_VS_tim14
-Mcu.Pin54=VP_TIM4_VS_ClockSourceINT
+Mcu.Pin54=VP_TIM1_VS_ClockSourceINT
-Mcu.Pin55=VP_TIM5_VS_ClockSourceINT
+Mcu.Pin55=VP_TIM4_VS_ClockSourceINT
-Mcu.Pin56=VP_TIM8_VS_ClockSourceINT
+Mcu.Pin56=VP_TIM5_VS_ClockSourceINT
-Mcu.Pin57=VP_TIM10_VS_ClockSourceINT
+Mcu.Pin57=VP_TIM8_VS_ClockSourceINT
-Mcu.Pin58=VP_USB_DEVICE_VS_USB_DEVICE_CDC_FS
+Mcu.Pin58=VP_TIM10_VS_ClockSourceINT
-Mcu.Pin59=VP_STMicroelectronics.X-CUBE-ALGOBUILD_VS_DSPOoLibraryJjLibrary_1.3.0_1.3.0
+Mcu.Pin59=VP_USB_DEVICE_VS_USB_DEVICE_CDC_FS
 Mcu.Pin6=PB7
+Mcu.Pin60=VP_STMicroelectronics.X-CUBE-ALGOBUILD_VS_DSPOoLibraryJjLibrary_1.3.0_1.3.0
 Mcu.Pin7=PB6
 Mcu.Pin8=PD0
 Mcu.Pin9=PC11
-Mcu.PinsNb=60
+Mcu.PinsNb=61
 Mcu.ThirdParty0=STMicroelectronics.X-CUBE-ALGOBUILD.1.3.0
 Mcu.ThirdPartyNb=1
 Mcu.UserConstants=
@@ -336,6 +337,8 @@ PA0-WKUP.GPIO_Label=KEY
 PA0-WKUP.GPIO_PuPd=GPIO_PULLUP
 PA0-WKUP.Locked=true
 PA0-WKUP.Signal=GPIO_Input
+PA10.Locked=true
+PA10.Signal=USB_OTG_FS_ID
 PA11.Mode=Device_Only
 PA11.Signal=USB_OTG_FS_DM
 PA12.Mode=Device_Only
@@ -517,7 +520,7 @@ ProjectManager.StackSize=0x4000
 ProjectManager.TargetToolchain=Makefile
 ProjectManager.ToolChainLocation=
 ProjectManager.UnderRoot=false
-ProjectManager.functionlistsort=1-MX_GPIO_Init-GPIO-false-HAL-true,2-MX_DMA_Init-DMA-false-HAL-true,3-SystemClock_Config-RCC-false-HAL-false,4-MX_ADC1_Init-ADC1-false-HAL-true,5-MX_CAN1_Init-CAN1-false-HAL-true,6-MX_CAN2_Init-CAN2-false-HAL-true,7-MX_SPI1_Init-SPI1-false-HAL-true,8-MX_TIM4_Init-TIM4-false-HAL-true,9-MX_TIM5_Init-TIM5-false-HAL-true,10-MX_USART3_UART_Init-USART3-false-HAL-true,11-MX_RNG_Init-RNG-false-HAL-true,12-MX_RTC_Init-RTC-false-HAL-true,13-MX_TIM1_Init-TIM1-false-HAL-true,14-MX_TIM10_Init-TIM10-false-HAL-true,15-MX_USART1_UART_Init-USART1-false-HAL-true,16-MX_USART6_UART_Init-USART6-false-HAL-true,17-MX_USB_DEVICE_Init-USB_DEVICE-false-HAL-false,18-MX_TIM8_Init-TIM8-false-HAL-true,19-MX_I2C2_Init-I2C2-false-HAL-true,20-MX_I2C3_Init-I2C3-false-HAL-true,21-MX_SPI2_Init-SPI2-false-HAL-true,22-MX_CRC_Init-CRC-false-HAL-true,23-MX_DAC_Init-DAC-false-HAL-true
+ProjectManager.functionlistsort=1-MX_GPIO_Init-GPIO-false-HAL-true,2-MX_DMA_Init-DMA-false-HAL-true,3-SystemClock_Config-RCC-false-HAL-false,4-MX_ADC1_Init-ADC1-false-HAL-true,5-MX_CAN1_Init-CAN1-false-HAL-true,6-MX_CAN2_Init-CAN2-false-HAL-true,7-MX_SPI1_Init-SPI1-false-HAL-true,8-MX_TIM4_Init-TIM4-false-HAL-true,9-MX_TIM5_Init-TIM5-false-HAL-true,10-MX_USART3_UART_Init-USART3-false-HAL-true,11-MX_RNG_Init-RNG-false-HAL-true,12-MX_RTC_Init-RTC-false-HAL-true,13-MX_TIM1_Init-TIM1-false-HAL-true,14-MX_TIM10_Init-TIM10-false-HAL-true,15-MX_USART1_UART_Init-USART1-false-HAL-true,16-MX_USART6_UART_Init-USART6-false-HAL-true,17-MX_TIM8_Init-TIM8-false-HAL-true,18-MX_I2C2_Init-I2C2-false-HAL-true,19-MX_I2C3_Init-I2C3-false-HAL-true,20-MX_SPI2_Init-SPI2-false-HAL-true,21-MX_CRC_Init-CRC-false-HAL-true,22-MX_DAC_Init-DAC-false-HAL-true,23-MX_USB_OTG_FS_USB_Init-USB_OTG_FS-false-HAL-true
 RCC.48MHZClocksFreq_Value=48000000
 RCC.AHBFreq_Value=168000000
 RCC.APB1CLKDivider=RCC_HCLK_DIV4
@@ -646,7 +649,10 @@ USART3.WordLength=WORDLENGTH_9B
 USART6.IPParameters=VirtualMode
 USART6.VirtualMode=VM_ASYNC
 USB_DEVICE.CLASS_NAME_FS=CDC
-USB_DEVICE.IPParameters=VirtualMode-CDC_FS,VirtualModeFS,CLASS_NAME_FS
+USB_DEVICE.IPParameters=VirtualMode-CDC_FS,VirtualModeFS,CLASS_NAME_FS,MANUFACTURER_STRING-CDC_FS,PID_CDC_FS,PRODUCT_STRING_CDC_FS
+USB_DEVICE.MANUFACTURER_STRING-CDC_FS=YueLuEmbedded
+USB_DEVICE.PID_CDC_FS=202
+USB_DEVICE.PRODUCT_STRING_CDC_FS=Vision Comm port
 USB_DEVICE.VirtualMode-CDC_FS=Cdc
 USB_DEVICE.VirtualModeFS=Cdc_FS
 USB_OTG_FS.IPParameters=VirtualMode

bsp/usb/bsp_usb.c

@@ -10,31 +10,16 @@
  */
 
 #include "bsp_usb.h"
+#include "bsp_log.h"
 
 static uint8_t *bsp_usb_rx_buffer; // 接收到的数据会被放在这里,buffer size为2048
 // 注意usb单个数据包(Full speed模式下)最大为64byte,超出可能会出现丢包情况
 
-// 这是传输完成的回调函数,在usbd_cdc_if.c中被调用
+uint8_t *USBInit(USB_Init_Config_s usb_conf)
-__weak void USBTransmitCpltCallback(uint32_t len)
-{
-    // 本次发送的数据长度
-    UNUSED(len);
-    // 传输完成会调用此函数,to do something
-}
-
-// 这是接收回调函数
-__weak void USBReceiveCpltCallback(uint32_t len)
-{
-    // 本次接收的数据长度
-    UNUSED(len);
-    // 传输完成会调用此函数,to do something
-}
-
-uint8_t *USBInit()
 {
     // 上电后重新枚举usb设备
-    USBTransmit((uint8_t *)"USB DEVICE READY", sizeof("USB DEVICE READY")); // 发送初始化完成信息
+    LOGINFO("USB init success");
-    bsp_usb_rx_buffer = CDCInitRxbufferNcallback(USBTransmitCpltCallback,USBReceiveCpltCallback);  // 获取接收数据指针
+    bsp_usb_rx_buffer = CDCInitRxbufferNcallback(usb_conf.tx_cbk, usb_conf.rx_cbk); // 获取接收数据指针
     // usb的接收回调函数会在这里被设置,并将数据保存在bsp_usb_rx_buffer中
     return bsp_usb_rx_buffer;
 }

bsp/usb/bsp_usb.h

@@ -17,9 +17,15 @@
 #include "usbd_desc.h"
 #include "usbd_cdc_if.h"
 
+typedef struct
+{
+    USBCallback tx_cbk;
+    USBCallback rx_cbk;
+} USB_Init_Config_s;
+
 /* @note 虚拟串口的波特率/校验位/数据位等动态可变,取决于上位机的设定 */
 /* 使用时不需要关心这些设置(作为从机) */
 
-uint8_t *USBInit(); // bsp初始化时调用会重新枚举设备
+uint8_t *USBInit(USB_Init_Config_s usb_conf); // bsp初始化时调用会重新枚举设备
 
 void USBTransmit(uint8_t *buffer, uint16_t len); // 通过usb发送数据

modules/imu/ins_task.c

@@ -46,7 +46,7 @@ static void IMU_Temperature_Ctrl(void)
 }
 
 // 使用加速度计的数据初始化Roll和Pitch,而Yaw置0,这样可以避免在初始时候的姿态估计误差
-static void InitQuaternion(float* init_q4)
+static void InitQuaternion(float *init_q4)
 {
     float acc_init[3] = {0};
     float gravity_norm[3] = {0, 0, 1}; // 导航系重力加速度矢量,归一化后为(0,0,1)
@@ -68,7 +68,7 @@ static void InitQuaternion(float* init_q4)
     Cross3d(acc_init, gravity_norm, axis_rot);
     Norm3d(axis_rot);
     init_q4[0] = cosf(angle / 2.0f);
-    for(uint8_t i = 0; i < 2; ++i)
+    for (uint8_t i = 0; i < 2; ++i)
         init_q4[i + 1] = axis_rot[i] * sinf(angle / 2.0f); // 轴角公式,第三轴为0(没有z轴分量)
 }
 
@@ -155,7 +155,7 @@ void INS_Task(void)
         INS.Roll = QEKF_INS.Roll;
         INS.YawTotalAngle = QEKF_INS.YawTotalAngle;
 
-        VisionSetAltitude(INS.Yaw,INS.Pitch);
+        VisionSetAltitude(INS.Yaw, INS.Pitch, INS.Roll);
     }
 
     // temperature control

modules/master_machine/master_process.c

@@ -9,14 +9,32 @@
  *
  */
 #include "master_process.h"
-#include "bsp_usart.h"
-#include "usart.h"
 #include "seasky_protocol.h"
-#include "daemon.h"
 #include "bsp_log.h"
+#include "robot_def.h"
 
 static Vision_Recv_s recv_data;
 static Vision_Send_s send_data;
+
+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, float roll)
+{
+    send_data.yaw = yaw;
+    send_data.pitch = pitch;
+    send_data.roll = roll;
+}
+
+#ifdef VISION_USE_UART
+
+#include "bsp_usart.h"
+#include "daemon.h"
+
 static USARTInstance *vision_usart_instance;
 static DaemonInstance *vision_daemon_instance;
 
@@ -45,9 +63,9 @@ static void VisionOfflineCallback(void *id)
     USARTServiceInit(vision_usart_instance);
 }
 
-/* 视觉通信初始化 */
 Vision_Recv_s *VisionInit(UART_HandleTypeDef *_handle)
 {
+#ifdef VISION_USE_UART
     USART_Init_Config_s conf;
     conf.module_callback = DecodeVision;
     conf.recv_buff_size = VISION_RECV_SIZE;
@@ -61,6 +79,7 @@ Vision_Recv_s *VisionInit(UART_HandleTypeDef *_handle)
         .reload_count = 10,
     };
     vision_daemon_instance = DaemonRegister(&daemon_conf);
+#endif // VISION_USE_UART
 
     return &recv_data;
 }
@@ -88,15 +107,40 @@ void VisionSend()
     // 若使用了daemon,则也可以使用DMA发送.
 }
 
-void VisionSetFlag(Enemy_Color_e enemy_color, Work_Mode_e work_mode, Bullet_Speed_e bullet_speed)
+#endif // VISION_USE_UART
+
+#ifdef VISION_USE_VCP
+
+#include "bsp_usb.h"
+static uint8_t *vis_recv_buff;
+
+static void DecodeVision(uint16_t recv_len)
 {
-    send_data.enemy_color = enemy_color;
+    uint16_t flag_register;
-    send_data.work_mode = work_mode;
+    get_protocol_info(vis_recv_buff, &flag_register, (uint8_t *)&recv_data.pitch);
-    send_data.bullet_speed = bullet_speed;
+    // TODO: code to resolve flag_register;
 }
 
-void VisionSetAltitude(float yaw, float pitch)
+/* 视觉通信初始化 */
+Vision_Recv_s *VisionInit(UART_HandleTypeDef *_handle)
 {
-    send_data.yaw = yaw;
+    UNUSED(_handle); // 仅为了消除警告
-    send_data.pitch = pitch;
+    USB_Init_Config_s conf = {0};
+    conf.rx_cbk = DecodeVision;
+    vis_recv_buff = USBInit(conf);
+    return &recv_data;
 }
+
+void VisionSend()
+{
+    static uint16_t flag_register;
+    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_data.yaw, 3, send_buff, &tx_len);
+    USBTransmit(send_buff, tx_len);
+}
+
+#endif // VISION_USE_VCP

modules/master_machine/master_process.h

@@ -77,6 +77,7 @@ typedef struct
 
 	float yaw;
 	float pitch;
+	float roll;
 } Vision_Send_s;
 #pragma pack()
 
@@ -108,6 +109,6 @@ void VisionSetFlag(Enemy_Color_e enemy_color, Work_Mode_e work_mode, Bullet_Spee
  * @param yaw
  * @param pitch
  */
-void VisionSetAltitude(float yaw, float pitch);
+void VisionSetAltitude(float yaw, float pitch, float roll);
 
 #endif // !MASTER_PROCESS_H