diff --git a/MDK-ARM/R2/R2.axf b/MDK-ARM/R2/R2.axf index 6593a15..40d3460 100644 Binary files a/MDK-ARM/R2/R2.axf and b/MDK-ARM/R2/R2.axf differ diff --git a/Middlewares/Third_Party/Protocol/protocol.h b/Middlewares/Third_Party/Protocol/protocol.h new file mode 100644 index 0000000..330770c --- /dev/null +++ b/Middlewares/Third_Party/Protocol/protocol.h @@ -0,0 +1,100 @@ +/* + 视觉与电控通信协议 +*/ + +#pragma once + +#include +#include + +#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 diff --git a/User/bsp/protocol.h b/User/bsp/protocol.h index da06407..c0d7390 100644 --- a/User/bsp/protocol.h +++ b/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)) { diff --git a/User/device/nuc.c b/User/device/nuc.c index f7eb9f2..0685a15 100644 --- a/User/device/nuc.c +++ b/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; + } +} + diff --git a/User/device/nuc.h b/User/device/nuc.h index fad848d..866483a 100644 --- a/User/device/nuc.h +++ b/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_HandleOffline(CMD_NUC_t *cmd); +int8_t NUC_RawParse(CMD_NUC_t *n, NUC_t *nuc); +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 diff --git a/User/task/atti_esti.c b/User/task/atti_esti.c index d689212..ccda8a6 100644 --- a/User/task/atti_esti.c +++ b/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, diff --git a/User/task/init.c b/User/task/init.c index 9b3e7af..ff67827 100644 --- a/User/task/init.c +++ b/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 */ diff --git a/User/task/nuc_task.c b/User/task/nuc_task.c index 29feee1..9be52b0 100644 --- a/User/task/nuc_task.c +++ b/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); - osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc_send, &NUC_send, NULL, 0); - - + + if (nuc_raw.unc_online) { + 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); - } - - - + } diff --git a/User/task/user_task.h b/User/task/user_task.h index 81c836e..9f2ba0f 100644 --- a/User/task/user_task.h +++ b/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;