数字信号处理滤波器

本例程演示如何使用 librm 中的 DSP 滤波器模块。这些滤波器来自 ArduPilot 项目，经过了大量实际应用的验证，适用于传感器数据滤波、控制信号平滑等场景。

librm 采用类似 C++ 标准库的方式（如 <cmath> 包装 math.h）将这些滤波器封装到了 rm::modules 命名空间中。

低通滤波器（Low Pass Filter）

低通滤波器允许低频信号通过，衰减高频噪声。适用于传感器数据去噪、平滑控制信号等场景。

一阶低通滤波器

应用场景：

• 传感器数据降噪（陀螺仪、加速度计等）
• 控制信号平滑
• 电压/电流测量滤波

LowPassFilter（采样周期可变）：

#include "cmsis_os.h"
#include <librm.hpp>

extern "C" void FilterTask(const void *pv_arg) {
  // 创建一个浮点型低通滤波器
  rm::modules::LowPassFilter<float> lpf;

  // 设置截止频率为 10Hz
  lpf.set_cutoff_frequency(10.0f);

  for (;;) {
    float raw_value = ReadSensor();  // 读取传感器原始值
    float dt = 0.001f;  // 采样周期 1ms

    // 应用滤波器，传入原始值和采样周期
    float filtered_value = lpf.apply(raw_value, dt);

    // 使用滤波后的值...

    osDelay(1);
  }
}

LowPassFilterConstDt（采样周期恒定，效率更高）：

extern "C" void FilterTask(const void *pv_arg) {
  rm::modules::LowPassFilterConstDt<float> lpf;

  // 一次性设置采样频率和截止频率
  lpf.set_cutoff_frequency(1000.0f, 10.0f);  // 采样频率 1kHz，截止频率 10Hz

  for (;;) {
    float raw_value = ReadSensor();

    // 不需要传入 dt，效率更高
    float filtered_value = lpf.apply(raw_value);

    osDelay(1);
  }
}

陀螺仪数据滤波示例：

#include "spi.h"
#include "cmsis_os.h"
#include <librm.hpp>

extern "C" void ImuFilterTask(const void *pv_arg) {
  rm::device::BMI088 imu(hspi1, CS1_ACCEL_GPIO_Port, CS1_ACCEL_Pin,
                         CS1_GYRO_GPIO_Port, CS1_GYRO_Pin);

  // 为三个轴分别创建滤波器
  rm::modules::LowPassFilterConstDt<float> gyro_x_filter;
  rm::modules::LowPassFilterConstDt<float> gyro_y_filter;
  rm::modules::LowPassFilterConstDt<float> gyro_z_filter;

  // 采样频率 1kHz，截止频率 50Hz
  gyro_x_filter.set_cutoff_frequency(1000.0f, 50.0f);
  gyro_y_filter.set_cutoff_frequency(1000.0f, 50.0f);
  gyro_z_filter.set_cutoff_frequency(1000.0f, 50.0f);

  for (;;) {
    imu.Update();

    // 滤波陀螺仪数据
    float gyro_x = gyro_x_filter.apply(imu.gyro_x());
    float gyro_y = gyro_y_filter.apply(imu.gyro_y());
    float gyro_z = gyro_z_filter.apply(imu.gyro_z());

    // 使用滤波后的数据进行姿态解算...

    osDelay(1);
  }
}

二阶低通滤波器（LowPassFilter2p）

应用场景：

• 需要更陡峭滚降特性的场合
• 高噪声环境下的传感器数据滤波
• 对相位延迟敏感的应用

二阶滤波器相比一阶滤波器有更好的衰减特性（40dB/decade vs 20dB/decade），但会引入更大的相位延迟。

