添加视觉通讯

Middlewares/Third_Party/Protocol/protocol.h

@@ -7,13 +7,15 @@
 #include <stdint.h>
 
 #ifdef __cplusplus
-extern "C" {
+extern "C"
+{
 #endif
 
 #define AI_NOTICE_AUTOAIM (1 << 0)
 #define AI_NOTICE_HITBUFF (1 << 1)
 #define AI_NOTICE_AUTOMATIC (1 << 2)
 #define AI_NOTICE_FIRE (1 << 3)
+#define AI_NOTICE_SEARCH_ARMOR (1 << 4)
 
 #define AI_ID_MCU (0xC4)
 #define AI_ID_REF (0xA8)
@@ -21,66 +23,70 @@ extern "C" {
 #define AI_TEAM_RED (0x01)
 #define AI_TEAM_BLUE (0x02)
 
-typedef uint8_t Protocol_ID_t;
+  typedef uint8_t Protocol_ID_t;
 
-/* 电控 -> 视觉 MCU数据结构体*/
+  /* 电控 -> 视觉 MCU数据结构体*/
-typedef struct __attribute__((packed)) {
+  typedef struct __attribute__((packed))
-  struct __attribute__((packed)) {
+  {
+    struct __attribute__((packed))
+    {
       float q0;
       float q1;
       float q2;
       float q3;
     } quat; /* 四元数 */
-
+    struct __attribute__((packed))
+    {
+      float yaw;
+      float pit;
+      float rol;
+    } gimbal;       /* 欧拉角 */
     uint8_t notice; /* 控制命令 */
+  } Protocol_UpDataMCU_t;
 
-  float ball_speed; /* 子弹初速度 */
+  /* 电控 -> 视觉 裁判系统数据结构体*/
-
+  typedef struct __attribute__((packed))
-  struct __attribute__((packed)) {
+  {
-    float left;
-    float right;
-  } distance; /* 左右距离(哨兵) */
-
-  float chassis_speed; /* 底盘速度(哨兵) */
-} Protocol_UpDataMCU_t;
-
-/* 电控 -> 视觉 裁判系统数据结构体*/
-typedef struct __attribute__((packed)) {
     uint16_t team; /* 本身队伍 */
     uint16_t time; /* 比赛开始时间 */
-} Protocol_UpDataReferee_t;
+  } Protocol_UpDataReferee_t;
 
-/* 视觉 -> 电控 数据结构体*/
+  /* 视觉 -> 电控 数据结构体*/
-typedef struct __attribute__((packed)) {
+  typedef struct __attribute__((packed))
-  struct __attribute__((packed)) {
+  {
+    struct __attribute__((packed))
+    {
       float yaw; /* 偏航角（Yaw angle） */
       float pit; /* 俯仰角（Pitch angle） */
       float rol; /* 翻滚角（Roll angle） */
     } gimbal;    /* 欧拉角 */
 
-  uint8_t notice; /* 控制命令 */
+    struct __attribute__((packed))
-
+    {
-  struct __attribute__((packed)) {
       float vx;         /* x轴移动速度 */
       float vy;         /* y轴移动速度 */
       float wz;         /* z轴转动速度 */
     } chassis_move_vec; /* 底盘移动向量 */
-} Protocol_DownData_t;
+    uint8_t notice; /* 控制命令 */
+  } Protocol_DownData_t;
 
-typedef struct __attribute__((packed)) {
+  typedef struct __attribute__((packed))
+  {
     Protocol_UpDataMCU_t data;
     uint16_t crc16;
-} Protocol_UpPackageMCU_t;
+  } Protocol_UpPackageMCU_t;
 
-typedef struct __attribute__((packed)) {
+  typedef struct __attribute__((packed))
+  {
     Protocol_UpDataReferee_t data;
     uint16_t crc16;
-} Protocol_UpPackageReferee_t;
+  } Protocol_UpPackageReferee_t;
 
-typedef struct __attribute__((packed)) {
+  typedef struct __attribute__((packed))
+  {
     Protocol_DownData_t data;
     uint16_t crc16;
-} Protocol_DownPackage_t;
+  } Protocol_DownPackage_t;
 
 #ifdef __cplusplus
 }

User/component/user_math.c

@@ -27,6 +27,10 @@ inline float AbsClip(float in, float limit) {
   return (in < -limit) ? -limit : ((in > limit) ? limit : in);
 }
 
+float fAbs(float in){
+  return (in > 0) ? in : -in;
+}
+
 inline void Clip(float *origin, float min, float max) {
   if (*origin > max) *origin = max;
   if (*origin < min) *origin = min;
@@ -103,7 +107,6 @@ inline float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm) {
   if (is17mm) {
     if (bullet_speed == 15.0f) return 4670.f;
     if (bullet_speed == 18.0f) return 5200.f;
-    if (bullet_speed == 25.0f) return 6600.f;
     if (bullet_speed == 30.0f) return 7350.f;
   } else {
     if (bullet_speed == 10.0f) return 4450.f;
@@ -113,3 +116,17 @@ inline float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm) {
   /* 不为裁判系统设定值时,计算转速 */
   return 60.0f * (float)bullet_speed / (M_2PI * fric_radius);
 }
+
+/**
+ * @brief 断言失败处理
+ *
+ * @param file 文件名
+ * @param line 行号
+ */
+void VerifyFailed(const char *file, uint32_t line) {
+  UNUSED(file);
+  UNUSED(line);
+  while (1) {
+    __NOP();
+  }
+}

User/component/user_math.h

@@ -51,6 +51,8 @@ float InvSqrt(float x);
 
 float AbsClip(float in, float limit);
 
+float fAbs(float in);
+
 void Clip(float *origin, float min, float max);
 
 float Sign(float in);
@@ -104,3 +106,53 @@ float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm);
 #ifdef __cplusplus
 }
 #endif
+
+#ifdef DEBUG
+
+/**
+ * @brief 如果表达式的值为假则运行处理函数
+ *
+ */
+#define ASSERT(expr)                    \
+  do {                                  \
+    if (!(expr)) {                      \
+      VerifyFailed(__FILE__, __LINE__); \
+    }                                   \
+  } while (0)
+#else
+
+/**
+ * @brief 未定DEBUG，表达式不会运行，断言被忽略
+ *
+ */
+#define ASSERT(expr) ((void)(0))
+#endif
+
+#ifdef DEBUG
+
+/**
+ * @brief 如果表达式的值为假则运行处理函数
+ *
+ */
+#define VERIFY(expr)                    \
+  do {                                  \
+    if (!(expr)) {                      \
+      VerifyFailed(__FILE__, __LINE__); \
+    }                                   \
+  } while (0)
+#else
+
+/**
+ * @brief 表达式会运行，忽略表达式结果
+ *
+ */
+#define VERIFY(expr) ((void)(expr))
+#endif
+
+/**
+ * @brief 断言失败处理
+ *
+ * @param file 文件名
+ * @param line 行号
+ */
+void VerifyFailed(const char *file, uint32_t line);

User/device/ai.c

@@ -1,5 +1,5 @@
 /*
-  AI
+AI
 */
 
 /* Includes ----------------------------------------------------------------- */
@@ -12,31 +12,45 @@
 #include "component\crc16.h"
 #include "component\crc8.h"
 #include "component\user_math.h"
+#include "component\filter.h"
+
 
 /* Private define ----------------------------------------------------------- */
+#define AI_LEN_RX_BUFF (sizeof(Protocol_DownPackage_t))
+
 /* Private macro ------------------------------------------------------------ */
 /* Private typedef ---------------------------------------------------------- */
 /* Private variables -------------------------------------------------------- */
 static volatile uint32_t drop_message = 0;
 
-static osThreadId_t thread_alert;
+static uint8_t rxbuf[AI_LEN_RX_BUFF];
 
 static bool inited = false;
 
+static osThreadId_t thread_alert;
+
 /* Private function  -------------------------------------------------------- */
 
 static void Ai_RxCpltCallback(void) {
   osThreadFlagsSet(thread_alert, SIGNAL_AI_RAW_REDY);
 }
 
+static void Ai_IdleLineCallback(void) {
+  osThreadFlagsSet(thread_alert, SIGNAL_AI_RAW_REDY);
+}
+
 /* Exported functions ------------------------------------------------------- */
 int8_t AI_Init(AI_t *ai) {
-  if (ai == NULL) return DEVICE_ERR_NULL;
+  UNUSED(ai);
+  ASSERT(ai);
   if (inited) return DEVICE_ERR_INITED;
-  if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
+  VERIFY((thread_alert = osThreadGetId()) != NULL);
 
   BSP_UART_RegisterCallback(BSP_UART_AI, BSP_UART_RX_CPLT_CB,
                             Ai_RxCpltCallback);
+  BSP_UART_RegisterCallback(BSP_UART_AI, BSP_UART_IDLE_LINE_CB,
+                            Ai_IdleLineCallback);
+  
   inited = true;
   return 0;
 }
@@ -48,9 +62,9 @@ int8_t AI_Restart(void) {
 }
 
 int8_t AI_StartReceiving(AI_t *ai) {
-  if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_AI),
+  UNUSED(ai);
-                           (uint8_t *)&(ai->form_host),
+  if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_AI), rxbuf,
-                           sizeof(ai->form_host)) == HAL_OK)
+                           AI_LEN_RX_BUFF) == HAL_OK)
     return DEVICE_OK;
   return DEVICE_ERR;
 }
