第一次提交

2025-07-05 20:11:15 +08:00
commit 89b0708dfb
56 changed files with 10876 additions and 0 deletions
--- a/app/app_pid.c
+++ b/app/app_pid.c
@@ -0,0 +1,130 @@
+#include "app_config.h"
+
+#include "app_pid.h"
+
+
+
+
+// PID控制器初始化
+void initializePid(PID_t *pid, pid_mode_t mode, float dtMin)
+{
+    pid->mode = mode;
+    pid->dt_min = dtMin > SIGMA ? dtMin : SIGMA;
+    pid->kp = 0.0f;
+    pid->ki = 0.0f;
+    pid->kd = 0.0f;
+    pid->integral = 0.0f;
+    pid->integral_limit = 0.0f;
+    pid->output_limit = 0.0f;
+    pid->error_previous = 0.0f;
+    pid->last_output = 0.0f;
+}
+
+// 设置PID参数
+int32_t setPidParameters(PID_t *pid, float kp, float ki, float kd, float integralLimit, float outputLimit)
+{
+    int32_t ret = 0;
+
+    if (isfinite(kp)) {
+        pid->kp = kp;
+    } else {
+        ret = -1;
+    }
+
+    if (isfinite(ki)) {
+        pid->ki = ki;
+    } else {
+        ret = -2;
+    }
+
+    if (isfinite(kd)) {
+        pid->kd = kd;
+    } else {
+        ret = -3;
+    }
+
+    if (isfinite(integralLimit)) {
+        pid->integral_limit = integralLimit;
+    } else {
+        ret = -4;
+    }
+
+    if (isfinite(outputLimit)) {
+        pid->output_limit = outputLimit;
+    } else {
+        ret = -5;
+    }
+
+    return ret;
+}
+
+/**
+ * @brief 计算PID控制器的输出
+ *
+ * @param pid 指向PID_t结构体的指针，包含PID控制器的状态和参数
+ * @param sp  设定值(Setpoint)，期望系统达到的目标值
+ * @param val 当前值(Current Value)，系统的实际测量值
+ * @param val_dot 当前值的导数(Derivative of Current Value)，即测量值的变化率（用于微分项计算）
+ * @param dt  时间增量(Time Increment)，两次调用之间的时间间隔，用于计算积分和微分
+ *
+ * @return float 返回PID控制器计算出的输出值
+ */
+float calculatePidOutput(PID_t *pid, float sp, float val, float val_dot, float dt)
+{
+    // 检查输入参数的有效性
+    if (!isfinite(sp) || !isfinite(val) || !isfinite(val_dot) || !isfinite(dt) || dt < pid->dt_min) {
+        return pid->last_output;
+    }
+
+    // 计算误差
+    float error = sp - val;
+
+    // 根据模式计算微分项
+    float derivative = 0.0f;
+    switch (pid->mode) {
+        case PID_MODE_DERIVATIVE_CALC:
+            derivative = (error - pid->error_previous) / dt;
+            pid->error_previous = error;
+            break;
+        case PID_MODE_DERIVATIVE_CALC_NO_SP:
+            derivative = (-val - pid->error_previous) / dt;
+            pid->error_previous = -val;
+            break;
+        case PID_MODE_DERIVATIVE_SET:
+            derivative = -val_dot;
+            break;
+        default:
+            derivative = 0.0f;
+            break;
+    }
+
+    // 计算比例和微分项的输出
+    float output = (pid->kp * error) + (pid->kd * derivative);
+
+    // 计算积分项，并检查积分饱和
+    if (pid->ki > SIGMA) {
+        pid->integral += error * dt;
+        if (pid->integral > pid->integral_limit) {
+            pid->integral = pid->integral_limit;
+        } else if (pid->integral < -pid->integral_limit) {
+            pid->integral = -pid->integral_limit;
+        }
+        output += pid->ki * pid->integral;
+    }
+
+    // 限制输出范围
+    if (output > pid->output_limit) {
+        output = pid->output_limit;
+    } else if (output < -pid->output_limit) {
+        output = -pid->output_limit;
+    }
+
+    pid->last_output = output;
+    return output;
+}
+
+// 重置积分器
+void resetPidIntegral(PID_t *pid)
+{
+    pid->integral = 0.0f;
+}