extern "C" void Filter2pTask(const void *pv_arg) {
  rm::modules::LowPassFilter2p<float> lpf2p;

  // 设置采样频率和截止频率
  lpf2p.set_cutoff_frequency(1000.0f, 20.0f);  // 1kHz 采样，20Hz 截止

  for (;;) {
    float raw_value = ReadNoisySensor();
    float filtered_value = lpf2p.apply(raw_value);

    osDelay(1);
  }
}

电机电流滤波示例：

#include "can.h"
#include "cmsis_os.h"
#include <librm.hpp>

extern "C" void MotorCurrentFilterTask(const void *pv_arg) {
  rm::hal::Can can1(hcan1);
  rm::device::M3508 motor(can1, 1);
  can1.SetFilter(0, 0);
  can1.Begin();

  // 创建二阶低通滤波器，用于电流反馈滤波
  rm::modules::LowPassFilter2p<float> current_filter;
  current_filter.set_cutoff_frequency(1000.0f, 15.0f);  // 15Hz 截止频率

  for (;;) {
    // 获取电流反馈并滤波
    float raw_current = motor.current();
    float filtered_current = current_filter.apply(raw_current);

    // 使用滤波后的电流进行过载检测或其他处理...
    if (filtered_current > 10000) {
      // 过载保护
      motor.SetCurrent(0);
    }

    osDelay(1);
  }
}

陷波滤波器（Notch Filter）

陷波滤波器用于抑制特定频率的干扰信号，对其他频率影响最小。

应用场景：

• 消除电机共振频率
• 抑制机械结构固有频率的振动
• 消除电源工频干扰（50Hz/60Hz）
• 云台/机械臂的振动抑制

基本陷波滤波器

extern "C" void NotchFilterTask(const void *pv_arg) {
  // 创建陷波滤波器
  rm::modules::NotchFilter<float> notch;

  // 初始化参数：
  // - 采样频率: 1000Hz
  // - 中心频率: 50Hz (要抑制的频率)
  // - 带宽: 10Hz
  // - 衰减: 20dB
  notch.init(1000.0f, 50.0f, 10.0f, 20.0f);

  for (;;) {
    float input = ReadSignal();
    float output = notch.apply(input);

    osDelay(1);
  }
}

云台电机振动抑制示例：

#include "can.h"
#include "cmsis_os.h"
#include <librm.hpp>

extern "C" void GimbalNotchTask(const void *pv_arg) {
  rm::hal::Can can1(hcan1);
  rm::device::GM6020 pitch_motor(can1, 1);
  can1.SetFilter(0, 0);
  can1.Begin();

  // 创建 PID 控制器
  rm::modules::PID pid(8, 0, 0, 30000, 0);

  // 创建陷波滤波器，抑制云台在 35Hz 附近的共振
  rm::modules::NotchFilter<float> notch;
  notch.init(1000.0f, 35.0f, 5.0f, 25.0f);  // 抑制 35Hz，带宽 5Hz，衰减 25dB

  for (;;) {
    float target = GetTargetAngle();
    float feedback = pitch_motor.angle();

    // PID 计算
    pid.Update(target, feedback);
    float control_output = pid.out();

    // 对控制输出应用陷波滤波器，消除共振频率成分
    float filtered_output = notch.apply(control_output);

    pitch_motor.SetCurrent(static_cast<int16_t>(filtered_output));
    rm::device::DjiMotor<>::SendCommand();

    osDelay(1);
  }
}

谐波陷波滤波器（HarmonicNotchFilter）

应用场景：

• 抑制电机转速相关的振动（基频及其谐波）
• 多旋翼无人机螺旋桨振动抑制
• 云台多个共振频率的同时抑制

谐波陷波滤波器可以同时抑制基频和其多个谐波分量。

