biquad.comp

来自「CNC 的开放码,EMC2 V2.2.8版」· COMP 代码 · 共 174 行

/******************************************************************************
 *
 * Copyright (C) 2007 Peter G. Vavaroutsos <pete AT vavaroutsos DOT com>
 *
 * $RCSfile: biquad.comp,v $
 * $Author: petev $
 * $Locker:  $
 * $Revision: 1.4 $
 * $State: Exp $
 * $Date: 2007/06/10 04:16:58 $
 *
 * This module implements a biquad IIR filter.
 *
 ******************************************************************************
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of version 2 of the GNU General
 * Public License as published by the Free Software Foundation.
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111 USA
 *
 * THE AUTHORS OF THIS LIBRARY ACCEPT ABSOLUTELY NO LIABILITY FOR
 * ANY HARM OR LOSS RESULTING FROM ITS USE.  IT IS _EXTREMELY_ UNWISE
 * TO RELY ON SOFTWARE ALONE FOR SAFETY.  Any machinery capable of
 * harming persons must have provisions for completely removing power
 * from all motors, etc, before persons enter any danger area.  All
 * machinery must be designed to comply with local and national safety
 * codes, and the authors of this software can not, and do not, take
 * any responsibility for such compliance.
 *
 * This code was written as part of the EMC HAL project.  For more
 * information, go to www.linuxcnc.org.
 *
 ******************************************************************************/

component biquad "Biquad IIR filter";

description """Biquad IIR filter. Implements the following transfer function:
H(z) = (n0 + n1z-1 + n2z-2) / (1+ d1z-1 + d2z-2)""";

pin in float in "Filter input.";
pin out float out "Filter output.";
pin in bit enable = 0 "Filter enable. When false, the in is passed to out \
without any filtering. A transition from false to true causes filter \
coefficients to be calculated according to parameters";
pin out bit valid = 0 "When false, indicates an error occured when caclulating \
filter coefficients.";

param rw u32 type_ = 0 "Filter type determines the type of filter \
coefficients calculated. When 0, coefficients must be loaded directly. When 1, \
a low pass filter is created. When 2, a notch filter is created.";
param rw float f0 = 250.0 "The corner frequency of the filter.";
param rw float Q = 0.7071 "The Q of the filter.";

param rw float d1 = 0.0 "1st-delayed denominator coef";
param rw float d2 = 0.0 "2nd-delayed denominator coef";
param rw float n0 = 1.0 "non-delayed numerator coef";
param rw float n1 = 0.0 "1st-delayed numerator coef";
param rw float n2 = 0.0 "2nd-delayed numerator coef";
param rw float s1 = 0.0;
param rw float s2 = 0.0;

option data Internal;
option extra_setup;

function _;

license "GPL";
;;


#include <float.h>
#include <rtapi_math.h>


#define PI                      3.141592653589


void                            Biquad_CalcCoeffs(unsigned long period);


typedef enum {
    TYPE_DIRECT,
    TYPE_LOW_PASS,
    TYPE_NOTCH,
} Type;


typedef struct {
    hal_bit_t                   lastEnable;
} Internal;


EXTRA_SETUP()
{
    data.lastEnable = 0;

    return(0);
}


FUNCTION(_)
{
    if(data.lastEnable != enable){
        data.lastEnable = enable;

        if(enable)
            Biquad_CalcCoeffs(period);
    }

    if(!enable || !valid){
        out = in;
    }else{
        // Transposed direct form II.
        out = in * n0 + s1;
        s1 = in * n1 - out * d1 + s2;
        s2 = in * n2 - out * d2;
    }
}


void
Biquad_CalcCoeffs(unsigned long period)
{
    double                      sampleRate, w0, alpha;
    double                      b0, b1, b2, a0, a1, a2;

    // If not direct coefficient loading.
    if(type_ != TYPE_DIRECT){
        valid = 0;

        sampleRate = 1.0 / (period * 1e-9);

        if((f0 > sampleRate / 2.0) || (Q > 2.0) || (Q < 0.5))
            return;

        w0 = 2.0 * PI * f0 / sampleRate;
        alpha = sin(w0) / (2.0 * Q);

        if(type_ == TYPE_LOW_PASS){
            b0 = (1.0 - cos(w0)) / 2.0;
            b1 = 1.0 - cos(w0);
            b2 = (1.0 - cos(w0)) / 2.0;
            a0 = 1.0 + alpha;
            a1 = -2.0 * cos(w0);
            a2 = 1.0 - alpha;
        }else if(type_ == TYPE_NOTCH){
            b0 = 1.0;
            b1 = -2.0 * cos(w0);
            b2 = 1.0;
            a0 = 1.0 + alpha;
            a1 = -2.0 * cos(w0);
            a2 = 1.0 - alpha;
        }else{
            return;
        }

        n0 = b0 / a0;
        n1 = b1 / a0;
        n2 = b2 / a0;
        d1 = a1 / a0;
        d2 = a2 / a0;
        s1 = s2 = 0.0;
    }

    valid = 1;
}