kopia lustrzana https://github.com/r-burg/rbfilter
162 wiersze
3.5 KiB
C++
162 wiersze
3.5 KiB
C++
/******************************************************************************
|
|
|
|
Discrete.cpp
|
|
|
|
Copyright (c) Roger Burghall 2014..2017
|
|
|
|
******************************************************************************/
|
|
|
|
|
|
/** \file
|
|
'Discrete.cpp' holds the code for implementing discrete, i.e. z-transform, filters.
|
|
*/
|
|
|
|
#include <cmath>
|
|
#include "math.h"
|
|
#include "Calcs.h"
|
|
#include <iostream>
|
|
#include <fstream>
|
|
#include <complex>
|
|
#include <stdlib.h>
|
|
#include "Discrete.h"
|
|
|
|
|
|
iir iir1;
|
|
|
|
|
|
/// 'iir::step(...)' implements the Direct Form 1 realisation.
|
|
double iir::step(double in)
|
|
{
|
|
int i;
|
|
|
|
for(i=2; i>0; i--) x[i] = x[i-1], y[i] = y[i-1];
|
|
x[0] = in * k;
|
|
y[0] = -a1*y[1] - a2*y[2] + b0*x[0] + b1*x[1] + b2*x[2];
|
|
|
|
return(y[0]);
|
|
}
|
|
|
|
|
|
|
|
|
|
/** Set the iir filter objects coefficients. */
|
|
void iir::setup(double kk, double aa0, double aa1, double aa2, double bb1, double bb2)
|
|
{
|
|
k = kk;
|
|
a1 = aa1, a2 = aa2;
|
|
b1 = bb1, b2 = bb2;
|
|
}
|
|
|
|
|
|
|
|
void iir::write(int stage)
|
|
{
|
|
char str[256];
|
|
std::ofstream out;
|
|
if(stage == 0) {
|
|
out.open("./out.c", std::ofstream::trunc);
|
|
/// Write initial code to 'out.c'
|
|
}
|
|
else out.open("./out.c", std::ofstream::app);
|
|
sprintf(str, "y%1d = -a%1d1*y%1d[1] - a%1d2*y%1d[2] + b%1d0*x%1d[0] + b%1d1*x%1d[1] + b%1d2*x%1d[2];\n",
|
|
stage, stage, stage, stage, stage, stage, stage, stage, stage, stage, stage);
|
|
out << str;
|
|
if(out.is_open( )) { out.close( ); }
|
|
}
|
|
|
|
|
|
|
|
/** Pre-warp the continuous filter. */
|
|
void prewarp(void)
|
|
{
|
|
/// Not yet designed.
|
|
}
|
|
|
|
|
|
|
|
/** Transform the continuous filter into a discrete one. */
|
|
void iir::transform(filter_class fclass, double Omega_ac, double q)
|
|
{
|
|
double ro2 = 1.0 / (Omega_ac * Omega_ac);
|
|
double roq = 1.0 / (Omega_ac * q);
|
|
b0 = 1.0;
|
|
switch(fclass) {
|
|
case Lowpass:
|
|
b1 = 2.0;
|
|
b2 = 1.0;
|
|
break;
|
|
case Highpass:
|
|
b0 = ro2;
|
|
b1 = -2.0 * ro2;
|
|
b2 = ro2;
|
|
break;
|
|
case Bandpass:
|
|
b0 = 1.0;
|
|
b1 = 0.0;
|
|
b2 = -1.0;
|
|
break;
|
|
default: cout << "\nUnknown filter type.\n"; return;
|
|
}
|
|
a0 = ro2 + roq + 1.0;
|
|
std::cout << "a0 =" << a0 << "\n";
|
|
// k = 1.0 / a0;
|
|
a1 = 2.0 - 2.0 * ro2;
|
|
a2 = ro2 - roq + 1.0;
|
|
|
|
a1 /= a0;
|
|
a2 /= a0;
|
|
b0 /= a0;
|
|
b1 /= a0;
|
|
b2 /= a0;
|
|
std::cout << "(" << b0 << "+(z^-1)*" << b1 << "+(z^-2)*" << b2 << ")/(" << 1.0 << "+(z^-1)*" << a1 << "+(z^-2)*" << a2 << ")\n";
|
|
std::cout << "-->" << (b0 + b1 + b2) / (1.0 + a1 + a2) << "\n";
|
|
|
|
std::cout << "[a0 = 1.0, a1 =" << a1 << ", a2 =" << a2 << "; b0 =" << b0 << ", b1 =" << b1 << ", b2 =" << b2 << "]\n";
|
|
|
|
/* std::cout << "Direct Form 2 realisation.\n--------------------------\n";
|
|
std::cout << "y[n] = b0 * w[n] + b1 * w[n-1] + b2 * w[n-2], w[n] = x[n] - a1 * w[n-1] - a2 * w[n-2]";
|
|
*/
|
|
}
|
|
|
|
|
|
|
|
/** 'normalise(...)' sets 'omega' */
|
|
double normalise(double fc, double fs)
|
|
{
|
|
double omega = 2.0 * M_PI * fc / fs;
|
|
return(omega);
|
|
}
|
|
|
|
|
|
#if false
|
|
int main( )
|
|
{
|
|
int i;
|
|
double fc, fs, omega;
|
|
std::cout << "cut-off freq: ";
|
|
std::cin >> fc;
|
|
std::cout << "sampling freq: ";
|
|
std::cin >> fs;
|
|
if(fc >= fs/2.0) {
|
|
std::cout << "That doesn't make sense! Cut-off must be less than sampling freq / 2.\n";
|
|
exit(-1);
|
|
}
|
|
omega = normalise(fc, fs);
|
|
std::cout << "omega =" << omega << "\n";
|
|
double omega_ac = tan(omega/2.0);
|
|
std::cout << "omega_ac =" << omega_ac << "\n\n";
|
|
|
|
iir1.transform(omega_ac, 0.7);
|
|
// iir1.setup(1.384, 0.1311136, 0.2162924, 0.1311136, -0.829328, 0.307046);
|
|
iir1.step(0.0);
|
|
std::cout << "\n";
|
|
std::cout << "Direct Form 1 realisation.\n--------------------------\n";
|
|
for(i=0; i<200; i++) std::cout << iir1.step(1.0) << "\n";
|
|
return(0);
|
|
}
|
|
#endif
|
|
|
|
|
|
|
|
|