添加pid和flitter

User/component/dependencies.csv

@@ -0,0 +1,2 @@
+pid,component/filter,component/user_math
+filter,component/user_math

User/component/filter.c

@@ -0,0 +1,185 @@
+/*
+  各类滤波器。
+*/
+
+#include "filter.h"
+
+#include "user_math.h"
+
+/**
+ * @brief 初始化滤波器
+ *
+ * @param f 滤波器
+ * @param sample_freq 采样频率
+ * @param cutoff_freq 截止频率
+ */
+void LowPassFilter2p_Init(LowPassFilter2p_t *f, float sample_freq,
+                          float cutoff_freq) {
+  if (f == NULL) return;
+
+  f->cutoff_freq = cutoff_freq;
+
+  f->delay_element_1 = 0.0f;
+  f->delay_element_2 = 0.0f;
+
+  if (f->cutoff_freq <= 0.0f) {
+    /* no filtering */
+    f->b0 = 1.0f;
+    f->b1 = 0.0f;
+    f->b2 = 0.0f;
+
+    f->a1 = 0.0f;
+    f->a2 = 0.0f;
+
+    return;
+  }
+  const float fr = sample_freq / f->cutoff_freq;
+  const float ohm = tanf(M_PI / fr);
+  const float c = 1.0f + 2.0f * cosf(M_PI / 4.0f) * ohm + ohm * ohm;
+
+  f->b0 = ohm * ohm / c;
+  f->b1 = 2.0f * f->b0;
+  f->b2 = f->b0;
+
+  f->a1 = 2.0f * (ohm * ohm - 1.0f) / c;
+  f->a2 = (1.0f - 2.0f * cosf(M_PI / 4.0f) * ohm + ohm * ohm) / c;
+}
+
+/**
+ * @brief 施加一次滤波计算
+ *
+ * @param f 滤波器
+ * @param sample 采样的值
+ * @return float 滤波后的值
+ */
+float LowPassFilter2p_Apply(LowPassFilter2p_t *f, float sample) {
+  if (f == NULL) return 0.0f;
+
+  /* do the filtering */
+  float delay_element_0 =
+      sample - f->delay_element_1 * f->a1 - f->delay_element_2 * f->a2;
+
+  if (isinf(delay_element_0)) {
+    /* don't allow bad values to propagate via the filter */
+    delay_element_0 = sample;
+  }
+
+  const float output = delay_element_0 * f->b0 + f->delay_element_1 * f->b1 +
+                       f->delay_element_2 * f->b2;
+
+  f->delay_element_2 = f->delay_element_1;
+  f->delay_element_1 = delay_element_0;
+
+  /* return the value. Should be no need to check limits */
+  return output;
+}
+
+/**
+ * @brief 重置滤波器
+ *
+ * @param f 滤波器
+ * @param sample 采样的值
+ * @return float 滤波后的值
+ */
+float LowPassFilter2p_Reset(LowPassFilter2p_t *f, float sample) {
+  if (f == NULL) return 0.0f;
+
+  const float dval = sample / (f->b0 + f->b1 + f->b2);
+
+  if (isfinite(dval)) {
+    f->delay_element_1 = dval;
+    f->delay_element_2 = dval;
+
+  } else {
+    f->delay_element_1 = sample;
+    f->delay_element_2 = sample;
+  }
+
+  return LowPassFilter2p_Apply(f, sample);
+}
+
+/**
+ * @brief 初始化滤波器
+ *
+ * @param f 滤波器
+ * @param sample_freq 采样频率
+ * @param notch_freq 中心频率
+ * @param bandwidth 带宽
+ */
+void NotchFilter_Init(NotchFilter_t *f, float sample_freq, float notch_freq,
+                      float bandwidth) {
+  if (f == NULL) return;
+
+  f->notch_freq = notch_freq;
+  f->bandwidth = bandwidth;
+
+  f->delay_element_1 = 0.0f;
+  f->delay_element_2 = 0.0f;
+
+  if (notch_freq <= 0.0f) {
+    /* no filtering */
+    f->b0 = 1.0f;
+    f->b1 = 0.0f;
+    f->b2 = 0.0f;
+
+    f->a1 = 0.0f;
+    f->a2 = 0.0f;
+
+    return;
+  }
+
+  const float alpha = tanf(M_PI * bandwidth / sample_freq);
+  const float beta = -cosf(M_2PI * notch_freq / sample_freq);
+  const float a0_inv = 1.0f / (alpha + 1.0f);
+
+  f->b0 = a0_inv;
+  f->b1 = 2.0f * beta * a0_inv;
+  f->b2 = a0_inv;
+
+  f->a1 = f->b1;
+  f->a2 = (1.0f - alpha) * a0_inv;
+}
+
+/**
+ * @brief 施加一次滤波计算
+ *
+ * @param f 滤波器
+ * @param sample 采样的值
+ * @return float 滤波后的值
+ */
+inline float NotchFilter_Apply(NotchFilter_t *f, float sample) {
+  if (f == NULL) return 0.0f;
+
+  /* Direct Form II implementation */
+  const float delay_element_0 =
+      sample - f->delay_element_1 * f->a1 - f->delay_element_2 * f->a2;
+  const float output = delay_element_0 * f->b0 + f->delay_element_1 * f->b1 +
+                       f->delay_element_2 * f->b2;
+
+  f->delay_element_2 = f->delay_element_1;
+  f->delay_element_1 = delay_element_0;
+
+  return output;
+}
+
+/**
+ * @brief 重置滤波器
+ *
+ * @param f 滤波器
+ * @param sample 采样的值
+ * @return float 滤波后的值
+ */
+float NotchFilter_Reset(NotchFilter_t *f, float sample) {
+  if (f == NULL) return 0.0f;
+
+  float dval = sample;
+
+  if (fabsf(f->b0 + f->b1 + f->b2) > FLT_EPSILON) {
+    dval = dval / (f->b0 + f->b1 + f->b2);
+  }
+
+  f->delay_element_1 = dval;
+  f->delay_element_2 = dval;
+
+  return NotchFilter_Apply(f, sample);
+}

