diff --git a/Makefile b/Makefile
index 1d537ce..456a6bd 100644
--- a/Makefile
+++ b/Makefile
@@ -199,7 +199,7 @@ FPU = -mfpu=fpv4-sp-d16
 FLOAT-ABI = -mfloat-abi=hard
 
 # mcu
-MCU = $(CPU) -mthumb $(FPU) $(FLOAT-ABI)
+MCU = $(CPU) -mthumb -mthumb-interwork $(FPU) $(FLOAT-ABI)
 
 # macros for gcc
 # AS defines
@@ -211,7 +211,9 @@ C_DEFS =  \
 -DSTM32F407xx \
 -DARM_MATH_CM4 \
 -DARM_MATH_MATRIX_CHECK \
--DARM_MATH_ROUNDING 
+-DARM_MATH_ROUNDING \
+-DARM_MATH_LOOPUNROLL \
+-DISABLEFLOAT16
 
 # AS includes
 AS_INCLUDES =  \
@@ -277,7 +279,7 @@ C_INCLUDES =  \
 # compile gcc flags
 ASFLAGS = $(MCU) $(AS_DEFS) $(AS_INCLUDES) $(OPT) -fdata-sections -ffunction-sections
 
-CFLAGS += $(MCU) $(C_DEFS) $(C_INCLUDES) $(OPT) -fdata-sections -ffunction-sections
+CFLAGS += $(MCU) $(C_DEFS) $(C_INCLUDES) $(OPT) -fdata-sections -ffunction-sections -fmessage-length=0
 
 ifeq ($(DEBUG), 1)
 CFLAGS += -g -gdwarf-2
@@ -299,7 +301,7 @@ LIBS = -lc -lm -lnosys  \
 -l:libarm_cortexM4lf_math.a
 LIBDIR =  \
 -LMiddlewares/ST/ARM/DSP/Lib
-LDFLAGS = $(MCU) -specs=nano.specs -T$(LDSCRIPT) $(LIBDIR) $(LIBS) -Wl,-Map=$(BUILD_DIR)/$(TARGET).map,--cref -Wl,--gc-sections
+LDFLAGS = $(MCU) -specs=nano.specs -T$(LDSCRIPT) $(LIBDIR) $(LIBS) -Wl,-Map=$(BUILD_DIR)/$(TARGET).map,--cref -Wl,--gc-sections -flto
 
 # default action: build all
 all: $(BUILD_DIR)/$(TARGET).elf $(BUILD_DIR)/$(TARGET).hex $(BUILD_DIR)/$(TARGET).bin
@@ -339,7 +341,7 @@ $(BUILD_DIR):
 # clean up
 #######################################
 clean:
-	rd /s/q $(BUILD_DIR)
+	rd $(BUILD_DIR) /s/q
 
 
 #######################################
diff --git a/Src/main.c b/Src/main.c
index 5a1f4b0..ce57ebb 100644
--- a/Src/main.c
+++ b/Src/main.c
@@ -36,6 +36,7 @@
 /* Private includes ----------------------------------------------------------*/
 /* USER CODE BEGIN Includes */
 #include "robot.h"
+#include "bsp_log.h"
 /* USER CODE END Includes */
 
 /* Private typedef -----------------------------------------------------------*/
@@ -120,6 +121,7 @@ int main(void)
   MX_DAC_Init();
   /* USER CODE BEGIN 2 */
 	RobotInit(); // 唯一的初始化函数
+  LOGINFO("[main] SystemInit() and RobotInit() done");
   /* USER CODE END 2 */
 
   /* Call init function for freertos objects (in freertos.c) */
diff --git a/bsp/log/bsp_log.h b/bsp/log/bsp_log.h
index 7709ffe..002be77 100644
--- a/bsp/log/bsp_log.h
+++ b/bsp/log/bsp_log.h
@@ -7,44 +7,46 @@
 
 #define BUFFER_INDEX 0
 
-
-
 /**
  * @brief 日志系统初始化
- * 
+ *
  */
 void BSPLogInit();
 
 /**
  * @brief 日志功能原型,供下面的LOGI,LOGW,LOGE等使用
- * 
+ *
  */
