diff --git a/.vscode/settings.json b/.vscode/settings.json
index f8f50e8..cf7cc80 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -41,7 +41,8 @@
         "motor_def.h": "c",
         "quaternionekf.h": "c",
         "shoot.h": "c",
-        "lk9025.h": "c"
+        "lk9025.h": "c",
+        "ht04.h": "c"
     },
     "C_Cpp.default.configurationProvider": "ms-vscode.makefile-tools",
 }
\ No newline at end of file
diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c
index 35b2a89..a50da6a 100644
--- a/application/cmd/robot_cmd.c
+++ b/application/cmd/robot_cmd.c
@@ -8,8 +8,8 @@
 #include "dji_motor.h"
 
 // 私有宏,自动将编码器转换成角度值
-#define YAW_ALIGN_ANGLE (YAW_CHASSIS_ALIGN_ECD * ECD_ANGLE_COEF)
-#define PTICH_HORIZON_ANGLE (PITCH_HORIZON_ECD * ECD_ANGLE_COEF)
+#define YAW_ALIGN_ANGLE (YAW_CHASSIS_ALIGN_ECD * ECD_ANGLE_COEF_DJI)
+#define PTICH_HORIZON_ANGLE (PITCH_HORIZON_ECD * ECD_ANGLE_COEF_DJI)
 
 /* gimbal_cmd应用包含的模块实例指针和交互信息存储*/
 #ifdef GIMBAL_BOARD // 对双板的兼容,条件编译
diff --git a/bsp/bsp_can.c b/bsp/bsp_can.c
index 7c0785e..f018026 100644
--- a/bsp/bsp_can.c
+++ b/bsp/bsp_can.c
@@ -115,7 +115,7 @@ static void CANFIFOxCallback(CAN_HandleTypeDef *_hcan, uint32_t fifox)
     uint8_t can_rx_buff[8];
     CAN_RxHeaderTypeDef rxconf;
     HAL_CAN_GetRxMessage(_hcan, fifox, &rxconf, can_rx_buff);