User/component/filter.h

@@ -0,0 +1,102 @@
+/*
+  各类滤波器。
+*/
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* 二阶低通滤波器 */
+typedef struct {
+  float cutoff_freq; /* 截止频率 */
+
+  float a1;
+  float a2;
+
+  float b0;
+  float b1;
+  float b2;
+
+  float delay_element_1;
+  float delay_element_2;
+
+} LowPassFilter2p_t;
+
+/* 带阻滤波器 */
+typedef struct {
+  float notch_freq; /* 阻止频率 */
+  float bandwidth;  /* 带宽 */
+
+  float a1;
+  float a2;
+
+  float b0;
+  float b1;
+  float b2;
+  float delay_element_1;
+  float delay_element_2;
+
+} NotchFilter_t;
+
+/**
+ * @brief 初始化滤波器
+ *
+ * @param f 滤波器
+ * @param sample_freq 采样频率
+ * @param cutoff_freq 截止频率
+ */
+void LowPassFilter2p_Init(LowPassFilter2p_t *f, float sample_freq,
+                          float cutoff_freq);
+
+/**
+ * @brief 施加一次滤波计算
+ *
+ * @param f 滤波器
+ * @param sample 采样的值
+ * @return float 滤波后的值
+ */
+float LowPassFilter2p_Apply(LowPassFilter2p_t *f, float sample);
+
+/**
+ * @brief 重置滤波器
+ *
+ * @param f 滤波器
+ * @param sample 采样的值
+ * @return float 滤波后的值
+ */
+float LowPassFilter2p_Reset(LowPassFilter2p_t *f, float sample);
+
+/**
+ * @brief 初始化滤波器
+ *
+ * @param f 滤波器
+ * @param sample_freq 采样频率
+ * @param notch_freq 中心频率
+ * @param bandwidth 带宽
+ */
+void NotchFilter_Init(NotchFilter_t *f, float sample_freq, float notch_freq,
+                      float bandwidth);
+
+/**
+ * @brief 施加一次滤波计算
+ *
+ * @param f 滤波器
+ * @param sample 采样的值
+ * @return float 滤波后的值
+ */
+float NotchFilter_Apply(NotchFilter_t *f, float sample);
+
+/**
+ * @brief 重置滤波器
+ *
+ * @param f 滤波器
+ * @param sample 采样的值
+ * @return float 滤波后的值
+ */
+float NotchFilter_Reset(NotchFilter_t *f, float sample);
+
+#ifdef __cplusplus
+}
+#endif