extern "C" void HarmonicNotchTask(const void *pv_arg) {
  // 创建谐波陷波滤波器
  rm::modules::HarmonicNotchFilter<float> harmonic_notch;

  // 分配滤波器组（滤波器数量、谐波、复合陷波数量）
  // 这里配置为抑制基频、2倍频、3倍频
  harmonic_notch.allocate_filters(3, 0b111, 1);  // 0b111 = 抑制 1,2,3 次谐波

  // 初始化：采样频率 1000Hz，中心频率 50Hz，带宽 10Hz，衰减 20dB
  harmonic_notch.init(1000.0f, 50.0f, 10.0f, 20.0f);

  for (;;) {
    float motor_rpm = GetMotorRPM();

    // 根据电机转速更新陷波频率
    float freq = motor_rpm / 60.0f;  // RPM 转换为 Hz
    harmonic_notch.update(freq);

    float input = ReadSignal();
    float output = harmonic_notch.apply(input);

    osDelay(1);
  }
}

平均滤波器（Average Filter）

平均滤波器对最近 N 个采样值求平均，可以有效抑制随机噪声。

应用场景：

• 模拟传感器（电位器、温度传感器等）降噪
• 电压/电流测量平滑
• 脉冲干扰抑制

滑动平均滤波

extern "C" void AverageFilterTask(const void *pv_arg) {
  // 创建一个 5 点浮点型平均滤波器
  // 参数：<数据类型, 累加类型, 滤波器大小>
  rm::modules::AverageFilter<float, float, 5> avg_filter;

  for (;;) {
    float raw_value = ReadAnalogSensor();
    float filtered = avg_filter.apply(raw_value);

    osDelay(1);
  }
}

也可以使用预定义的类型别名：

// 浮点型，5 点平均
rm::modules::AverageFilterFloat_Size5 avg_float;

// int16 型，3 点平均
rm::modules::AverageFilterInt16_Size3 avg_int16;

// uint32 型，4 点平均
rm::modules::AverageFilterUInt32_Size4 avg_uint32;

电位器读取滤波示例：

#include "adc.h"
#include "cmsis_os.h"
#include <librm.hpp>

extern "C" void PotentiometerTask(const void *pv_arg) {
  // 使用 5 点平均滤波器
  rm::modules::AverageFilterUInt16_Size5 adc_filter;

  for (;;) {
    // 读取 ADC 值
    uint16_t raw_adc = HAL_ADC_GetValue(&hadc1);

    // 应用平均滤波
    uint16_t filtered_adc = adc_filter.apply(raw_adc);

    // 转换为角度或其他物理量
    float angle = filtered_adc * 360.0f / 4096.0f;

    osDelay(10);
  }
}

导数滤波器（Derivative Filter）

导数滤波器计算信号的导数（变化率），同时抑制高频噪声。

应用场景：

• 从位置信号计算速度
• 从速度信号计算加速度
• 边沿检测

extern "C" void DerivativeFilterTask(const void *pv_arg) {
  // 创建导数滤波器，缓冲区大小为 7
  rm::modules::DerivativeFilter<float, 7> deriv_filter;

  const float dt = 0.001f;  // 采样周期 1ms

  for (;;) {
    float position = ReadPosition();

    // 计算速度（位置的导数）
    float velocity = deriv_filter.apply(position, dt);

    osDelay(1);
  }
}

从编码器位置计算速度示例：

#include "can.h"
#include "cmsis_os.h"
#include <librm.hpp>

extern "C" void VelocityEstimationTask(const void *pv_arg) {
  rm::hal::Can can1(hcan1);
  rm::device::M3508 motor(can1, 1);
  can1.SetFilter(0, 0);
  can1.Begin();

  // 使用导数滤波器从位置估计速度
  rm::modules::DerivativeFilterFloat_Size7 vel_estimator;

  const float dt = 0.001f;

  for (;;) {
    // 获取编码器角度
    float angle = motor.angle();

    // 计算角速度
    float angular_velocity = vel_estimator.apply(angle, dt);

    // 对比电机内部的速度测量
    float motor_velocity = motor.velocity();

    // angular_velocity 会更平滑，噪声更小

    osDelay(1);
  }
}

