更新电机

app/code_page/bsp_interface.py

@@ -481,10 +481,11 @@ class bsp_can(BspPeripheralBase):
             f"    can{can_num}_filter.FilterFIFOAssignment = {fifo_assignment};",
             f"    HAL_CAN_ConfigFilter(&hcan{can_num}, &can{can_num}_filter);",
             f"    HAL_CAN_Start(&hcan{can_num});",
-            f"    HAL_CAN_ActivateNotification(&hcan{can_num}, CAN_IT_RX_FIFO0_MSG_PENDING);",
             "",
-            f"    // 注册回调函数",
-            f"    BSP_CAN_RegisterCallback({self.enum_prefix}_{name}, HAL_CAN_RX_FIFO0_MSG_PENDING_CB, BSP_CAN_RxFifoCallback);",
+            f"    // 注册{instance}回调函数",
+            f"    BSP_CAN_RegisterCallback({self.enum_prefix}_{name}, HAL_CAN_RX_FIFO0_MSG_PENDING_CB, BSP_CAN_RxFifo0Callback);",
+            "",
+            f"    HAL_CAN_ActivateNotification(&hcan{can_num}, CAN_IT_RX_FIFO0_MSG_PENDING);",
             ""
         ])
 
@@ -511,10 +512,11 @@ class bsp_can(BspPeripheralBase):
                 f"    can1_filter.FilterFIFOAssignment = CAN_RX_FIFO0;",
                 f"    HAL_CAN_ConfigFilter(&hcan1, &can1_filter);",
                 f"    HAL_CAN_Start(&hcan1);",
-                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_RxFifo0Callback);",
+                "",
+                f"    HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING);",
                 ""
             ])
         
@@ -522,22 +524,14 @@ class bsp_can(BspPeripheralBase):
             name, instance = can2_config
             init_lines.extend([
                 f"    // 初始化 CAN2 - 使用 FIFO1",
-                f"    CAN_FilterTypeDef can2_filter = {{0}};",
-                f"    can2_filter.FilterBank = 14;",
-                f"    can2_filter.FilterIdHigh = 0;",
-                f"    can2_filter.FilterIdLow = 0;",
-                f"    can2_filter.FilterMode = CAN_FILTERMODE_IDMASK;",
-                f"    can2_filter.FilterScale = CAN_FILTERSCALE_32BIT;",
-                f"    can2_filter.FilterMaskIdHigh = 0;",
-                f"    can2_filter.FilterMaskIdLow = 0;",
-                f"    can2_filter.FilterActivation = ENABLE;",
-                f"    can2_filter.FilterFIFOAssignment = CAN_RX_FIFO0;",
-                f"    HAL_CAN_ConfigFilter(&hcan1, &can2_filter);  // 通过 CAN1 配置",
+                f"    can1_filter.FilterBank = 14;",
+                f"    can1_filter.FilterFIFOAssignment = CAN_RX_FIFO1;",
+                f"    HAL_CAN_ConfigFilter(&hcan2, &can1_filter);  // 通过 CAN1 配置",
                 f"    HAL_CAN_Start(&hcan2);",
-                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_RxFifo0Callback);",
+                f"    BSP_CAN_RegisterCallback({self.enum_prefix}_{name}, HAL_CAN_RX_FIFO1_MSG_PENDING_CB, BSP_CAN_RxFifo1Callback);",
+                f"    HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO1_MSG_PENDING);",
                 ""
             ])
 
@@ -565,34 +559,27 @@ class bsp_can(BspPeripheralBase):
                 f"    can1_filter.FilterFIFOAssignment = CAN_RX_FIFO0;",
                 f"    HAL_CAN_ConfigFilter(&hcan1, &can1_filter);",
                 f"    HAL_CAN_Start(&hcan1);",
-                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_RxFifo0Callback);",
+                "",
+                f"    HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING);",
                 ""
             ])
         
-        # CAN2 - FIFO0
+        # CAN2 - FIFO0  
         if can2_config:
             name, instance = can2_config
             init_lines.extend([
                 f"    // 初始化 CAN2 - 使用 FIFO0",
-                f"    CAN_FilterTypeDef can2_filter = {{0}};",
-                f"    can2_filter.FilterBank = 14;",
-                f"    can2_filter.FilterIdHigh = 0;",
-                f"    can2_filter.FilterIdLow = 0;",
-                f"    can2_filter.FilterMode = CAN_FILTERMODE_IDMASK;",
-                f"    can2_filter.FilterScale = CAN_FILTERSCALE_32BIT;",
-                f"    can2_filter.FilterMaskIdHigh = 0;",
-                f"    can2_filter.FilterMaskIdLow = 0;",
-                f"    can2_filter.FilterActivation = ENABLE;",
-                f"    can2_filter.FilterFIFOAssignment = CAN_RX_FIFO0;",
-                f"    HAL_CAN_ConfigFilter(&hcan1, &can2_filter);  // 通过 CAN1 配置",
+                f"    can1_filter.FilterBank = 14;",
+                f"    can1_filter.FilterFIFOAssignment = CAN_RX_FIFO0;",
+                f"    HAL_CAN_ConfigFilter(&hcan2, &can1_filter);  // 通过 CAN1 配置",
                 f"    HAL_CAN_Start(&hcan2);",
-                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_RxFifo0Callback);",
+                f"    HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO0_MSG_PENDING);",
                 ""
             ])
         
@@ -603,22 +590,14 @@ class bsp_can(BspPeripheralBase):
             can_num = ''.join(filter(str.isdigit, instance))
             init_lines.extend([
                 f"    // 初始化 {instance} - 使用 FIFO1",
-                f"    CAN_FilterTypeDef can{can_num}_filter = {{0}};",
-                f"    can{can_num}_filter.FilterBank = {filter_bank};",
-                f"    can{can_num}_filter.FilterIdHigh = 0;",
-                f"    can{can_num}_filter.FilterIdLow = 0;",
-                f"    can{can_num}_filter.FilterMode = CAN_FILTERMODE_IDMASK;",
-                f"    can{can_num}_filter.FilterScale = CAN_FILTERSCALE_32BIT;",
-                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_FIFO0;",
-                f"    HAL_CAN_ConfigFilter(&hcan1, &can{can_num}_filter);  // 通过 CAN1 配置",
+                f"    can1_filter.FilterBank = {filter_bank};",
+                f"    can1_filter.FilterFIFOAssignment = CAN_RX_FIFO1;",
+                f"    HAL_CAN_ConfigFilter(&hcan1, &can1_filter);  // 通过 CAN1 配置",
                 f"    HAL_CAN_Start(&hcan{can_num});",
-                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_FIFO0_MSG_PENDING_CB, BSP_CAN_RxFifo0Callback);",
+                f"    BSP_CAN_RegisterCallback({self.enum_prefix}_{name}, HAL_CAN_RX_FIFO1_MSG_PENDING_CB, BSP_CAN_RxFifo1Callback);",
+                f"    HAL_CAN_ActivateNotification(&hcan{can_num}, CAN_IT_RX_FIFO1_MSG_PENDING);",
                 ""
             ])
             filter_bank += 1  # 为下一个CAN分配不同的过滤器组

app/code_page/device_interface.py

@@ -28,6 +28,10 @@ def get_available_bsp_devices(project_path, bsp_type, gpio_type=None):
             if gpio_type:
                 configs = [cfg for cfg in configs if cfg.get('type', '').lower() == gpio_type.lower()]
             return [f"BSP_GPIO_{cfg['custom_name']}" for cfg in configs]
+        elif bsp_type == "pwm" and bsp_config.get("pwm", {}).get("enabled", False):
+            # PWM使用configs结构
+            configs = bsp_config["pwm"].get("configs", [])
+            return [f"BSP_PWM_{cfg['custom_name']}" for cfg in configs]
         elif bsp_type in bsp_config and bsp_config[bsp_type].get('enabled', False):
             devices = bsp_config[bsp_type].get('devices', [])
             return [f"BSP_{bsp_type.upper()}_{device['name']}" for device in devices]

assets/User_code/bsp/can.c

@@ -176,6 +176,38 @@ static void BSP_CAN_RxFifo0Callback(void) {
     }
 }
 
