hpm6750/controller_yy_app/matrix/flt_butter.h

/******************************************************************************
 * Copyright 2020 The Firmament Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#ifndef BUTTER_H__
#define BUTTER_H__

#ifdef __cplusplus
extern "C"
{
#endif

/* 30Hz cut-off frequency, 1000Hz sampling frequency */
#define BUTTER3_B_1000S_30C        \
  {                                \
    0.0007, 0.0021, 0.0021, 0.0007 \
  }
#define BUTTER3_A_1000S_30C       \
  {                               \
    1.0, -2.6236, 2.3147, -0.6855 \
  }

  typedef struct
  {
    float A[4];
    float B[4];
    float X[4];
    float Y[4];
  } LpfButter3;

  typedef struct
  {
    // 历史采样数据
    float _delay_element1;
    float _delay_element2;

    // 权重系数
    float _a1;
    float _a2;
    float _b0;
    float _b1;
    float _b2;

    // 截至频率
    float _cutoff_freq;

    // 采样频率
    float _sample_freq;
  } LpfButter2;

  int butter3_filter_init(LpfButter3 *butter, const float b[4], const float a[4]);
  int butter3_filter_reset(float sample, LpfButter3 *butter);
  float butter3_filter_apply(float in, LpfButter3 *butter);

  int butter2_filter_init(LpfButter2 *butter, float sample_freq,
                          float cuttoff_freq);
  float butter2_filter_reset(LpfButter2 *lp, float sample);
  float butter2_filter_apply(LpfButter2 *lp, float sample);

#ifdef __cplusplus
}
#endif

#endif