更新imu数据

Middlewares/Third_Party/Protocol/protocol.h

@@ -0,0 +1,100 @@
+/*
+  视觉与电控通信协议
+*/
+
+#pragma once
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+#define NUC_HEAD (0xFF)
+#define NUC_END (0xFE)
+#define NUC_ID (0x09)
+#define NUC_CTRL (0x01)
+
+#define IMU_ID (0x01)
+#define CMD_ID (0x02)
+
+
+  /* 电控 -> 视觉 IMU数据结构体*/
+  typedef struct __attribute__((packed))
+  {
+    struct __attribute__((packed))
+    {
+      float x;
+      float y;
+      float z;
+    } gyro; /* 陀螺仪数据 */
+
+    struct __attribute__((packed))
+    {
+      float x;
+      float y;
+      float z;
+    } accl; /* 四元数 */
+    struct __attribute__((packed))
+    {
+      float q0;
+      float q1;
+      float q2;
+      float q3;
+    } quat; /* 四元数 */
+
+  } Protocol_UpDataIMU_t;
+
+    /* 电控 -> 视觉 控制数据结构体*/
+  typedef struct __attribute__((packed))
+  {
+    int8_t cmd; /* 控制命令 */
+  } Protocol_UpDataCMD_t;
+
+  /* 视觉 -> 电控 数据结构体*/
+  typedef struct __attribute__((packed))
+  {
+    struct __attribute__((packed))
+    {
+      float vx;         /* x轴移动速度 */
+      float vy;         /* y轴移动速度 */
+      float wz;         /* z轴转动速度 */
+    } chassis_move_vec; /* 底盘移动向量 */
+
+    float distance; /* 距离（单位：米） */
+
+    float angle; /* 角度（单位：弧度） */
+
+    bool notice; /* 控制命令 */
+  } Protocol_DownData_t;
+
+  typedef struct __attribute__((packed))
+  {
+    uint8_t head;
+    uint8_t id; /* 数据包ID */
+    uint8_t ctrl; /* 控制字 */
+    Protocol_UpDataIMU_t data;
+    uint8_t end; /* 数据包结束符 */
+
+  } Protocol_UpPackageIMU_t;
+
+  typedef struct __attribute__((packed))
+  {
+    uint8_t head;
+    uint8_t id;
+    uint8_t ctrl;
+    Protocol_UpDataCMD_t data;
+    uint8_t end;
+  } Protocol_UpPackageCMD_t;
+
+  typedef struct __attribute__((packed))
+  {
+    Protocol_DownData_t data;
+    uint16_t crc16;
+  } Protocol_DownPackage_t;
+
+#ifdef __cplusplus
+}
+#endif

User/bsp/protocol.h

@@ -14,6 +14,11 @@ extern "C" {
 #define HEAD (0xFF)
 #define TAIL (0xFE)
 
+#define IMU_ID (0x01)
+#define CMD_ID (0x02)
+
+#define TYPE (0x09)
+
 #define NAVI (0x05)
 #define PICK (0x06)
 
@@ -49,6 +54,38 @@ typedef struct __attribute__((packed)) {
 
 } Protocol_DownDataChassis_t;
 
+
+  /* 电控 -> 视觉 IMU数据结构体*/
+  typedef struct __attribute__((packed))
+  {
+    struct __attribute__((packed))
+    {
+      float x;
+      float y;
+      float z;
+    } gyro; /* 陀螺仪数据 */
+
+    struct __attribute__((packed))
+    {
+      float x;
+      float y;
+      float z;
+    } accl; /* 四元数 */
+    struct __attribute__((packed))
+    {
+      float q0;
+      float q1;
+      float q2;
+      float q3;
+    } quat; /* 四元数 */
+
+  } Protocol_UpDataIMU_t;
+
+  typedef struct __attribute__((packed))
+  {
+    uint8_t status;
+  } Protocol_UpDataCMD_t;
+
 /* 视觉 -> 电控 上层机构数据结构体*/
 typedef struct __attribute__((packed)) {
 

User/device/nuc.c

@@ -75,11 +75,11 @@ int8_t NUC_Restart(void)
 }
 bool_t NUC_WaitDmaCplt(void)
 {
-  return (osThreadFlagsWait(SIGNAL_NUC_RAW_REDY, osFlagsWaitAll, 20) ==
+  return (osThreadFlagsWait(SIGNAL_NUC_RAW_REDY, osFlagsWaitAll, 0) ==
           SIGNAL_NUC_RAW_REDY);
 }
 
-int8_t NUC_RawParse(CMD_NUC_t *n)
+int8_t NUC_RawParse(CMD_NUC_t *n, NUC_t *nuc)
 {
   if (n == NULL)
     return DEVICE_ERR_NULL;
@@ -165,6 +165,7 @@ int8_t NUC_RawParse(CMD_NUC_t *n)
 
       break;
     }
+    nuc->unc_online = true; // 设置为在线状态
     return DEVICE_OK;
   }
 error:
@@ -173,10 +174,51 @@ error:
   return DEVICE_ERR;
 }
 