-#define LOG_PROTO(type,color,format,...)            \
-        SEGGER_RTT_printf(BUFFER_INDEX,"  %s%s"format"\r\n%s", \
-                          color,                    \
-                          type,                     \
-                          ##__VA_ARGS__,            \
+#define LOG_PROTO(type, color, format, ...)                       \
+        SEGGER_RTT_printf(BUFFER_INDEX, "  %s%s" format "\r\n%s", \
+                          color,                                  \
+                          type,                                   \
+                          ##__VA_ARGS__,                          \
                           RTT_CTRL_RESET)
 
-/*------下面是日志输出的接口--------*/
+/*----------------------------------------下面是日志输出的接口-------------------------------------------------*/
 
 /* 清屏 */
-#define LOG_CLEAR() SEGGER_RTT_WriteString(0, "  "RTT_CTRL_CLEAR)
+#define LOG_CLEAR() SEGGER_RTT_WriteString(0, "  " RTT_CTRL_CLEAR)
 
 /* 无颜色日志输出 */
-#define LOG(format,...) LOG_PROTO("","",format,##__VA_ARGS__)
+#define LOG(format, ...) LOG_PROTO("", "", format, ##__VA_ARGS__)
 
-/* 有颜色格式日志输出,建议使用这些宏来输出日志 */
+/**
+ *  有颜色格式日志输出,建议使用这些宏来输出日志
+ *  @attention 注意这些接口不支持浮点格式化输出,若有需要,请使用Float2Str()函数进行转换后再打印
+ */
 // information level
-#define LOGINFO(format,...) LOG_PROTO("I:", RTT_CTRL_TEXT_BRIGHT_GREEN , format, ##__VA_ARGS__)
+#define LOGINFO(format, ...) LOG_PROTO("I:", RTT_CTRL_TEXT_BRIGHT_GREEN, format, ##__VA_ARGS__)
 // warning level
-#define LOGWARNING(format,...) LOG_PROTO("W:", RTT_CTRL_TEXT_BRIGHT_YELLOW, format, ##__VA_ARGS__)
+#define LOGWARNING(format, ...) LOG_PROTO("W:", RTT_CTRL_TEXT_BRIGHT_YELLOW, format, ##__VA_ARGS__)
 // error level
-#define LOGERROR(format,...) LOG_PROTO("E:", RTT_CTRL_TEXT_BRIGHT_RED   , format, ##__VA_ARGS__)
+#define LOGERROR(format, ...) LOG_PROTO("E:", RTT_CTRL_TEXT_BRIGHT_RED, format, ##__VA_ARGS__)
 
 /**
  * @brief 通过segger RTT打印日志,支持格式化输出,格式化输出的实现参考printf.
- * 
+ * @attention !! 此函数不支持浮点格式化,若有需要,请使用Float2Str()函数进行转换后再打印 !!
+ *
  * @param fmt 格式字符串
  * @param ... 参数列表
  * @return int 打印的log字符数
@@ -53,7 +55,8 @@ int PrintLog(const char *fmt, ...);
 
 /**
  * @brief 利用sprintf(),将float转换为字符串进行打印
- * 
+ * @attention 浮点数需要转换为字符串后才能通过RTT打印
+ *
  * @param str 转换后的字符串
  * @param va 待转换的float
  */
diff --git a/bsp/log/bsp_log.md b/bsp/log/bsp_log.md
index 64be4a3..259c296 100644
--- a/bsp/log/bsp_log.md
+++ b/bsp/log/bsp_log.md
@@ -2,10 +2,6 @@
 
 <p align='right'>neozng1@hnu.edu.cn</p>
 
-> TODO:
->
-> 1. 在未接入调试器的时候，将日志写入flash中，并提供接口读取
-
 ## 使用说明
 
 bsp_log是基于segger RTT实现的日志打印模块。
@@ -18,11 +14,11 @@ bsp_log是基于segger RTT实现的日志打印模块。
 #define LOGERROR(format,...)
 ```
 
-分别用于输出不同等级的日志。
+分别用于输出不同等级的日志。注意RTT不支持直接使用`%f`进行浮点格式化,要使用`void Float2Str(char *str, float va);`转化成字符串之后再发送。
 
-**若想启用RTT，必须通过`launch.json`的`debug-jlink`启动调试（不论使用什么调试器）。**
+**若想启用RTT，必须通过`launch.json`的`debug-jlink`启动调试（不论使用什么调试器）。** 按照`VSCode+Ozone环境配置`完成配置之后的cmsis dap和daplink是可以支持Jlink全家桶的。
 
-注意，若你使用的是cmsis-dap和daplink，**请在调试任务启动之后再打开`log`任务。**（均在项目文件夹下的.vsocde/task.json中，有注释自行查看）。
+另外，若你使用的是cmsis-dap和daplink，**请在 *jlink* 调试任务启动之后再打开`log`任务。**（均在项目文件夹下的.vsocde/task.json中，有注释自行查看）。否则可能出线RTT viewer无法连接客户端的情况。
 
 在ozone中查看log输出，直接打开console调试任务台和terminal调试中断便可看到调试输出。
 
@@ -34,7 +30,7 @@ bsp_log是基于segger RTT实现的日志打印模块。
 
 ```c
 int printf_log(const char *fmt, ...);
-void Float2Str(char *str, float va);
+void Float2Str(char *str, float va); // 输出浮点需要先用此函数进行转换
 ```
 
 调用第一个函数，可以通过jlink或dap-link向调试器连接的上位机发送信息，格式和printf相同，示例如下：
@@ -52,8 +48,3 @@ char* str_buff[64];
 Float2Str(str_buff,current_feedback);
 printf_log("Motor %d met some problem, error code %d!\n",3,1);
 ```
-
-或直接通过`%f`格式符直接使用`printf_log()`发送日志，可以设置小数点位数以降低带宽开销。
-
-
-
diff --git a/bsp/pwm/bsp_pwm.c b/bsp/pwm/bsp_pwm.c
index 9e56ef8..403b75c 100644
--- a/bsp/pwm/bsp_pwm.c
+++ b/bsp/pwm/bsp_pwm.c
@@ -8,6 +8,8 @@ static PWMInstance *pwm_instance[PWM_DEVICE_CNT] = {NULL}; // 所有的pwm insta
 
 /**
  * @brief pwm dma传输完成回调函数
+ * @attention 由于HAL库的设计问题,当一个pulse完成(即tim的计数超过比较寄存器)也会调用此函数
+ *            故对于那些开启了PWM的TIM,务必关闭其全局中断,仅保持DMA传输完成中断打开
  * 
  * @param htim 发生中断的定时器句柄
  */
diff --git a/modules/algorithm/user_lib.c b/modules/algorithm/user_lib.c
index b511bb9..5029109 100644
--- a/modules/algorithm/user_lib.c
+++ b/modules/algorithm/user_lib.c
@@ -206,3 +206,9 @@ float AverageFilter(float new_data, float *buf, uint8_t len)
     return sum / len;
 }
 