@@ -60,17 +74,12 @@ bool AI_WaitDmaCplt(void) {
           SIGNAL_AI_RAW_REDY);
 }
 
-int8_t AI_ParseHost(AI_t *ai, CMD_Host_t *cmd_host) {
+int8_t AI_ParseHost(AI_t *ai) {
-  (void)cmd_host;
+  if (!CRC16_Verify((const uint8_t *)&(rxbuf), sizeof(ai->from_host)))
-  if (!CRC16_Verify((const uint8_t *)&(ai->form_host), sizeof(ai->form_host)))
     goto error;
-  cmd_host->gimbal_delta.pit = ai->form_host.data.gimbal.pit;
+  ai->ai_online = true;
-  cmd_host->gimbal_delta.yaw = ai->form_host.data.gimbal.yaw;
+  memcpy(&(ai->from_host), rxbuf, sizeof(ai->from_host));
-  cmd_host->gimbal_delta.rol = ai->form_host.data.gimbal.rol;
+  memset(rxbuf, 0, AI_LEN_RX_BUFF);
-  cmd_host->fire = (ai->form_host.data.notice & AI_NOTICE_FIRE);
-  cmd_host->chassis_move_vec.vx = ai->form_host.data.chassis_move_vec.vx;
-  cmd_host->chassis_move_vec.vy = ai->form_host.data.chassis_move_vec.vy;
-  cmd_host->chassis_move_vec.wz = ai->form_host.data.chassis_move_vec.wz;
   return DEVICE_OK;
 
 error:
@@ -78,11 +87,20 @@ error:
   return DEVICE_ERR;
 }
 
+void AI_PackCmd(AI_t *ai, CMD_Host_t *cmd_host) {
+  cmd_host->gimbal_delta.yaw = ai->from_host.data.gimbal.yaw;
+  cmd_host->gimbal_delta.pit = ai->from_host.data.gimbal.pit;
+  cmd_host->fire = (ai->from_host.data.notice & AI_NOTICE_FIRE);
+  cmd_host->chassis_move_vec.vx = ai->from_host.data.chassis_move_vec.vx;
+  cmd_host->chassis_move_vec.vy = ai->from_host.data.chassis_move_vec.vy;
+  cmd_host->chassis_move_vec.wz = ai->from_host.data.chassis_move_vec.wz;
+}
+
 int8_t AI_HandleOffline(AI_t *ai, CMD_Host_t *cmd_host) {
   if (ai == NULL) return DEVICE_ERR_NULL;
   if (cmd_host == NULL) return DEVICE_ERR_NULL;
-
+  ai->ai_online = false;
-  memset(&(ai->form_host), 0, sizeof(ai->form_host));
+  memset(&(ai->from_host), 0, sizeof(ai->from_host));
   memset(cmd_host, 0, sizeof(*cmd_host));
   return 0;
 }
@@ -131,3 +149,4 @@ int8_t AI_StartSend(AI_t *ai, bool ref_update) {
       return DEVICE_ERR;
   }
 }