User/component/pid.c

@@ -0,0 +1,160 @@
+/*
+   Modified from
+   https://github.com/PX4/Firmware/blob/master/src/lib/pid/pid.cpp
+
+   参考资料：
+   https://github.com/PX4/Firmware/issues/12362
+   https://dev.px4.io/master/en/flight_stack/controller_diagrams.html
+   https://docs.px4.io/master/en/config_mc/pid_tuning_guide_multicopter.html#standard_form
+   https://www.controleng.com/articles/not-all-pid-controllers-are-the-same/
+   https://en.wikipedia.org/wiki/PID_controller
+   http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/
+*/
+
+#include "pid.h"
+
+#include "user_math.h"
+
+#define SIGMA 0.000001f
+
+/**
+ * @brief 初始化PID
+ *
+ * @param pid PID结构体
+ * @param mode PID模式
+ * @param sample_freq 采样频率
+ * @param param PID参数
+ * @return int8_t 0对应没有错误
+ */
+int8_t PID_Init(KPID_t *pid, KPID_Mode_t mode, float sample_freq,
+                const KPID_Params_t *param) {
+  if (pid == NULL) return -1;
+
+  if (!isfinite(param->p)) return -1;
+  if (!isfinite(param->i)) return -1;
+  if (!isfinite(param->d)) return -1;
+  if (!isfinite(param->i_limit)) return -1;
+  if (!isfinite(param->out_limit)) return -1;
+  pid->param = param;
+
+  float dt_min = 1.0f / sample_freq;
+  if (isfinite(dt_min))
+    pid->dt_min = dt_min;
+  else
+    return -1;
+
+  LowPassFilter2p_Init(&(pid->dfilter), sample_freq, pid->param->d_cutoff_freq);
+
+  pid->mode = mode;
+  PID_Reset(pid);
+  return 0;
+}
+
+/**
+ * @brief PID计算
+ *
+ * @param pid PID结构体
+ * @param sp 设定值
+ * @param fb 反馈值
+ * @param fb_dot 反馈值微分
+ * @param dt 间隔时间
+ * @return float 计算的输出
+ */
+float PID_Calc(KPID_t *pid, float sp, float fb, float fb_dot, float dt) {
+  if (!isfinite(sp) || !isfinite(fb) || !isfinite(fb_dot) || !isfinite(dt)) {
+    return pid->last.out;
+  }
+
+  /* 计算误差值 */
+  const float err = CircleError(sp, fb, pid->param->range);
+
+  /* 计算P项 */
+  const float k_err = err * pid->param->k;
+
+  /* 计算D项 */
+  const float k_fb = pid->param->k * fb;
+  const float filtered_k_fb = LowPassFilter2p_Apply(&(pid->dfilter), k_fb);
+
+  float d;
+  switch (pid->mode) {
+    case KPID_MODE_CALC_D:
+      /* 通过fb计算D，避免了由于sp变化导致err突变的问题 */
+      /* 当sp不变时，err的微分等于负的fb的微分 */
+      d = (filtered_k_fb - pid->last.k_fb) / fmaxf(dt, pid->dt_min);
+      break;
+
+    case KPID_MODE_SET_D:
+      d = fb_dot;
+      break;
+
+    case KPID_MODE_NO_D:
+      d = 0.0f;
+      break;
+  }
+  pid->last.err = err;
+  pid->last.k_fb = filtered_k_fb;
+
+  if (!isfinite(d)) d = 0.0f;
+
+  /* 计算PD输出 */
+  float output = (k_err * pid->param->p) - (d * pid->param->d);
+
+  /* 计算I项 */
+  const float i = pid->i + (k_err * dt);
+  const float i_out = i * pid->param->i;
+  
+  if (pid->param->i > SIGMA) {
+    /* 检查是否饱和 */
+    if (isfinite(i)) {
+      if ((fabsf(output + i_out) <= pid->param->out_limit) &&
+          (fabsf(i) <= pid->param->i_limit)) {
+        /* 未饱和，使用新积分 */
+        pid->i = i;
+      }
+    }
+  }
+  
+  /* 计算PID输出 */
+  output += i_out;
+ 
+  /* 限制输出 */
+  if (isfinite(output)) {
+    if (pid->param->out_limit > SIGMA) {
+      output = AbsClip(output, pid->param->out_limit);
+    }
+    pid->last.out = output;
+  }
+  return pid->last.out;
+}
+
+/**
+ * @brief 重置微分项
+ *
+ * @param pid PID结构体
+ * @return int8_t 0对应没有错误
+ */
+int8_t PID_ResetIntegral(KPID_t *pid) {
+  if (pid == NULL) return -1;
+
+  pid->i = 0.0f;
+
+  return 0;
+}
+
+/**
+ * @brief 重置PID
+ *
+ * @param pid PID结构体
+ * @return int8_t 0对应没有错误
+ */
+int8_t PID_Reset(KPID_t *pid) {
+  if (pid == NULL) return -1;
+
+  pid->i = 0.0f;
+  pid->last.err = 0.0f;
+  pid->last.k_fb = 0.0f;
+  pid->last.out = 0.0f;
+  LowPassFilter2p_Reset(&(pid->dfilter), 0.0f);
+
+  return 0;
+}