-int8_t NUC_HandleOffline(CMD_NUC_t *cmd)
+int8_t NUC_HandleOffline(CMD_NUC_t *cmd, NUC_t *nuc)
 {
   if (cmd == NULL)
     return DEVICE_ERR_NULL;
+  nuc->unc_online = false; // 设置为离线状态
   //	 memset(cmd, 0, sizeof(*cmd));
   return 0;
 }
+
+int8_t NUC_PackIMU(NUC_t *nuc, const AHRS_Quaternion_t *quat, const AHRS_Accl_t *accl, const AHRS_Gyro_t *gyro){
+  nuc->to_nuc.imu.head = HEAD;
+  nuc->to_nuc.imu.id = IMU_ID;
+  nuc->to_nuc.imu.type = TYPE;
+  nuc->to_nuc.imu.end = TAIL;
+  memcpy((void *)&(nuc->to_nuc.imu.package.quat), (const void *)quat,sizeof(*quat));
+  memcpy((void *)&(nuc->to_nuc.imu.package.gyro), (const void *)gyro,sizeof(*gyro));
+  memcpy((void *)&(nuc->to_nuc.imu.package.accl), (const void *)accl,sizeof(*accl));
+  return DEVICE_OK;
+}
+
+int8_t NUC_PackCMD(NUC_t *nuc, const NUC_send_t *send){
+  nuc->to_nuc.cmd.head = HEAD;
+  nuc->to_nuc.cmd.id = IMU_ID;
+  nuc->to_nuc.cmd.type = TYPE;
+  nuc->to_nuc.cmd.end = TAIL;
+  // memcpy((void *)&(nuc->to_nuc.cmd.package.status), (const void *)send,sizeof(*send));
+  /*在这里添加你们的控制命令*/
+  return DEVICE_OK;
+}
+
+int8_t NUC_StartSend(NUC_t *nuc, bool cmd_update){
+  if (cmd_update) {
+    if (HAL_UART_Transmit_DMA(
+            BSP_UART_GetHandle(BSP_UART_NUC), (uint8_t *)&(nuc->to_nuc.cmd),
+            sizeof(nuc->to_nuc.cmd) + sizeof(nuc->to_nuc.imu)) == HAL_OK)
+      return DEVICE_OK;
+    else
+      return DEVICE_ERR;
+  } else {
+    if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_NUC),
+                              (uint8_t *)&(nuc->to_nuc.imu),
+                              sizeof(nuc->to_nuc.imu)) == HAL_OK)
+      return DEVICE_OK;
+    else
+      return DEVICE_ERR;
+  }
+}
+

User/device/nuc.h

@@ -6,8 +6,8 @@
 #include "bsp_usart.h"
 #include "cmd.h"
 #include "protocol.h"
