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.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
biomurph 0:675506e540be 19 #include <math.h>
biomurph 0:675506e540be 20 #include "Biquad.h"
biomurph 0:675506e540be 21
biomurph 0:675506e540be 22 Biquad::Biquad() {
biomurph 0:675506e540be 23 type = bq_type_lowpass;
biomurph 0:675506e540be 24 a0 = 1.0;
biomurph 0:675506e540be 25 a1 = a2 = b1 = b2 = 0.0;
biomurph 0:675506e540be 26 Fc = 0.50;
biomurph 0:675506e540be 27 Q = 0.707;
biomurph 0:675506e540be 28 peakGain = 0.0;
biomurph 0:675506e540be 29 z1 = z2 = 0.0;
biomurph 0:675506e540be 30 }
biomurph 0:675506e540be 31
biomurph 0:675506e540be 32 Biquad::Biquad(int type, double Fc, double Q, double peakGainDB) {
biomurph 0:675506e540be 33 setBiquad(type, Fc, Q, peakGainDB);
biomurph 0:675506e540be 34 z1 = z2 = 0.0;
biomurph 0:675506e540be 35 }
biomurph 0:675506e540be 36
biomurph 0:675506e540be 37 Biquad::~Biquad() {
biomurph 0:675506e540be 38 }
biomurph 0:675506e540be 39
biomurph 0:675506e540be 40 void Biquad::setType(int type) {
biomurph 0:675506e540be 41 this->type = type;
biomurph 0:675506e540be 42 calcBiquad();
biomurph 0:675506e540be 43 }
biomurph 0:675506e540be 44
biomurph 0:675506e540be 45 void Biquad::setQ(double Q) {
biomurph 0:675506e540be 46 this->Q = Q;
biomurph 0:675506e540be 47 calcBiquad();
biomurph 0:675506e540be 48 }
biomurph 0:675506e540be 49
biomurph 0:675506e540be 50 void Biquad::setFc(double Fc) {
biomurph 0:675506e540be 51 this->Fc = Fc;
biomurph 0:675506e540be 52 calcBiquad();
biomurph 0:675506e540be 53 }
biomurph 0:675506e540be 54
biomurph 0:675506e540be 55 void Biquad::setPeakGain(double peakGainDB) {
biomurph 0:675506e540be 56 this->peakGain = peakGainDB;
biomurph 0:675506e540be 57 calcBiquad();
biomurph 0:675506e540be 58 }
biomurph 0:675506e540be 59
biomurph 0:675506e540be 60 void Biquad::setBiquad(int type, double Fc, double Q, double peakGainDB) {
biomurph 0:675506e540be 61 this->type = type;
biomurph 0:675506e540be 62 this->Q = Q;
biomurph 0:675506e540be 63 this->Fc = Fc;
biomurph 0:675506e540be 64 setPeakGain(peakGainDB);
biomurph 0:675506e540be 65 }
biomurph 0:675506e540be 66
biomurph 0:675506e540be 67 void Biquad::calcBiquad(void) {
biomurph 0:675506e540be 68 double norm;
biomurph 0:675506e540be 69 double V = pow(10, fabs(peakGain) / 20.0);
biomurph 0:675506e540be 70 double K = tan(M_PI * Fc);
biomurph 0:675506e540be 71 switch (this->type) {
biomurph 0:675506e540be 72 case bq_type_lowpass:
biomurph 0:675506e540be 73 norm = 1 / (1 + K / Q + K * K);
biomurph 0:675506e540be 74 a0 = K * K * norm;
biomurph 0:675506e540be 75 a1 = 2 * a0;
biomurph 0:675506e540be 76 a2 = a0;
biomurph 0:675506e540be 77 b1 = 2 * (K * K - 1) * norm;
biomurph 0:675506e540be 78 b2 = (1 - K / Q + K * K) * norm;
biomurph 0:675506e540be 79 break;
biomurph 0:675506e540be 80
biomurph 0:675506e540be 81 case bq_type_highpass:
biomurph 0:675506e540be 82 norm = 1 / (1 + K / Q + K * K);
biomurph 0:675506e540be 83 a0 = 1 * norm;
biomurph 0:675506e540be 84 a1 = -2 * a0;
biomurph 0:675506e540be 85 a2 = a0;
biomurph 0:675506e540be 86 b1 = 2 * (K * K - 1) * norm;
biomurph 0:675506e540be 87 b2 = (1 - K / Q + K * K) * norm;
biomurph 0:675506e540be 88 break;
biomurph 0:675506e540be 89
biomurph 0:675506e540be 90 case bq_type_bandpass:
biomurph 0:675506e540be 91 norm = 1 / (1 + K / Q + K * K);
biomurph 0:675506e540be 92 a0 = K / Q * norm;
biomurph 0:675506e540be 93 a1 = 0;
biomurph 0:675506e540be 94 a2 = -a0;
biomurph 0:675506e540be 95 b1 = 2 * (K * K - 1) * norm;
biomurph 0:675506e540be 96 b2 = (1 - K / Q + K * K) * norm;
biomurph 0:675506e540be 97 break;
biomurph 0:675506e540be 98
biomurph 0:675506e540be 99 case bq_type_notch:
biomurph 0:675506e540be 100 norm = 1 / (1 + K / Q + K * K);
biomurph 0:675506e540be 101 a0 = (1 + K * K) * norm;
biomurph 0:675506e540be 102 a1 = 2 * (K * K - 1) * norm;
biomurph 0:675506e540be 103 a2 = a0;
biomurph 0:675506e540be 104 b1 = a1;
biomurph 0:675506e540be 105 b2 = (1 - K / Q + K * K) * norm;
biomurph 0:675506e540be 106 break;
biomurph 0:675506e540be 107
biomurph 0:675506e540be 108 case bq_type_peak:
biomurph 0:675506e540be 109 if (peakGain >= 0) { // boost
biomurph 0:675506e540be 110 norm = 1 / (1 + 1/Q * K + K * K);
biomurph 0:675506e540be 111 a0 = (1 + V/Q * K + K * K) * norm;
biomurph 0:675506e540be 112 a1 = 2 * (K * K - 1) * norm;
biomurph 0:675506e540be 113 a2 = (1 - V/Q * K + K * K) * norm;
biomurph 0:675506e540be 114 b1 = a1;
biomurph 0:675506e540be 115 b2 = (1 - 1/Q * K + K * K) * norm;
biomurph 0:675506e540be 116 }
biomurph 0:675506e540be 117 else { // cut
biomurph 0:675506e540be 118 norm = 1 / (1 + V/Q * K + K * K);
biomurph 0:675506e540be 119 a0 = (1 + 1/Q * K + K * K) * norm;
biomurph 0:675506e540be 120 a1 = 2 * (K * K - 1) * norm;
biomurph 0:675506e540be 121 a2 = (1 - 1/Q * K + K * K) * norm;
biomurph 0:675506e540be 122 b1 = a1;
biomurph 0:675506e540be 123 b2 = (1 - V/Q * K + K * K) * norm;
biomurph 0:675506e540be 124 }
biomurph 0:675506e540be 125 break;
biomurph 0:675506e540be 126 case bq_type_lowshelf:
biomurph 0:675506e540be 127 if (peakGain >= 0) { // boost
biomurph 0:675506e540be 128 norm = 1 / (1 + sqrt(2) * K + K * K);
biomurph 0:675506e540be 129 a0 = (1 + sqrt(2*V) * K + V * K * K) * norm;
biomurph 0:675506e540be 130 a1 = 2 * (V * K * K - 1) * norm;
biomurph 0:675506e540be 131 a2 = (1 - sqrt(2*V) * K + V * K * K) * norm;
biomurph 0:675506e540be 132 b1 = 2 * (K * K - 1) * norm;
biomurph 0:675506e540be 133 b2 = (1 - sqrt(2) * K + K * K) * norm;
biomurph 0:675506e540be 134 }
biomurph 0:675506e540be 135 else { // cut
biomurph 0:675506e540be 136 norm = 1 / (1 + sqrt(2*V) * K + V * K * K);
biomurph 0:675506e540be 137 a0 = (1 + sqrt(2) * K + K * K) * norm;
biomurph 0:675506e540be 138 a1 = 2 * (K * K - 1) * norm;
biomurph 0:675506e540be 139 a2 = (1 - sqrt(2) * K + K * K) * norm;
biomurph 0:675506e540be 140 b1 = 2 * (V * K * K - 1) * norm;
biomurph 0:675506e540be 141 b2 = (1 - sqrt(2*V) * K + V * K * K) * norm;
biomurph 0:675506e540be 142 }
biomurph 0:675506e540be 143 break;
biomurph 0:675506e540be 144 case bq_type_highshelf:
biomurph 0:675506e540be 145 if (peakGain >= 0) { // boost
biomurph 0:675506e540be 146 norm = 1 / (1 + sqrt(2) * K + K * K);
biomurph 0:675506e540be 147 a0 = (V + sqrt(2*V) * K + K * K) * norm;
biomurph 0:675506e540be 148 a1 = 2 * (K * K - V) * norm;
biomurph 0:675506e540be 149 a2 = (V - sqrt(2*V) * K + K * K) * norm;
biomurph 0:675506e540be 150 b1 = 2 * (K * K - 1) * norm;
biomurph 0:675506e540be 151 b2 = (1 - sqrt(2) * K + K * K) * norm;
biomurph 0:675506e540be 152 }
biomurph 0:675506e540be 153 else { // cut
biomurph 0:675506e540be 154 norm = 1 / (V + sqrt(2*V) * K + K * K);
biomurph 0:675506e540be 155 a0 = (1 + sqrt(2) * K + K * K) * norm;
biomurph 0:675506e540be 156 a1 = 2 * (K * K - 1) * norm;
biomurph 0:675506e540be 157 a2 = (1 - sqrt(2) * K + K * K) * norm;
biomurph 0:675506e540be 158 b1 = 2 * (K * K - V) * norm;
biomurph 0:675506e540be 159 b2 = (V - sqrt(2*V) * K + K * K) * norm;
biomurph 0:675506e540be 160 }
biomurph 0:675506e540be 161 break;
biomurph 0:675506e540be 162 }
biomurph 0:675506e540be 163
biomurph 0:675506e540be 164 return;
biomurph 0:675506e540be 165 }