User/component/pid.h

@@ -0,0 +1,94 @@
+/*
+  Modified from
+  https://github.com/PX4/Firmware/blob/master/src/lib/pid/pid.h
+*/
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+
+#include "filter.h"
+
+/* PID模式 */
+typedef enum {
+  KPID_MODE_NO_D = 0, /* 不使用微分项，PI控制器 */
+  KPID_MODE_CALC_D, /* 根据反馈的值计算离散微分，忽略PID_Calc中的fb_dot */
+  KPID_MODE_SET_D /* 直接提供微分值，PID_Calc中的fb_dot将被使用，(Gyros) */
+} KPID_Mode_t;
+
+/* PID参数 */
+typedef struct {
+  float k;             /* 控制器增益，设置为1用于并行模式 */
+  float p;             /* 比例项增益，设置为1用于标准形式 */
+  float i;             /* 积分项增益 */
+  float d;             /* 微分项增益 */
+  float i_limit;       /* 积分项上限 */
+  float out_limit;     /* 输出绝对值限制 */
+  float d_cutoff_freq; /* D项低通截止频率 */
+  float range;         /* 计算循环误差时使用，大于0时启用 */
+} KPID_Params_t;
+
+/* PID主结构体 */
+typedef struct {
+  KPID_Mode_t mode;
+  const KPID_Params_t *param;
+
+  float dt_min; /* 最小PID_Calc调用间隔 */
+  float i;      /* 积分 */
+
+  struct {
+    float err;  /* 上次误差 */
+    float k_fb; /* 上次反馈值 */
+    float out;  /* 上次输出 */
+  } last;
+
+  LowPassFilter2p_t dfilter; /* D项低通滤波器 */
+} KPID_t;
+
+/**
+ * @brief 初始化PID
+ *
+ * @param pid PID结构体
+ * @param mode PID模式
+ * @param sample_freq 采样频率
+ * @param param PID参数
+ * @return int8_t 0对应没有错误
+ */
+int8_t PID_Init(KPID_t *pid, KPID_Mode_t mode, float sample_freq,
+                const KPID_Params_t *param);
+
+/**
+ * @brief PID计算
+ *
+ * @param pid PID结构体
+ * @param sp 设定值
+ * @param fb 反馈值
+ * @param fb_dot 反馈值微分
+ * @param dt 间隔时间
+ * @return float 计算的输出
+ */
+float PID_Calc(KPID_t *pid, float sp, float fb, float fb_dot, float dt);
+
+/**
+ * @brief 重置微分项
+ *
+ * @param pid PID结构体
+ * @return int8_t 0对应没有错误
+ */
+int8_t PID_ResetIntegral(KPID_t *pid);
+
+/**
+ * @brief 重置PID
+ *
+ * @param pid PID结构体
+ * @return int8_t 0对应没有错误
+ */
+int8_t PID_Reset(KPID_t *pid);
+
+#ifdef __cplusplus
+}
+#endif