-
+#include "Algorithm/ahrs.h"
-
+#include "Module/Chassis.h"
 
 
 typedef struct {
@@ -16,11 +16,33 @@ typedef struct {
 } NUC_UpPackageMCU_t;
 
 typedef struct {
-//  osThreadId_t thread_alert;
+  uint8_t head;
+  uint8_t type;
+  uint8_t id; // 0x01 IMU帧
+  Protocol_UpDataIMU_t package;
+  uint8_t end;
+} NUC_UpPackageIMU_t;
 
+typedef struct {
+  uint8_t head;
+  uint8_t type; // 0x01 控制帧
+  uint8_t id;
+  Protocol_UpDataCMD_t package;
+  uint8_t end;
+} NUC_UpPackageCMD_t;
+
+
+typedef struct {
+//  osThreadId_t thread_alert;
+  bool unc_online; //是否在线
   Protocol_DownPackageChassis_t Nucfor_chassis; //接收的数据协议
 
-  NUC_UpPackageMCU_t to_nuc; //发送的数据协议
+  // NUC_UpPackageMCU_t to_nuc; //发送的数据协议
+
+  struct {
+    NUC_UpPackageIMU_t imu;
+    NUC_UpPackageCMD_t cmd;
+  } to_nuc;
 
   uint8_t status;//控制状态
 
@@ -31,9 +53,13 @@ int8_t NUC_Init(NUC_t *nuc);
 int8_t NUC_StartReceiving(void);
 int8_t NUC_StartSending(fp32 *data) ;
 bool_t NUC_WaitDmaCplt(void);
-int8_t NUC_RawParse(CMD_NUC_t *n);
+int8_t NUC_RawParse(CMD_NUC_t *n, NUC_t *nuc);
-int8_t NUC_HandleOffline(CMD_NUC_t *cmd);
+int8_t NUC_HandleOffline(CMD_NUC_t *cmd, NUC_t *nuc);
 int8_t NUC_Restart(void);
 
+int8_t NUC_PackIMU(NUC_t *nuc, const AHRS_Quaternion_t *quat, const AHRS_Accl_t *accl, const AHRS_Gyro_t *gyro);
+int8_t NUC_PackCMD(NUC_t *nuc, const NUC_send_t *send);
+int8_t NUC_StartSend(NUC_t *nuc, bool cmd_update);
+
 #endif
 

User/task/atti_esti.c

@@ -119,7 +119,7 @@ void Task_AttiEsti(void *argument) {
     AHRS_GetEulr(&imu_eulr, &gimbal_ahrs);
 		
 		detect_hook(IMU_TOE);
-
+    osKernelUnlock();
     osMessageQueueReset(task_runtime.msgq.imu.accl);
     osMessageQueuePut(task_runtime.msgq.imu.accl, &bmi088.accl, 0, 0);
 		
@@ -128,7 +128,13 @@ void Task_AttiEsti(void *argument) {
 		
 		osMessageQueueReset(task_runtime.msgq.imu.eulr);
 		osMessageQueuePut(task_runtime.msgq.imu.eulr, &imu_eulr, 0, 0);
-    osKernelUnlock();
+    
+    osMessageQueueReset(task_runtime.msgq.nuc.gyro);
+    osMessageQueuePut(task_runtime.msgq.nuc.gyro, &bmi088.gyro, 0, 0);
+    osMessageQueueReset(task_runtime.msgq.nuc.accl);
+    osMessageQueuePut(task_runtime.msgq.nuc.accl, &bmi088.accl, 0, 0);
+    osMessageQueueReset(task_runtime.msgq.nuc.quat);
+    osMessageQueuePut(task_runtime.msgq.nuc.quat, &gimbal_ahrs.quat, 0, 0);
 		
     /* PID控制IMU温度，PWM输出 */
     BSP_PWM_Set(BSP_PWM_IMU_HEAT,

User/task/init.c

@@ -64,6 +64,13 @@ void Task_Init(void *argument) {
 	task_runtime.msgq.can.output.up_dribble_3508 = 
 	    osMessageQueueNew(2u, sizeof(CAN_DJIOutput_t), NULL);
 
+    /*nuc*/
+    task_runtime.msgq.nuc.quat =
+        osMessageQueueNew(2u, sizeof(AHRS_Quaternion_t), NULL);
+    task_runtime.msgq.nuc.accl =
+        osMessageQueueNew(2u, sizeof(AHRS_Accl_t), NULL);
+    task_runtime.msgq.nuc.gyro =
+        osMessageQueueNew(2u, sizeof(AHRS_Gyro_t), NULL);
 
 
 	/* imu */

User/task/nuc_task.c

@@ -5,11 +5,17 @@
 
 NUC_t nuc_raw;
 CMD_NUC_t cmd_fromnuc;
+AHRS_Quaternion_t quat;
+AHRS_Accl_t accl;
+AHRS_Gyro_t gyro;
 
 NUC_send_t NUC_send;
 #else 
 static NUC_t nuc_raw;
 static CMD_NUC_t cmd_fromnuc;
+static AHRS_Quaternion_t quat;
+static AHRS_Accl_t accl;
+static AHRS_Gyro_t gyro;
 NUC_send_t NUC_send;
 
 #endif
@@ -24,7 +30,7 @@ void Task_nuc(void *argument){
    NUC_Init(&nuc_raw);
 
    uint32_t tick = osKernelGetTickCount();
-   
+   uint32_t last_online_tick = tick;
    while (1) 
 		 {	 
 #ifdef DEBUG
@@ -35,25 +41,39 @@ void Task_nuc(void *argument){
 #endif
 			 a++;
 			NUC_StartReceiving();
-			// NUC_StartSending(NUC_send.send);
-			//  NUC_StartSending (send);
       if (NUC_WaitDmaCplt()){ 
-         NUC_RawParse(&cmd_fromnuc);
+         NUC_RawParse(&cmd_fromnuc, &nuc_raw);
+         last_online_tick = tick;
       }
       else{
-        NUC_HandleOffline(&cmd_fromnuc);
+        if (tick - last_online_tick > 300) NUC_HandleOffline(&cmd_fromnuc, &nuc_raw);
       }
-     osMessageQueueReset(task_runtime.msgq.cmd.raw.nuc);
+
-     osMessageQueuePut(task_runtime.msgq.cmd.raw.nuc,&(cmd_fromnuc),0,0);
+      if (nuc_raw.unc_online) {
-		 osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc_send, &NUC_send, NULL, 0);
+        osMessageQueueReset(task_runtime.msgq.cmd.raw.nuc);
-	
+        osMessageQueuePut(task_runtime.msgq.cmd.raw.nuc,&(cmd_fromnuc),0,0);
-     
+      }
+
+      osMessageQueueGet(task_runtime.msgq.nuc.quat, &(quat), NULL, 0);
+      osMessageQueueGet(task_runtime.msgq.nuc.accl, &(accl), NULL, 0);
+      osMessageQueueGet(task_runtime.msgq.nuc.gyro, &(gyro), NULL, 0);
+
+		//  osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc_send, &NUC_send, NULL, 0);
+
+     bool cmd_update = (osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc_send,
+                                         &(NUC_send), NULL, 0) == osOK);
+
+      NUC_PackIMU(&nuc_raw, &quat, &accl, &gyro);
+      if (cmd_update) {
+        NUC_PackCMD(&nuc_raw,&NUC_send);
+      }
+
+
+     NUC_StartSend(&nuc_raw, cmd_update);
+
 		 tick += delay_tick; /* 计算下一个唤醒时刄1ķ*/
 		 osDelayUntil(tick);
-			
 		 }
-		 
+		
-		 
-		 
 }
 	 

User/task/user_task.h

@@ -18,7 +18,7 @@
 #define TASK_FREQ_UP (900u)
 
 #define TASK_FREQ_CTRL_CMD (500u)
-#define TASK_FREQ_NUC (500u)
+#define TASK_FREQ_NUC (250u)
 #define TASK_FREQ_CAN (1000u)
 #define TASK_FREQ_RC (1000u)
 
@@ -54,15 +54,20 @@ typedef struct {
       osMessageQueueId_t eulr; /* 姿态结算得到欧拉角 */
     } imu;	
      /* 控制指令 */
+    struct {
+        osMessageQueueId_t quat; /* 姿态解算得到 */
+        osMessageQueueId_t accl; /* IMU读取 */
+        osMessageQueueId_t gyro; /* IMU读取 */
+      } nuc;
+
     struct {
       struct {
         osMessageQueueId_t rc;
         osMessageQueueId_t nuc; //
         osMessageQueueId_t eulr;/*姿态结算得到的欧拉角*/
 				osMessageQueueId_t nuc_send; //给nuc发
-		 
       }raw;
-			
+
 			/*控制分离*/
       osMessageQueueId_t UP_cmd_ctrl_t;
 			osMessageQueueId_t CHASSIS_cmd_ctrl_t;