LYL/R2_NEW
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

更新imu数据

Browse Source
This commit is contained in:
Robofish 2025-07-03 22:47:46 +08:00
parent 40ca2de038
commit b7e4d04550
9 changed files with 271 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
MDK-ARM/R2/R2.axf
View File

Binary file not shown.

100
Middlewares/Third_Party/Protocol/protocol.h vendored Normal file
View File

@ -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

37
User/bsp/protocol.h
View File

@ -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)) {

48
User/device/nuc.c
View File

@ -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;
}
}

38
User/device/nuc.h
View File

@ -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

10
User/task/atti_esti.c
View File

@ -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,

7
User/task/init.c
View File

@ -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 */

48
User/task/nuc_task.c
View File

@ -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);
}
}

11
User/task/user_task.h
View File

@ -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;