+/**
+ * @brief FIFO1接收处理函数
+ */
+static void BSP_CAN_RxFifo1Callback(void) {
+    CAN_RxHeaderTypeDef rx_header;
+    uint8_t rx_data[BSP_CAN_MAX_DLC];
+    for (int can_idx = 0; can_idx < BSP_CAN_NUM; can_idx++) {
+        CAN_HandleTypeDef *hcan = BSP_CAN_GetHandle((BSP_CAN_t)can_idx);
+        if (hcan == NULL) continue;
+        while (HAL_CAN_GetRxFifoFillLevel(hcan, CAN_RX_FIFO1) > 0) {
+            if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO1, &rx_header, rx_data) == HAL_OK) {
+                uint32_t original_id = (rx_header.IDE == CAN_ID_STD) ? rx_header.StdId : rx_header.ExtId;
+                BSP_CAN_FrameType_t frame_type = BSP_CAN_GetFrameType(&rx_header);
+                uint32_t parsed_id = BSP_CAN_ParseId(original_id, frame_type);
+                osMessageQueueId_t queue = BSP_CAN_FindQueue((BSP_CAN_t)can_idx, parsed_id);
+                if (queue != NULL) {
+                    BSP_CAN_Message_t msg = {0};
+                    msg.frame_type = frame_type;
+                    msg.original_id = original_id;
+                    msg.parsed_id = parsed_id;
+                    msg.dlc = rx_header.DLC;
+                    if (rx_header.RTR == CAN_RTR_DATA) {
+                        memcpy(msg.data, rx_data, rx_header.DLC);
+                    }
+                    msg.timestamp = HAL_GetTick();
+                    osMessageQueuePut(queue, &msg, 0, BSP_CAN_TIMEOUT_IMMEDIATE);
+                }
+            }
+        }
+    }
+}
+
 /* HAL Callback Functions --------------------------------------------------- */
 void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef *hcan) {
   BSP_CAN_t bsp_can = CAN_Get(hcan);

assets/User_code/bsp/pwm.c

@@ -1,9 +1,8 @@
 /* Includes ----------------------------------------------------------------- */
+#include "tim.h"
 #include "bsp/pwm.h"
 #include "bsp.h"
 
-#include <tim.h>
-
 /* Private define ----------------------------------------------------------- */
 /* Private macro ------------------------------------------------------------ */
 /* Private typedef ---------------------------------------------------------- */
@@ -67,4 +66,33 @@ int8_t BSP_PWM_Stop(BSP_PWM_Channel_t ch) {
   
   HAL_TIM_PWM_Stop(PWM_Map[ch].tim, PWM_Map[ch].channel);
   return BSP_OK;
+}
+
+uint32_t BSP_PWM_GetAutoReloadPreload(BSP_PWM_Channel_t ch) {
+    if (ch >= BSP_PWM_NUM) return BSP_ERR;
+    return PWM_Map[ch].tim->Init.AutoReloadPreload;  
+}
+
+TIM_HandleTypeDef* BSP_PWM_GetHandle(BSP_PWM_Channel_t ch) {
+  return PWM_Map[ch].tim;
+}
+
+
+uint16_t BSP_PWM_GetChannel(BSP_PWM_Channel_t ch) {
+  if (ch >= BSP_PWM_NUM) return BSP_ERR;
+  return PWM_Map[ch].channel;
+}
+
+int8_t BSP_PWM_Start_DMA(BSP_PWM_Channel_t ch, uint32_t *pData, uint16_t Length) {
+  if (ch >= BSP_PWM_NUM) return BSP_ERR;
+  
+  HAL_TIM_PWM_Start_DMA(PWM_Map[ch].tim, PWM_Map[ch].channel, pData, Length);
+  return BSP_OK;
+}
+
+int8_t BSP_PWM_Stop_DMA(BSP_PWM_Channel_t ch) {
+  if (ch >= BSP_PWM_NUM) return BSP_ERR;
+  
+  HAL_TIM_PWM_Stop_DMA(PWM_Map[ch].tim, PWM_Map[ch].channel);
+  return BSP_OK;
 }

assets/User_code/bsp/pwm.h

@@ -6,9 +6,10 @@ extern "C" {
 
 /* Includes ----------------------------------------------------------------- */
 #include <stdint.h>
-
+#include "tim.h"
 #include "bsp.h"
 
+
 /* Exported constants ------------------------------------------------------- */
 /* Exported macro ----------------------------------------------------------- */
 /* Exported types ----------------------------------------------------------- */
@@ -24,6 +25,11 @@ int8_t BSP_PWM_Start(BSP_PWM_Channel_t ch);
 int8_t BSP_PWM_SetComp(BSP_PWM_Channel_t ch, float duty_cycle);
 int8_t BSP_PWM_SetFreq(BSP_PWM_Channel_t ch, float freq);
 int8_t BSP_PWM_Stop(BSP_PWM_Channel_t ch);
+uint32_t BSP_PWM_GetAutoReloadPreload(BSP_PWM_Channel_t ch);
+uint16_t BSP_PWM_GetChannel(BSP_PWM_Channel_t ch);
+TIM_HandleTypeDef* BSP_PWM_GetHandle(BSP_PWM_Channel_t ch);
+int8_t BSP_PWM_Start_DMA(BSP_PWM_Channel_t ch, uint32_t *pData, uint16_t Length);
+int8_t BSP_PWM_Stop_DMA(BSP_PWM_Channel_t ch);
 
 #ifdef __cplusplus
 }

assets/User_code/config.csv

@@ -1,4 +1,4 @@
 bsp,can,dwt,gpio,i2c,mm,spi,uart,pwm,time
 component,ahrs,ballistics,capacity,cmd,crc8,crc16,error_detect,filter,FreeRTOS_CLI,limiter,mixer,pid,ui,user_math
-device,dr16,bmi088,ist8310,motor,motor_rm,motor_vesc,motor_odrive,dm_imu
+device,dr16,bmi088,ist8310,motor,motor_rm,motor_vesc,motor_lk,motor_lz,motor_odrive,dm_imu,servo,buzzer,led,ws2812
 module,

assets/User_code/device/buzzer.c

@@ -0,0 +1,44 @@
+#include "device/buzzer.h"
+
+
+int8_t BUZZER_Init(BUZZER_t *buzzer, BSP_PWM_Channel_t channel) {
+    if (buzzer == NULL) return DEVICE_ERR;
+    
+    buzzer->channel = channel;
+    buzzer->header.online = true;
+    
+    BUZZER_Stop(buzzer);
+    
+    return DEVICE_OK ;
+}
+
+int8_t BUZZER_Start(BUZZER_t *buzzer) {
+    if (buzzer == NULL || !buzzer->header.online) 
+        return DEVICE_ERR;
+    
+    return (BSP_PWM_Start(buzzer->channel) == BSP_OK) ? 
+           DEVICE_OK  : DEVICE_ERR;
+}
+
+int8_t BUZZER_Stop(BUZZER_t *buzzer) {
+    if (buzzer == NULL || !buzzer->header.online) 
+        return DEVICE_ERR;
+    
+    return (BSP_PWM_Stop(buzzer->channel) == BSP_OK) ? 
+           DEVICE_OK  : DEVICE_ERR;
+}
+
+int8_t BUZZER_Set(BUZZER_t *buzzer, float freq, float duty_cycle) {
+    if (buzzer == NULL || !buzzer->header.online) 
+        return DEVICE_ERR;
+    
+    int result = DEVICE_OK ;
+    
+    if (BSP_PWM_SetFreq(buzzer->channel, freq) != BSP_OK) 
+        result = DEVICE_ERR;
+    
+    if (BSP_PWM_SetComp(buzzer->channel, duty_cycle) != BSP_OK) 
+        result = DEVICE_ERR;
+    
+    return result;
+}

assets/User_code/device/buzzer.h

@@ -0,0 +1,35 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include "device.h"          
+#include "bsp/pwm.h"         
+#include <stddef.h>
+
+/* Exported constants ------------------------------------------------------- */
+
+/* Exported types ----------------------------------------------------------- */
+typedef struct {
+    DEVICE_Header_t header;    
+    BSP_PWM_Channel_t channel; 
+} BUZZER_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+
+int8_t BUZZER_Init(BUZZER_t *buzzer, BSP_PWM_Channel_t channel);
+
+
+int8_t BUZZER_Start(BUZZER_t *buzzer);
+
+
+int8_t BUZZER_Stop(BUZZER_t *buzzer);
+
+
+int8_t BUZZER_Set(BUZZER_t *buzzer, float freq, float duty_cycle);
+
+#ifdef __cplusplus
+}
+#endif

assets/User_code/device/config.yaml

@@ -107,4 +107,19 @@ devices:
     description: "通用电机驱动"
     files:
       header: "motor.h"
-      source: "motor.c"
+      source: "motor.c"
+
+  ws2812:
+    name: "WS2812 LED 灯"
+    description: "WS2812 RGB LED 灯驱动"
+    dependencies:
+      bsp: ["pwm", "time"]
+      component: []
+    bsp_requirements:
+      - type: "pwm"
+        var_name: "BSP_PWM_WS2812"
+        description: "用于WS2812数据传输的PWM通道"
+    thread_signals: []
+    files:
+      header: "ws2812.h"
+      source: "ws2812.c"

assets/User_code/device/dm_imu.c

@@ -7,6 +7,7 @@
 
 #include "bsp/can.h"
 #include "bsp/time.h"
+#include "component/user_math.h"
 #include <string.h>
 
 /* Private define ----------------------------------------------------------- */
@@ -86,13 +87,12 @@ static int8_t DM_IMU_ParseEulerData(DM_IMU_t *imu, uint8_t *data, uint8_t len) {
     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);
+    imu->data.euler.pit = uint_to_float(pit_raw, PITCH_CAN_MIN, PITCH_CAN_MAX, 16) * M_DEG2RAD_MULT;
+    imu->data.euler.yaw = uint_to_float(yaw_raw, YAW_CAN_MIN, YAW_CAN_MAX, 16) * M_DEG2RAD_MULT;
+    imu->data.euler.rol = uint_to_float(rol_raw, ROLL_CAN_MIN, ROLL_CAN_MAX, 16) * M_DEG2RAD_MULT;
     return DEVICE_OK;
 }
 
-
 /**
  * @brief 解析四元数数据
  */