-    for (size_t i = 0; i < MX_REGISTER_DEVICE_CNT; i++)
+    for (size_t i = 0; i < MX_REGISTER_DEVICE_CNT; ++i)
     {
         if (instance[i] != NULL) // 碰到NULL说明已经遍历完所有实例
         {                        // 两者相等说明这是要找的实例
diff --git a/bsp/bsp_usart.c b/bsp/bsp_usart.c
index 98b7a44..c77e97a 100644
--- a/bsp/bsp_usart.c
+++ b/bsp/bsp_usart.c
@@ -70,7 +70,7 @@ void USARTSend(USARTInstance *_instance, uint8_t *send_buf, uint16_t send_size)
  */
 void HAL_UARTEx_RxEventCallback(UART_HandleTypeDef *huart, uint16_t Size)
 {
-    for (uint8_t i = 0; i < 3; i++)
+    for (uint8_t i = 0; i < 3; ++i)
     {
         if (huart == instance[i]->usart_handle)
         {
@@ -93,7 +93,7 @@ void HAL_UARTEx_RxEventCallback(UART_HandleTypeDef *huart, uint16_t Size)
  */
 void HAL_UART_ErrorCallback(UART_HandleTypeDef *huart)
 {
-    for (uint8_t i = 0; i < 3; i++)
+    for (uint8_t i = 0; i < 3; ++i)
     {
         if (huart == instance[i]->usart_handle)
         {
diff --git a/modules/algorithm/QuaternionEKF.c b/modules/algorithm/QuaternionEKF.c
index 08a33df..5513486 100644
--- a/modules/algorithm/QuaternionEKF.c
+++ b/modules/algorithm/QuaternionEKF.c
@@ -156,7 +156,7 @@ void IMU_QuaternionEKF_Update(float gx, float gy, float gz, float ax, float ay,
 
     // set z,单位化重力加速度向量
     accelInvNorm = invSqrt(QEKF_INS.Accel[0] * QEKF_INS.Accel[0] + QEKF_INS.Accel[1] * QEKF_INS.Accel[1] + QEKF_INS.Accel[2] * QEKF_INS.Accel[2]);
-    for (uint8_t i = 0; i < 3; i++)
+    for (uint8_t i = 0; i < 3; ++i)
     {
         QEKF_INS.IMU_QuaternionEKF.MeasuredVector[i] = QEKF_INS.Accel[i] * accelInvNorm; // 用加速度向量更新量测值
     }
@@ -238,7 +238,7 @@ static void IMU_QuaternionEKF_F_Linearization_P_Fading(KalmanFilter_t *kf)
 
     // quaternion normalize
     qInvNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
-    for (uint8_t i = 0; i < 4; i++)
+    for (uint8_t i = 0; i < 4; ++i)
     {
         kf->xhatminus_data[i] *= qInvNorm;
     }
@@ -352,7 +352,7 @@ static void IMU_QuaternionEKF_xhatUpdate(KalmanFilter_t *kf)
     kf->temp_vector_data[2] = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3; // temp_vector = h(xhat'(k))
 
     // 计算预测值和各个轴的方向余弦
-    for (uint8_t i = 0; i < 3; i++)
+    for (uint8_t i = 0; i < 3; ++i)
     {
         QEKF_INS.OrientationCosine[i] = acosf(fabsf(kf->temp_vector_data[i]));
     }
@@ -426,13 +426,13 @@ static void IMU_QuaternionEKF_xhatUpdate(KalmanFilter_t *kf)
     kf->MatStatus = Matrix_Multiply(&kf->temp_matrix, &kf->temp_matrix1, &kf->K);
 
     // implement adaptive
-    for (uint8_t i = 0; i < kf->K.numRows * kf->K.numCols; i++)
+    for (uint8_t i = 0; i < kf->K.numRows * kf->K.numCols; ++i)
     {
         kf->K_data[i] *= QEKF_INS.AdaptiveGainScale;
     }
-    for (uint8_t i = 4; i < 6; i++)
+    for (uint8_t i = 4; i < 6; ++i)
     {
-        for (uint8_t j = 0; j < 3; j++)
+        for (uint8_t j = 0; j < 3; ++j)
         {
             kf->K_data[i * 3 + j] *= QEKF_INS.OrientationCosine[i - 4] / 1.5707963f; // 1 rad
         }
@@ -445,7 +445,7 @@ static void IMU_QuaternionEKF_xhatUpdate(KalmanFilter_t *kf)
     // 零漂修正限幅,一般不会有过大的漂移
     if (QEKF_INS.ConvergeFlag)
     {
-        for (uint8_t i = 4; i < 6; i++)
+        for (uint8_t i = 4; i < 6; ++i)
         {
             if (kf->temp_vector.pData[i] > 1e-2f * QEKF_INS.dt)
             {
diff --git a/modules/algorithm/kalman_filter.c b/modules/algorithm/kalman_filter.c
index adb7ddb..4f5460f 100644
--- a/modules/algorithm/kalman_filter.c
+++ b/modules/algorithm/kalman_filter.c
@@ -420,7 +420,7 @@ float *Kalman_Filter_Update(KalmanFilter_t *kf)
 
     // 避免滤波器过度收敛
     // suppress filter excessive convergence
-    for (uint8_t i = 0; i < kf->xhatSize; i++)
+    for (uint8_t i = 0; i < kf->xhatSize; ++i)
     {
         if (kf->P_data[i * kf->xhatSize + i] < kf->StateMinVariance[i])
             kf->P_data[i * kf->xhatSize + i] = kf->StateMinVariance[i];
@@ -445,7 +445,7 @@ static void H_K_R_Adjustment(KalmanFilter_t *kf)
     // recognize measurement validity and adjust matrices H R K
     memset(kf->R_data, 0, sizeof_float * kf->zSize * kf->zSize);
     memset(kf->H_data, 0, sizeof_float * kf->xhatSize * kf->zSize);
-    for (uint8_t i = 0; i < kf->zSize; i++)
+    for (uint8_t i = 0; i < kf->zSize; ++i)
     {
         if (kf->z_data[i] != 0)
         {
@@ -459,7 +459,7 @@ static void H_K_R_Adjustment(KalmanFilter_t *kf)
             kf->MeasurementValidNum++;
         }
     }
-    for (uint8_t i = 0; i < kf->MeasurementValidNum; i++)
+    for (uint8_t i = 0; i < kf->MeasurementValidNum; ++i)
     {
         // 重构矩阵R
         // rebuild matrix R
diff --git a/modules/can_comm/can_comm.c b/modules/can_comm/can_comm.c
index 946cc1a..efe2eec 100644
--- a/modules/can_comm/can_comm.c
+++ b/modules/can_comm/can_comm.c
@@ -26,52 +26,49 @@ static void CANCommResetRx(CANCommInstance *ins)
  */
 static void CANCommRxCallback(CANInstance *_instance)
 {
-    for (size_t i = 0; i < idx; i++)
+    static CANCommInstance *comm;
+    comm = (CANCommInstance *)_instance->id;
+
+    /* 接收状态判断 */
+    if (_instance->rx_buff[0] == CAN_COMM_HEADER && comm->recv_state == 0) // 尚未开始接收且新的一包里有帧头
     {
-        if (can_comm_instance[i]->can_ins == _instance) // 遍历,找到对应的接收CAN COMM实例
+        if (_instance->rx_buff[1] == comm->recv_data_len) // 接收长度等于设定接收长度
         {
-            /* 接收状态判断 */
-            if (_instance->rx_buff[0] == CAN_COMM_HEADER && can_comm_instance[i]->recv_state == 0) // 尚未开始接收且新的一包里有帧头
+            comm->recv_state = 1;
+        }
+        else
+            return; // 直接跳过即可
+    }
+    if (comm->recv_state) // 已经收到过帧头
+    {                     // 如果已经接收到的长度加上当前一包的长度大于总buf len,说明接收错误
+        if (comm->cur_recv_len + _instance->rx_len > comm->recv_buf_len)
+        {
+            CANCommResetRx(comm);
+            return; // 重置状态然后返回
+        }
+
+        // 直接拷贝到当前的接收buffer后面
+        memcpy(comm->raw_recvbuf + comm->cur_recv_len, _instance->rx_buff, _instance->rx_len);
+        comm->cur_recv_len += _instance->rx_len;
+
+        // 当前已经收满
+        if (comm->cur_recv_len == comm->recv_buf_len)
+        { // buff里本该是tail的位置不等于CAN_COMM_TAIL
+            if (comm->raw_recvbuf[comm->recv_buf_len - 1] != CAN_COMM_TAIL)
             {
-                if (_instance->rx_buff[1] == can_comm_instance[i]->recv_data_len) // 接收长度等于设定接收长度
-                {
-                    can_comm_instance[i]->recv_state = 1;
-                }
-                else
-                    return; // 直接跳过即可
+                CANCommResetRx(comm);
+                return; // 重置状态然后返回
             }
-            if (can_comm_instance[i]->recv_state) // 已经收到过帧头
-            {                                     // 如果已经接收到的长度加上当前一包的长度大于总buf len,说明接收错误
-                if (can_comm_instance[i]->cur_recv_len + _instance->rx_len > can_comm_instance[i]->recv_buf_len)
-                {
-                    CANCommResetRx(can_comm_instance[i]);
-                    return; // 重置状态然后返回
-                }
-
-                // 直接拷贝到当前的接收buffer后面
-                memcpy(can_comm_instance[i]->raw_recvbuf + can_comm_instance[i]->cur_recv_len, _instance->rx_buff, _instance->rx_len);
-                can_comm_instance[i]->cur_recv_len += _instance->rx_len;
-
-                // 当前已经收满
-                if (can_comm_instance[i]->cur_recv_len == can_comm_instance[i]->recv_buf_len)
-                { // buff里本该是tail的位置不等于CAN_COMM_TAIL
-                    if (can_comm_instance[i]->raw_recvbuf[can_comm_instance[i]->recv_buf_len - 1] != CAN_COMM_TAIL)
-                    {
-                        CANCommResetRx(can_comm_instance[i]);
-                        return; // 重置状态然后返回
-                    }
-                    else // tail正确, 对数据进行crc8校验
-                    {
-                        if (can_comm_instance[i]->raw_recvbuf[can_comm_instance[i]->recv_buf_len - 2] ==
-                            crc_8(can_comm_instance[i]->raw_recvbuf + 2, can_comm_instance[i]->recv_data_len))
-                        { // 通过校验,复制数据到unpack_data中
-                            memcpy(can_comm_instance[i]->unpacked_recv_data, can_comm_instance[i]->raw_recvbuf + 2, can_comm_instance[i]->recv_data_len);
-                            can_comm_instance[i]->update_flag = 1; // 数据更新flag置为1
-                        }
-                        CANCommResetRx(can_comm_instance[i]);
-                        return; // 重置状态然后返回
-                    }
+            else // tail正确, 对数据进行crc8校验
+            {
+                if (comm->raw_recvbuf[comm->recv_buf_len - 2] ==
+                    crc_8(comm->raw_recvbuf + 2, comm->recv_data_len))
+                { // 通过校验,复制数据到unpack_data中
+                    memcpy(comm->unpacked_recv_data, comm->raw_recvbuf + 2, comm->recv_data_len);
+                    comm->update_flag = 1; // 数据更新flag置为1
                 }
+                CANCommResetRx(comm);
+                return; // 重置状态然后返回
             }
             return; // 访问完一个can comm直接退出,一次中断只会也只可能会处理一个实例的回调
         }
@@ -90,6 +87,7 @@ CANCommInstance *CANCommInit(CANComm_Init_Config_s *comm_config)
     can_comm_instance[idx]->raw_sendbuf[1] = comm_config->send_data_len;
     can_comm_instance[idx]->raw_sendbuf[comm_config->send_data_len + CAN_COMM_OFFSET_BYTES - 1] = CAN_COMM_TAIL;
 
+    comm_config->can_config.id = can_comm_instance[idx];
     comm_config->can_config.can_module_callback = CANCommRxCallback;
     can_comm_instance[idx]->can_ins = CANRegister(&comm_config->can_config);
     return can_comm_instance[idx++];
diff --git a/modules/daemon/daemon.c b/modules/daemon/daemon.c
index 6520c8b..49d6677 100644
--- a/modules/daemon/daemon.c
+++ b/modules/daemon/daemon.c
@@ -31,7 +31,7 @@ uint8_t DaemonIsOnline(DaemonInstance *instance)
 void DaemonTask()
 {
     static DaemonInstance* pins; //提高可读性同时降低访存开销
-    for (size_t i = 0; i < idx; i++)
+    for (size_t i = 0; i < idx; ++i)
     {
         pins=daemon_instances[i];
         if(pins->temp_count>0)
diff --git a/modules/imu/BMI088driver.c b/modules/imu/BMI088driver.c
index 4a008d2..787a304 100644
--- a/modules/imu/BMI088driver.c
+++ b/modules/imu/BMI088driver.c
@@ -144,7 +144,7 @@ void Calibrate_MPU_Offset(IMU_Data_t *bmi088)
         bmi088->GyroOffset[1] = 0;
         bmi088->GyroOffset[2] = 0;
 
-        for (uint16_t i = 0; i < CaliTimes; i++)
+        for (uint16_t i = 0; i < CaliTimes; ++i)
         {
             BMI088_accel_read_muli_reg(BMI088_ACCEL_XOUT_L, buf, 6);
             bmi088_raw_temp = (int16_t)((buf[1]) << 8) | buf[0];
@@ -176,7 +176,7 @@ void Calibrate_MPU_Offset(IMU_Data_t *bmi088)
             {
                 gNormMax = gNormTemp;
                 gNormMin = gNormTemp;
-                for (uint8_t j = 0; j < 3; j++)
+                for (uint8_t j = 0; j < 3; ++j)
                 {
                     gyroMax[j] = bmi088->Gyro[j];
                     gyroMin[j] = bmi088->Gyro[j];
@@ -188,7 +188,7 @@ void Calibrate_MPU_Offset(IMU_Data_t *bmi088)
                     gNormMax = gNormTemp;
                 if (gNormTemp < gNormMin)
                     gNormMin = gNormTemp;
-                for (uint8_t j = 0; j < 3; j++)
+                for (uint8_t j = 0; j < 3; ++j)
                 {
                     if (bmi088->Gyro[j] > gyroMax[j])
                         gyroMax[j] = bmi088->Gyro[j];
@@ -198,7 +198,7 @@ void Calibrate_MPU_Offset(IMU_Data_t *bmi088)
             }
 
             gNormDiff = gNormMax - gNormMin;
-            for (uint8_t j = 0; j < 3; j++)
+            for (uint8_t j = 0; j < 3; ++j)
                 gyroDiff[j] = gyroMax[j] - gyroMin[j];
             if (gNormDiff > 0.5f ||
                 gyroDiff[0] > 0.15f ||
@@ -209,7 +209,7 @@ void Calibrate_MPU_Offset(IMU_Data_t *bmi088)
         }
 
         bmi088->gNorm /= (float)CaliTimes;
-        for (uint8_t i = 0; i < 3; i++)
+        for (uint8_t i = 0; i < 3; ++i)
             bmi088->GyroOffset[i] /= (float)CaliTimes;
 
         BMI088_accel_read_muli_reg(BMI088_TEMP_M, buf, 2);
diff --git a/modules/imu/ins_task.c b/modules/imu/ins_task.c
index d70f575..87d81d1 100644
--- a/modules/imu/ins_task.c
+++ b/modules/imu/ins_task.c
@@ -112,7 +112,7 @@ void INS_Task(void)
         // 将重力从导航坐标系n转换到机体系b,随后根据加速度计数据计算运动加速度
         float gravity_b[3];
         EarthFrameToBodyFrame(gravity, gravity_b, INS.q);
-        for (uint8_t i = 0; i < 3; i++) // 同样过一个低通滤波
+        for (uint8_t i = 0; i < 3; ++i) // 同样过一个低通滤波
         {
             INS.MotionAccel_b[i] = (INS.Accel[i] - gravity_b[i]) * dt / (INS.AccelLPF + dt) + INS.MotionAccel_b[i] * INS.AccelLPF / (INS.AccelLPF + dt);
         }
@@ -218,7 +218,7 @@ static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[
         param->flag = 0;
     }
     float gyro_temp[3];
-    for (uint8_t i = 0; i < 3; i++)
+    for (uint8_t i = 0; i < 3; ++i)
         gyro_temp[i] = gyro[i] * param->scale[i];
 
     gyro[X] = c_11 * gyro_temp[X] +
@@ -232,7 +232,7 @@ static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[
               c_33 * gyro_temp[Z];
 
     float accel_temp[3];
-    for (uint8_t i = 0; i < 3; i++)
+    for (uint8_t i = 0; i < 3; ++i)
         accel_temp[i] = accel[i];
 
     accel[X] = c_11 * accel_temp[X] +
diff --git a/modules/led_light/led_task.c b/modules/led_light/led_task.c
index 76a9fdc..21037b7 100644
--- a/modules/led_light/led_task.c
+++ b/modules/led_light/led_task.c
@@ -29,7 +29,7 @@ void led_RGB_flow_task()
 	static float alpha, red, green, blue;
 	static uint32_t aRGB;
 
-	for (size_t i = 0; i < RGB_FLOW_COLOR_LENGHT; i++)
+	for (size_t i = 0; i < RGB_FLOW_COLOR_LENGHT; ++i)
 	{
 		alpha = (RGB_flow_color[i] & 0xFF000000) >> 24;
 		red = ((RGB_flow_color[i] & 0x00FF0000) >> 16);
@@ -45,7 +45,7 @@ void led_RGB_flow_task()
 		delta_red /= RGB_FLOW_COLOR_CHANGE_TIME;
 		delta_green /= RGB_FLOW_COLOR_CHANGE_TIME;
 		delta_blue /= RGB_FLOW_COLOR_CHANGE_TIME;
-		for (size_t j = 0; j < RGB_FLOW_COLOR_CHANGE_TIME; j++)
+		for (size_t j = 0; j < RGB_FLOW_COLOR_CHANGE_TIME; ++j)
 		{
 			alpha += delta_alpha;
 			red += delta_red;
diff --git a/modules/message_center/message_center.c b/modules/message_center/message_center.c
index 0beded9..443bff8 100644
--- a/modules/message_center/message_center.c
+++ b/modules/message_center/message_center.c
@@ -14,11 +14,11 @@ void MessageInit()
 {
     // pub必须唯一,即消息名称不能重复,不得有多个pub发布相同消息名称
     // 对每一个subscriber,寻找相同消息名称的publisher,可能有多个sub从相同pub获取消息
-    for (size_t i = 0; i < MAX_EVENT_COUNT; i++)
+    for (size_t i = 0; i < MAX_EVENT_COUNT; ++i)
     {
         if (s_pptr[i] != NULL)
         {
-            for (size_t j = 0; j < MAX_EVENT_COUNT; j++) // 遍历publisher
+            for (size_t j = 0; j < MAX_EVENT_COUNT; ++j) // 遍历publisher
             {
                 if (p_ptr[j] != NULL) // 不为空
                 {
@@ -46,7 +46,7 @@ void MessageInit()
 void PublisherRegister(char *name, void *data)
 {
     static uint8_t idx;
-    for (size_t i = 0; i < idx; i++)
+    for (size_t i = 0; i < idx; ++i)
     {
         if (strcmp(pname[i], name) == 0)
             while (1)
@@ -105,7 +105,7 @@ Subscriber_t *SubRegister(char *name, uint8_t data_len)
             memset(ret, 0, sizeof(Subscriber_t));
             // 对新建的Subscriber进行初始化
             ret->data_len = data_len; // 设定数据长度
-            for (size_t i = 0; i < QUEUE_SIZE; i++)
+            for (size_t i = 0; i < QUEUE_SIZE; ++i)
             { // 给消息队列的每一个元素分配空间,queue里保存的实际上是数据执指针,这样可以兼容不同的数据长度
                 ret->queue[i] = malloc(sizeof(data_len));
             }
@@ -135,7 +135,7 @@ Subscriber_t *SubRegister(char *name, uint8_t data_len)
     Subscriber_t *ret = (Subscriber_t *)malloc(sizeof(Subscriber_t));
     memset(ret, 0, sizeof(Subscriber_t));
     ret->data_len = data_len;
-    for (size_t i = 0; i < QUEUE_SIZE; i++)
+    for (size_t i = 0; i < QUEUE_SIZE; ++i)
     { // 给消息队列分配空间
         ret->queue[i] = malloc(sizeof(data_len));
     }
diff --git a/modules/motor/HT04.c b/modules/motor/HT04.c
index 4b04d15..f3a7e74 100644
--- a/modules/motor/HT04.c
+++ b/modules/motor/HT04.c
@@ -1,15 +1,31 @@
 #include "HT04.h"
 #include "memory.h"
+#include "general_def.h"
 
-HKMotor_Measure_t *joint_motor_info[HT_MOTOR_CNT];
+static uint8_t idx;
+HTMotorInstance *ht_motor_info[HT_MOTOR_CNT];
 
+/**
+ * @brief
+ *
+ * @param cmd
+ * @param motor
+ */
+static void HTMotorSetMode(HTMotor_Mode_t cmd, HTMotorInstance *motor)
+{
+    static uint8_t buf[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00};
+    buf[7] = (uint8_t)cmd;
+    memcpy(motor->motor_can_instace->tx_buff, buf, sizeof(buf));
+    CANTransmit(motor->motor_can_instace);
+}
+
+/* 两个用于将uint值和float值进行映射的函数,在设定发送值和解析反馈值时使用 */
 static uint16_t float_to_uint(float x, float x_min, float x_max, uint8_t bits)
 {
     float span = x_max - x_min;
     float offset = x_min;
     return (uint16_t)((x - offset) * ((float)((1 << bits) - 1)) / span);
 }
-
 static float uint_to_float(int x_int, float x_min, float x_max, int bits)
 {
     float span = x_max - x_min;
@@ -17,47 +33,84 @@ static float uint_to_float(int x_int, float x_min, float x_max, int bits)
     return ((float)x_int) * span / ((float)((1 << bits) - 1)) + offset;
 }
 
-static void DecodeJoint(CANInstance *motor_instance)
+/**
+ * @brief 解析电机反馈值
+ *
+ * @param motor_can 收到
+ */
+static void HTMotorDecode(CANInstance *motor_can)
 {
-    uint16_t tmp;
-    for (size_t i = 0; i < HT_MOTOR_CNT; i++)
+    static uint16_t tmp; // 用于暂存解析值,稍后转换成float数据,避免多次创建临时变量
+    static HTMotor_Measure_t *measure;
+    static uint8_t *rxbuff;
+
+    rxbuff = motor_can->rx_buff;
+    measure = &((HTMotorInstance *)motor_can->id)->motor_measure;
+
+    measure->last_angle = measure->total_angle;
+
+    tmp = (uint16_t)((rxbuff[1] << 8) | rxbuff[2]);
+    measure->total_angle = RAD_2_ANGLE * uint_to_float(tmp, P_MAX, P_MIN, 16);
+
+    tmp = (uint16_t)((rxbuff[3] << 4) | (rxbuff[4] >> 4));
+    measure->speed_aps = RAD_2_ANGLE * SPEED_SMOOTH_COEF * uint_to_float(tmp, V_MAX, V_MIN, 12) +
+                         (1 - SPEED_SMOOTH_COEF) * measure->speed_aps;
+
+    tmp = (uint16_t)(((rxbuff[4] & 0x0f) << 8) | rxbuff[5]);
+    measure->real_current = CURRENT_SMOOTH_COEF * uint_to_float(tmp, T_MAX, T_MIN, 12) +
+                            (1 - CURRENT_SMOOTH_COEF) * measure->real_current;
+}
+
+HTMotorInstance *HTMotorInit(Motor_Init_Config_s *config)
+{
+
+    ht_motor_info[idx] = (HTMotorInstance *)malloc(sizeof(HTMotorInstance));
+    memset(ht_motor_info[idx], 0, sizeof(HTMotorInstance));
+
+    ht_motor_info[idx]->motor_settings = config->controller_setting_init_config;
+    PID_Init(&ht_motor_info[idx]->current_PID, &config->controller_param_init_config.current_PID);
+    PID_Init(&ht_motor_info[idx]->speed_PID, &config->controller_param_init_config.speed_PID);
+    PID_Init(&ht_motor_info[idx]->angle_PID, &config->controller_param_init_config.angle_PID);
+    ht_motor_info[idx]->other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr;
+    ht_motor_info[idx]->other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr;
+
+    config->can_init_config.can_module_callback = HTMotorDecode;
+    config->can_init_config.id = ht_motor_info[idx];
+    ht_motor_info[idx]->motor_can_instace = CANRegister(&config->can_init_config);
+
+    HTMotorEnable(ht_motor_info[idx]);
+    return ht_motor_info[idx++];
+}
+
+void HTMotorSetRef(HTMotorInstance *motor, float ref)
+{
+    motor->pid_ref = ref;
+}
+
+void HTMotorControl()
+{
+    static uint16_t tmp;
+    for (size_t i = 0; i < idx; i++)
     {
-        if (joint_motor_info[i]->motor_can_instace == motor_instance)
-        {
-            tmp = (motor_instance->rx_buff[1] << 8) | motor_instance->rx_buff[2];
-            joint_motor_info[i]->last_ecd = joint_motor_info[i]->ecd;
-            joint_motor_info[i]->ecd = uint_to_float(tmp, P_MAX, P_MIN, 16);
-            tmp = (motor_instance->rx_buff[3] << 4) | (motor_instance->rx_buff[4] >> 4);
-            joint_motor_info[i]->speed_rpm = uint_to_float(tmp, V_MAX, V_MIN, 12);
-            tmp = ((motor_instance->rx_buff[4] & 0xf) << 8) | motor_instance->rx_buff[5];
-            joint_motor_info[i]->real_current = uint_to_float(tmp, T_MAX, T_MIN, 12);
-            break;
-        }
+        // tmp=float_to_uint()
+        // _instance->motor_can_instace->rx_buff[6] = tmp >> 8;
+        // _instance->motor_can_instace->rx_buff[7] = tmp & 0xff;
+        // CANTransmit(_instance->motor_can_instace);
     }
+    
 }
 
-HKMotor_Measure_t *HTMotorInit(CAN_Init_Config_s config)
+void HTMotorStop(HTMotorInstance *motor)
 {
-    static uint8_t idx;
-    joint_motor_info[idx] = (HKMotor_Measure_t *)malloc(sizeof(HKMotor_Measure_t));
-    joint_motor_info[idx]->motor_can_instace = CANRegister(&config);
-    return joint_motor_info[idx++];
+    HTMotorSetMode(CMD_RESET_MODE, motor);
 }
 
-void JointControl(HKMotor_Measure_t *_instance, float current)
+void HTMotorEnable(HTMotorInstance *motor)
 {
-    uint16_t tmp;
-    LIMIT_MIN_MAX(current, T_MIN, T_MAX);
-    tmp = float_to_uint(current, T_MIN, T_MAX, 12);
-    _instance->motor_can_instace->rx_buff[6] = tmp >> 8;
-    _instance->motor_can_instace->rx_buff[7] = tmp & 0xff;
-    CANTransmit(_instance->motor_can_instace);
+    HTMotorSetMode(CMD_MOTOR_MODE, motor);
 }
 
-void SetJointMode(joint_mode cmd, HKMotor_Measure_t *_instance)
+void HTMotorCalibEncoder(HTMotorInstance *motor)
 {
-    static uint8_t buf[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00};
-    buf[7] = (uint8_t)cmd;
-    memcpy(_instance->motor_can_instace->rx_buff, buf, 8 * sizeof(uint8_t));
-    CANTransmit(_instance->motor_can_instace);
+    HTMotorSetMode(CMD_ZERO_POSITION, motor);
 }
diff --git a/modules/motor/HT04.h b/modules/motor/HT04.h
index 1e8bce4..21317f0 100644
--- a/modules/motor/HT04.h
+++ b/modules/motor/HT04.h
@@ -7,36 +7,94 @@
 #include "motor_def.h"
 
 #define HT_MOTOR_CNT 4
+#define CURRENT_SMOOTH_COEF 0.9f
+#define SPEED_SMOOTH_COEF 0.85f
 
 #define P_MIN -95.5f // Radians
 #define P_MAX 95.5f
 #define V_MIN -45.0f // Rad/s
 #define V_MAX 45.0f
-#define T_MIN -18.0f
+#define T_MIN -18.0f // N·m
 #define T_MAX 18.0f
 
 typedef struct // HT04
-{
-    float last_ecd;
-    float ecd;
-    float speed_rpm;
+{              // 角度为多圈角度,范围是-95.5~95.5,单位为rad
+    float last_angle;
+    float total_angle;
+    float speed_aps;
     float real_current;
+} HTMotor_Measure_t;
+
+/* HT电机类型定义*/
+typedef struct
+{
+    HTMotor_Measure_t motor_measure;
+
+    Motor_Control_Setting_s motor_settings;
+
+    PIDInstance current_PID;
+    PIDInstance speed_PID;
+    PIDInstance angle_PID;
+    float *other_angle_feedback_ptr;
+    float *other_speed_feedback_ptr;
+    float pid_ref;
 
-    PIDInstance pid;
     CANInstance *motor_can_instace;
-} HKMotor_Measure_t;
+} HTMotorInstance;
 
+/* HT电机模式,初始化时自动进入CMD_MOTOR_MODE*/
 typedef enum
 {
-    CMD_MOTOR_MODE = 0xfc,
-    CMD_RESET_MODE = 0xfd,
-    CMD_ZERO_POSITION = 0xfe
-} joint_mode;
+    CMD_MOTOR_MODE = 0xfc,   // 使能,会响应指令
+    CMD_RESET_MODE = 0xfd,   // 停止
+    CMD_ZERO_POSITION = 0xfe // 将当前的位置设置为编码器零位
+} HTMotor_Mode_t;
 
-HKMotor_Measure_t *HTMotorInit(CAN_Init_Config_s config);
+/**
+ * @brief 
+ * 
+ * @param config 
+ * @return HTMotorInstance* 
+ */
+HTMotorInstance *HTMotorInit(Motor_Init_Config_s *config);
 
-void JointControl(HKMotor_Measure_t *_instance, float current);
+/**
+ * @brief 设定电机的参考值
+ * 
+ * @param motor 要设定的电机
+ * @param current   设定值
+ */
+void HTMotorSetRef(HTMotorInstance *motor, float ref);
 
-void SetJointMode(joint_mode cmd, HKMotor_Measure_t *_instance);
+/**
+ * @brief 给所有的HT电机发送控制指令
+ * 
+ */
+void HTMotorControl();
+
+/**
+ * @brief 停止电机,之后电机不会响应HTMotorSetRef设定的值
+ * 
+ * @param motor 
+ */
+void HTMotorStop(HTMotorInstance *motor);
+
+/**
+ * @brief 启动电机
+ * 
+ * @param motor 要启动的电机
+ */
+void HTMotorEnable(HTMotorInstance *motor);
+
+/**
+ * @brief 校准电机编码器
+ * @attention 注意,校准时务必将电机和其他机构分离,电机会旋转360°!
+ *            注意,校准时务必将电机和其他机构分离,电机会旋转360°!
+ *            注意,校准时务必将电机和其他机构分离,电机会旋转360°!
+ *            注意,校准时务必将电机和其他机构分离,电机会旋转360°!
+ * 
+ * @param motor 
+ */
+void HTMotorCalibEncoder(HTMotorInstance *motor);
 
 #endif // !HT04_H#define HT04_H
\ No newline at end of file
diff --git a/modules/motor/LK9025.c b/modules/motor/LK9025.c
index 82ca1a2..c265faf 100644
--- a/modules/motor/LK9025.c
+++ b/modules/motor/LK9025.c
@@ -2,25 +2,25 @@
 #include "stdlib.h"
 
 static uint8_t idx;
-static LKMotorInstance* lkmotor_instance[LK_MOTOR_MX_CNT]={NULL};
+static LKMotorInstance *lkmotor_instance[LK_MOTOR_MX_CNT] = {NULL};
 
 static void LKMotorDecode(CANInstance *_instance)
 {
-    static LKMotor_Measure_t* measure;
-    static uint8_t* rx_buff;
-    rx_buff=_instance->rx_buff;
-    measure=&((LKMotorInstance*)_instance)->measure;
-    
-    measure->last_ecd=measure->ecd;
-    measure->ecd=(uint16_t)((rx_buff[7] << 8) | rx_buff[6]);
-    measure->angle_single_round=ECD_ANGLE_COEF*measure->ecd;
-    measure->speed_aps=(1-SPEED_SMOOTH_COEF)*measure->speed_aps+
-                       SPEED_SMOOTH_COEF*(float)((int16_t)(rx_buff[5] << 8 | rx_buff[4]));
-    measure->real_current=(1-CURRENT_SMOOTH_COEF)*measure->real_current+
-                          CURRENT_SMOOTH_COEF*(float)((int16_t)(rx_buff[3] << 8 | rx_buff[2]));
-    measure->temperate=rx_buff[1];
+    static LKMotor_Measure_t *measure;
+    static uint8_t *rx_buff;
+    rx_buff = _instance->rx_buff;
+    measure = &((LKMotorInstance *)_instance)->measure;
 
-    //计算多圈角度
+    measure->last_ecd = measure->ecd;
+    measure->ecd = (uint16_t)((rx_buff[7] << 8) | rx_buff[6]);
+    measure->angle_single_round = ECD_ANGLE_COEF_LK * measure->ecd;
+    measure->speed_aps = (1 - SPEED_SMOOTH_COEF) * measure->speed_aps +
+                         SPEED_SMOOTH_COEF * (float)((int16_t)(rx_buff[5] << 8 | rx_buff[4]));
+    measure->real_current = (1 - CURRENT_SMOOTH_COEF) * measure->real_current +
+                            CURRENT_SMOOTH_COEF * (float)((int16_t)(rx_buff[3] << 8 | rx_buff[2]));
+    measure->temperate = rx_buff[1];
+
+    // 计算多圈角度
     if (measure->ecd - measure->last_ecd > 32678)
         measure->total_round--;
     else if (measure->ecd - measure->last_ecd < -32678)
@@ -28,48 +28,46 @@ static void LKMotorDecode(CANInstance *_instance)
     measure->total_angle = measure->total_round * 360 + measure->angle_single_round;
 }
 
-
 void LKMotorControl()
 {
-    for (size_t i = 0; i < idx; i++)
+    for (size_t i = 0; i < idx; ++i)
     {
-        
     }
 }
 
 LKMotorInstance *LKMotroInit(Motor_Init_Config_s *config)
 {
-    lkmotor_instance[idx]=(LKMotorInstance*)malloc(sizeof(LKMotorInstance));
-    memset(lkmotor_instance[idx],0,sizeof(LKMotorInstance));
+    lkmotor_instance[idx] = (LKMotorInstance *)malloc(sizeof(LKMotorInstance));
+    memset(lkmotor_instance[idx], 0, sizeof(LKMotorInstance));
 
-    lkmotor_instance[idx]->motor_settings=config->controller_setting_init_config;
-    PID_Init(&lkmotor_instance[idx]->current_PID,&config->controller_param_init_config.current_PID);
-    PID_Init(&lkmotor_instance[idx]->current_PID,&config->controller_param_init_config.current_PID);
-    PID_Init(&lkmotor_instance[idx]->current_PID,&config->controller_param_init_config.current_PID);
-    lkmotor_instance[idx]->other_angle_feedback_ptr=config->controller_param_init_config.other_angle_feedback_ptr;
-    lkmotor_instance[idx]->other_speed_feedback_ptr=config->controller_param_init_config.other_speed_feedback_ptr;
+    lkmotor_instance[idx]->motor_settings = config->controller_setting_init_config;
+    PID_Init(&lkmotor_instance[idx]->current_PID, &config->controller_param_init_config.current_PID);
+    PID_Init(&lkmotor_instance[idx]->speed_PID, &config->controller_param_init_config.speed_PID);
+    PID_Init(&lkmotor_instance[idx]->angle_PID, &config->controller_param_init_config.angle_PID);
+    lkmotor_instance[idx]->other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr;
+    lkmotor_instance[idx]->other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr;
 
-    config->can_init_config.can_module_callback=LKMotorDecode;
-    config->can_init_config.rx_id=0x140+config->can_init_config.tx_id;
-    config->can_init_config.tx_id=config->can_init_config.tx_id+0x240;
-    lkmotor_instance[idx]->motor_can_ins=CANRegister(&config->can_init_config);
+    config->can_init_config.id = lkmotor_instance[idx];
+    config->can_init_config.can_module_callback = LKMotorDecode;
+    config->can_init_config.rx_id = 0x140 + config->can_init_config.tx_id;
+    config->can_init_config.tx_id = config->can_init_config.tx_id + 0x240;
+    lkmotor_instance[idx]->motor_can_ins = CANRegister(&config->can_init_config);
 
     LKMotorEnable(lkmotor_instance[idx]);
     return lkmotor_instance[idx++];
 }
 
-
 void LKMotorStop(LKMotorInstance *motor)
 {
-    motor->stop_flag=MOTOR_STOP;
+    motor->stop_flag = MOTOR_STOP;
 }
 
 void LKMotorEnable(LKMotorInstance *motor)
 {
-    motor->stop_flag=MOTOR_ENALBED;
+    motor->stop_flag = MOTOR_ENALBED;
 }
 
 void LKMotorSetRef(LKMotorInstance *motor, float ref)
 {
-    motor->pid_ref=ref;
+    motor->pid_ref = ref;
 }
diff --git a/modules/motor/LK9025.h b/modules/motor/LK9025.h
index 7c9360c..592111e 100644
--- a/modules/motor/LK9025.h
+++ b/modules/motor/LK9025.h
@@ -13,7 +13,7 @@
 #define CURRENT_SMOOTH_COEF 0.9f
 #define SPEED_SMOOTH_COEF 0.85f
 #define REDUCTION_RATIO_DRIVEN 1
-#define ECD_ANGLE_COEF (360.0f/65536.0f)
+#define ECD_ANGLE_COEF_LK (360.0f/65536.0f)
 
 typedef struct // 9025
 {
diff --git a/modules/motor/dji_motor.c b/modules/motor/dji_motor.c
index 4fce573..bda25c2 100644
--- a/modules/motor/dji_motor.c
+++ b/modules/motor/dji_motor.c
@@ -76,7 +76,7 @@ static void MotorSenderGrouping(CAN_Init_Config_s *config)
         dji_motor_info[idx]->sender_group = motor_grouping;
 
         // 检查是否发生id冲突
-        for (size_t i = 0; i < idx; i++)
+        for (size_t i = 0; i < idx; ++i)
         {
             if (dji_motor_info[i]->motor_can_instance->can_handle == config->can_handle && dji_motor_info[i]->motor_can_instance->rx_id == config->rx_id)
                 IDcrash_Handler(i, idx);
@@ -100,7 +100,7 @@ static void MotorSenderGrouping(CAN_Init_Config_s *config)
         dji_motor_info[idx]->message_num = motor_send_num;
         dji_motor_info[idx]->sender_group = motor_grouping;
 
-        for (size_t i = 0; i < idx; i++)
+        for (size_t i = 0; i < idx; ++i)
         {
             if (dji_motor_info[i]->motor_can_instance->can_handle == config->can_handle && dji_motor_info[i]->motor_can_instance->rx_id == config->rx_id)
                 IDcrash_Handler(i, idx);
@@ -124,15 +124,14 @@ static void DecodeDJIMotor(CANInstance *_instance)
     static uint8_t *rxbuff;
     static DJI_Motor_Measure_s *measure;
     rxbuff = _instance->rx_buff;
-    measure = &((DJIMotorInstance*)_instance->id)->motor_measure; // measure要多次使用,保存指针减小访存开销
+    measure = &((DJIMotorInstance *)_instance->id)->motor_measure; // measure要多次使用,保存指针减小访存开销
 
-    uint8_t nice;
     // resolve data and apply filter to current and speed
     measure->last_ecd = measure->ecd;
     measure->ecd = ((uint16_t)rxbuff[0]) << 8 | rxbuff[1];
-    measure->angle_single_round = ECD_ANGLE_COEF * (float)measure->ecd;
-    measure->speed_aps = (1.0f - SPEED_SMOOTH_COEF) * measure->speed_aps + RPM_2_ANGLE_PER_SEC * 
-                            SPEED_SMOOTH_COEF *(float)((int16_t)(rxbuff[2] << 8 | rxbuff[3])) ;
+    measure->angle_single_round = ECD_ANGLE_COEF_DJI * (float)measure->ecd;
+    measure->speed_aps = (1.0f - SPEED_SMOOTH_COEF) * measure->speed_aps + 
+                        RPM_2_ANGLE_PER_SEC * SPEED_SMOOTH_COEF * (float)((int16_t)(rxbuff[2] << 8 | rxbuff[3]));
     measure->real_current = (1.0f - CURRENT_SMOOTH_COEF) * measure->real_current +
                             CURRENT_SMOOTH_COEF * (float)((int16_t)(rxbuff[4] << 8 | rxbuff[5]));
     measure->temperate = rxbuff[6];
@@ -167,7 +166,7 @@ DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config)
 
     // register motor to CAN bus
     config->can_init_config.can_module_callback = DecodeDJIMotor; // set callback
-    config->can_init_config.id=dji_motor_info[idx]; //set id,eq to address(it is identity)
+    config->can_init_config.id = dji_motor_info[idx];             // set id,eq to address(it is identity)
     dji_motor_info[idx]->motor_can_instance = CANRegister(&config->can_init_config);
 
     DJIMotorEnable(dji_motor_info[idx]);
@@ -220,7 +219,7 @@ void DJIMotorControl()
     static DJI_Motor_Measure_s *motor_measure;
     static float pid_measure, pid_ref;
     // 遍历所有电机实例,进行串级PID的计算并设置发送报文的值
-    for (size_t i = 0; i < idx; i++)
+    for (size_t i = 0; i < idx; ++i)
     {
         if (dji_motor_info[i])
         {
@@ -279,7 +278,7 @@ void DJIMotorControl()
     }
 
     // 遍历flag,检查是否要发送这一帧报文
-    for (size_t i = 0; i < 6; i++)
+    for (size_t i = 0; i < 6; ++i)
     {
         if (sender_enable_flag[i])
         {
diff --git a/modules/motor/dji_motor.h b/modules/motor/dji_motor.h
index 5dde915..10ef101 100644
--- a/modules/motor/dji_motor.h
+++ b/modules/motor/dji_motor.h
@@ -24,7 +24,7 @@
 /* 滤波系数设置为1的时候即关闭滤波 */
 #define SPEED_SMOOTH_COEF 0.85f    // better to be greater than 0.85
 #define CURRENT_SMOOTH_COEF 0.9f // this coef *must* be greater than 0.9
-#define ECD_ANGLE_COEF 0.043945f  // 360/8192,将编码器值转化为角度制
+#define ECD_ANGLE_COEF_DJI (360.0f/8192.0f)  // ,将编码器值转化为角度制
 
 /* DJI电机CAN反馈信息*/
 typedef struct
diff --git a/modules/motor/dji_motor.md b/modules/motor/dji_motor.md
index 3f048d8..b383020 100644
--- a/modules/motor/dji_motor.md
+++ b/modules/motor/dji_motor.md
@@ -387,7 +387,7 @@ static dji_motor_instance *dji_motor_info[DJI_MOTOR_CNT] = {NULL};
 
 ```c
 #define PI2 (3.141592f * 2)
-#define ECD_ANGLE_COEF 3.835e-4 // ecd/8192*pi
+#define ECD_ANGLE_COEF_DJI 3.835e-4 // ecd/8192*pi
 ```
 
 这两个宏用于在电机反馈信息中的多圈角度计算，将编码器的0~8192转化为角度表示。
diff --git a/modules/motor/motor_def.h b/modules/motor/motor_def.h
index 2cf2cce..46c9842 100644
--- a/modules/motor/motor_def.h
+++ b/modules/motor/motor_def.h
@@ -99,13 +99,15 @@ typedef enum
  */
 typedef struct
 {
-    float *other_angle_feedback_ptr;
-    float *other_speed_feedback_ptr;
+    float *other_angle_feedback_ptr; // 角度反馈数据指针,注意电机使用total_angle
+    float *other_speed_feedback_ptr; // 速度反馈数据指针,单位为angle per sec
+
+    float *speed_feedforward_ptr;    // 速度前馈数据指针
+    float *current_feedforward_ptr;  // 电流前馈数据指针
 
     PID_Init_config_s current_PID;
     PID_Init_config_s speed_PID;
     PID_Init_config_s angle_PID;
-
 } Motor_Controller_Init_s;
 
 /* 用于初始化CAN电机的结构体,各类电机通用 */
diff --git a/modules/remote/remote_control.c b/modules/remote/remote_control.c
index 9960294..a017199 100644
--- a/modules/remote/remote_control.c
+++ b/modules/remote/remote_control.c
@@ -15,7 +15,7 @@ static USARTInstance *rc_usart_instance;
  */
 static void RectifyRCjoystick()
 {
-    for (uint8_t i = 0; i < 5; i++)
+    for (uint8_t i = 0; i < 5; ++i)
     {
         if (*(&rc_ctrl[TEMP].rc.rocker_l_+i) > 660 || *(&rc_ctrl[TEMP].rc.rocker_l_+i) < -660)
             *(&rc_ctrl[TEMP].rc.rocker_l_+i) = 0;