finish all bsp utilities

modules/algorithm/controller.h

@@ -1,19 +1,18 @@
 /**
-  ******************************************************************************
-  * @file	 controller.h
-  * @author  Wang Hongxi
-  * @version V1.1.3
-  * @date    2021/7/3
-  * @brief   
-  ******************************************************************************
-  * @attention 
-  *
-  ******************************************************************************
-  */
+ ******************************************************************************
+ * @file	 controller.h
+ * @author  Wang Hongxi
+ * @version V1.1.3
+ * @date    2021/7/3
+ * @brief
+ ******************************************************************************
+ * @attention
+ *
+ ******************************************************************************
+ */
 #ifndef _CONTROLLER_H
 #define _CONTROLLER_H
 
-
 #include "main.h"
 #include "stdint.h"
 #include "string.h"
@@ -26,36 +25,37 @@
 #define abs(x) ((x > 0) ? x : -x)
 #endif
 
-
-/******************************* PID CONTROL *********************************/
+// PID 优化环节使能标志位
 typedef enum pid_Improvement_e
 {
-    NONE = 0X00,                        //0000 0000
-    Integral_Limit = 0x01,              //0000 0001
-    Derivative_On_Measurement = 0x02,   //0000 0010
-    Trapezoid_Intergral = 0x04,         //0000 0100
-    Proportional_On_Measurement = 0x08, //0000 1000
-    OutputFilter = 0x10,                //0001 0000
-    ChangingIntegrationRate = 0x20,     //0010 0000
-    DerivativeFilter = 0x40,            //0100 0000
-    ErrorHandle = 0x80,                 //1000 0000
+    NONE = 0b00000000,                        // 0000 0000
+    Integral_Limit = 0b00000001,              // 0000 0001
+    Derivative_On_Measurement = 0b00000010,   // 0000 0010
+    Trapezoid_Intergral = 0b00000100,         // 0000 0100
+    Proportional_On_Measurement = 0b00001000, // 0000 1000
+    OutputFilter = 0b00010000,                // 0001 0000
+    ChangingIntegrationRate = 0b00100000,     // 0010 0000
+    DerivativeFilter = 0b01000000,            // 0100 0000
+    ErrorHandle = 0b10000000,                 // 1000 0000
 } PID_Improvement_e;
 
+/* PID 报错类型枚举*/
 typedef enum errorType_e
 {
     PID_ERROR_NONE = 0x00U,
     Motor_Blocked = 0x01U
 } ErrorType_e;
 
-typedef  struct
+typedef struct
 {
     uint64_t ERRORCount;
     ErrorType_e ERRORType;
 } PID_ErrorHandler_t;
 
-typedef struct 
+/* PID结构体 */
+typedef struct
 {
-//---------------------------------- init config block
+    //---------------------------------- init config block
     // config parameter
     float Kp;
     float Ki;
@@ -64,13 +64,13 @@ typedef struct
     float MaxOut;
     float IntegralLimit;
     float DeadBand;
-    float CoefA;         //For Changing Integral
-    float CoefB;         //ITerm = Err*((A-abs(err)+B)/A)  when B<|err|<A+B
+    float CoefA;         // For Changing Integral
+    float CoefB;         // ITerm = Err*((A-abs(err)+B)/A)  when B<|err|<A+B
     float Output_LPF_RC; // RC = 1/omegac
     float Derivative_LPF_RC;
 
     uint8_t Improve;
-//-----------------------------------
+    //-----------------------------------
     // for calculating
     float Measure;
     float Last_Measure;
@@ -96,7 +96,7 @@ typedef struct
 } PID_t;
 
 /* 用于PID初始化的结构体*/
-typedef struct 
+typedef struct
 {
     // config parameter
     float Kp;
@@ -106,8 +106,8 @@ typedef struct
     float MaxOut;
     float IntegralLimit;
     float DeadBand;
-    float CoefA;         //For Changing Integral
-    float CoefB;         //ITerm = Err*((A-abs(err)+B)/A)  when B<|err|<A+B
+    float CoefA;         // For Changing Integral
+    float CoefB;         // ITerm = Err*((A-abs(err)+B)/A)  when B<|err|<A+B
     float Output_LPF_RC; // RC = 1/omegac
     float Derivative_LPF_RC;
 
@@ -115,32 +115,22 @@ typedef struct
 
 } PID_Init_config_s;
 
+/**
+ * @brief 初始化PID实例
+ *
+ * @param pid    PID实例指针
+ * @param config PID初始化配置
+ */
+void PID_Init(PID_t *pid, PID_Init_config_s *config);
 
-
-void PID_Init(PID_t* pid,PID_Init_config_s* config);
+/**
+ * @brief 计算PID输出
+ *
+ * @param pid     PID实例指针
+ * @param measure 反馈值
+ * @param ref     设定值
+ * @return float  PID计算输出
+ */
 float PID_Calculate(PID_t *pid, float measure, float ref);
 
-
-/*************************** Tracking Differentiator ***************************/
-typedef  struct
-{
-    float Input;
-
-    float h0;
-    float r;
-
-    float x;
-    float dx;
-    float ddx;
-
-    float last_dx;
-    float last_ddx;
-
-    uint32_t DWT_CNT;
-    float dt;
-} TD_t;
-
-void TD_Init(TD_t *td, float r, float h0);
-float TD_Calculate(TD_t *td, float input);
-
-#endif
+#endif