@@ -117,21 +117,26 @@ static int8_t DM_IMU_ParseQuaternionData(DM_IMU_t *imu, uint8_t *data, uint8_t l
 /**
  * @brief 初始化DM IMU设备
  */
-int8_t DM_IMU_Init(DM_IMU_t *imu, BSP_CAN_t can) {
-    if (imu == NULL) {
+int8_t DM_IMU_Init(DM_IMU_t *imu, DM_IMU_Param_t *param) {
+    if (imu == NULL || param == NULL) {
         return DEVICE_ERR_NULL;
     }
     
     // 初始化设备头部
     imu->header.online = false;
     imu->header.last_online_time = 0;
-    imu->can = can;
+    
+    // 配置参数
+    imu->param.can = param->can;
+    imu->param.can_id = param->can_id;
+    imu->param.device_id = param->device_id;
+    imu->param.master_id = param->master_id;
     
     // 清零数据
     memset(&imu->data, 0, sizeof(DM_IMU_Data_t));
     
     // 注册CAN接收队列，用于接收回复报文
-    int8_t result = BSP_CAN_RegisterId(imu->can, DM_IMU_MST_ID, 10);
+    int8_t result = BSP_CAN_RegisterId(imu->param.can, imu->param.master_id, 10);
     if (result != BSP_OK) {
         return DEVICE_ERR;
     }
@@ -149,20 +154,20 @@ int8_t DM_IMU_Request(DM_IMU_t *imu, DM_IMU_RID_t rid) {
     
     // 构造发送数据：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                     // 固定值
+        imu->param.device_id & 0xFF,        // id_L
+        (imu->param.device_id >> 8) & 0xFF, // id_H
+        (uint8_t)rid,                       // RID
+        0xCC                                // 固定值
     };
     
     // 发送标准数据帧
     BSP_CAN_StdDataFrame_t frame = {
-        .id = DM_IMU_CAN_ID,
+        .id = imu->param.can_id,
         .dlc = 4,
     };
     memcpy(frame.data, tx_data, 4);
     
-    int8_t result = BSP_CAN_TransmitStdDataFrame(imu->can, &frame);
+    int8_t result = BSP_CAN_TransmitStdDataFrame(imu->param.can, &frame);
     return (result == BSP_OK) ? DEVICE_OK : DEVICE_ERR;
 }
 
@@ -179,7 +184,7 @@ int8_t DM_IMU_Update(DM_IMU_t *imu) {
     bool data_received = false;
     
     // 持续接收所有可用消息
-    while ((result = BSP_CAN_GetMessage(imu->can, DM_IMU_MST_ID, &msg, BSP_CAN_TIMEOUT_IMMEDIATE)) == BSP_OK) {
+    while ((result = BSP_CAN_GetMessage(imu->param.can, imu->param.master_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE)) == BSP_OK) {
         // 验证回复数据格式（至少检查数据长度）
         if (msg.dlc < 3) {
             continue; // 跳过无效消息

assets/User_code/device/dm_imu.h

@@ -10,12 +10,19 @@ extern "C" {
 #include "bsp/can.h"
 /* Exported constants ------------------------------------------------------- */
 
-#define DM_IMU_CAN_ID 0x6FF
-#define DM_IMU_ID 0x42
-#define DM_IMU_MST_ID 0x43
+#define DM_IMU_CAN_ID_DEFAULT 0x6FF
+#define DM_IMU_ID_DEFAULT 0x42
+#define DM_IMU_MST_ID_DEFAULT 0x43
 
 /* Exported macro ----------------------------------------------------------- */
 /* Exported types ----------------------------------------------------------- */
+
+typedef struct {
+  BSP_CAN_t can;       // CAN总线句柄
+  uint16_t can_id;     // CAN通信ID
+  uint8_t device_id;   // 设备ID
+  uint8_t master_id;   // 主机ID
+} DM_IMU_Param_t;
 typedef enum {
   RID_ACCL = 0x01, // 加速度计
   RID_GYRO = 0x02, // 陀螺仪
@@ -33,7 +40,7 @@ typedef struct {
 
 typedef struct {
   DEVICE_Header_t header;
-  BSP_CAN_t can;
+  DM_IMU_Param_t param; // IMU参数配置
   DM_IMU_Data_t data; // IMU数据
 } DM_IMU_t;
 
@@ -42,9 +49,10 @@ typedef struct {
 /**
  * @brief 初始化DM IMU设备
  * @param imu DM IMU设备结构体指针
+ * @param param IMU参数配置指针
  * @return DEVICE_OK 成功，其他值失败
  */
-int8_t DM_IMU_Init(DM_IMU_t *imu, BSP_CAN_t can);
+int8_t DM_IMU_Init(DM_IMU_t *imu, DM_IMU_Param_t *param);
 
 /**
  * @brief 请求IMU数据

assets/User_code/device/led.c

@@ -0,0 +1,47 @@
+/*
+   led控制
+*/
+/*Includes   -----------------------------------------*/
+#include "device/led.h"
+#include "bsp/gpio.h"
+#include "bsp/pwm.h"
+#include "device.h"
+
+
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+
+DEVICE_LED_t LED_Map={
+ BSP_GPIO_BLUE,
+BSP_PWM_TIM5_CH1,
+};
+
+int8_t BSP_LED_Set(char sign,DEVICE_LED_t ch,bool value,float duty_cycle)
+{
+	switch(sign){
+		case 'p':
+		case 'P':
+			    if (duty_cycle < 0.0f || duty_cycle > 1.0f) {
+                return DEVICE_ERR_NULL; // 错误：占空比超出范围
+            }
+				uint16_t pulse = (uint16_t)(duty_cycle * (float)UINT16_MAX);
+		BSP_PWM_Start(LED_Map.channel);
+		BSP_PWM_SetComp(LED_Map.channel, pulse);
+			break;
+	
+		case 'g':
+		case 'G':
+		BSP_GPIO_WritePin(LED_Map.gpio,value);
+		break;
+	default:
+            return DEVICE_ERR_INITED; // 错误：无效的控制方式
+	}
+	return DEVICE_OK;
+}
+
+
+
+
+
+

assets/User_code/device/led.h

@@ -0,0 +1,33 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include <stdint.h>
+#include "bsp/gpio.h"
+#include "bsp/pwm.h"
+#include "bsp/bsp.h"
+
+/* Exported constants ------------------------------------------------------- */
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+
+
+/* LED ??? */
+typedef struct {
+BSP_GPIO_t gpio;
+BSP_PWM_Channel_t channel;	
+} DEVICE_LED_t;
+
+
+ extern DEVICE_LED_t LED_Map;
+/* Exported functions prototypes -------------------------------------------- */
+
+
+int8_t BSP_LED_Set(char sign,DEVICE_LED_t ch,bool value,float duty_cycle);
+
+#ifdef __cplusplus
+}
+#endif

assets/User_code/device/motor.h

@@ -17,6 +17,24 @@ typedef struct {
     float temp; /* 温度 */
 } MOTOR_Feedback_t;
 
+/**
+ * @brief mit电机输出参数结构体
+ */
+typedef struct {
+    float torque;      /* 目标力矩 */
+    float velocity;    /* 目标速度 */
+    float angle;       /* 目标位置 */
+    float kp;         /* 位置环增益 */
+    float kd;         /* 速度环增益 */
+} MOTOR_MIT_Output_t;
+
+/**
+ * @brief 转矩电流控制模式参数结构体
+ */
+typedef struct {
+    float current;    /* 目标电流 */
+} MOTOR_Current_Output_t;
+
 typedef struct {
     DEVICE_Header_t header;
     bool reverse; /* 是否反装 true表示反装 */

assets/User_code/device/motor_lk.c

@@ -0,0 +1,318 @@
+/*
+  LK电机驱动
+*/
+/* Includes ----------------------------------------------------------------- */
+#include "motor_lk.h"
+#include <stdbool.h>
+#include <string.h>
+#include "bsp/can.h"
+#include "bsp/mm.h"
+#include "bsp/time.h"
+#include "component/user_math.h"
+
+/* Private define ----------------------------------------------------------- */
+#define LK_CTRL_ID_BASE        (0x140)
+#define LK_FB_ID_BASE          (0x240)
+
+// LK电机命令字节定义
+#define LK_CMD_FEEDBACK        (0x9C)    // 反馈命令字节
+#define LK_CMD_MOTOR_OFF       (0x80)    // 电机关闭命令
+#define LK_CMD_MOTOR_ON        (0x88)    // 电机运行命令
+#define LK_CMD_TORQUE_CTRL     (0xA1)    // 转矩闭环控制命令
+
+// LK电机参数定义
+#define LK_CURR_LSB_MF         (33.0f / 4096.0f)   // MF电机转矩电流分辨率 A/LSB
+#define LK_CURR_LSB_MG         (66.0f / 4096.0f)   // MG电机转矩电流分辨率 A/LSB
+#define LK_POWER_RANGE_MS      (1000)              // MS电机功率范围 ±1000
+#define LK_TORQUE_RANGE        (2048)              // 转矩控制值范围 ±2048
+#define LK_TORQUE_CURRENT_MF   (16.5f)             // MF电机最大转矩电流 A
+#define LK_TORQUE_CURRENT_MG   (33.0f)             // MG电机最大转矩电流 A
+
+#define MOTOR_TX_BUF_SIZE      (8)
+#define MOTOR_RX_BUF_SIZE      (8)
+
+// 编码器分辨率定义
+#define LK_ENC_14BIT_MAX       (16383)   // 14位编码器最大值
+#define LK_ENC_15BIT_MAX       (32767)   // 15位编码器最大值  
+#define LK_ENC_16BIT_MAX       (65535)   // 16位编码器最大值
+
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+/* Private variables -------------------------------------------------------- */
+static MOTOR_LK_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL};
+
+/* Private functions -------------------------------------------------------- */
+static float MOTOR_LK_GetCurrentLSB(MOTOR_LK_Module_t module) {
+    switch (module) {
+        case MOTOR_LK_MF9025:
+        case MOTOR_LK_MF9035:
+            return LK_CURR_LSB_MF;
+        default:
+            return LK_CURR_LSB_MG;  // 默认使用MG的分辨率
+    }
+}
+
+static uint16_t MOTOR_LK_GetEncoderMax(MOTOR_LK_Module_t module) {
+    // 根据电机型号返回编码器最大值，这里假设都使用16位编码器
+    // 实际使用时需要根据具体电机型号配置
+    return LK_ENC_16BIT_MAX;
+}
+
+static MOTOR_LK_CANManager_t* MOTOR_LK_GetCANManager(BSP_CAN_t can) {
+    if (can >= BSP_CAN_NUM) return NULL;
+    return can_managers[can];
+}
+
+static int8_t MOTOR_LK_CreateCANManager(BSP_CAN_t can) {
+    if (can >= BSP_CAN_NUM) return DEVICE_ERR;
+    if (can_managers[can] != NULL) return DEVICE_OK;
+    
+    can_managers[can] = (MOTOR_LK_CANManager_t*)BSP_Malloc(sizeof(MOTOR_LK_CANManager_t));
+    if (can_managers[can] == NULL) return DEVICE_ERR;
+    
+    memset(can_managers[can], 0, sizeof(MOTOR_LK_CANManager_t));
+    can_managers[can]->can = can;
+    return DEVICE_OK;
+}
+
+static void MOTOR_LK_Decode(MOTOR_LK_t *motor, BSP_CAN_Message_t *msg) {
+    // 调试信息：打印接收到的数据
+    // printf("LK Motor ID:%d, CMD:0x%02X, Data: %02X %02X %02X %02X %02X %02X %02X %02X\n", 
+    //        motor->param.id, msg->data[0], msg->data[0], msg->data[1], msg->data[2], 
+    //        msg->data[3], msg->data[4], msg->data[5], msg->data[6], msg->data[7]);
+    
+    // 检查命令字节是否为反馈命令
+    if (msg->data[0] != LK_CMD_FEEDBACK) {
+        // 如果不是标准反馈命令，可能是其他格式的数据
+        // 临时跳过命令字节检查，直接解析数据
+        // return;
+    }
+    
+    // 解析温度 (DATA[1])
+    motor->motor.feedback.temp = (int8_t)msg->data[1];
+    
+    // 解析转矩电流值或功率值 (DATA[2], DATA[3])
+    int16_t raw_current_or_power = (int16_t)((msg->data[3] << 8) | msg->data[2]);
+    
+    // 根据电机类型解析电流或功率
+    switch (motor->param.module) {
+        case MOTOR_LK_MF9025:
+        case MOTOR_LK_MF9035:
+            // MF/MG电机：转矩电流值
+            motor->motor.feedback.torque_current = raw_current_or_power * MOTOR_LK_GetCurrentLSB(motor->param.module);
+            break;
+        default:
+            // MS电机：功率值（范围-1000~1000）
+            motor->motor.feedback.torque_current = (float)raw_current_or_power;  // 将功率存储在torque_current字段中
+            break;
+    }
+    
+    // 解析转速 (DATA[4], DATA[5]) - 单位：1dps/LSB
+    motor->motor.feedback.rotor_speed = (int16_t)((msg->data[5] << 8) | msg->data[4]);
+    
+    // 解析编码器值 (DATA[6], DATA[7])
+    uint16_t raw_encoder = (uint16_t)((msg->data[7] << 8) | msg->data[6]);
+    uint16_t encoder_max = MOTOR_LK_GetEncoderMax(motor->param.module);
+    
+    // 将编码器值转换为弧度 (0 ~ 2π)
+    motor->motor.feedback.rotor_abs_angle = (float)raw_encoder / (float)encoder_max * M_2PI;
+}
+
+/* Exported functions ------------------------------------------------------- */
+
+int8_t MOTOR_LK_Register(MOTOR_LK_Param_t *param) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+    
+    if (MOTOR_LK_CreateCANManager(param->can) != DEVICE_OK) return DEVICE_ERR;
+    MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager(param->can);
+    if (manager == NULL) return DEVICE_ERR;
+    
+    // 检查是否已注册
+    for (int i = 0; i < manager->motor_count; i++) {
+        if (manager->motors[i] && manager->motors[i]->param.id == param->id) {
+            return DEVICE_ERR_INITED;
+        }
+    }
+    
+    // 检查数量
+    if (manager->motor_count >= MOTOR_LK_MAX_MOTORS) return DEVICE_ERR;
+    
+    // 创建新电机实例
+    MOTOR_LK_t *new_motor = (MOTOR_LK_t*)BSP_Malloc(sizeof(MOTOR_LK_t));
+    if (new_motor == NULL) return DEVICE_ERR;
+    
+    memcpy(&new_motor->param, param, sizeof(MOTOR_LK_Param_t));
+    memset(&new_motor->motor, 0, sizeof(MOTOR_t));
+    new_motor->motor.reverse = param->reverse;
+    
+    // 对于某些LK电机，反馈数据可能通过命令ID发送
+    // 根据实际测试，使用命令ID接收反馈数据
+    uint16_t feedback_id = 0x140 + param->id;  // 使用命令ID作为反馈ID
+    
+    // 注册CAN接收ID
+    if (BSP_CAN_RegisterId(param->can, feedback_id, 3) != BSP_OK) {
+        BSP_Free(new_motor);
+        return DEVICE_ERR;
+    }
+    
+    manager->motors[manager->motor_count] = new_motor;
+    manager->motor_count++;
+    return DEVICE_OK;
+}
+
+int8_t MOTOR_LK_Update(MOTOR_LK_Param_t *param) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+    
+    MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager(param->can);
+    if (manager == NULL) return DEVICE_ERR_NO_DEV;
+    
+    for (int i = 0; i < manager->motor_count; i++) {
+        MOTOR_LK_t *motor = manager->motors[i];
+        if (motor && motor->param.id == param->id) {
+            // 对于某些LK电机，反馈数据通过命令ID发送
+            uint16_t feedback_id = 0x140 + param->id;
+            
+            BSP_CAN_Message_t rx_msg;
+            if (BSP_CAN_GetMessage(param->can, feedback_id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) {
+                uint64_t now_time = BSP_TIME_Get();
+                if (now_time - motor->motor.header.last_online_time > 1000) {
+                    motor->motor.header.online = false;
+                    return DEVICE_ERR_NO_DEV;
+                }
+                return DEVICE_ERR;
+            }
+            
+            motor->motor.header.online = true;
+            motor->motor.header.last_online_time = BSP_TIME_Get();
+            MOTOR_LK_Decode(motor, &rx_msg);
+            return DEVICE_OK;
+        }
+    }
+    return DEVICE_ERR_NO_DEV;
+}
+
+int8_t MOTOR_LK_UpdateAll(void) {
+    int8_t ret = DEVICE_OK;
+    for (int can = 0; can < BSP_CAN_NUM; can++) {
+        MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager((BSP_CAN_t)can);
+        if (manager == NULL) continue;
+        
+        for (int i = 0; i < manager->motor_count; i++) {
+            MOTOR_LK_t *motor = manager->motors[i];
+            if (motor != NULL) {
+                if (MOTOR_LK_Update(&motor->param) != DEVICE_OK) {
+                    ret = DEVICE_ERR;
+                }
+            }
+        }
+    }
+    return ret;
+}
+
+int8_t MOTOR_LK_SetOutput(MOTOR_LK_Param_t *param, float value) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+    
+    MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager(param->can);
+    if (manager == NULL) return DEVICE_ERR_NO_DEV;
+    
+    // 限制输出值范围
+    if (value > 1.0f) value = 1.0f;
+    if (value < -1.0f) value = -1.0f;
+    
+    MOTOR_LK_t *motor = MOTOR_LK_GetMotor(param);
+    if (motor == NULL) return DEVICE_ERR_NO_DEV;
+    
+    // 转矩闭环控制命令 - 将输出值转换为转矩控制值
+    int16_t torque_control = (int16_t)(value * (float)LK_TORQUE_RANGE);
+    
+    // 构建CAN帧（根据协议：命令报文标识符 = 0x140 + ID）
+    BSP_CAN_StdDataFrame_t tx_frame;
+    tx_frame.id = 0x140 + param->id;
+    tx_frame.dlc = MOTOR_TX_BUF_SIZE;
+    
+    // 设置转矩闭环控制命令数据
+    tx_frame.data[0] = LK_CMD_TORQUE_CTRL;  // 命令字节
+    tx_frame.data[1] = 0x00;                // NULL
+    tx_frame.data[2] = 0x00;                // NULL
+    tx_frame.data[3] = 0x00;                // NULL
+    tx_frame.data[4] = (uint8_t)(torque_control & 0xFF);        // 转矩电流控制值低字节
+    tx_frame.data[5] = (uint8_t)((torque_control >> 8) & 0xFF); // 转矩电流控制值高字节
+    tx_frame.data[6] = 0x00;                // NULL
+    tx_frame.data[7] = 0x00;                // NULL
+    
+    return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
+}
+
+int8_t MOTOR_LK_Ctrl(MOTOR_LK_Param_t *param) {
+    // 对于LK电机，每次设置输出时就直接发送控制命令
+    // 这个函数可以用于发送其他控制命令，如电机开启/关闭
+    return DEVICE_OK;
+}
+
+int8_t MOTOR_LK_MotorOn(MOTOR_LK_Param_t *param) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+    
+    BSP_CAN_StdDataFrame_t tx_frame;
+    tx_frame.id = 0x140 + param->id;
+    tx_frame.dlc = MOTOR_TX_BUF_SIZE;
+    
+    // 电机运行命令
+    tx_frame.data[0] = LK_CMD_MOTOR_ON;     // 命令字节
+    tx_frame.data[1] = 0x00;
+    tx_frame.data[2] = 0x00;
+    tx_frame.data[3] = 0x00;
+    tx_frame.data[4] = 0x00;
+    tx_frame.data[5] = 0x00;
+    tx_frame.data[6] = 0x00;
+    tx_frame.data[7] = 0x00;
+    
+    return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
+}
+
+int8_t MOTOR_LK_MotorOff(MOTOR_LK_Param_t *param) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+    
+    BSP_CAN_StdDataFrame_t tx_frame;
+    tx_frame.id = 0x140 + param->id;
+    tx_frame.dlc = MOTOR_TX_BUF_SIZE;
+    
+    // 电机关闭命令
+    tx_frame.data[0] = LK_CMD_MOTOR_OFF;    // 命令字节
+    tx_frame.data[1] = 0x00;
+    tx_frame.data[2] = 0x00;
+    tx_frame.data[3] = 0x00;
+    tx_frame.data[4] = 0x00;
+    tx_frame.data[5] = 0x00;
+    tx_frame.data[6] = 0x00;
+    tx_frame.data[7] = 0x00;
+    
+    return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
+}
+
+MOTOR_LK_t* MOTOR_LK_GetMotor(MOTOR_LK_Param_t *param) {
+    if (param == NULL) return NULL;
+    
+    MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager(param->can);
+    if (manager == NULL) return NULL;
+    
+    for (int i = 0; i < manager->motor_count; i++) {
+        MOTOR_LK_t *motor = manager->motors[i];
+        if (motor && motor->param.id == param->id) {
+            return motor;
+        }
+    }
+    return NULL;
+}
+
+int8_t MOTOR_LK_Relax(MOTOR_LK_Param_t *param) {
+    return MOTOR_LK_SetOutput(param, 0.0f);
+}
+
+int8_t MOTOR_LK_Offine(MOTOR_LK_Param_t *param) {
+    MOTOR_LK_t *motor = MOTOR_LK_GetMotor(param);
+    if (motor) {
+        motor->motor.header.online = false;
+        return DEVICE_OK;
+    }
+    return DEVICE_ERR_NO_DEV;
+}

assets/User_code/device/motor_lk.h

@@ -0,0 +1,119 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include "device/device.h"
+#include "device/motor.h"
+#include "bsp/can.h"
+
+/* Exported constants ------------------------------------------------------- */
+#define MOTOR_LK_MAX_MOTORS 32
+
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+typedef enum {
+    MOTOR_LK_MF9025,
+    MOTOR_LK_MF9035,
+} MOTOR_LK_Module_t;
+
+
+/*每个电机需要的参数*/
+typedef struct {
+    BSP_CAN_t can;
+    uint16_t id;
+    MOTOR_LK_Module_t module;
+    bool reverse;
+} MOTOR_LK_Param_t;
+
+/*电机实例*/
+typedef struct{
+    MOTOR_LK_Param_t param;
+    MOTOR_t motor;
+} MOTOR_LK_t;
+
+/*CAN管理器，管理一个CAN总线上所有的电机*/
+typedef struct {
+    BSP_CAN_t can;
+    MOTOR_LK_t *motors[MOTOR_LK_MAX_MOTORS];
+    uint8_t motor_count;
+} MOTOR_LK_CANManager_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+
+/**
+ * @brief 注册一个LK电机
+ * @param param 电机参数
+ * @return 
+ */
+int8_t MOTOR_LK_Register(MOTOR_LK_Param_t *param);
+
+/**
+ * @brief 更新指定电机数据
+ * @param param 电机参数
+ * @return 
+ */
+int8_t MOTOR_LK_Update(MOTOR_LK_Param_t *param);
+
+/**
+ * @brief 设置一个电机的输出
+ * @param param 电机参数
+ * @param value 输出值，范围[-1.0, 1.0]
+ * @return 
+ */
+int8_t MOTOR_LK_SetOutput(MOTOR_LK_Param_t *param, float value);
+
+/**
+ * @brief 发送控制命令到电机，注意一个CAN可以控制多个电机，所以只需要发送一次即可
+ * @param param 电机参数
+ * @return 
+ */
+int8_t MOTOR_LK_Ctrl(MOTOR_LK_Param_t *param);
+
+/**
+ * @brief 发送电机开启命令
+ * @param param 电机参数
+ * @return 
+ */
+int8_t MOTOR_LK_MotorOn(MOTOR_LK_Param_t *param);
+
+/**
+ * @brief 发送电机关闭命令
+ * @param param 电机参数
+ * @return 
+ */
+int8_t MOTOR_LK_MotorOff(MOTOR_LK_Param_t *param);
+
+/**
+ * @brief 获取指定电机的实例指针
+ * @param param 电机参数
+ * @return 
+ */
+MOTOR_LK_t* MOTOR_LK_GetMotor(MOTOR_LK_Param_t *param);
+
+/**
+ * @brief 使电机松弛（设置输出为0）
+ * @param param 
+ * @return 
+ */
+int8_t MOTOR_LK_Relax(MOTOR_LK_Param_t *param);
+
+/**
+ * @brief 使电机离线（设置在线状态为false）
+ * @param param 
+ * @return 
+ */
+int8_t MOTOR_LK_Offine(MOTOR_LK_Param_t *param);
+
+/**
+ * @brief 
+ * @param  
+ * @return 
+ */
+int8_t MOTOR_LK_UpdateAll(void);
+
+#ifdef __cplusplus
+}
+#endif

assets/User_code/device/motor_lz.c

@@ -0,0 +1,532 @@
+/*
+  灵足电机驱动
+  
+  灵足电机通信协议：
+  - CAN 2.0通信接口，波特率1Mbps
+  - 采用扩展帧格式(29位ID)
+  - ID格式：Bit28~24(通信类型) + Bit23~8(数据区2) + Bit7~0(目标地址)
+*/
+/* Includes ----------------------------------------------------------------- */
+#include "motor_lz.h"
+#include <stdint.h>
+#include <stdbool.h>
+#include <string.h>
+#include "bsp/can.h"
+#include "bsp/mm.h"
+#include "bsp/time.h"
+#include "component/user_math.h"
+
+/* Private define ----------------------------------------------------------- */
+// 灵足电机协议参数
+#define LZ_ANGLE_RANGE_RAD          (12.57f)        /* 角度范围 ±12.57 rad */
+#define LZ_VELOCITY_RANGE_RAD_S     (20.0f)         /* 角速度范围 ±20 rad/s */
+#define LZ_TORQUE_RANGE_NM          (60.0f)         /* 力矩范围 ±60 Nm */
+#define LZ_KP_MAX                   (5000.0f)       /* Kp最大值 */
+#define LZ_KD_MAX                   (100.0f)        /* Kd最大值 */
+
+#define LZ_RAW_VALUE_MAX            (65535)         /* 16位原始值最大值 */
+#define LZ_TEMP_SCALE               (10.0f)         /* 温度缩放因子 */
+
+#define LZ_MAX_RECOVER_DIFF_RAD    (0.4f)
+#define MOTOR_TX_BUF_SIZE           (8)
+#define MOTOR_RX_BUF_SIZE           (8)
+
+/* Private macro ------------------------------------------------------------ */
+
+MOTOR_LZ_MotionParam_t lz_recover_param = {
+    .target_angle = 0.0f,
+    .target_velocity = 0.0f,
+    .kp = 20.0f,
+    .kd = 1.0f,
+    .torque = 0.0f,
+};
+/* Private typedef ---------------------------------------------------------- */
+/* Private variables -------------------------------------------------------- */
+static MOTOR_LZ_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL};
+
+/* Private function prototypes ---------------------------------------------- */
+static MOTOR_LZ_CANManager_t* MOTOR_LZ_GetCANManager(BSP_CAN_t can);
+static int8_t MOTOR_LZ_CreateCANManager(BSP_CAN_t can);
+static void MOTOR_LZ_Decode(MOTOR_LZ_t *motor, BSP_CAN_Message_t *msg);
+static uint32_t MOTOR_LZ_BuildExtID(MOTOR_LZ_CmdType_t cmd_type, uint16_t data2, uint8_t target_id);
+static uint16_t MOTOR_LZ_FloatToRaw(float value, float max_value);
+static float MOTOR_LZ_RawToFloat(uint16_t raw_value, float max_value);
+static int8_t MOTOR_LZ_SendExtFrame(BSP_CAN_t can, uint32_t ext_id, uint8_t *data, uint8_t dlc);
+static uint32_t MOTOR_LZ_IdParser(uint32_t original_id, BSP_CAN_FrameType_t frame_type);
+
+/* Private functions -------------------------------------------------------- */
+
+/**
+ * @brief 获取CAN管理器
+ */
+static MOTOR_LZ_CANManager_t* MOTOR_LZ_GetCANManager(BSP_CAN_t can) {
+    if (can >= BSP_CAN_NUM) return NULL;
+    return can_managers[can];
+}
+
+/**
+ * @brief 创建CAN管理器
+ */
+static int8_t MOTOR_LZ_CreateCANManager(BSP_CAN_t can) {
+    if (can >= BSP_CAN_NUM) return DEVICE_ERR;
+    if (can_managers[can] != NULL) return DEVICE_OK;
+    
+    can_managers[can] = (MOTOR_LZ_CANManager_t*)BSP_Malloc(sizeof(MOTOR_LZ_CANManager_t));
+    if (can_managers[can] == NULL) return DEVICE_ERR;
+    
+    memset(can_managers[can], 0, sizeof(MOTOR_LZ_CANManager_t));
+    can_managers[can]->can = can;
+    return DEVICE_OK;
+}
+
+/**
+ * @brief 构建扩展ID
+ */
+static uint32_t MOTOR_LZ_BuildExtID(MOTOR_LZ_CmdType_t cmd_type, uint16_t data2, uint8_t target_id) {
+    uint32_t ext_id = 0;
+    ext_id |= ((uint32_t)cmd_type & 0x1F) << 24;      // Bit28~24: 通信类型
+    ext_id |= ((uint32_t)data2 & 0xFFFF) << 8;        // Bit23~8: 数据区2
+    ext_id |= ((uint32_t)target_id & 0xFF);           // Bit7~0: 目标地址
+    return ext_id;
+}
+
+/**
+ * @brief 浮点值转换为原始值（对称范围：-max_value ~ +max_value）
+ */
+static uint16_t MOTOR_LZ_FloatToRaw(float value, float max_value) {
+    // 限制范围
+    if (value > max_value) value = max_value;
+    if (value < -max_value) value = -max_value;
+    
+    // 转换为0~65535范围，对应-max_value~max_value
+    return (uint16_t)((value + max_value) / (2.0f * max_value) * (float)LZ_RAW_VALUE_MAX);
+}
+
+/**
+ * @brief 浮点值转换为原始值（单向范围：0 ~ +max_value）
+ */
+static uint16_t MOTOR_LZ_FloatToRawPositive(float value, float max_value) {
+    // 限制范围
+    if (value > max_value) value = max_value;
+    if (value < 0.0f) value = 0.0f;
+    
+    // 转换为0~65535范围，对应0~max_value
+    return (uint16_t)(value / max_value * (float)LZ_RAW_VALUE_MAX);
+}
+
+/**
+ * @brief 原始值转换为浮点值
+ */
+static float MOTOR_LZ_RawToFloat(uint16_t raw_value, float max_value) {
+    // 将0~65535范围转换为-max_value~max_value
+    return ((float)raw_value / (float)LZ_RAW_VALUE_MAX) * (2.0f * max_value) - max_value;
+}
+
+/**
+ * @brief 发送扩展帧
+ */
+static int8_t MOTOR_LZ_SendExtFrame(BSP_CAN_t can, uint32_t ext_id, uint8_t *data, uint8_t dlc) {
+    BSP_CAN_ExtDataFrame_t tx_frame;
+    tx_frame.id = ext_id;
+    tx_frame.dlc = dlc;
+    if (data != NULL) {
+        memcpy(tx_frame.data, data, dlc);
+    } else {
+        memset(tx_frame.data, 0, dlc);
+    }
+    
+    return BSP_CAN_TransmitExtDataFrame(can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
+}
+
+/**
+ * @brief 灵足电机ID解析器
+ * @param original_id 原始CAN ID（29位扩展帧）
+ * @param frame_type 帧类型
+ * @return 解析后的ID（用于队列匹配）
+ * 
+ * 灵足电机扩展ID格式：
+ * Bit28~24: 通信类型 (0x1=运控控制, 0x2=反馈数据, 0x3=使能, 0x4=停止, 0x6=设零位)
+ * Bit23~8:  数据区2 (根据通信类型而定)
+ * Bit7~0:   目标地址 (目标电机CAN ID)
+ */
+static uint32_t MOTOR_LZ_IdParser(uint32_t original_id, BSP_CAN_FrameType_t frame_type) {
+    // 只处理扩展数据帧
+    if (frame_type != BSP_CAN_FRAME_EXT_DATA) {
+        return original_id; // 非扩展帧直接返回原始ID
+    }
+    
+    // 解析扩展ID各个字段
+    uint8_t cmd_type = (original_id >> 24) & 0x1F;    // Bit28~24: 通信类型
+    uint16_t data2 = (original_id >> 8) & 0xFFFF;     // Bit23~8: 数据区2
+    uint8_t host_id = (uint8_t)(original_id & 0xFF);  // Bit7~0: 主机CAN ID
+    
+    // 对于反馈数据帧，我们使用特殊的解析规则
+    if (cmd_type == MOTOR_LZ_CMD_FEEDBACK) {
+        // 反馈数据的data2字段包含：
+        // Bit8~15: 当前电机CAN ID
+        // Bit16~21: 故障信息
+        // Bit22~23: 模式状态
+        uint8_t motor_can_id = data2 & 0xFF;          // bit8~15: 当前电机CAN ID
+        
+        // 返回格式化的ID，便于匹配
+        // 格式：0x02HHMMTT (02=反馈命令, HH=主机ID, MM=电机ID, TT=主机ID)
+        return (0x02000000) | (host_id << 16) | (motor_can_id << 8) | host_id;
+    }
+    
+    // 对于其他命令类型，直接返回原始ID
+    return original_id;
+}
+
+/**
+ * @brief 解码灵足电机反馈数据
+ */
+static void MOTOR_LZ_Decode(MOTOR_LZ_t *motor, BSP_CAN_Message_t *msg) {
+    if (motor == NULL || msg == NULL) return;
+    
+    // 检查是否为反馈数据帧 (通信类型2)
+    // 需要使用原始ID来解析，因为parsed_id已经被IdParser处理过了
+    uint8_t cmd_type = (msg->original_id >> 24) & 0x1F;
+    if (cmd_type != MOTOR_LZ_CMD_FEEDBACK) return;
+    
+    // 解析原始ID中的数据区2 (bit23~8)
+    uint16_t id_data2 = (msg->original_id >> 8) & 0xFFFF;
+    uint8_t motor_can_id = id_data2 & 0xFF;          // Bit8~15: 当前电机CAN ID
+    uint8_t fault_info = (id_data2 >> 8) & 0x3F;     // Bit16~21: 故障信息
+    uint8_t mode_state = (id_data2 >> 14) & 0x03;    // Bit22~23: 模式状态
+    
+    // 更新电机CAN ID
+    motor->lz_feedback.motor_can_id = motor_can_id;
+    
+    // 解析故障信息
+    motor->lz_feedback.fault.under_voltage = (fault_info & 0x01) != 0;      // bit16
+    motor->lz_feedback.fault.driver_fault = (fault_info & 0x02) != 0;       // bit17
+    motor->lz_feedback.fault.over_temp = (fault_info & 0x04) != 0;          // bit18
+    motor->lz_feedback.fault.encoder_fault = (fault_info & 0x08) != 0;      // bit19
+    motor->lz_feedback.fault.stall_overload = (fault_info & 0x10) != 0;     // bit20
+    motor->lz_feedback.fault.uncalibrated = (fault_info & 0x20) != 0;       // bit21
+    
+    // 解析模式状态
+    motor->lz_feedback.state = (MOTOR_LZ_State_t)mode_state;
+    
+    // 解析数据区
+    // Byte0~1: 当前角度 (高字节在前，低字节在后)
+    uint16_t raw_angle = (uint16_t)((msg->data[0] << 8) | msg->data[1]);
+    motor->lz_feedback.current_angle = MOTOR_LZ_RawToFloat(raw_angle, LZ_ANGLE_RANGE_RAD);
+    
+    // Byte2~3: 当前角速度 (高字节在前，低字节在后)
+    uint16_t raw_velocity = (uint16_t)((msg->data[2] << 8) | msg->data[3]);
+    motor->lz_feedback.current_velocity = MOTOR_LZ_RawToFloat(raw_velocity, LZ_VELOCITY_RANGE_RAD_S);
+    
+    // Byte4~5: 当前力矩 (高字节在前，低字节在后)
+    uint16_t raw_torque = (uint16_t)((msg->data[4] << 8) | msg->data[5]);
+    motor->lz_feedback.current_torque = MOTOR_LZ_RawToFloat(raw_torque, LZ_TORQUE_RANGE_NM);
+    
+    // Byte6~7: 当前温度 (温度*10) (高字节在前，低字节在后)
+    uint16_t raw_temp = (uint16_t)((msg->data[6] << 8) | msg->data[7]);
+    motor->lz_feedback.temperature = (float)raw_temp / LZ_TEMP_SCALE;
+    
+    // 更新通用电机反馈信息
+    motor->motor.feedback.rotor_abs_angle = motor->lz_feedback.current_angle;
+    motor->motor.feedback.rotor_speed = motor->lz_feedback.current_velocity * 180.0f / M_PI * 6.0f; // 转换为RPM
+    motor->motor.feedback.torque_current = motor->lz_feedback.current_torque; // 使用力矩作为电流反馈
+    motor->motor.feedback.temp = (int8_t)motor->lz_feedback.temperature;
+    
+    // 更新在线状态
+    motor->motor.header.online = true;
+    motor->motor.header.last_online_time = BSP_TIME_Get();
+}
+
+/* Exported functions ------------------------------------------------------- */
+
+/**
+ * @brief 初始化灵足电机驱动系统
+ * @return 设备状态码
+ */
+int8_t MOTOR_LZ_Init(void) {
+    // 注册灵足电机专用的ID解析器
+    return BSP_CAN_RegisterIdParser(MOTOR_LZ_IdParser) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
+}
+
+/**
+ * @brief 反初始化灵足电机驱动系统
+ * @return 设备状态码
+ */
+int8_t MOTOR_LZ_DeInit(void) {
+    // 注销ID解析器
+    return BSP_CAN_UnregisterIdParser() == BSP_OK ? DEVICE_OK : DEVICE_ERR;
+}
+
+int8_t MOTOR_LZ_Register(MOTOR_LZ_Param_t *param) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+    
+    if (MOTOR_LZ_CreateCANManager(param->can) != DEVICE_OK) return DEVICE_ERR;
+    MOTOR_LZ_CANManager_t *manager = MOTOR_LZ_GetCANManager(param->can);
+    if (manager == NULL) return DEVICE_ERR;
+    
+    // 检查是否已注册
+    for (int i = 0; i < manager->motor_count; i++) {
+        if (manager->motors[i] && manager->motors[i]->param.motor_id == param->motor_id) {
+            return DEVICE_ERR; // 已注册
+        }
+    }
+    
+    // 检查数量
+    if (manager->motor_count >= MOTOR_LZ_MAX_MOTORS) return DEVICE_ERR;
+    
+    // 创建新电机实例
+    MOTOR_LZ_t *new_motor = (MOTOR_LZ_t*)BSP_Malloc(sizeof(MOTOR_LZ_t));
+    if (new_motor == NULL) return DEVICE_ERR;
+    
+    memcpy(&new_motor->param, param, sizeof(MOTOR_LZ_Param_t));
+    memset(&new_motor->motor, 0, sizeof(MOTOR_t));
+    memset(&new_motor->lz_feedback, 0, sizeof(MOTOR_LZ_Feedback_t));
+    memset(&new_motor->motion_param, 0, sizeof(MOTOR_LZ_MotionParam_t));
+    
+    new_motor->motor.reverse = param->reverse;
+    
+    // 注册CAN接收ID - 使用解析后的反馈数据ID
+    // 构建反馈数据的原始扩展ID
+    // 反馈数据：data2包含电机ID(bit8~15)，target_id是主机ID
+    uint32_t original_feedback_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_FEEDBACK, param->motor_id, param->host_id);
+    // 通过ID解析器得到解析后的ID
+    uint32_t parsed_feedback_id = MOTOR_LZ_IdParser(original_feedback_id, BSP_CAN_FRAME_EXT_DATA);
+    
+    if (BSP_CAN_RegisterId(param->can, parsed_feedback_id, 3) != BSP_OK) {
+        BSP_Free(new_motor);
+        return DEVICE_ERR;
+    }
+    
+    manager->motors[manager->motor_count] = new_motor;
+    manager->motor_count++;
+    return DEVICE_OK;
+}
+
+int8_t MOTOR_LZ_Update(MOTOR_LZ_Param_t *param) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+    
+    MOTOR_LZ_CANManager_t *manager = MOTOR_LZ_GetCANManager(param->can);
+    if (manager == NULL) return DEVICE_ERR_NO_DEV;
+    
+    for (int i = 0; i < manager->motor_count; i++) {
+        MOTOR_LZ_t *motor = manager->motors[i];
+        if (motor && motor->param.motor_id == param->motor_id) {
+            // 获取反馈数据 - 使用解析后的ID
+            uint32_t original_feedback_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_FEEDBACK, param->motor_id, param->host_id);
+            uint32_t parsed_feedback_id = MOTOR_LZ_IdParser(original_feedback_id, BSP_CAN_FRAME_EXT_DATA);
+            BSP_CAN_Message_t msg;
+            
+            while (BSP_CAN_GetMessage(param->can, parsed_feedback_id, &msg, 0) == BSP_OK) {
+                MOTOR_LZ_Decode(motor, &msg);
+            }
+            return DEVICE_OK;
+        }
+    }
+    return DEVICE_ERR_NO_DEV;
+}
+
+int8_t MOTOR_LZ_UpdateAll(void) {
+    int8_t ret = DEVICE_OK;
+    for (int can = 0; can < BSP_CAN_NUM; can++) {
+        MOTOR_LZ_CANManager_t *manager = MOTOR_LZ_GetCANManager((BSP_CAN_t)can);
+        if (manager == NULL) continue;
+        
+        for (int i = 0; i < manager->motor_count; i++) {
+            MOTOR_LZ_t *motor = manager->motors[i];
+            if (motor) {
+                if (MOTOR_LZ_Update(&motor->param) != DEVICE_OK) {
+                    ret = DEVICE_ERR;
+                }
+            }
+        }
+    }
+    return ret;
+}
+
+int8_t MOTOR_LZ_MotionControl(MOTOR_LZ_Param_t *param, MOTOR_LZ_MotionParam_t *motion_param) {
+    if (param == NULL || motion_param == NULL) return DEVICE_ERR_NULL;
+    
+    MOTOR_LZ_t *motor = MOTOR_LZ_GetMotor(param);
+    if (motor == NULL) return DEVICE_ERR_NO_DEV;
+    
+    // 更新运控参数
+    memcpy(&motor->motion_param, motion_param, sizeof(MOTOR_LZ_MotionParam_t));
+    
+    // 根据协议，bit23~8数据区2包含力矩信息
+    // 力矩范围：-60Nm~60Nm 对应 0~65535
+    uint16_t raw_torque = MOTOR_LZ_FloatToRaw(motion_param->torque, LZ_TORQUE_RANGE_NM);
+    
+    // 构建扩展ID - 运控模式控制指令
+    // bit28~24: 0x1 (运控模式)
+    // bit23~8:  力矩数据 (0~65535)，协议中描述为"Byte2:力矩"
+    // bit7~0:   目标电机CAN_ID
+    uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_MOTION, raw_torque, param->motor_id);
+    
+    // 准备8字节数据区
+    uint8_t data[8];
+    
+    // Byte0~1: 目标角度 [0~65535] 对应 (-12.57f~12.57f rad) (高字节在前，低字节在后)
+    uint16_t raw_angle = MOTOR_LZ_FloatToRaw(motion_param->target_angle, LZ_ANGLE_RANGE_RAD);
+    data[0] = (raw_angle >> 8) & 0xFF;  // 高字节
+    data[1] = raw_angle & 0xFF;         // 低字节
+    
+    // Byte2~3: 目标角速度 [0~65535] 对应 (-20rad/s~20rad/s) (高字节在前，低字节在后)
+    uint16_t raw_velocity = MOTOR_LZ_FloatToRaw(motion_param->target_velocity, LZ_VELOCITY_RANGE_RAD_S);
+    data[2] = (raw_velocity >> 8) & 0xFF;  // 高字节
+    data[3] = raw_velocity & 0xFF;         // 低字节
+    
+    // Byte4~5: Kp [0~65535] 对应 (0.0~5000.0) (高字节在前，低字节在后)
+    uint16_t raw_kp = MOTOR_LZ_FloatToRawPositive(motion_param->kp, LZ_KP_MAX);
+    data[4] = (raw_kp >> 8) & 0xFF;     // 高字节
+    data[5] = raw_kp & 0xFF;            // 低字节
+    
+    // Byte6~7: Kd [0~65535] 对应 (0.0~100.0) (高字节在前，低字节在后)
+    uint16_t raw_kd = MOTOR_LZ_FloatToRawPositive(motion_param->kd, LZ_KD_MAX);
+    data[6] = (raw_kd >> 8) & 0xFF;     // 高字节
+    data[7] = raw_kd & 0xFF;            // 低字节
+    
+    return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8);
+}
+
+int8_t MOTOR_LZ_Enable(MOTOR_LZ_Param_t *param) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+    
+    // 构建扩展ID - 使能命令
+    uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_ENABLE, param->host_id, param->motor_id);
+    
+    // 数据区清零
+    uint8_t data[8] = {0};
+    
+    return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8);
+}
+
+int8_t MOTOR_LZ_Disable(MOTOR_LZ_Param_t *param, bool clear_fault) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+    
+    // 构建扩展ID - 停止命令
+    uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_DISABLE, param->host_id, param->motor_id);
+    
+    // 数据区
+    uint8_t data[8] = {0};
+    if (clear_fault) {
+        data[0] = 1; // Byte[0]=1时清故障
+    }
+    
+    return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8);
+}
+
+int8_t MOTOR_LZ_SetZero(MOTOR_LZ_Param_t *param) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+    
+    // 构建扩展ID - 设置零位命令
+    uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_SET_ZERO, param->host_id, param->motor_id);
+    
+    // 数据区 - Byte[0]=1
+    uint8_t data[8] = {1, 0, 0, 0, 0, 0, 0, 0};
+    
+    return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8);
+}
+
+MOTOR_LZ_t* MOTOR_LZ_GetMotor(MOTOR_LZ_Param_t *param) {
+    if (param == NULL) return NULL;
+    
+    MOTOR_LZ_CANManager_t *manager = MOTOR_LZ_GetCANManager(param->can);
+    if (manager == NULL) return NULL;
+    
+    for (int i = 0; i < manager->motor_count; i++) {
+        MOTOR_LZ_t *motor = manager->motors[i];
+        if (motor && motor->param.motor_id == param->motor_id) {
+            return motor;
+        }
+    }
+    return NULL;
+}
+
+int8_t MOTOR_LZ_Relax(MOTOR_LZ_Param_t *param) {
+    return MOTOR_LZ_Disable(param, false);
+}
+
+int8_t MOTOR_LZ_Offline(MOTOR_LZ_Param_t *param) {
+    MOTOR_LZ_t *motor = MOTOR_LZ_GetMotor(param);
+    if (motor) {
+        motor->motor.header.online = false;
+        return DEVICE_OK;
+    }
+    return DEVICE_ERR_NO_DEV;
+}
+
+static MOTOR_LZ_Feedback_t* MOTOR_LZ_GetFeedback(MOTOR_LZ_Param_t *param) {
+    MOTOR_LZ_t *motor = MOTOR_LZ_GetMotor(param);
+    if (motor && motor->motor.header.online) {
+        return &motor->lz_feedback;
+    }
+    return NULL;
+}
+
+int8_t MOTOR_LZ_TorqueControl(MOTOR_LZ_Param_t *param, float torque) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+    
+    // 创建运控参数，只设置力矩，其他参数为0
+    MOTOR_LZ_MotionParam_t motion_param = {0};
+    motion_param.torque = torque;
+    motion_param.target_angle = 0.0f;
+    motion_param.target_velocity = 0.0f;
+    motion_param.kp = 0.0f;
+    motion_param.kd = 0.0f;
+    
+    return MOTOR_LZ_MotionControl(param, &motion_param);
+}
+
+int8_t MOTOR_LZ_PositionControl(MOTOR_LZ_Param_t *param, float target_angle, float max_velocity) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+    
+    // 创建运控参数，设置位置和速度限制
+    MOTOR_LZ_MotionParam_t motion_param = {0};
+    motion_param.target_angle = target_angle;
+    motion_param.target_velocity = max_velocity;
+    motion_param.torque = 0.0f;
+    motion_param.kp = 100.0f;  // 默认位置增益
+    motion_param.kd = 5.0f;    // 默认微分增益
+    
+    return MOTOR_LZ_MotionControl(param, &motion_param);
+}
+
+int8_t MOTOR_LZ_VelocityControl(MOTOR_LZ_Param_t *param, float target_velocity) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+    
+    // 创建运控参数，只设置速度
+    MOTOR_LZ_MotionParam_t motion_param = {0};
+    motion_param.target_angle = 0.0f;
+    motion_param.target_velocity = target_velocity;
+    motion_param.torque = 0.0f;
+    motion_param.kp = 0.0f;
+    motion_param.kd = 1.0f;    // 少量阻尼
+    
+    return MOTOR_LZ_MotionControl(param, &motion_param);
+}
+
+int8_t MOTOR_LZ_RecoverToZero(MOTOR_LZ_Param_t *param) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+    
+    MOTOR_LZ_t *motor = MOTOR_LZ_GetMotor(param);
+    if (motor == NULL) return DEVICE_ERR_NO_DEV;
+    
+    // 获取当前角度
+    MOTOR_LZ_Feedback_t *feedback = MOTOR_LZ_GetFeedback(param);
+    if (feedback == NULL) return DEVICE_ERR_NO_DEV;
+    
+    float current_angle = feedback->current_angle;
+    
+    // 计算目标角度为0时的最短路径
+    float angle_diff = -current_angle; // 目标是0，所以差值就是-current_angle
+    // 限制最大差值，防止过大跳变
+    if (angle_diff > LZ_MAX_RECOVER_DIFF_RAD) angle_diff = LZ_MAX_RECOVER_DIFF_RAD;
+    if (angle_diff < -LZ_MAX_RECOVER_DIFF_RAD) angle_diff = -LZ_MAX_RECOVER_DIFF_RAD;
+    
+    float target_angle = current_angle + angle_diff;
+    
+    // 创建运控参数，设置位置和速度限制
+    MOTOR_LZ_MotionParam_t motion_param = lz_recover_param; // 使用预设的恢复参数
+    motion_param.target_angle = target_angle;
+
+    return MOTOR_LZ_MotionControl(param, &motion_param);
+}

