diff --git a/.vscode/tasks.json b/.vscode/tasks.json
index a0cf025..8cac119 100644
--- a/.vscode/tasks.json
+++ b/.vscode/tasks.json
@@ -5,7 +5,7 @@
         {
             "label": "build task",         // 任务标签
             "type": "shell",               // 任务类型,因为要调用mingw32-make,是在终端(CMD)里运行的,所以是shell任务
-            "command": "mingw32-make -j24 -l12",// 任务命令,线程数可以根据自己的电脑修改,建议为核数的4~8倍
+            "command": "mingw32-make -j24 -l12",// 任务命令,线程数可以根据自己的电脑修改,建议与cpu核数相同
             "problemMatcher": [],          
             "group": {
                 "kind": "build",
diff --git a/Makefile b/Makefile
index c551cca..96a72d2 100644
--- a/Makefile
+++ b/Makefile
@@ -156,7 +156,6 @@ application/gimbal/gimbal.c \
 application/chassis/chassis.c \
 application/shoot/shoot.c \
 application/cmd/robot_cmd.c \
-application/balance_chassis/balance.c \
 application/robot.c
 
 # ASM sources
@@ -235,7 +234,6 @@ C_INCLUDES =  \
 -Iapplication/shoot \
 -Iapplication/gimbal \
 -Iapplication/cmd \
--Iapplication/balance_chassis \
 -Iapplication \
 -Ibsp/dwt \
 -Ibsp/can \
diff --git a/Src/freertos.c b/Src/freertos.c
index 0ecf8a9..5d0b689 100644
--- a/Src/freertos.c
+++ b/Src/freertos.c
@@ -170,7 +170,7 @@ void StartINSTASK(void const *argument)
   {
     // 1kHz
     INS_Task();
-    VisionSend(); // 解算完成后发送视觉数据
+    VisionSend(); // 解算完成后发送视觉数�?
     osDelay(1);
   }
 }
diff --git a/Src/spi.c b/Src/spi.c
index 5f1bce7..d4680c8 100644
--- a/Src/spi.c
+++ b/Src/spi.c
@@ -49,7 +49,7 @@ void MX_SPI1_Init(void)
   hspi1.Init.CLKPolarity = SPI_POLARITY_HIGH;
   hspi1.Init.CLKPhase = SPI_PHASE_2EDGE;
   hspi1.Init.NSS = SPI_NSS_SOFT;
-  hspi1.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_256;
+  hspi1.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_8;
   hspi1.Init.FirstBit = SPI_FIRSTBIT_MSB;
   hspi1.Init.TIMode = SPI_TIMODE_DISABLE;
   hspi1.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
diff --git a/application/grab/grab.md b/application/grab/grab.md
deleted file mode 100644
index 47ab839..0000000
--- a/application/grab/grab.md
+++ /dev/null
@@ -1 +0,0 @@
-工程机器人夹爪  
\ No newline at end of file
diff --git a/application/lift/lift.md b/application/lift/lift.md
deleted file mode 100644
index 98507b2..0000000
--- a/application/lift/lift.md
+++ /dev/null
@@ -1,3 +0,0 @@
-抬升/横移机构
-
-气缸和电磁阀似乎没有必要构建模块,但是也可以组合微动开关和编码器+继电器等传感器构成模块,降低lift的复杂度,完成解耦.
\ No newline at end of file
diff --git a/application/manipulator/manipulator.md b/application/manipulator/manipulator.md
deleted file mode 100644
index 6ab8126..0000000
--- a/application/manipulator/manipulator.md
+++ /dev/null
@@ -1 +0,0 @@
-机械臂(不包含末端执行机构)
\ No newline at end of file
diff --git a/application/robot_def.h b/application/robot_def.h
index b2e8de1..d21e764 100644
--- a/application/robot_def.h
+++ b/application/robot_def.h
@@ -25,16 +25,6 @@
 #define VISION_USE_VCP  // 使用虚拟串口发送视觉数据
 // #define VISION_USE_UART // 使用串口发送视觉数据
 
-// @todo: 增加机器人类型定义,后续是否要兼容所有机器人?(只兼容步兵英雄哨兵似乎就够了)
-// 通过该宏,你可以直接将所有机器人的参数保存在一处,然后每次只需要修改这个宏就可以替换所有参数
-/* 机器人类型定义 */
-// #define ROBOT_HERO 1     // 英雄机器人
-// #define ROBOT_ENINEER 2  // 工程机器人
-#define ROBOT_INFANTRY 3 // 步兵机器人3
-// #define ROBOT_INFANTRY 4 // 步兵机器人4
-// #define ROBOT_INFANTRY 5 // 步兵机器人5
-// #define ROBOT_SENTRY 6   // 哨兵机器人
-
 /* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */
 // 云台参数
 #define YAW_CHASSIS_ALIGN_ECD 2711  // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改
@@ -188,7 +178,6 @@ typedef struct
  *
  */
 
-/* @todo : 对于平衡底盘,需要新增控制模式和控制数据 */
 typedef struct
 {
 #if defined(CHASSIS_BOARD) || defined(GIMBAL_BOARD) // 非单板的时候底盘还将imu数据回传(若有必要)
@@ -205,7 +194,7 @@ typedef struct
 
 } Chassis_Upload_Data_s;
 
-/* @todo : 对于平衡底盘,需要不同的反馈数据 */
+
 typedef struct
 {
     attitude_t gimbal_imu_data;
diff --git a/basic_framework.ioc b/basic_framework.ioc
index 7c04482..ac4b75b 100644
--- a/basic_framework.ioc
+++ b/basic_framework.ioc
@@ -520,7 +520,7 @@ ProjectManager.StackSize=0x4000
 ProjectManager.TargetToolchain=Makefile
 ProjectManager.ToolChainLocation=
 ProjectManager.UnderRoot=false
-ProjectManager.functionlistsort=1-MX_GPIO_Init-GPIO-false-HAL-true,2-MX_DMA_Init-DMA-false-HAL-true,3-SystemClock_Config-RCC-false-HAL-false,4-MX_ADC1_Init-ADC1-false-HAL-true,5-MX_CAN1_Init-CAN1-false-HAL-true,6-MX_CAN2_Init-CAN2-false-HAL-true,7-MX_SPI1_Init-SPI1-false-HAL-true,8-MX_TIM4_Init-TIM4-false-HAL-true,9-MX_TIM5_Init-TIM5-false-HAL-true,10-MX_USART3_UART_Init-USART3-false-HAL-true,11-MX_RNG_Init-RNG-false-HAL-true,12-MX_RTC_Init-RTC-false-HAL-true,13-MX_TIM1_Init-TIM1-false-HAL-true,14-MX_TIM10_Init-TIM10-false-HAL-true,15-MX_USART1_UART_Init-USART1-false-HAL-true,16-MX_USART6_UART_Init-USART6-false-HAL-true,17-MX_TIM8_Init-TIM8-false-HAL-true,18-MX_I2C2_Init-I2C2-false-HAL-true,19-MX_I2C3_Init-I2C3-false-HAL-true,20-MX_SPI2_Init-SPI2-false-HAL-true,21-MX_CRC_Init-CRC-false-HAL-true,22-MX_DAC_Init-DAC-false-HAL-true,23-MX_USB_OTG_FS_USB_Init-USB_OTG_FS-false-HAL-true
+ProjectManager.functionlistsort=1-MX_GPIO_Init-GPIO-false-HAL-true,2-MX_DMA_Init-DMA-false-HAL-true,3-SystemClock_Config-RCC-false-HAL-false,4-MX_ADC1_Init-ADC1-false-HAL-true,5-MX_CAN1_Init-CAN1-false-HAL-true,6-MX_CAN2_Init-CAN2-false-HAL-true,7-MX_SPI1_Init-SPI1-false-HAL-true,8-MX_TIM4_Init-TIM4-false-HAL-true,9-MX_TIM5_Init-TIM5-false-HAL-true,10-MX_USART3_UART_Init-USART3-false-HAL-true,11-MX_RNG_Init-RNG-false-HAL-true,12-MX_RTC_Init-RTC-false-HAL-true,13-MX_TIM1_Init-TIM1-false-HAL-true,14-MX_TIM10_Init-TIM10-false-HAL-true,15-MX_USART1_UART_Init-USART1-false-HAL-true,16-MX_USART6_UART_Init-USART6-false-HAL-true,17-MX_TIM8_Init-TIM8-false-HAL-true,18-MX_I2C2_Init-I2C2-false-HAL-true,19-MX_I2C3_Init-I2C3-false-HAL-true,20-MX_SPI2_Init-SPI2-false-HAL-true,21-MX_CRC_Init-CRC-false-HAL-true,22-MX_DAC_Init-DAC-false-HAL-true,23-MX_USB_DEVICE_Init-USB_DEVICE-false-HAL-false
 RCC.48MHZClocksFreq_Value=48000000
 RCC.AHBFreq_Value=168000000
 RCC.APB1CLKDivider=RCC_HCLK_DIV4
@@ -591,10 +591,10 @@ SH.S_TIM8_CH2.0=TIM8_CH2,Output Compare2 CH2
 SH.S_TIM8_CH2.ConfNb=1
 SH.S_TIM8_CH3.0=TIM8_CH3,Output Compare3 CH3
 SH.S_TIM8_CH3.ConfNb=1
-SPI1.BaudRatePrescaler=SPI_BAUDRATEPRESCALER_256
+SPI1.BaudRatePrescaler=SPI_BAUDRATEPRESCALER_8
 SPI1.CLKPhase=SPI_PHASE_2EDGE
 SPI1.CLKPolarity=SPI_POLARITY_HIGH
-SPI1.CalculateBaudRate=328.125 KBits/s
+SPI1.CalculateBaudRate=10.5 MBits/s
 SPI1.Direction=SPI_DIRECTION_2LINES
 SPI1.IPParameters=VirtualType,Mode,Direction,CalculateBaudRate,BaudRatePrescaler,CLKPolarity,CLKPhase
 SPI1.Mode=SPI_MODE_MASTER
diff --git a/bsp/bsp_init.c b/bsp/bsp_init.c
index 6d33f25..d85f809 100644
--- a/bsp/bsp_init.c
+++ b/bsp/bsp_init.c
@@ -11,7 +11,6 @@ void BSPInit()
 {
     DWT_Init(168);
     BSPLogInit();
-    // USBInit(); // 务必在进入操作系统之前执行USBInit
 
 
     // legacy support，待删除,将在实现了led/tempctrl/buzzer的module之后移动到app层进行XXXRegister()
diff --git a/bsp/can/bsp_can.c b/bsp/can/bsp_can.c
index 0b73035..051dc31 100644
--- a/bsp/can/bsp_can.c
+++ b/bsp/can/bsp_can.c
@@ -3,6 +3,7 @@
 #include "memory.h"
 #include "stdlib.h"
 #include "bsp_dwt.h"
+#include "bsp_log.h"
 
 /* can instance ptrs storage, used for recv callback */
 // 在CAN产生接收中断会遍历数组,选出hcan和rxid与发生中断的实例相同的那个,调用其回调函数
@@ -30,14 +31,14 @@ static void CANAddFilter(CANInstance *_instance)
     CAN_FilterTypeDef can_filter_conf;
     static uint8_t can1_filter_idx = 0, can2_filter_idx = 14; // 0-13给can1用,14-27给can2用
 
-    can_filter_conf.FilterMode = CAN_FILTERMODE_IDLIST;  //使用id list模式,即只有将rxid添加到过滤器中才会接收到,其他报文会被过滤
-    can_filter_conf.FilterScale = CAN_FILTERSCALE_16BIT; //使用16位id模式,即只有低16位有效
-    can_filter_conf.FilterFIFOAssignment = (_instance->tx_id & 1) ? CAN_RX_FIFO0 : CAN_RX_FIFO1; //奇数id的模块会被分配到FIFO0,偶数id的模块会被分配到FIFO1
-    can_filter_conf.SlaveStartFilterBank = 14; // 从第14个过滤器开始配置从机过滤器(在STM32的BxCAN控制器中CAN2是CAN1的从机)
-    can_filter_conf.FilterIdLow = _instance->rx_id << 5; // 过滤器寄存器的低16位,因为使用STDID,所以只有低11位有效,高5位要填0
+    can_filter_conf.FilterMode = CAN_FILTERMODE_IDLIST;                                                       // 使用id list模式,即只有将rxid添加到过滤器中才会接收到,其他报文会被过滤
+    can_filter_conf.FilterScale = CAN_FILTERSCALE_16BIT;                                                      // 使用16位id模式,即只有低16位有效
+    can_filter_conf.FilterFIFOAssignment = (_instance->tx_id & 1) ? CAN_RX_FIFO0 : CAN_RX_FIFO1;              // 奇数id的模块会被分配到FIFO0,偶数id的模块会被分配到FIFO1
+    can_filter_conf.SlaveStartFilterBank = 14;                                                                // 从第14个过滤器开始配置从机过滤器(在STM32的BxCAN控制器中CAN2是CAN1的从机)
+    can_filter_conf.FilterIdLow = _instance->rx_id << 5;                                                      // 过滤器寄存器的低16位,因为使用STDID,所以只有低11位有效,高5位要填0
     can_filter_conf.FilterBank = _instance->can_handle == &hcan1 ? (can1_filter_idx++) : (can2_filter_idx++); // 根据can_handle判断是CAN1还是CAN2,然后自增
-    can_filter_conf.FilterActivation = CAN_FILTER_ENABLE; // 启用过滤器
-    
+    can_filter_conf.FilterActivation = CAN_FILTER_ENABLE;                                                     // 启用过滤器
+
     HAL_CAN_ConfigFilter(_instance->can_handle, &can_filter_conf);
 }
 
@@ -69,12 +70,12 @@ CANInstance *CANRegister(CAN_Init_Config_s *config)
         while (1)
             ;
     for (size_t i = 0; i < idx; i++)
-    {   // 重复注册 | id重复
-        if (can_instance[i]->rx_id == config->rx_id && can_instance[i]->can_handle == config->can_handle) 
+    { // 重复注册 | id重复
+        if (can_instance[i]->rx_id == config->rx_id && can_instance[i]->can_handle == config->can_handle)
             while (1)
                 ;
     }
-    
+
     CANInstance *instance = (CANInstance *)malloc(sizeof(CANInstance)); // 分配空间
     memset(instance, 0, sizeof(CANInstance));                           // 分配的空间未必是0,所以要先清空
     // 进行发送报文的配置
@@ -97,18 +98,28 @@ CANInstance *CANRegister(CAN_Init_Config_s *config)
 
 /* @todo 目前似乎封装过度,应该添加一个指向tx_buff的指针,tx_buff不应该由CAN instance保存 */
 /* 如果让CANinstance保存txbuff,会增加一次复制的开销 */
-uint8_t CANTransmit(CANInstance *_instance,uint8_t timeout)
+uint8_t CANTransmit(CANInstance *_instance, float timeout)
 {
+    static uint32_t busy_count;
+    static float wait_time;
     float dwt_start = DWT_GetTimeline_ms();
     while (HAL_CAN_GetTxMailboxesFreeLevel(_instance->can_handle) == 0) // 等待邮箱空闲
-    {    
+    {
         if (DWT_GetTimeline_ms() - dwt_start > timeout) // 超时
         {
+            LOGWARNING("CAN BUSY sending! cnt:%d", busy_count);
+            busy_count++;
             return 0;
         }
     }
+    wait_time = DWT_GetTimeline_ms() - dwt_start;
     // tx_mailbox会保存实际填入了这一帧消息的邮箱,但是知道是哪个邮箱发的似乎也没啥用
-    HAL_CAN_AddTxMessage(_instance->can_handle, &_instance->txconf, _instance->tx_buff, &_instance->tx_mailbox);
+    if (HAL_CAN_AddTxMessage(_instance->can_handle, &_instance->txconf, _instance->tx_buff, &_instance->tx_mailbox))
+    {
+        LOGWARNING("CAN BUSY bus! cnt:%d", busy_count);
+        busy_count++;
+        return 0;
+    }
     return 1; // 发送成功
 }
 
@@ -132,8 +143,8 @@ void CANSetDLC(CANInstance *_instance, uint8_t length)
 static void CANFIFOxCallback(CAN_HandleTypeDef *_hcan, uint32_t fifox)
 {
     static CAN_RxHeaderTypeDef rxconf; // 同上
-    uint8_t can_rx_buff[8];    
-    
+    uint8_t can_rx_buff[8];
+
     HAL_CAN_GetRxMessage(_hcan, fifox, &rxconf, can_rx_buff); // 从FIFO中获取数据
     for (size_t i = 0; i < idx; ++i)
     { // 两者相等说明这是要找的实例
@@ -177,6 +188,4 @@ void HAL_CAN_RxFifo1MsgPendingCallback(CAN_HandleTypeDef *hcan)
     CANFIFOxCallback(hcan, CAN_RX_FIFO1); // 调用我们自己写的函数来处理消息
 }
 
-
 // 人工智能会毁灭人类吗？请在补全回答
-
diff --git a/bsp/can/bsp_can.h b/bsp/can/bsp_can.h
index ea10b7a..62b7f5c 100644
--- a/bsp/can/bsp_can.h
+++ b/bsp/can/bsp_can.h
@@ -63,6 +63,6 @@ void CANSetDLC(CANInstance *_instance, uint8_t length);
  * @param timeout 超时时间,单位为ms;后续改为us,获得更精确的控制
  * @param _instance* can instance owned by module
  */
-uint8_t CANTransmit(CANInstance *_instance,uint8_t timeout);
+uint8_t CANTransmit(CANInstance *_instance,float timeout);
 
 #endif
diff --git a/bsp/usb/bsp_usb.h b/bsp/usb/bsp_usb.h
index 5234fc7..5ab1588 100644
--- a/bsp/usb/bsp_usb.h
+++ b/bsp/usb/bsp_usb.h
@@ -2,7 +2,6 @@
  * @file bsp_usb.h
  * @author your name (you@domain.com)
  * @brief 提供对usb vpc(virtal com port)的操作接口,hid和msf考虑后续添加
- * @todo 与usart的接口统一完成兼容
  * @attention 这一版usb修改了usbd_cdc_if.c中的CDC_Receive_FS函数,若使用cube生成后会被覆盖.后续需要由usbcdciftemplate创建一套自己的模板
  * @version 0.1
  * @date 2023-02-09
diff --git a/modules/BMI088/bmi088.c b/modules/BMI088/bmi088.c
index 1a45fdd..b59bc36 100644
--- a/modules/BMI088/bmi088.c
+++ b/modules/BMI088/bmi088.c
@@ -227,7 +227,7 @@ static void BMI088GyroINTCallback(GPIOInstance *gpio)
 
 /**
  * @brief
- * @todo 现在要考虑一下数据返回的方式,指针还是结构体? 7个float数据有点费时,不然用DMA? or memcpy
+ * @todo 现在要考虑一下数据返回的方式,指针还是结构体?
  *
  * @param bmi088
  * @return BMI088_Data_t
diff --git a/modules/BMI088/bmi088.h b/modules/BMI088/bmi088.h
index 39bc1f8..1ed12da 100644
--- a/modules/BMI088/bmi088.h
+++ b/modules/BMI088/bmi088.h
@@ -100,7 +100,6 @@ BMI088Instance *BMI088Register(BMI088_Init_Config_s *config);
 
 /**
  * @brief 读取BMI088数据
- * @todo  7个float数据开销较大,后续考虑使用DMA或双缓冲区直接传递指针
  * @param bmi088 BMI088实例指针
  * @return BMI088_Data_t 读取到的数据
  */
diff --git a/modules/algorithm/algorithm.md b/modules/algorithm/algorithm.md
index c2b4c6d..d2007ad 100644
--- a/modules/algorithm/algorithm.md
+++ b/modules/algorithm/algorithm.md
@@ -3,11 +3,8 @@
 <p align='right'>neozng1@hnu.edu.cn</p>
 
 > TODO:
->
 > 1. 实现麦轮和全向轮的速度解算
-> 2. 实现LQR
-> 3. 实现一些通用的滤波器，如指数平均，窗平均，低通等
-> 4. 实现系统辨识
+> 2. 实现一些通用的滤波器，如指数平均，窗平均，低通等
 
 ## 总览和使用
 
diff --git a/modules/algorithm/controller.c b/modules/algorithm/controller.c
index 0658910..5f0af7b 100644
--- a/modules/algorithm/controller.c
+++ b/modules/algorithm/controller.c
@@ -134,6 +134,7 @@ void PIDInit(PIDInstance *pid, PID_Init_Config_s *config)
     // utilize the quality of struct that its memeory is continuous
     memcpy(pid, config, sizeof(PID_Init_Config_s));
     // set rest of memory to 0
+    DWT_GetDeltaT(&pid->DWT_CNT);
 }
 
 /**
diff --git a/modules/daemon/daemon.c b/modules/daemon/daemon.c
index 783d0d5..b5c945c 100644
--- a/modules/daemon/daemon.c
+++ b/modules/daemon/daemon.c
@@ -13,7 +13,7 @@ DaemonInstance *DaemonRegister(Daemon_Init_Config_s *config)
     memset(instance, 0, sizeof(DaemonInstance));
 
     instance->owner_id = config->owner_id;
-    instance->reload_count = config->reload_count;
+    instance->reload_count = config->reload_count == 0 ? 100 : config->reload_count;
     instance->callback = config->callback;
 
     daemon_instances[idx++] = instance;
diff --git a/modules/master_machine/master_process.md b/modules/master_machine/master_process.md
index 055c332..e81e949 100644
--- a/modules/master_machine/master_process.md
+++ b/modules/master_machine/master_process.md
@@ -94,7 +94,6 @@ typedef struct
 	float yaw;
 	float pitch;
 	float roll;
-	// uint32_t time_stamp; // @todo 用于和相机的时间戳对齐
 } Vision_Send_s;
 ```
 
diff --git a/modules/message_center/message_center.h b/modules/message_center/message_center.h
index d383700..f9e89e7 100644
--- a/modules/message_center/message_center.h
+++ b/modules/message_center/message_center.h
@@ -2,7 +2,6 @@
  * @file message_center.h
  * @author NeoZeng neozng1@hnu.edu.cn
  * @brief 这是一个伪pubsub机制,仅对应用之间的通信进行了隔离
- * @todo 实现基于链表-队列的pubsub机制
  * @version 0.1
  * @date 2022-11-30
  *
diff --git a/modules/motor/DJImotor/dji_motor.c b/modules/motor/DJImotor/dji_motor.c
index 414d37e..370f215 100644
--- a/modules/motor/DJImotor/dji_motor.c
+++ b/modules/motor/DJImotor/dji_motor.c
@@ -160,6 +160,8 @@ DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config)
     PIDInit(&instance->motor_controller.angle_PID, &config->controller_param_init_config.angle_PID);
     instance->motor_controller.other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr;
     instance->motor_controller.other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr;
+    instance->motor_controller.current_feedforward_ptr = config->controller_param_init_config.current_feedforward_ptr;
+    instance->motor_controller.speed_feedforward_ptr = config->controller_param_init_config.speed_feedforward_ptr;
     // 后续增加电机前馈控制器(速度和电流)
 
     // 电机分组,因为至多4个电机可以共用一帧CAN控制报文
@@ -251,6 +253,9 @@ void DJIMotorControl()
         // 计算速度环,(外层闭环为速度或位置)且(启用速度环)时会计算速度环
         if ((motor_setting->close_loop_type & SPEED_LOOP) && (motor_setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP)))
         {
+            if (motor_setting->feedforward_flag & SPEED_FEEDFORWARD)
+                pid_ref += *motor_controller->speed_feedforward_ptr;
+
             if (motor_setting->speed_feedback_source == OTHER_FEED)
                 pid_measure = *motor_controller->other_speed_feedback_ptr;
             else // MOTOR_FEED
@@ -260,6 +265,8 @@ void DJIMotorControl()
         }
 
         // 计算电流环,目前只要启用了电流环就计算,不管外层闭环是什么,并且电流只有电机自身传感器的反馈
+        if (motor_setting->feedforward_flag & CURRENT_FEEDFORWARD)
+            pid_ref += *motor_controller->current_feedforward_ptr;
         if (motor_setting->close_loop_type & CURRENT_LOOP)
         {
             pid_ref = PIDCalculate(&motor_controller->current_PID, measure->real_current, pid_ref);
diff --git a/modules/motor/DJImotor/dji_motor.h b/modules/motor/DJImotor/dji_motor.h
index 4f97beb..c2477f2 100644
--- a/modules/motor/DJImotor/dji_motor.h
+++ b/modules/motor/DJImotor/dji_motor.h
@@ -7,7 +7,6 @@
  *
  * @todo  1. 给不同的电机设置不同的低通滤波器惯性系数而不是统一使用宏
           2. 为M2006和M3508增加开环的零位校准函数,并在初始化时调用(根据用户配置决定是否调用)
-          3. 完成前馈功能（已经添加了前馈数据指针）
 
  * @copyright Copyright (c) 2022 HNU YueLu EC all rights reserved
  *
@@ -34,7 +33,7 @@ typedef struct
     uint16_t last_ecd;        // 上一次读取的编码器值
     uint16_t ecd;             // 0-8191,刻度总共有8192格
     float angle_single_round; // 单圈角度
-    float speed_aps;          // 角速度,单位为:度/秒 rpm:rounds per minute
+    float speed_aps;          // 角速度,单位为:度/秒
     int16_t real_current;     // 实际电流
     uint8_t temperate;        // 温度 Celsius
 
@@ -100,7 +99,6 @@ void DJIMotorChangeFeed(DJIMotorInstance *motor, Closeloop_Type_e loop, Feedback
 
 /**
  * @brief 该函数被motor_task调用运行在rtos上,motor_stask内通过osDelay()确定控制频率
- * @todo  增加前馈功能
  */
 void DJIMotorControl();
 
diff --git a/modules/motor/HTmotor/HT04.c b/modules/motor/HTmotor/HT04.c
index 57d14e7..f9f9b1b 100644
--- a/modules/motor/HTmotor/HT04.c
+++ b/modules/motor/HTmotor/HT04.c
@@ -1,6 +1,7 @@
 #include "HT04.h"
 #include "memory.h"
 #include "general_def.h"
+#include "bsp_dwt.h"
 
 static uint8_t idx;
 HTMotorInstance *ht_motor_instance[HT_MOTOR_CNT];
@@ -43,19 +44,27 @@ static void HTMotorDecode(CANInstance *motor_can)
     uint16_t tmp; // 用于暂存解析值,稍后转换成float数据,避免多次创建临时变量
     uint8_t *rxbuff = motor_can->rx_buff;
     HTMotor_Measure_t *measure = &((HTMotorInstance *)motor_can->id)->measure; // 将can实例中保存的id转换成电机实例的指针
+    measure->feedback_dt = DWT_GetDeltaT(&measure->count);
 
     measure->last_angle = measure->total_angle;
-
     tmp = (uint16_t)((rxbuff[1] << 8) | rxbuff[2]);
-    measure->total_angle =  uint_to_float(tmp, P_MIN, P_MAX, 16);
+    measure->total_angle = uint_to_float(tmp, P_MIN, P_MAX, 16) - measure->angle_bias;
 
-    tmp = (uint16_t)((rxbuff[3] << 4) | (rxbuff[4] >> 4));
-    measure->speed_rads = SPEED_SMOOTH_COEF * uint_to_float(tmp, V_MIN, V_MAX, 12) +
-                          (1 - SPEED_SMOOTH_COEF) * measure->speed_rads;
+    measure->speed_rads = // SPEED_SMOOTH_COEF * measure->speed_rads + (1 - SPEED_SMOOTH_COEF) *
+        (measure->total_angle - measure->last_angle) / measure->feedback_dt;
 
-    tmp = (uint16_t)(((rxbuff[4] & 0x0f) << 8) | rxbuff[5]);
-    measure->real_current = CURRENT_SMOOTH_COEF * uint_to_float(tmp, T_MIN, T_MAX, 12) +
-                            (1 - CURRENT_SMOOTH_COEF) * measure->real_current;
+    measure->real_current = uint_to_float(((uint16_t)(rxbuff[3] << 4) | (uint16_t)(rxbuff[4] >> 4)), V_MIN, V_MAX, 12);
+
+    if (!measure->first_feedback_flag) // 初始化的时候设置偏置
+    {
+        measure->first_feedback_flag = 1;
+        measure->angle_bias = measure->total_angle;
+        measure->total_angle = 0;
+        measure->speed_rads = 0;
+        DWT_GetDeltaT(&measure->count);
+    }
+
+    // measure->real_current = uint_to_float((rxbuff[3] << 8) | rxbuff[4], I_MIN, I_MAX, 16);
 }
 
 HTMotorInstance *HTMotorInit(Motor_Init_Config_s *config)
@@ -81,7 +90,7 @@ HTMotorInstance *HTMotorInit(Motor_Init_Config_s *config)
 
 void HTMotorSetRef(HTMotorInstance *motor, float ref)
 {
-    motor->pid_ref = ref;
+    motor->pid_ref = ref * 0.285f;
 }
 
 void HTMotorControl()
@@ -107,7 +116,7 @@ void HTMotorControl()
             if (setting->angle_feedback_source == OTHER_FEED)
                 pid_measure = *motor->other_angle_feedback_ptr;
             else
-                pid_measure = measure->real_current;
+                pid_measure = measure->total_angle;
             // measure单位是rad,ref是角度,统一到angle下计算,方便建模
             pid_ref = PIDCalculate(&motor->angle_PID, pid_measure * RAD_2_DEGREE, pid_ref);
         }
@@ -125,11 +134,10 @@ void HTMotorControl()
             pid_ref = PIDCalculate(&motor->speed_PID, pid_measure * RAD_2_DEGREE, pid_ref);
         }
 
+        if (setting->feedforward_flag & CURRENT_FEEDFORWARD)
+            pid_ref += *motor->current_feedforward_ptr;
         if (setting->close_loop_type & CURRENT_LOOP)
         {
-            if (setting->feedforward_flag & CURRENT_FEEDFORWARD)
-                pid_ref += *motor->current_feedforward_ptr;
-
             pid_ref = PIDCalculate(&motor->current_PID, measure->real_current, pid_ref);
         }
 
@@ -139,30 +147,34 @@ void HTMotorControl()
 
         LIMIT_MIN_MAX(set, T_MIN, T_MAX);           // 限幅,实际上这似乎和pid输出限幅重复了
         tmp = float_to_uint(set, T_MIN, T_MAX, 12); // 数值最后在 -12~+12之间
+        if (motor->stop_flag == MOTOR_STOP)
+            tmp = float_to_uint(0, T_MIN, T_MAX, 12);
         motor_can->tx_buff[6] = (tmp >> 8);
         motor_can->tx_buff[7] = tmp & 0xff;
 
-        if (motor->stop_flag == MOTOR_STOP)
-        { // 若该电机处于停止状态,直接将发送buff置零
-            memset(motor_can->tx_buff + 6, 0, sizeof(uint16_t));
-        }
         CANTransmit(motor_can, 1);
     }
 }
 
 void HTMotorStop(HTMotorInstance *motor)
 {
+    if (motor->stop_flag == MOTOR_STOP)
+        return;
     HTMotorSetMode(CMD_RESET_MODE, motor);
+    HTMotorSetMode(CMD_RESET_MODE, motor); // 发两次,确保电机停止
     motor->stop_flag = MOTOR_STOP;
 }
 
 void HTMotorEnable(HTMotorInstance *motor)
 {
+    if (motor->stop_flag == MOTOR_ENALBED)
+        return;
+    HTMotorSetMode(CMD_MOTOR_MODE, motor);
     HTMotorSetMode(CMD_MOTOR_MODE, motor);
     motor->stop_flag = MOTOR_ENALBED;
 }
 