+void MatInit(mat *m, uint8_t row, uint8_t col)
+{
+    m->numCols = col;
+    m->numRows = row;
+    m->pData = (float *)zmalloc(row * col * sizeof(float));
+}
diff --git a/modules/algorithm/user_lib.h b/modules/algorithm/user_lib.h
index fc90e5a..9a15097 100644
--- a/modules/algorithm/user_lib.h
+++ b/modules/algorithm/user_lib.h
@@ -13,14 +13,11 @@
 #ifndef _USER_LIB_H
 #define _USER_LIB_H
 
-
 #include "stdint.h"
 #include "main.h"
 #include "cmsis_os.h"
-
-
-#define msin(x) (arm_sin_f32(x))
-#define mcos(x) (arm_cos_f32(x))
+#include "stm32f407xx.h"
+#include "arm_math.h"
 
 
 #ifndef user_malloc
@@ -31,6 +28,18 @@
 #endif
 #endif
 
+#define msin(x) (arm_sin_f32(x))
+#define mcos(x) (arm_cos_f32(x))
+
+typedef arm_matrix_instance_f32 mat;
+// 若运算速度不够,可以使用q31代替f32,但是精度会降低
+#define MatAdd arm_mat_add_f32
+#define MatSubtract arm_mat_sub_f32
+#define MatMultiply arm_mat_mult_f32
+#define MatTranspose arm_mat_trans_f32
+#define MatInverse arm_mat_inverse_f32
+void MatInit(mat *m, uint8_t row, uint8_t col);
+
 /* boolean type definitions */
 #ifndef TRUE
 #define TRUE 1 /**< boolean true  */
@@ -40,12 +49,6 @@
 #define FALSE 0 /**< boolean fails */
 #endif
 
-/* math relevant */
-/* radian coefficient */
-#ifndef RADIAN_COEF
-#define RADIAN_COEF 57.295779513f
-#endif
-
 /* circumference ratio */
 #ifndef PI
 #define PI 3.14159265354f
diff --git a/合理地进行PID参数整定.md b/合理地进行PID参数整定.md
index 17195b2..59ea115 100644
--- a/合理地进行PID参数整定.md
+++ b/合理地进行PID参数整定.md
@@ -1,3 +1,7 @@
 # 利用Ozone进行model-based PID tunning
 
-Ozone的实时变量可视化监测(示波器)功能可以很好地帮助我们观察控制器在时域的表现，典型的有上升时间、超调量和稳态时间等。
\ No newline at end of file
+Ozone的实时变量可视化监测(示波器)功能可以很好地帮助我们观察控制器在时域的表现，典型的有上升时间、超调量和稳态时间等。
+
+## 调试顺序
+
+先内环，后外环。若有已知的外部扰动如阻力、重力等可以在**保持kp不变**的情况下添加积分环节，并查看达到稳态时积分的输出，该输出值可以作为**前馈**作用通过feedforward_ptr一同送入下一个串级控制器。
\ No newline at end of file