From 89b6af6138b89e90632dcc347387129bd02ce16b Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Tue, 19 Aug 2025 01:50:17 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E5=A4=8D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- app/code_page/bsp_interface.py | 18 +-- assets/.DS_Store | Bin 6148 -> 6148 bytes assets/User_code/.DS_Store | Bin 6148 -> 8196 bytes assets/User_code/bsp/can.c | 7 +- assets/User_code/bsp/i2c.c | 4 +- assets/User_code/config.csv | 2 +- assets/User_code/device/dm_imu.c | 266 +++++++++++++++++++++++++++++++ assets/User_code/device/dm_imu.h | 82 ++++++++++ 8 files changed, 364 insertions(+), 15 deletions(-) create mode 100644 assets/User_code/device/dm_imu.c create mode 100644 assets/User_code/device/dm_imu.h diff --git a/app/code_page/bsp_interface.py b/app/code_page/bsp_interface.py index ce39c7a..83848d5 100644 --- a/app/code_page/bsp_interface.py +++ b/app/code_page/bsp_interface.py @@ -514,7 +514,7 @@ class bsp_can(BspPeripheralBase): f" HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING);", "", f" // 注册CAN1回调函数", - f" BSP_CAN_RegisterCallback({self.enum_prefix}_{name}, HAL_CAN_RX_FIFO0_MSG_PENDING_CB, BSP_CAN_RxFifoCallback);", + f" BSP_CAN_RegisterCallback({self.enum_prefix}_{name}, HAL_CAN_RX_FIFO0_MSG_PENDING_CB, BSP_CAN_RxFifo0Callback);", "" ]) @@ -531,13 +531,13 @@ class bsp_can(BspPeripheralBase): f" can2_filter.FilterMaskIdHigh = 0;", f" can2_filter.FilterMaskIdLow = 0;", f" can2_filter.FilterActivation = ENABLE;", - f" can2_filter.FilterFIFOAssignment = CAN_RX_FIFO1;", + f" can2_filter.FilterFIFOAssignment = CAN_RX_FIFO0;", f" HAL_CAN_ConfigFilter(&hcan1, &can2_filter); // 通过 CAN1 配置", f" HAL_CAN_Start(&hcan2);", - f" HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO1_MSG_PENDING);", + f" HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO0_MSG_PENDING);", "", f" // 注册CAN2回调函数", - f" BSP_CAN_RegisterCallback({self.enum_prefix}_{name}, HAL_CAN_RX_FIFO1_MSG_PENDING_CB, BSP_CAN_RxFifoCallback);", + f" BSP_CAN_RegisterCallback({self.enum_prefix}_{name}, HAL_CAN_RX_FIFO0_MSG_PENDING_CB, BSP_CAN_RxFifo0Callback);", "" ]) @@ -568,7 +568,7 @@ class bsp_can(BspPeripheralBase): f" HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING);", "", f" // 注册CAN1回调函数", - f" BSP_CAN_RegisterCallback({self.enum_prefix}_{name}, HAL_CAN_RX_FIFO0_MSG_PENDING_CB, BSP_CAN_RxFifoCallback);", + f" BSP_CAN_RegisterCallback({self.enum_prefix}_{name}, HAL_CAN_RX_FIFO0_MSG_PENDING_CB, BSP_CAN_RxFifo0Callback);", "" ]) @@ -592,7 +592,7 @@ class bsp_can(BspPeripheralBase): f" HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO0_MSG_PENDING);", "", f" // 注册CAN2回调函数", - f" BSP_CAN_RegisterCallback({self.enum_prefix}_{name}, HAL_CAN_RX_FIFO0_MSG_PENDING_CB, BSP_CAN_RxFifoCallback);", + f" BSP_CAN_RegisterCallback({self.enum_prefix}_{name}, HAL_CAN_RX_FIFO0_MSG_PENDING_CB, BSP_CAN_RxFifo0Callback);", "" ]) @@ -612,13 +612,13 @@ class bsp_can(BspPeripheralBase): f" can{can_num}_filter.FilterMaskIdHigh = 0;", f" can{can_num}_filter.FilterMaskIdLow = 0;", f" can{can_num}_filter.FilterActivation = ENABLE;", - f" can{can_num}_filter.FilterFIFOAssignment = CAN_RX_FIFO1;", + f" can{can_num}_filter.FilterFIFOAssignment = CAN_RX_FIFO0;", f" HAL_CAN_ConfigFilter(&hcan1, &can{can_num}_filter); // 通过 CAN1 配置", f" HAL_CAN_Start(&hcan{can_num});", - f" HAL_CAN_ActivateNotification(&hcan{can_num}, CAN_IT_RX_FIFO1_MSG_PENDING);", + f" HAL_CAN_ActivateNotification(&hcan{can_num}, CAN_IT_RX_FIFO0_MSG_PENDING);", "", f" // 注册{instance}回调函数", - f" BSP_CAN_RegisterCallback({self.enum_prefix}_{name}, HAL_CAN_RX_FIFO1_MSG_PENDING_CB, BSP_CAN_RxFifoCallback);", + f" BSP_CAN_RegisterCallback({self.enum_prefix}_{name}, HAL_CAN_RX_FIFO0_MSG_PENDING_CB, BSP_CAN_RxFifo0Callback);", "" ]) filter_bank += 1 # 为下一个CAN分配不同的过滤器组 diff --git a/assets/.DS_Store b/assets/.DS_Store index 306d69802ef1deb33d9c6ca91192de1954c6ca35..2e7fb4d356a63b795c9d15b478c69a7a205e6397 100644 GIT binary patch delta 107 zcmZoMXfc@J&&akhU^gQp+h!gn1x8^@LmdTUBg0x9g=z~EV;uz(OY_P0%%P^d4519g z45t-G%1x8_G104lJbMsmqg=$M9Alt;uWU@VT=;kZT#w-&X L95%Ca{N)D#n)weW diff --git a/assets/User_code/.DS_Store b/assets/User_code/.DS_Store index 08591375155ab3be091f39b61c922fe3d7612f77..087482cf52d84cb4277124a8045f150d07666f41 100644 GIT binary patch delta 243 zcmZoMXmOBWU|?W$DortDU;r^WfEYvza8E20o2aMA$g?qEH}hr%jz7$c**Q2SHn1@A zOy*(PAZ%%JHTL;Bw=j7()cTK*}A}`ww)Wybt zWLjBpQC?1dUOG^madH=X=H@pd63i1Dcm$b&B0wO)4J2GaPTwrZ@tt`xzli5#e;y7F OMu@K%HplbKVFm!yRyUXc delta 131 zcmZp1XfcprU|?W$DortDU=RQ@Ie-{Mvv5r;6q~50$jG%ZU^g=(*JK`n4Z_9-Itqs7 z=CwKs)s{v;wuza^=JNsvm?qbYq)aRnS + +/* Private define ----------------------------------------------------------- */ +#define DM_IMU_OFFLINE_TIMEOUT 1000 // 设备离线判定时间1000ms + +#define ACCEL_CAN_MAX (58.8f) +#define ACCEL_CAN_MIN (-58.8f) +#define GYRO_CAN_MAX (34.88f) +#define GYRO_CAN_MIN (-34.88f) +#define PITCH_CAN_MAX (90.0f) +#define PITCH_CAN_MIN (-90.0f) +#define ROLL_CAN_MAX (180.0f) +#define ROLL_CAN_MIN (-180.0f) +#define YAW_CAN_MAX (180.0f) +#define YAW_CAN_MIN (-180.0f) +#define TEMP_MIN (0.0f) +#define TEMP_MAX (60.0f) +#define Quaternion_MIN (-1.0f) +#define Quaternion_MAX (1.0f) + + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* Private variables -------------------------------------------------------- */ +/* Private function --------------------------------------------------------- */ + +static uint8_t count = 0; // 计数器，用于判断设备是否离线 +/** + * @brief: 无符号整数转换为浮点数函数 + */ +static float uint_to_float(int x_int, float x_min, float x_max, int bits) +{ + float span = x_max - x_min; + float offset = x_min; + return ((float)x_int)*span/((float)((1<data.temp = (float)temp; + imu->data.accl.x = uint_to_float(acc_x_raw, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16); + imu->data.accl.y = uint_to_float(acc_y_raw, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16); + imu->data.accl.z = uint_to_float(acc_z_raw, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16); + return DEVICE_OK; +} +/** + * @brief 解析陀螺仪数据 + */ +static int8_t DM_IMU_ParseGyroData(DM_IMU_t *imu, uint8_t *data, uint8_t len) { + if (imu == NULL || data == NULL || len < 8) { + return DEVICE_ERR; + } + uint16_t gyro_x_raw = (data[3] << 8) | data[2]; + uint16_t gyro_y_raw = (data[5] << 8) | data[4]; + uint16_t gyro_z_raw = (data[7] << 8) | data[6]; + imu->data.gyro.x = uint_to_float(gyro_x_raw, GYRO_CAN_MIN, GYRO_CAN_MAX, 16); + imu->data.gyro.y = uint_to_float(gyro_y_raw, GYRO_CAN_MIN, GYRO_CAN_MAX, 16); + imu->data.gyro.z = uint_to_float(gyro_z_raw, GYRO_CAN_MIN, GYRO_CAN_MAX, 16); + return DEVICE_OK; +} +/** + * @brief 解析欧拉角数据 + */ +static int8_t DM_IMU_ParseEulerData(DM_IMU_t *imu, uint8_t *data, uint8_t len) { + if (imu == NULL || data == NULL || len < 8) { + return DEVICE_ERR; + } + uint16_t pit_raw = (data[3] << 8) | data[2]; + uint16_t yaw_raw = (data[5] << 8) | data[4]; + uint16_t rol_raw = (data[7] << 8) | data[6]; + imu->data.euler.pit = uint_to_float(pit_raw, PITCH_CAN_MIN, PITCH_CAN_MAX, 16); + imu->data.euler.yaw = uint_to_float(yaw_raw, YAW_CAN_MIN, YAW_CAN_MAX, 16); + imu->data.euler.rol = uint_to_float(rol_raw, ROLL_CAN_MIN, ROLL_CAN_MAX, 16); + return DEVICE_OK; +} + + +/** + * @brief 解析四元数数据 + */ +static int8_t DM_IMU_ParseQuaternionData(DM_IMU_t *imu, uint8_t *data, uint8_t len) { + if (imu == NULL || data == NULL || len < 8) { + return DEVICE_ERR; + } + int w = (data[1] << 6) | ((data[2] & 0xF8) >> 2); + int x = ((data[2] & 0x03) << 12) | (data[3] << 4) | ((data[4] & 0xF0) >> 4); + int y = ((data[4] & 0x0F) << 10) | (data[5] << 2) | ((data[6] & 0xC0) >> 6); + int z = ((data[6] & 0x3F) << 8) | data[7]; + imu->data.quat.q0 = uint_to_float(w, Quaternion_MIN, Quaternion_MAX, 14); + imu->data.quat.q1 = uint_to_float(x, Quaternion_MIN, Quaternion_MAX, 14); + imu->data.quat.q2 = uint_to_float(y, Quaternion_MIN, Quaternion_MAX, 14); + imu->data.quat.q3 = uint_to_float(z, Quaternion_MIN, Quaternion_MAX, 14); + return DEVICE_OK; +} + + +/* Exported functions ------------------------------------------------------- */ + +/** + * @brief 初始化DM IMU设备 + */ +int8_t DM_IMU_Init(DM_IMU_t *imu, BSP_CAN_t can) { + if (imu == NULL) { + return DEVICE_ERR_NULL; + } + + // 初始化设备头部 + imu->header.online = false; + imu->header.last_online_time = 0; + imu->can = can; + + // 清零数据 + memset(&imu->data, 0, sizeof(DM_IMU_Data_t)); + + // 注册CAN接收队列，用于接收回复报文 + int8_t result = BSP_CAN_RegisterId(imu->can, DM_IMU_MST_ID, 10); + if (result != BSP_OK) { + return DEVICE_ERR; + } + + return DEVICE_OK; +} + +/** + * @brief 请求IMU数据 + */ +int8_t DM_IMU_Request(DM_IMU_t *imu, DM_IMU_RID_t rid) { + if (imu == NULL) { + return DEVICE_ERR_NULL; + } + + // 构造发送数据：id_L, id_H(DM_IMU_ID), RID, 0xcc + uint8_t tx_data[4] = { + DM_IMU_ID & 0xFF, // id_L + (DM_IMU_ID >> 8) & 0xFF, // id_H + (uint8_t)rid, // RID + 0xCC // 固定值 + }; + + // 发送标准数据帧 + BSP_CAN_StdDataFrame_t frame = { + .id = DM_IMU_CAN_ID, + .dlc = 4, + }; + memcpy(frame.data, tx_data, 4); + + int8_t result = BSP_CAN_TransmitStdDataFrame(imu->can, &frame); + return (result == BSP_OK) ? DEVICE_OK : DEVICE_ERR; +} + +/** + * @brief 更新IMU数据（从CAN中获取所有数据并解析） + */ +int8_t DM_IMU_Update(DM_IMU_t *imu) { + if (imu == NULL) { + return DEVICE_ERR_NULL; + } + + BSP_CAN_Message_t msg; + int8_t result; + bool data_received = false; + + // 持续接收所有可用消息 + while ((result = BSP_CAN_GetMessage(imu->can, DM_IMU_MST_ID, &msg, BSP_CAN_TIMEOUT_IMMEDIATE)) == BSP_OK) { + // 验证回复数据格式（至少检查数据长度） + if (msg.dlc < 3) { + continue; // 跳过无效消息 + } + + // 根据数据位的第0位确定反馈报文类型 + uint8_t rid = msg.data[0] & 0x0F; // 取第0位的低4位作为RID + + // 根据RID类型解析数据 + int8_t parse_result = DEVICE_ERR; + switch (rid) { + case 0x01: // RID_ACCL + parse_result = DM_IMU_ParseAccelData(imu, msg.data, msg.dlc); + break; + case 0x02: // RID_GYRO + parse_result = DM_IMU_ParseGyroData(imu, msg.data, msg.dlc); + break; + case 0x03: // RID_EULER + parse_result = DM_IMU_ParseEulerData(imu, msg.data, msg.dlc); + break; + case 0x04: // RID_QUATERNION + parse_result = DM_IMU_ParseQuaternionData(imu, msg.data, msg.dlc); + break; + default: + continue; // 跳过未知类型的消息 + } + + // 如果解析成功，标记为收到数据 + if (parse_result == DEVICE_OK) { + data_received = true; + } + } + + // 如果收到任何有效数据，更新设备状态 + if (data_received) { + imu->header.online = true; + imu->header.last_online_time = BSP_TIME_Get_ms(); + return DEVICE_OK; + } + + return DEVICE_ERR; +} + +/** + * @brief 自动更新IMU所有数据（包括加速度计、陀螺仪、欧拉角和四元数,最高1khz） + */ +int8_t DM_IMU_AutoUpdateAll(DM_IMU_t *imu){ + if (imu == NULL) { + return DEVICE_ERR_NULL; + } + switch (count) { + case 0: + DM_IMU_Request(imu, RID_ACCL); + break; + case 1: + DM_IMU_Request(imu, RID_GYRO); + break; + case 2: + DM_IMU_Request(imu, RID_EULER); + break; + case 3: + DM_IMU_Request(imu, RID_QUATERNION); + DM_IMU_Update(imu); // 更新所有数据 + break; + } + count++; + if (count >= 4) { + count = 0; // 重置计数器 + } + return DEVICE_OK; +} + +/** + * @brief 检查设备是否在线 + */ +bool DM_IMU_IsOnline(DM_IMU_t *imu) { + if (imu == NULL) { + return false; + } + + uint32_t current_time = BSP_TIME_Get_ms(); + return imu->header.online && + (current_time - imu->header.last_online_time < DM_IMU_OFFLINE_TIMEOUT); +} diff --git a/assets/User_code/device/dm_imu.h b/assets/User_code/device/dm_imu.h new file mode 100644 index 0000000..f12f054 --- /dev/null +++ b/assets/User_code/device/dm_imu.h @@ -0,0 +1,82 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include "device/device.h" +#include "component/ahrs.h" +#include "bsp/can.h" +/* Exported constants ------------------------------------------------------- */ + +#define DM_IMU_CAN_ID 0x6FF +#define DM_IMU_ID 0x42 +#define DM_IMU_MST_ID 0x43 + +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ +typedef enum { + RID_ACCL = 0x01, // 加速度计 + RID_GYRO = 0x02, // 陀螺仪 + RID_EULER = 0x03, // 欧拉角 + RID_QUATERNION = 0x04, // 四元数 +} DM_IMU_RID_t; + +typedef struct { + AHRS_Accl_t accl; // 加速度计 + AHRS_Gyro_t gyro; // 陀螺仪 + AHRS_Eulr_t euler; // 欧拉角 + AHRS_Quaternion_t quat; // 四元数 + float temp; // 温度 +} DM_IMU_Data_t; + +typedef struct { + DEVICE_Header_t header; + BSP_CAN_t can; + DM_IMU_Data_t data; // IMU数据 +} DM_IMU_t; + +/* Exported functions prototypes -------------------------------------------- */ + +/** + * @brief 初始化DM IMU设备 + * @param imu DM IMU设备结构体指针 + * @return DEVICE_OK 成功，其他值失败 + */ +int8_t DM_IMU_Init(DM_IMU_t *imu, BSP_CAN_t can); + +/** + * @brief 请求IMU数据 + * @param imu DM IMU设备结构体指针 + * @param rid 请求的数据类型 + * @return DEVICE_OK 成功，其他值失败 + */ +int8_t DM_IMU_Request(DM_IMU_t *imu, DM_IMU_RID_t rid); + + +/** + * @brief 更新IMU数据（从CAN中获取所有数据并解析） + * @param imu DM IMU设备结构体指针 + * @return DEVICE_OK 成功，其他值失败 + */ +int8_t DM_IMU_Update(DM_IMU_t *imu); + +/** + * @brief 自动更新IMU所有数据（包括加速度计、陀螺仪、欧拉角和四元数,最高1khz，运行4次才有完整数据） + * @param imu DM IMU设备结构体指针 + * @return DEVICE_OK 成功，其他值失败 + */ +int8_t DM_IMU_AutoUpdateAll(DM_IMU_t *imu); + +/** + * @brief 检查设备是否在线 + * @param imu DM IMU设备结构体指针 + * @return true 在线，false 离线 + */ +bool DM_IMU_IsOnline(DM_IMU_t *imu); + + +#ifdef __cplusplus +} +#endif