-void HTMotorCalibEncoder(HTMotorInstance *motor)
-{
-    HTMotorSetMode(CMD_ZERO_POSITION, motor);
-}
+// void HTMotorCalibEncoder(HTMotorInstance *motor)
+// {
+//     HTMotorSetMode(CMD_ZERO_POSITION, motor);
+// }
diff --git a/modules/motor/HTmotor/HT04.h b/modules/motor/HTmotor/HT04.h
index 0ff482b..fdd8d7b 100644
--- a/modules/motor/HTmotor/HT04.h
+++ b/modules/motor/HTmotor/HT04.h
@@ -19,10 +19,15 @@
 
 typedef struct // HT04
 {
-    float last_angle;
     float total_angle; // 角度为多圈角度,范围是-95.5~95.5,单位为rad
+    float last_angle;
     float speed_rads;
     float real_current;
+    float angle_bias;
+    uint8_t first_feedback_flag;
+
+    float feedback_dt;
+    uint32_t count;
 } HTMotor_Measure_t;
 
 /* HT电机类型定义*/
@@ -91,11 +96,7 @@ void HTMotorStop(HTMotorInstance *motor);
 void HTMotorEnable(HTMotorInstance *motor);
 
 /**
- * @brief 校准电机编码器
- * @attention 注意,校准时务必将电机和其他机构分离,电机会旋转360°!
- *            注意,校准时务必将电机和其他机构分离,电机会旋转360°!
- *            注意,校准时务必将电机和其他机构分离,电机会旋转360°!
- *            注意,校准时务必将电机和其他机构分离,电机会旋转360°!
+ * @brief 校准电机编码器,设置的当前位置为0
  *
  * @param motor
  */