assets/User_code/device/motor_lz.h

@@ -0,0 +1,220 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include "device/device.h"
+#include "device/motor.h"
+#include "bsp/can.h"
+
+/* Exported constants ------------------------------------------------------- */
+#define MOTOR_LZ_MAX_MOTORS 32
+
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+typedef enum {
+    MOTOR_LZ_RSO0,
+    MOTOR_LZ_RSO1,
+    MOTOR_LZ_RSO2,
+    MOTOR_LZ_RSO3,
+    MOTOR_LZ_RSO4,
+    MOTOR_LZ_RSO5,
+    MOTOR_LZ_RSO6,
+} MOTOR_LZ_Module_t;
+
+/* 灵足电机控制模式 */
+typedef enum {
+    MOTOR_LZ_MODE_MOTION = 0x1,     /* 运控模式 */
+    MOTOR_LZ_MODE_CURRENT = 0x2,    /* 电流模式 */
+    MOTOR_LZ_MODE_VELOCITY = 0x3,   /* 速度模式 */
+    MOTOR_LZ_MODE_POSITION = 0x4,   /* 位置模式 */
+} MOTOR_LZ_ControlMode_t;
+
+/* 灵足电机通信类型 */
+typedef enum {
+    MOTOR_LZ_CMD_MOTION = 0x1,      /* 运控模式控制 */
+    MOTOR_LZ_CMD_FEEDBACK = 0x2,    /* 电机反馈数据 */
+    MOTOR_LZ_CMD_ENABLE = 0x3,      /* 电机使能运行 */
+    MOTOR_LZ_CMD_DISABLE = 0x4,     /* 电机停止运行 */
+    MOTOR_LZ_CMD_SET_ZERO = 0x6,    /* 设置电机机械零位 */
+} MOTOR_LZ_CmdType_t;
+
+/* 灵足电机运行状态 */
+typedef enum {
+    MOTOR_LZ_STATE_RESET = 0,       /* Reset模式[复位] */
+    MOTOR_LZ_STATE_CALI = 1,        /* Cali模式[标定] */
+    MOTOR_LZ_STATE_MOTOR = 2,       /* Motor模式[运行] */
+} MOTOR_LZ_State_t;
+
+/* 灵足电机故障信息 */
+typedef struct {
+    bool uncalibrated;      /* bit21: 未标定 */
+    bool stall_overload;    /* bit20: 堵转过载故障 */
+    bool encoder_fault;     /* bit19: 磁编码故障 */
+    bool over_temp;         /* bit18: 过温 */
+    bool driver_fault;      /* bit17: 驱动故障 */
+    bool under_voltage;     /* bit16: 欠压故障 */
+} MOTOR_LZ_Fault_t;
+
+/* 灵足电机运控参数 */
+typedef struct {
+    float target_angle;     /* 目标角度 (-12.57f~12.57f rad) */
+    float target_velocity;  /* 目标角速度 (-20~20 rad/s) */
+    float kp;               /* 位置增益 (0.0~5000.0) */
+    float kd;               /* 微分增益 (0.0~100.0) */
+    float torque;           /* 力矩 (-60~60 Nm) */
+} MOTOR_LZ_MotionParam_t;
+
+/*每个电机需要的参数*/
+typedef struct {
+    BSP_CAN_t can;              /* CAN总线 */
+    uint8_t motor_id;           /* 电机CAN ID */
+    uint8_t host_id;            /* 主机CAN ID */
+    MOTOR_LZ_Module_t module;   /* 电机型号 */
+    bool reverse;               /* 是否反向 */
+    MOTOR_LZ_ControlMode_t mode; /* 控制模式 */
+} MOTOR_LZ_Param_t;
+
+/*电机反馈信息扩展*/
+typedef struct {
+    float current_angle;        /* 当前角度 (-12.57f~12.57f rad) */
+    float current_velocity;     /* 当前角速度 (-20~20 rad/s) */
+    float current_torque;       /* 当前力矩 (-60~60 Nm) */
+    float temperature;          /* 当前温度 (摄氏度) */
+    MOTOR_LZ_State_t state;     /* 运行状态 */
+    MOTOR_LZ_Fault_t fault;     /* 故障信息 */
+    uint8_t motor_can_id;       /* 当前电机CAN ID */
+} MOTOR_LZ_Feedback_t;
+
+/*电机实例*/
+typedef struct {
+    MOTOR_LZ_Param_t param;
+    MOTOR_t motor;
+    MOTOR_LZ_Feedback_t lz_feedback;    /* 灵足电机特有反馈信息 */
+    MOTOR_LZ_MotionParam_t motion_param; /* 运控模式参数 */
+} MOTOR_LZ_t;
+
+/*CAN管理器，管理一个CAN总线上所有的电机*/
+typedef struct {
+    BSP_CAN_t can;
+    MOTOR_LZ_t *motors[MOTOR_LZ_MAX_MOTORS];
+    uint8_t motor_count;
+} MOTOR_LZ_CANManager_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+
+/**
+ * @brief 初始化灵足电机驱动系统
+ * @return 设备状态码
+ */
+int8_t MOTOR_LZ_Init(void);
+
+/**
+ * @brief 反初始化灵足电机驱动系统
+ * @return 设备状态码
+ */
+int8_t MOTOR_LZ_DeInit(void);
+
+/**
+ * @brief 注册一个灵足电机
+ * @param param 电机参数
+ * @return 设备状态码
+ */
+int8_t MOTOR_LZ_Register(MOTOR_LZ_Param_t *param);
+
+/**
+ * @brief 更新指定电机数据
+ * @param param 电机参数
+ * @return 设备状态码
+ */
+int8_t MOTOR_LZ_Update(MOTOR_LZ_Param_t *param);
+
+/**
+ * @brief 更新所有电机数据
+ * @return 设备状态码
+ */
+int8_t MOTOR_LZ_UpdateAll(void);
+
+/**
+ * @brief 运控模式控制电机
+ * @param param 电机参数
+ * @param motion_param 运控参数
+ * @return 设备状态码
+ */
+int8_t MOTOR_LZ_MotionControl(MOTOR_LZ_Param_t *param, MOTOR_LZ_MotionParam_t *motion_param);
+
+/**
+ * @brief 电流(力矩)模式控制电机
+ * @param param 电机参数
+ * @param torque 目标力矩 (-60~60 Nm)
+ * @return 设备状态码
+ */
+int8_t MOTOR_LZ_TorqueControl(MOTOR_LZ_Param_t *param, float torque);
+
+/**
+ * @brief 位置模式控制电机
+ * @param param 电机参数
+ * @param target_angle 目标角度 (-12.57~12.57 rad)
+ * @param max_velocity 最大速度 (0~20 rad/s)
+ * @return 设备状态码
+ */
+int8_t MOTOR_LZ_PositionControl(MOTOR_LZ_Param_t *param, float target_angle, float max_velocity);
+
+/**
+ * @brief 速度模式控制电机
+ * @param param 电机参数
+ * @param target_velocity 目标速度 (-20~20 rad/s)
+ * @return 设备状态码
+ */
+int8_t MOTOR_LZ_VelocityControl(MOTOR_LZ_Param_t *param, float target_velocity);
+
+/**
+ * @brief 电机使能运行
+ * @param param 电机参数
+ * @return 设备状态码
+ */
+int8_t MOTOR_LZ_Enable(MOTOR_LZ_Param_t *param);
+
+/**
+ * @brief 电机停止运行
+ * @param param 电机参数
+ * @param clear_fault 是否清除故障
+ * @return 设备状态码
+ */
+int8_t MOTOR_LZ_Disable(MOTOR_LZ_Param_t *param, bool clear_fault);
+
+/**
+ * @brief 设置电机机械零位
+ * @param param 电机参数
+ * @return 设备状态码
+ */
+int8_t MOTOR_LZ_SetZero(MOTOR_LZ_Param_t *param);
+
+/**
+ * @brief 获取指定电机的实例指针
+ * @param param 电机参数
+ * @return 电机实例指针，失败返回NULL
+ */
+MOTOR_LZ_t* MOTOR_LZ_GetMotor(MOTOR_LZ_Param_t *param);
+
+/**
+ * @brief 使电机松弛（发送停止命令）
+ * @param param 电机参数
+ * @return 设备状态码
+ */
+int8_t MOTOR_LZ_Relax(MOTOR_LZ_Param_t *param);
+
+/**
+ * @brief 使电机离线（设置在线状态为false）
+ * @param param 电机参数
+ * @return 设备状态码
+ */
+int8_t MOTOR_LZ_Offline(MOTOR_LZ_Param_t *param);
+
+int8_t MOTOR_LZ_RecoverToZero(MOTOR_LZ_Param_t *param);
+
+#ifdef __cplusplus
+}
+#endif