众数滤波器（Mode Filter）

众数滤波器返回最近 N 个采样值中出现次数最多的值，对脉冲干扰有很好的抑制效果。

应用场景：

• 数字传感器读数去毛刺
• 脉冲干扰抑制
• 开关信号滤波

extern "C" void ModeFilterTask(const void *pv_arg) {
  // 创建 5 点众数滤波器
  rm::modules::ModeFilter<uint16_t, 5> mode_filter;

  for (;;) {
    uint16_t raw_value = ReadDigitalSensor();
    uint16_t filtered = mode_filter.apply(raw_value);

    osDelay(1);
  }
}

按键消抖示例：

#include "gpio.h"
#include "cmsis_os.h"
#include <librm.hpp>

extern "C" void ButtonTask(const void *pv_arg) {
  // 使用众数滤波器进行按键消抖
  rm::modules::ModeFilterUInt16_Size3 button_filter;

  for (;;) {
    // 读取按键状态（0 或 1）
    uint16_t raw_state = HAL_GPIO_ReadPin(BUTTON_GPIO_Port, BUTTON_Pin);

    // 应用众数滤波
    uint16_t filtered_state = button_filter.apply(raw_state);

    if (filtered_state == 1) {
      // 按键被按下（已消抖）
      OnButtonPressed();
    }

    osDelay(10);
  }
}

转换率限制器（Slew Limiter）

转换率限制器限制信号的变化速率，防止信号变化过快导致的振荡或过冲。

应用场景：

• 防止控制器输出突变
• 电机启动加速度限制
• 防止系统振荡

extern "C" void SlewLimiterTask(const void *pv_arg) {
  // 创建转换率限制器
  float max_slew_rate = 100.0f;  // 最大变化率
  float slew_rate_tau = 0.1f;    // 时间常数

  rm::modules::SlewLimiter slew_limiter(max_slew_rate, slew_rate_tau);

  for (;;) {
    float target_value = GetTargetValue();
    float dt = 0.001f;

    // 应用转换率限制，返回 0-1 的调节系数
    float modifier = slew_limiter.modifier(target_value, dt);

    // 实际输出 = 目标值 × 调节系数
    float actual_output = target_value * modifier;

    osDelay(1);
  }
}

云台控制输出平滑示例：

#include "can.h"
#include "cmsis_os.h"
#include <librm.hpp>

extern "C" void GimbalSlewTask(const void *pv_arg) {
  rm::hal::Can can1(hcan1);
  rm::device::GM6020 yaw_motor(can1, 1);
  can1.SetFilter(0, 0);
  can1.Begin();

  rm::modules::PID pid(8, 0, 0, 30000, 0);

  // 创建转换率限制器，防止输出突变
  float max_slew_rate = 5000.0f;  // 最大变化率 5000 单位/秒
  float tau = 0.05f;
  rm::modules::SlewLimiter slew_limiter(max_slew_rate, tau);

  for (;;) {
    float target = GetTargetAngle();
    pid.Update(target, yaw_motor.angle());

    float raw_output = pid.out();
    float dt = 0.001f;

    // 限制输出变化率
    float modifier = slew_limiter.modifier(raw_output, dt);
    float smooth_output = raw_output * modifier;

    yaw_motor.SetCurrent(static_cast<int16_t>(smooth_output));
    rm::device::DjiMotor<>::SendCommand();

    osDelay(1);
  }
}

注意事项

1. 某些滤波器需要在使用前调用初始化函数（如 NotchFilter::init()）
2. 确保传入的 dt 与实际采样周期一致

API 参考

完整的 API 文档请参考头文件 librm/modules/dsp_filters.hpp 中的注释。

主要类型都已在 rm::modules 命名空间中提供了别名：

rm::modules::LowPassFilter<float> lpf;
rm::modules::NotchFilter<float> notch;
rm::modules::AverageFilterFloat_Size5 avg;
// ...