User/component/user_math.c

@@ -3,5 +3,100 @@
 */
 
 #include "user_math.h"
-#include <stdint.h>
 
+#include <string.h>
+
+inline float InvSqrt(float x) {
+//#if 0
+  /* Fast inverse square-root */
+  /* See: http://en.wikipedia.org/wiki/Fast_inverse_square_root */
+	float halfx = 0.5f * x;
+	float y = x;
+	long i = *(long*)&y;
+	i = 0x5f3759df - (i>>1);
+	y = *(float*)&i;
+	y = y * (1.5f - (halfx * y * y));
+	y = y * (1.5f - (halfx * y * y));
+	return y;	
+//#else
+//  return 1.0f / sqrtf(x);
+//#endif
+}
+
+inline float AbsClip(float in, float limit) {
+  return (in < -limit) ? -limit : ((in > limit) ? limit : in);
+}
+
+float fAbs(float in){
+  return (in > 0) ? in : -in;
+}
+
+inline void Clip(float *origin, float min, float max) {
+  if (*origin > max) *origin = max;
+  if (*origin < min) *origin = min;
+}
+
+inline float Sign(float in) { return (in > 0) ? 1.0f : 0.0f; }
+
+/**
+ * \brief 计算循环值的误差，用于没有负数值，并在一定范围内变化的值
+ * 例如编码器：相差1.5PI其实等于相差-0.5PI
+ *
+ * \param sp 被操作的值
+ * \param fb 变化量
+ * \param range 被操作的值变化范围，正数时起效
+ *
+ * \return 函数运行结果
+ */
+inline float CircleError(float sp, float fb, float range) {
+  float error = sp - fb;
+  if (range > 0.0f) {
+    float half_range = range / 2.0f;
+
+    if (error > half_range)
+      error -= range;
+    else if (error < -half_range)
+      error += range;
+  }
+  return error;
+}
+
+/**
+ * \brief 循环加法，用于没有负数值，并在一定范围内变化的值
+ * 例如编码器，在0-2PI内变化，1.5PI + 1.5PI = 1PI
+ *
+ * \param origin 被操作的值
+ * \param delta 变化量
+ * \param range 被操作的值变化范围，正数时起效
+ */
+inline void CircleAdd(float *origin, float delta, float range) {
+  float out = *origin + delta;
+  if (range > 0.0f) {
+    if (out >= range)
+      out -= range;
+    else if (out < 0.0f)
+      out += range;
+  }
+  *origin = out;
+}
+
+/**
+ * @brief 循环值取反
+ *
+ * @param origin 被操作的值
+ */
+inline void CircleReverse(float *origin) { *origin = -(*origin) + M_2PI; }
+
+/**
+ * @brief 断言失败处理
+ *
+ * @param file 文件名
+ * @param line 行号
+ */
+void VerifyFailed(const char *file, uint32_t line) {
+  UNUSED(file);
+  UNUSED(line);
+  while (1) {
+    __NOP();
+  }
+}