assets/User_code/device/servo.c

@@ -0,0 +1,39 @@
+/*
+	pwm控制舵机
+*/
+
+/*Includes   -----------------------------------------*/ 
+
+#include "bsp/pwm.h"
+#include "servo.h"
+
+#define SERVO_MIN_DUTY  0.025f   
+#define SERVO_MAX_DUTY  0.125f   
+
+/**
+ * @brief  
+ * @param  
+ * @retval BSP_OK / BSP_ERR
+ */
+
+int8_t SERVO_Init(SERVO_t *servo) {
+    if (servo == NULL) return BSP_ERR;
+    return BSP_PWM_Start(servo->pwm_ch);
+}
+
+int8_t SERVO_SetAngle(SERVO_t *servo, float angle) {
+    if (servo == NULL) return BSP_ERR;
+    
+	  /*限制角度范围*/
+    if (angle < 0.0f)   angle = 0.0f;
+    if (angle > 180.0f) angle = 180.0f;
+    /*角度映射到占空比*/
+    float duty = servo->min_duty + (angle / 180.0f) * (servo->max_duty - servo->min_duty);
+
+    return BSP_PWM_Set(servo->pwm_ch, duty);
+}
+
+int8_t SERVO_Stop(SERVO_t *servo) {
+    if (servo == NULL) return BSP_ERR;
+    return BSP_PWM_Stop(servo->pwm_ch);
+}

