Joel Murphy / Mbed 2 deprecated ADS_StreamRawData

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Biquad_multiChan.cpp@0:675506e540be, 2015-03-23 (annotated)

Committer:
biomurph
Date:
Mon Mar 23 19:22:04 2015 +0000
Revision:
0:675506e540be
Publishing this old ADS1299 code for the first time!

Who changed what in which revision?

UserRevisionLine numberNew contents of line
biomurph 0:675506e540be 1 //
biomurph 0:675506e540be 2 // Biquad.cpp
biomurph 0:675506e540be 3 //
biomurph 0:675506e540be 4 // Created by Nigel Redmon on 11/24/12
biomurph 0:675506e540be 5 // EarLevel Engineering: earlevel.com
biomurph 0:675506e540be 6 // Copyright 2012 Nigel Redmon
biomurph 0:675506e540be 7 //
biomurph 0:675506e540be 8 // For a complete explanation of the Biquad code:
biomurph 0:675506e540be 9 // http://www.earlevel.com/main/2012/11/26/biquad-c-source-code/
biomurph 0:675506e540be 10 //
biomurph 0:675506e540be 11 // License:
biomurph 0:675506e540be 12 //
biomurph 0:675506e540be 13 // This source code is provided as is, without warranty.
biomurph 0:675506e540be 14 // You may copy and distribute verbatim copies of this document.
biomurph 0:675506e540be 15 // You may modify and use this source code to create binary code
biomurph 0:675506e540be 16 // for your own purposes, free or commercial.
biomurph 0:675506e540be 17 //
biomurph 0:675506e540be 18 // Extended by Chip Audette to handle multiple channels of data
biomurph 0:675506e540be 19 // that are being filtered by the same coefficients
biomurph 0:675506e540be 20 //
biomurph 0:675506e540be 21
biomurph 0:675506e540be 22 #include <math.h>
biomurph 0:675506e540be 23 #include "Biquad_multiChan.h"
biomurph 0:675506e540be 24
biomurph 0:675506e540be 25
biomurph 0:675506e540be 26 /*
biomurph 0:675506e540be 27 Biquad_multiChan::Biquad_multiChan() {
biomurph 0:675506e540be 28 type = bq_type_lowpass;
biomurph 0:675506e540be 29 a0 = 1.0;
biomurph 0:675506e540be 30 a1 = a2 = b1 = b2 = 0.0;
biomurph 0:675506e540be 31 Fc = 0.50;
biomurph 0:675506e540be 32 Q = 0.707;
biomurph 0:675506e540be 33 peakGain = 0.0;
biomurph 0:675506e540be 34 z1 = z2 = 0.0;
biomurph 0:675506e540be 35 }
biomurph 0:675506e540be 36 */
biomurph 0:675506e540be 37
biomurph 0:675506e540be 38 Biquad_multiChan::Biquad_multiChan(int N,int type, double Fc, double Q, double peakGainDB) {
biomurph 0:675506e540be 39 setBiquad(type, Fc, Q, peakGainDB);
biomurph 0:675506e540be 40
biomurph 0:675506e540be 41 Nchan = N;
biomurph 0:675506e540be 42 z1 = new double[Nchan];
biomurph 0:675506e540be 43 z2 = new double[Nchan];
biomurph 0:675506e540be 44 for (int Ichan=0;Ichan<Nchan;Ichan++) {
biomurph 0:675506e540be 45 z1[Ichan]=0.0;
biomurph 0:675506e540be 46 z2[Ichan]=0.0;
biomurph 0:675506e540be 47 }
biomurph 0:675506e540be 48 }
biomurph 0:675506e540be 49
biomurph 0:675506e540be 50 Biquad_multiChan::~Biquad_multiChan() {
biomurph 0:675506e540be 51 delete z2;
biomurph 0:675506e540be 52 delete z1;
biomurph 0:675506e540be 53 }
biomurph 0:675506e540be 54
biomurph 0:675506e540be 55 void Biquad_multiChan::setType(int type) {
biomurph 0:675506e540be 56 this->type = type;
biomurph 0:675506e540be 57 calcBiquad();
biomurph 0:675506e540be 58 }
biomurph 0:675506e540be 59
biomurph 0:675506e540be 60 void Biquad_multiChan::setQ(double Q) {
biomurph 0:675506e540be 61 this->Q = Q;
biomurph 0:675506e540be 62 calcBiquad();
biomurph 0:675506e540be 63 }
biomurph 0:675506e540be 64
biomurph 0:675506e540be 65 void Biquad_multiChan::setFc(double Fc) {
biomurph 0:675506e540be 66 this->Fc = Fc;
biomurph 0:675506e540be 67 calcBiquad();
biomurph 0:675506e540be 68 }
biomurph 0:675506e540be 69
biomurph 0:675506e540be 70 void Biquad_multiChan::setPeakGain(double peakGainDB) {
biomurph 0:675506e540be 71 this->peakGain = peakGainDB;
biomurph 0:675506e540be 72 calcBiquad();
biomurph 0:675506e540be 73 }
biomurph 0:675506e540be 74
biomurph 0:675506e540be 75 void Biquad_multiChan::setBiquad(int type, double Fc, double Q, double peakGainDB) {
biomurph 0:675506e540be 76 this->type = type;
biomurph 0:675506e540be 77 this->Q = Q;
biomurph 0:675506e540be 78 this->Fc = Fc;
biomurph 0:675506e540be 79 setPeakGain(peakGainDB);
biomurph 0:675506e540be 80 }
biomurph 0:675506e540be 81
biomurph 0:675506e540be 82 void Biquad_multiChan::calcBiquad(void) {
biomurph 0:675506e540be 83 double norm;
biomurph 0:675506e540be 84 double V = pow(10, fabs(peakGain) / 20.0);
biomurph 0:675506e540be 85 double K = tan(M_PI * Fc);
biomurph 0:675506e540be 86 switch (this->type) {
biomurph 0:675506e540be 87 case bq_type_lowpass:
biomurph 0:675506e540be 88 norm = 1 / (1 + K / Q + K * K);
biomurph 0:675506e540be 89 a0 = K * K * norm;
biomurph 0:675506e540be 90 a1 = 2 * a0;
biomurph 0:675506e540be 91 a2 = a0;
biomurph 0:675506e540be 92 b1 = 2 * (K * K - 1) * norm;
biomurph 0:675506e540be 93 b2 = (1 - K / Q + K * K) * norm;
biomurph 0:675506e540be 94 break;
biomurph 0:675506e540be 95
biomurph 0:675506e540be 96 case bq_type_highpass:
biomurph 0:675506e540be 97 norm = 1 / (1 + K / Q + K * K);
biomurph 0:675506e540be 98 a0 = 1 * norm;
biomurph 0:675506e540be 99 a1 = -2 * a0;
biomurph 0:675506e540be 100 a2 = a0;
biomurph 0:675506e540be 101 b1 = 2 * (K * K - 1) * norm;
biomurph 0:675506e540be 102 b2 = (1 - K / Q + K * K) * norm;
biomurph 0:675506e540be 103 break;
biomurph 0:675506e540be 104
biomurph 0:675506e540be 105 case bq_type_bandpass:
biomurph 0:675506e540be 106 norm = 1.0 / (1.0 + K / Q + K * K);
biomurph 0:675506e540be 107 a0 = K / Q * norm;
biomurph 0:675506e540be 108 a1 = 0.0;
biomurph 0:675506e540be 109 a2 = -a0;
biomurph 0:675506e540be 110 b1 = 2.0 * (K * K - 1.0) * norm;
biomurph 0:675506e540be 111 b2 = (1.0 - K / Q + K * K) * norm;
biomurph 0:675506e540be 112 break;
biomurph 0:675506e540be 113
biomurph 0:675506e540be 114 case bq_type_notch:
biomurph 0:675506e540be 115 norm = 1.0 / (1.0 + K / Q + K * K);
biomurph 0:675506e540be 116 a0 = (1 + K * K) * norm;
biomurph 0:675506e540be 117 a1 = 2.0 * (K * K - 1) * norm;
biomurph 0:675506e540be 118 a2 = a0;
biomurph 0:675506e540be 119 b1 = a1;
biomurph 0:675506e540be 120 b2 = (1.0 - K / Q + K * K) * norm;
biomurph 0:675506e540be 121 break;
biomurph 0:675506e540be 122
biomurph 0:675506e540be 123 case bq_type_peak:
biomurph 0:675506e540be 124 if (peakGain >= 0) { // boost
biomurph 0:675506e540be 125 norm = 1 / (1 + 1/Q * K + K * K);
biomurph 0:675506e540be 126 a0 = (1 + V/Q * K + K * K) * norm;
biomurph 0:675506e540be 127 a1 = 2 * (K * K - 1) * norm;
biomurph 0:675506e540be 128 a2 = (1 - V/Q * K + K * K) * norm;
biomurph 0:675506e540be 129 b1 = a1;
biomurph 0:675506e540be 130 b2 = (1 - 1/Q * K + K * K) * norm;
biomurph 0:675506e540be 131 }
biomurph 0:675506e540be 132 else { // cut
biomurph 0:675506e540be 133 norm = 1 / (1 + V/Q * K + K * K);
biomurph 0:675506e540be 134 a0 = (1 + 1/Q * K + K * K) * norm;
biomurph 0:675506e540be 135 a1 = 2 * (K * K - 1) * norm;
biomurph 0:675506e540be 136 a2 = (1 - 1/Q * K + K * K) * norm;
biomurph 0:675506e540be 137 b1 = a1;
biomurph 0:675506e540be 138 b2 = (1 - V/Q * K + K * K) * norm;
biomurph 0:675506e540be 139 }
biomurph 0:675506e540be 140 break;
biomurph 0:675506e540be 141 case bq_type_lowshelf:
biomurph 0:675506e540be 142 if (peakGain >= 0) { // boost
biomurph 0:675506e540be 143 norm = 1 / (1 + sqrt(2) * K + K * K);
biomurph 0:675506e540be 144 a0 = (1 + sqrt(2*V) * K + V * K * K) * norm;
biomurph 0:675506e540be 145 a1 = 2 * (V * K * K - 1) * norm;
biomurph 0:675506e540be 146 a2 = (1 - sqrt(2*V) * K + V * K * K) * norm;
biomurph 0:675506e540be 147 b1 = 2 * (K * K - 1) * norm;
biomurph 0:675506e540be 148 b2 = (1 - sqrt(2) * K + K * K) * norm;
biomurph 0:675506e540be 149 }
biomurph 0:675506e540be 150 else { // cut
biomurph 0:675506e540be 151 norm = 1 / (1 + sqrt(2*V) * K + V * K * K);
biomurph 0:675506e540be 152 a0 = (1 + sqrt(2) * K + K * K) * norm;
biomurph 0:675506e540be 153 a1 = 2 * (K * K - 1) * norm;
biomurph 0:675506e540be 154 a2 = (1 - sqrt(2) * K + K * K) * norm;
biomurph 0:675506e540be 155 b1 = 2 * (V * K * K - 1) * norm;
biomurph 0:675506e540be 156 b2 = (1 - sqrt(2*V) * K + V * K * K) * norm;
biomurph 0:675506e540be 157 }
biomurph 0:675506e540be 158 break;
biomurph 0:675506e540be 159 case bq_type_highshelf:
biomurph 0:675506e540be 160 if (peakGain >= 0) { // boost
biomurph 0:675506e540be 161 norm = 1 / (1 + sqrt(2) * K + K * K);
biomurph 0:675506e540be 162 a0 = (V + sqrt(2*V) * K + K * K) * norm;
biomurph 0:675506e540be 163 a1 = 2 * (K * K - V) * norm;
biomurph 0:675506e540be 164 a2 = (V - sqrt(2*V) * K + K * K) * norm;
biomurph 0:675506e540be 165 b1 = 2 * (K * K - 1) * norm;
biomurph 0:675506e540be 166 b2 = (1 - sqrt(2) * K + K * K) * norm;
biomurph 0:675506e540be 167 }
biomurph 0:675506e540be 168 else { // cut
biomurph 0:675506e540be 169 norm = 1 / (V + sqrt(2*V) * K + K * K);
biomurph 0:675506e540be 170 a0 = (1 + sqrt(2) * K + K * K) * norm;
biomurph 0:675506e540be 171 a1 = 2 * (K * K - 1) * norm;
biomurph 0:675506e540be 172 a2 = (1 - sqrt(2) * K + K * K) * norm;
biomurph 0:675506e540be 173 b1 = 2 * (K * K - V) * norm;
biomurph 0:675506e540be 174 b2 = (V - sqrt(2*V) * K + K * K) * norm;
biomurph 0:675506e540be 175 }
biomurph 0:675506e540be 176 break;
biomurph 0:675506e540be 177 }
biomurph 0:675506e540be 178
biomurph 0:675506e540be 179 return;
biomurph 0:675506e540be 180 }