diff --git a/modules/motor/LKmotor/LK9025.c b/modules/motor/LKmotor/LK9025.c
index 588ea2a..6a4143a 100644
--- a/modules/motor/LKmotor/LK9025.c
+++ b/modules/motor/LKmotor/LK9025.c
@@ -104,10 +104,10 @@ void LKMotorControl()
             else
                 pid_measure = measure->speed_rads;
             pid_ref = PIDCalculate(&motor->angle_PID, pid_measure, pid_ref);
-            if (setting->feedforward_flag & CURRENT_FEEDFORWARD)
-                pid_ref += *motor->current_feedforward_ptr;
         }
 
+        if (setting->feedforward_flag & CURRENT_FEEDFORWARD)
+            pid_ref += *motor->current_feedforward_ptr;
         if (setting->close_loop_type & CURRENT_LOOP)
         {
             pid_ref = PIDCalculate(&motor->current_PID, measure->real_current, pid_ref);
diff --git a/modules/motor/motor_def.h b/modules/motor/motor_def.h
index 482e4bf..d73476e 100644
--- a/modules/motor/motor_def.h
+++ b/modules/motor/motor_def.h
@@ -45,7 +45,7 @@ typedef enum
 /* 反馈来源设定,若设为OTHER_FEED则需要指定数据来源指针,详见Motor_Controller_s*/
 typedef enum
 {
-    MOTOR_FEED=0,
+    MOTOR_FEED = 0,
     OTHER_FEED,
 } Feedback_Source_e;
 
@@ -71,13 +71,13 @@ typedef enum
 /* 电机控制设置,包括闭环类型,反转标志和反馈来源 */
 typedef struct
 {
-    Closeloop_Type_e outer_loop_type;        // 最外层的闭环,未设置时默认为最高级的闭环
-    Closeloop_Type_e close_loop_type;        // 使用几个闭环(串级)
-    Motor_Reverse_Flag_e motor_reverse_flag; // 是否反转
+    Closeloop_Type_e outer_loop_type;              // 最外层的闭环,未设置时默认为最高级的闭环
+    Closeloop_Type_e close_loop_type;              // 使用几个闭环(串级)
+    Motor_Reverse_Flag_e motor_reverse_flag;       // 是否反转
     Feedback_Reverse_Flag_e feedback_reverse_flag; // 反馈是否反向
-    Feedback_Source_e angle_feedback_source; // 角度反馈类型
-    Feedback_Source_e speed_feedback_source; // 速度反馈类型
-    Feedfoward_Type_e feedforward_flag;      // 前馈标志
+    Feedback_Source_e angle_feedback_source;       // 角度反馈类型
+    Feedback_Source_e speed_feedback_source;       // 速度反馈类型
+    Feedfoward_Type_e feedforward_flag;            // 前馈标志
 
 } Motor_Control_Setting_s;
 
@@ -87,8 +87,8 @@ typedef struct
 {
     float *other_angle_feedback_ptr; // 其他反馈来源的反馈数据指针
     float *other_speed_feedback_ptr;
-    // float *speed_foward_ptr;
-    // float *current_foward_ptr;
+    float *speed_feedforward_ptr;
+    float *current_feedforward_ptr;
 
     PIDInstance current_PID;
     PIDInstance speed_PID;
diff --git a/modules/remote/remote_control.c b/modules/remote/remote_control.c
index 4dbef62..b03efa5 100644
--- a/modules/remote/remote_control.c
+++ b/modules/remote/remote_control.c
@@ -117,7 +117,6 @@ static void RemoteControlRxCallback()
  */
 static void RCLostCallback(void *id)
 {
-    // @todo 遥控器丢失的处理
     USARTServiceInit(rc_usart_instance); // 尝试重新启动接收
 }
 
@@ -130,7 +129,6 @@ RC_ctrl_t *RemoteControlInit(UART_HandleTypeDef *rc_usart_handle)
     rc_usart_instance = USARTRegister(&conf);
 
     // 进行守护进程的注册,用于定时检查遥控器是否正常工作
-    // @todo 当前守护进程直接在这里注册,后续考虑将其封装到遥控器的初始化函数中,即可以让用户决定reload_count的值(是否有必要?)
     Daemon_Init_Config_s daemon_conf = {
         .reload_count = 10, // 100ms未收到数据视为离线,遥控器的接收频率实际上是1000/14Hz(大约70Hz)
         .callback = RCLostCallback,