assets/User_code/device/servo.h

@@ -0,0 +1,52 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include <cmsis_os2.h>
+	
+#include "bsp/pwm.h"
+	
+/* Exported constants ------------------------------------------------------- */
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+
+/**
+ * @brief 
+ */
+typedef struct {
+   BSP_PWM_Channel_t pwm_ch;  
+   float min_duty;         
+   float max_duty;      
+} SERVO_t;
+
+/**
+ * @brief  
+ * @param  servo 
+ * @retval BSP_OK / BSP_ERR
+ */
+
+int8_t SERVO_Init(SERVO_t *servo);
+
+/**
+ * @brief 
+ * @param  servo
+ * @param  angle 
+ * @retval BSP_OK / BSP_ERR
+ */
+int8_t SERVO_SetAngle(SERVO_t *servo, float angle);
+
+/**
+ * @brief 
+ * @param  servo 
+ * @retval BSP_OK / BSP_ERR
+ */
+ 
+int8_t SERVO_Stop(SERVO_t *servo);
+
+	
+#ifdef __cplusplus
+}
+#endif

assets/User_code/device/ws2812.c

@@ -0,0 +1,96 @@
+/* Includes ----------------------------------------------------------------- */
+#include "ws2812.h"
+#include "device.h"
+
+#include "bsp/pwm.h"
+#include <stdlib.h>
+/* Private define ----------------------------------------------------------- */
+#define DEVICE_WS2812_T1H            (uint16_t)(BSP_PWM_GetAutoReloadPreload(BSP_PWM_WS2812) * 0.56)   // High-level width of logic-1 pulse
+#define DEVICE_WS2812_T0H            (BSP_PWM_GetAutoReloadPreload(BSP_PWM_WS2812) * 0.29)             // High-level width of logic-0 pulse
+#define DEVICE_WS2812_WS_REST        40   // Number of reset pulses (low level) after data stream
+#define DEVICE_WS2812_DATA_LEN       24   // WS2812 data length: 24 bits (GRB) per LED
+#define DEVICE_WS2812_RST_NUM        50   // Extra reset pulses reserved at the end of the buffer
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+/* Private variables -------------------------------------------------------- */
+static uint16_t DEVICE_WS2812_LED_NUM; 	       // Total number of LEDs
+static uint16_t *DEVICE_WS2812_RGB_Buff = NULL;// PWM duty buffer for DMA
+/* Private function  -------------------------------------------------------- */
+/* Exported functions ------------------------------------------------------- */
+/**
+ * Set color of a single WS2812 LED
+ * @param num LED index (1-based)
+ * @param R   Red value   (0-255)
+ * @param G   Green value (0-255)
+ * @param B   Blue value  (0-255)
+ * @return    DEVICE_OK on success, DEVICE_ERR if num is invalid
+ */
+uint8_t DEVICE_WS2812_Set(uint16_t num, uint8_t R, uint8_t G, uint8_t B)
+{
+  if(num<1 || num>DEVICE_WS2812_LED_NUM) return DEVICE_ERR;
+  uint32_t indexx = (num-1) * DEVICE_WS2812_DATA_LEN;
+  
+  /* WS2812 uses GRB order, MSB first */
+  for (uint8_t i = 0; i < 8; i++) {
+    // G
+    DEVICE_WS2812_RGB_Buff[indexx + i]      = (G & (0x80 >> i)) ? DEVICE_WS2812_T1H : DEVICE_WS2812_T0H;
+    // R
+    DEVICE_WS2812_RGB_Buff[indexx + i + 8]  = (R & (0x80 >> i)) ? DEVICE_WS2812_T1H : DEVICE_WS2812_T0H;
+    // B
+    DEVICE_WS2812_RGB_Buff[indexx + i + 16] = (B & (0x80 >> i)) ? DEVICE_WS2812_T1H : DEVICE_WS2812_T0H;
+  }
+  return DEVICE_OK;
+}
+
+/**
+ * Initialize WS2812 driver
+ * @param ledNum Number of LEDs in the strip
+ * @return DEVICE_OK on success, DEVICE_ERR if memory allocation or PWM setup fails
+ */
+uint8_t DEVICE_WS2812_Init(uint16_t ledNum)
+{
+	DEVICE_WS2812_LED_NUM = ledNum;
+
+    if (DEVICE_WS2812_RGB_Buff != NULL)
+    {
+        free(DEVICE_WS2812_RGB_Buff);
+        DEVICE_WS2812_RGB_Buff = NULL;
+    }
+
+    /* Allocate new buffer: 24 PWM samples per LED + reset pulses */
+    size_t bufLen = ledNum * DEVICE_WS2812_DATA_LEN + DEVICE_WS2812_RST_NUM;
+    DEVICE_WS2812_RGB_Buff = (uint16_t *)malloc(bufLen * sizeof(uint16_t));
+    if (DEVICE_WS2812_RGB_Buff == NULL)
+        return DEVICE_ERR;              
+
+    /* Initialize all LEDs to dim green */
+    for (int i = 1; i <= ledNum; i++)
+        DEVICE_WS2812_Set(i, 0, 20, 0);
+	
+	/* Configure PWM frequency to 800 kHz and start DMA */
+    if (BSP_PWM_SetFreq(BSP_PWM_WS2812, 800000) == DEVICE_OK)
+        BSP_PWM_Start_DMA(
+            BSP_PWM_WS2812,
+            (uint32_t *)DEVICE_WS2812_RGB_Buff,
+            bufLen);
+    else
+        return DEVICE_ERR;
+
+    return DEVICE_OK;
+}
+
+/**
+ * De-initialize WS2812 driver
+ * Frees the DMA buffer and stops PWM
+ */
+void DEVICE_WS2812_DeInit()
+{
+	for (int i = 1; i <= DEVICE_WS2812_LED_NUM; i++)
+        DEVICE_WS2812_Set(i, 0, 0, 0);
+	if (DEVICE_WS2812_RGB_Buff != NULL)
+    {
+        free(DEVICE_WS2812_RGB_Buff);
+        DEVICE_WS2812_RGB_Buff = NULL;
+    }
+	BSP_PWM_Stop_DMA(BSP_PWM_WS2812);
+}

assets/User_code/device/ws2812.h

@@ -0,0 +1,19 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include <stdint.h>
+/* Exported constants ------------------------------------------------------- */
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+/* Exported functions prototypes -------------------------------------------- */
+uint8_t DEVICE_WS2812_Init(uint16_t led_num);
+uint8_t DEVICE_WS2812_Set(uint16_t num, uint8_t R, uint8_t G, uint8_t B);
+void DEVICE_WS2812_DeInit();
+
+#ifdef __cplusplus
+}
+#endif