User/component/user_math.h

@@ -4,10 +4,135 @@
 
 #pragma once
 
+#ifdef __cplusplus
+extern "C" {
+#endif
 
+#include <float.h>
 #include <math.h>
-#include <stdint.h>
+#include <stdbool.h>
+
+#define M_DEG2RAD_MULT (0.01745329251f)
+#define M_RAD2DEG_MULT (57.2957795131f)
 
 #ifndef M_PI
 #define M_PI 3.14159265358979323846f
 #endif
+
+#ifndef M_2PI
+#define M_2PI 6.28318530717958647692f
+#endif
+
+#define max(a, b)           \
+  ({                        \
+    __typeof__(a) _a = (a); \
+    __typeof__(b) _b = (b); \
+    _a > _b ? _a : _b;      \
+  })
+
+#define min(a, b)           \
+  ({                        \
+    __typeof__(a) _a = (a); \
+    __typeof__(b) _b = (b); \
+    _a < _b ? _a : _b;      \
+  })
+
+/* 移动向量 */
+typedef struct {
+  float vx; /* 前后平移 */
+  float vy; /* 左右平移 */
+  float wz; /* 转动 */
+} MoveVector_t;
+
+float InvSqrt(float x);
+
+float AbsClip(float in, float limit);
+
+float fAbs(float in);
+
+void Clip(float *origin, float min, float max);
+
+float Sign(float in);
+
+/**
+ * \brief 计算循环值的误差，用于没有负数值，并在一定范围内变化的值
+ * 例如编码器：相差1.5PI其实等于相差-0.5PI
+ *
+ * \param sp 被操作的值
+ * \param fb 变化量
+ * \param range 被操作的值变化范围，正数时起效
+ *
+ * \return 函数运行结果
+ */
+float CircleError(float sp, float fb, float range);
+
+/**
+ * \brief 循环加法，用于没有负数值，并在一定范围内变化的值
+ * 例如编码器，在0-2PI内变化，1.5PI + 1.5PI = 1PI
+ *
+ * \param origin 被操作的值
+ * \param delta 变化量
+ * \param range 被操作的值变化范围，正数时起效
+ */
+void CircleAdd(float *origin, float delta, float range);
+
+/**
+ * @brief 循环值取反
+ *
+ * @param origin 被操作的值
+ */
+void CircleReverse(float *origin);
+
+#ifdef __cplusplus
+}
+#endif
+
+#ifdef DEBUG
+
+/**
+ * @brief 如果表达式的值为假则运行处理函数
+ *
+ */
+#define ASSERT(expr)                    \
+  do {                                  \
+    if (!(expr)) {                      \
+      VerifyFailed(__FILE__, __LINE__); \
+    }                                   \
+  } while (0)
+#else
+
+/**
+ * @brief 未定DEBUG，表达式不会运行，断言被忽略
+ *
+ */
+#define ASSERT(expr) ((void)(0))
+#endif
+
+#ifdef DEBUG
+
+/**
+ * @brief 如果表达式的值为假则运行处理函数
+ *
+ */
+#define VERIFY(expr)                    \
+  do {                                  \
+    if (!(expr)) {                      \
+      VerifyFailed(__FILE__, __LINE__); \
+    }                                   \
+  } while (0)
+#else
+
+/**
+ * @brief 表达式会运行，忽略表达式结果
+ *
+ */
+#define VERIFY(expr) ((void)(expr))
+#endif
+
+/**
+ * @brief 断言失败处理
+ *
+ * @param file 文件名
+ * @param line 行号
+ */
+void VerifyFailed(const char *file, uint32_t line);