+

User/device/ai.h

@@ -16,6 +16,7 @@ extern "C" {
 #include "component\ahrs.h"
 #include "component\cmd.h"
 #include "component\user_math.h"
+#include "component\filter.h"
 #include "device\device.h"
 #include "device\referee.h"
 #include "protocol.h"
@@ -37,7 +38,7 @@ typedef struct __packed {
 typedef struct __packed {
   osThreadId_t thread_alert;
 
-  Protocol_DownPackage_t form_host;
+  Protocol_DownPackage_t from_host;
 
   struct {
     AI_UpPackageReferee_t ref;
@@ -45,6 +46,7 @@ typedef struct __packed {
   } to_host;
 
   CMD_AI_Status_t status;
+  bool ai_online;
 } AI_t;
 
 /* Exported functions prototypes -------------------------------------------- */
@@ -53,11 +55,15 @@ int8_t AI_Restart(void);
 
 int8_t AI_StartReceiving(AI_t *ai);
 bool AI_WaitDmaCplt(void);
-int8_t AI_ParseHost(AI_t *ai, CMD_Host_t *cmd_host);
+int8_t AI_ParseHost(AI_t *ai);
 int8_t AI_HandleOffline(AI_t *ai, CMD_Host_t *cmd_host);
 int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat);
 int8_t AI_PackRef(AI_t *ai, const Referee_ForAI_t *ref);
 int8_t AI_StartSend(AI_t *ai, bool option);
+void AI_PackCmd(AI_t *ai, CMD_Host_t *cmd_host);
 #ifdef __cplusplus
 }
 #endif
+
+
+

User/task/ai.c

@@ -42,6 +42,7 @@ void Task_Ai(void *argument) {
   AI_Init(&ai);
 
   uint32_t tick = osKernelGetTickCount();
+  uint32_t last_online_tick = tick;
   while (1) {
 #ifdef DEBUG
     task_runtime.stack_water_mark.ai = osThreadGetStackSpace(osThreadGetId());
@@ -51,17 +52,23 @@ void Task_Ai(void *argument) {
 
     AI_StartReceiving(&ai);
     if (AI_WaitDmaCplt()) {
-      AI_ParseHost(&ai, &cmd_host);
+      AI_ParseHost(&ai);
+      last_online_tick = tick;
     } else {
-      AI_HandleOffline(&ai, &cmd_host);
+      if (tick - last_online_tick > 300) AI_HandleOffline(&ai,&cmd_host);
     }
+
+    if (ai.status != AI_STATUS_STOP && ai.ai_online){
+      AI_PackCmd(&ai, &cmd_host);
       osMessageQueueReset(task_runtime.msgq.cmd.raw.host);
       osMessageQueuePut(task_runtime.msgq.cmd.raw.host, &(cmd_host), 0, 0);
+    }
 
     osMessageQueueGet(task_runtime.msgq.ai.quat, &(quat), NULL, 0);
     osMessageQueueGet(task_runtime.msgq.cmd.ai, &(ai.status), NULL, 0);
     bool ref_update = (osMessageQueueGet(task_runtime.msgq.referee.ai,
                                          &(referee_ai), NULL, 0) == osOK);
+    
     AI_PackMCU(&ai, &quat);
     if (ref_update) AI_PackRef(&ai, &(referee_ai));