Mercurial > hg > beaglert

annotate projects/d-box/Biquad.cpp @ 39:638bc1ae2500 staging

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Improved readibility of the DIGITAL code in the PRU, using register names instead of aliases and expanding some of the macros, removing unused macros. Binaries were not modified
author Giulio Moro <giuliomoro@yahoo.it>
date Wed, 13 May 2015 12:18:10 +0100
parents 8a575ba3ab52
children
rev   line source
andrewm@0 1 //
andrewm@0 2 // Biquad.cpp
andrewm@0 3 //
andrewm@0 4 // Created by Nigel Redmon on 11/24/12
andrewm@0 5 // EarLevel Engineering: earlevel.com
andrewm@0 6 // Copyright 2012 Nigel Redmon
andrewm@0 7 //
andrewm@0 8 // For a complete explanation of the Biquad code:
andrewm@0 9 // http://www.earlevel.com/main/2012/11/26/biquad-c-source-code/
andrewm@0 10 //
andrewm@0 11 // License:
andrewm@0 12 //
andrewm@0 13 // This source code is provided as is, without warranty.
andrewm@0 14 // You may copy and distribute verbatim copies of this document.
andrewm@0 15 // You may modify and use this source code to create binary code
andrewm@0 16 // for your own purposes, free or commercial.
andrewm@0 17 //
andrewm@0 18
andrewm@0 19 #include <math.h>
andrewm@0 20 #include "Biquad.h"
andrewm@0 21 #include <iostream>
andrewm@0 22
andrewm@0 23 Biquad::Biquad() {
andrewm@0 24 type = bq_type_lowpass;
andrewm@0 25 a0 = 1.0;
andrewm@0 26 a1 = a2 = b1 = b2 = 0.0;
andrewm@0 27 Fc = 0.50;
andrewm@0 28 Q = 0.707;
andrewm@0 29 peakGain = 0.0;
andrewm@0 30 z1 = z2 = 0.0;
andrewm@0 31 }
andrewm@0 32
andrewm@0 33 Biquad::Biquad(int type, double Fc, double Q, double peakGainDB) {
andrewm@0 34 setBiquad(type, Fc, Q, peakGainDB);
andrewm@0 35 z1 = z2 = 0.0;
andrewm@0 36 }
andrewm@0 37
andrewm@0 38 Biquad::~Biquad() {
andrewm@0 39 }
andrewm@0 40
andrewm@0 41 void Biquad::setType(int type) {
andrewm@0 42 this->type = type;
andrewm@0 43 calcBiquad();
andrewm@0 44 }
andrewm@0 45
andrewm@0 46 void Biquad::setQ(double Q) {
andrewm@0 47 this->Q = Q;
andrewm@0 48 calcBiquad();
andrewm@0 49 }
andrewm@0 50
andrewm@0 51 void Biquad::setFc(double Fc) {
andrewm@0 52 this->Fc = Fc;
andrewm@0 53 calcBiquad();
andrewm@0 54 }
andrewm@0 55
andrewm@0 56 void Biquad::setPeakGain(double peakGainDB) {
andrewm@0 57 this->peakGain = peakGainDB;
andrewm@0 58 calcBiquad();
andrewm@0 59 }
andrewm@0 60
andrewm@0 61 void Biquad::setBiquad(int type, double Fc, double Q, double peakGainDB) {
andrewm@0 62 this->type = type;
andrewm@0 63 this->Q = Q;
andrewm@0 64 this->Fc = Fc;
andrewm@0 65 startFc = Fc;
andrewm@0 66 startQ = Q;
andrewm@0 67 startPeakGain = peakGainDB;
andrewm@0 68 setPeakGain(peakGainDB);
andrewm@0 69 }
andrewm@0 70
andrewm@0 71 void Biquad::calcBiquad(void) {
andrewm@0 72 double norm;
andrewm@0 73 double V = pow(10, fabs(peakGain) / 20.0);
andrewm@0 74 double K = tan(M_PI * Fc);
andrewm@0 75 switch (this->type) {
andrewm@0 76 case bq_type_lowpass:
andrewm@0 77 norm = 1 / (1 + K / Q + K * K);
andrewm@0 78 a0 = K * K * norm;
andrewm@0 79 a1 = 2 * a0;
andrewm@0 80 a2 = a0;
andrewm@0 81 b1 = 2 * (K * K - 1) * norm;
andrewm@0 82 b2 = (1 - K / Q + K * K) * norm;
andrewm@0 83 break;
andrewm@0 84
andrewm@0 85 case bq_type_highpass:
andrewm@0 86 norm = 1 / (1 + K / Q + K * K);
andrewm@0 87 a0 = 1 * norm;
andrewm@0 88 a1 = -2 * a0;
andrewm@0 89 a2 = a0;
andrewm@0 90 b1 = 2 * (K * K - 1) * norm;
andrewm@0 91 b2 = (1 - K / Q + K * K) * norm;
andrewm@0 92 break;
andrewm@0 93
andrewm@0 94 case bq_type_bandpass:
andrewm@0 95 norm = 1 / (1 + K / Q + K * K);
andrewm@0 96 a0 = K / Q * norm;
andrewm@0 97 a1 = 0;
andrewm@0 98 a2 = -a0;
andrewm@0 99 b1 = 2 * (K * K - 1) * norm;
andrewm@0 100 b2 = (1 - K / Q + K * K) * norm;
andrewm@0 101 break;
andrewm@0 102
andrewm@0 103 case bq_type_notch:
andrewm@0 104 norm = 1 / (1 + K / Q + K * K);
andrewm@0 105 a0 = (1 + K * K) * norm;
andrewm@0 106 a1 = 2 * (K * K - 1) * norm;
andrewm@0 107 a2 = a0;
andrewm@0 108 b1 = a1;
andrewm@0 109 b2 = (1 - K / Q + K * K) * norm;
andrewm@0 110 break;
andrewm@0 111
andrewm@0 112 case bq_type_peak:
andrewm@0 113 if (peakGain >= 0) { // boost
andrewm@0 114 norm = 1 / (1 + 1/Q * K + K * K);
andrewm@0 115 a0 = (1 + V/Q * K + K * K) * norm;
andrewm@0 116 a1 = 2 * (K * K - 1) * norm;
andrewm@0 117 a2 = (1 - V/Q * K + K * K) * norm;
andrewm@0 118 b1 = a1;
andrewm@0 119 b2 = (1 - 1/Q * K + K * K) * norm;
andrewm@0 120 }
andrewm@0 121 else { // cut
andrewm@0 122 norm = 1 / (1 + V/Q * K + K * K);
andrewm@0 123 a0 = (1 + 1/Q * K + K * K) * norm;
andrewm@0 124 a1 = 2 * (K * K - 1) * norm;
andrewm@0 125 a2 = (1 - 1/Q * K + K * K) * norm;
andrewm@0 126 b1 = a1;
andrewm@0 127 b2 = (1 - V/Q * K + K * K) * norm;
andrewm@0 128 }
andrewm@0 129 break;
andrewm@0 130 case bq_type_lowshelf:
andrewm@0 131 if (peakGain >= 0) { // boost
andrewm@0 132 norm = 1 / (1 + sqrt(2) * K + K * K);
andrewm@0 133 a0 = (1 + sqrt(2*V) * K + V * K * K) * norm;
andrewm@0 134 a1 = 2 * (V * K * K - 1) * norm;
andrewm@0 135 a2 = (1 - sqrt(2*V) * K + V * K * K) * norm;
andrewm@0 136 b1 = 2 * (K * K - 1) * norm;
andrewm@0 137 b2 = (1 - sqrt(2) * K + K * K) * norm;
andrewm@0 138 }
andrewm@0 139 else { // cut
andrewm@0 140 norm = 1 / (1 + sqrt(2*V) * K + V * K * K);
andrewm@0 141 a0 = (1 + sqrt(2) * K + K * K) * norm;
andrewm@0 142 a1 = 2 * (K * K - 1) * norm;
andrewm@0 143 a2 = (1 - sqrt(2) * K + K * K) * norm;
andrewm@0 144 b1 = 2 * (V * K * K - 1) * norm;
andrewm@0 145 b2 = (1 - sqrt(2*V) * K + V * K * K) * norm;
andrewm@0 146 }
andrewm@0 147 break;
andrewm@0 148 case bq_type_highshelf:
andrewm@0 149 if (peakGain >= 0) { // boost
andrewm@0 150 norm = 1 / (1 + sqrt(2) * K + K * K);
andrewm@0 151 a0 = (V + sqrt(2*V) * K + K * K) * norm;
andrewm@0 152 a1 = 2 * (K * K - V) * norm;
andrewm@0 153 a2 = (V - sqrt(2*V) * K + K * K) * norm;
andrewm@0 154 b1 = 2 * (K * K - 1) * norm;
andrewm@0 155 b2 = (1 - sqrt(2) * K + K * K) * norm;
andrewm@0 156 }
andrewm@0 157 else { // cut
andrewm@0 158 norm = 1 / (V + sqrt(2*V) * K + K * K);
andrewm@0 159 a0 = (1 + sqrt(2) * K + K * K) * norm;
andrewm@0 160 a1 = 2 * (K * K - 1) * norm;
andrewm@0 161 a2 = (1 - sqrt(2) * K + K * K) * norm;
andrewm@0 162 b1 = 2 * (K * K - V) * norm;
andrewm@0 163 b2 = (V - sqrt(2*V) * K + K * K) * norm;
andrewm@0 164 }
andrewm@0 165 break;
andrewm@0 166 }
andrewm@0 167
andrewm@0 168 return;
andrewm@0 169 }