Jakub Rusz / Mbed 2 deprecated Rizeni_ruky_K64F

Zdrojový kód k projektu Řízeni protetické ruky pomocí K64F do předmětu MPOA 2015

Dependencies:   mbed

main.cpp@2:d59911284528, 2016-01-17 (annotated)

Committer:
customer10123
Date:
Sun Jan 17 20:51:22 2016 +0000
Revision:
2:d59911284528
Parent:
0:5007c2a9b9d1 
Update 2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
customer10123 0:5007c2a9b9d1 1 #include "mbed.h"
customer10123 0:5007c2a9b9d1 2 #include "math.h"
customer10123 0:5007c2a9b9d1 3 #include "filter1.h"
customer10123 0:5007c2a9b9d1 4
customer10123 0:5007c2a9b9d1 5 #include <stdlib.h>
customer10123 0:5007c2a9b9d1 6 #include <string.h>
customer10123 0:5007c2a9b9d1 7
customer10123 0:5007c2a9b9d1 8 DigitalOut led(LED_RED); //deklarace led
customer10123 0:5007c2a9b9d1 9 DigitalOut led2(LED_GREEN);
customer10123 0:5007c2a9b9d1 10 DigitalOut led3(LED_BLUE);
customer10123 0:5007c2a9b9d1 11
customer10123 0:5007c2a9b9d1 12 Serial pc(USBTX, USBRX); //deklarace seriove linky a analogovych vstupu
customer10123 0:5007c2a9b9d1 13 AnalogIn ain(A0);
customer10123 0:5007c2a9b9d1 14 AnalogIn otoc(A5);
customer10123 0:5007c2a9b9d1 15
customer10123 0:5007c2a9b9d1 16 const int N=8000; //pocet vzorku ucici faze (fvz je 1kHz)
customer10123 0:5007c2a9b9d1 17 const int M=100; //pocet vzorku pro kontinualni mereni
customer10123 0:5007c2a9b9d1 18
customer10123 0:5007c2a9b9d1 19 float filter1_coefficients[20] =//definice filtru s IIR
customer10123 0:5007c2a9b9d1 20 {
customer10123 0:5007c2a9b9d1 21 0.47949725793876763, -0.9589945158775353, 0.47949725793876763, 1.9426378322419233, -0.9435968961395514,// b0, b1, b2, a1, a2
customer10123 0:5007c2a9b9d1 22 1, -2, 1, 1.9752697957988141, -0.9762449562056803,// b0, b1, b2, a1, a2
customer10123 0:5007c2a9b9d1 23 1, 2, 1, -1.998839289707791, -0.9988396842695652,// b0, b1, b2, a1, a2
customer10123 0:5007c2a9b9d1 24 2, 4, 2, -1.9995188301139024, -0.9995192248043052// b0, b1, b2, a1, a2
customer10123 0:5007c2a9b9d1 25
customer10123 0:5007c2a9b9d1 26 };
customer10123 0:5007c2a9b9d1 27
customer10123 0:5007c2a9b9d1 28 const int inputBufferSize = 1; // delky IO bufferu v B + alokace
customer10123 0:5007c2a9b9d1 29 const int outputBufferSize = 1;
customer10123 0:5007c2a9b9d1 30 float inputBuffer[inputBufferSize], outputBuffer[outputBufferSize];
customer10123 0:5007c2a9b9d1 31
customer10123 0:5007c2a9b9d1 32 int main()
customer10123 0:5007c2a9b9d1 33 {
customer10123 0:5007c2a9b9d1 34 float emg4abs[N]; //absolutni hodnota emg4 pro ucici fazi 8s
customer10123 0:5007c2a9b9d1 35 float emg4[M]; //namerene data 0,1s, emg4 = biceps
customer10123 0:5007c2a9b9d1 36 float emg4obal[N-M]; //obalka emg4abs
customer10123 0:5007c2a9b9d1 37
customer10123 0:5007c2a9b9d1 38 float emg3abs[N]; //to same pro emg3, emg3 = predlokti
customer10123 0:5007c2a9b9d1 39 float emg3[M];
customer10123 0:5007c2a9b9d1 40 float emg3obal[N-M];
customer10123 0:5007c2a9b9d1 41
customer10123 0:5007c2a9b9d1 42 float Maktual[M]; //aktualni okno pro vypocet obalky
customer10123 0:5007c2a9b9d1 43 float meze1; //meze pro vyhodnoceni emg3
customer10123 0:5007c2a9b9d1 44 float meze2; //meze pro vyhodnoceni emg4
customer10123 0:5007c2a9b9d1 45 float meze1zaloha; //pomocna promenna
customer10123 0:5007c2a9b9d1 46 float emg4krit; //zdali je emg4 nad mezi
customer10123 0:5007c2a9b9d1 47
customer10123 0:5007c2a9b9d1 48 uint32_t cyklus=0; //cyklus mereni
customer10123 0:5007c2a9b9d1 49 float sum4=0; //suma pro vypocet obalky emg4
customer10123 0:5007c2a9b9d1 50 float sum3=0; //suma pro vypocet obalky emg3
customer10123 0:5007c2a9b9d1 51 float s1[N]; //abs. hodnota obalky pro vypocet meze
customer10123 0:5007c2a9b9d1 52 float maxs1[200]; //200 submaxim abs. hodnoty obalky
customer10123 0:5007c2a9b9d1 53 float obal4; //aktualni hodnota obalky emg4 behem kontinualniho mereni
customer10123 0:5007c2a9b9d1 54 float obal3; //aktualni hodnota obalky emg3 behem kontinualniho mereni
customer10123 0:5007c2a9b9d1 55 float data4[M]; //namerena data se kteryma se dale pracuje na vyhodnoceni emg4
customer10123 0:5007c2a9b9d1 56 float data3[M]; //namerena data se kteryma se dale pracuje na vyhodnoceni emg3
customer10123 0:5007c2a9b9d1 57
customer10123 0:5007c2a9b9d1 58 uint32_t procenta=0; //aktualni pozice koncetiny v zavislosti na emg3 a emg4, emg4 kodovano mod1, emg3 mod1000
customer10123 0:5007c2a9b9d1 59 float zvysenimeze=3; //nastaveni modifikace meze pro otoceni (emg3)
customer10123 0:5007c2a9b9d1 60
customer10123 0:5007c2a9b9d1 61 int compare (const void * a, const void * b); //nastaveni pro quicksort
customer10123 0:5007c2a9b9d1 62
customer10123 0:5007c2a9b9d1 63 float vzorek[1]; //promenna do ktere se uklada hodnota analog. vstupu
customer10123 0:5007c2a9b9d1 64 int nProcessedSamples; //pocet vzorku po filtraci
customer10123 0:5007c2a9b9d1 65 filter1Type *filter = filter1_create(); //deklarace filtru
customer10123 0:5007c2a9b9d1 66
customer10123 0:5007c2a9b9d1 67 //########################################################
customer10123 0:5007c2a9b9d1 68 //## ucici phase emg4 - biceps 8 vterin ... cca 4 flexy ##
customer10123 0:5007c2a9b9d1 69 //########################################################
customer10123 0:5007c2a9b9d1 70 led = 1; //zhasnuti led
customer10123 0:5007c2a9b9d1 71 led2 =1;
customer10123 0:5007c2a9b9d1 72 led3 = 1;
customer10123 0:5007c2a9b9d1 73 pc.baud(115200); //nastaveni baudrate
customer10123 0:5007c2a9b9d1 74
customer10123 0:5007c2a9b9d1 75 for(int i=0;i<(N);i++){ //ulozeni hodnoty do vzorek, filtrace a abs. hodnota
customer10123 0:5007c2a9b9d1 76 vzorek[0] = ain.read_u16();
customer10123 0:5007c2a9b9d1 77 nProcessedSamples = filter1_filterBlock( filter, vzorek, outputBuffer, inputBufferSize );
customer10123 0:5007c2a9b9d1 78 emg4abs[i]=abs(outputBuffer[0]);
customer10123 0:5007c2a9b9d1 79 wait(0.001f); //fvz = 1kHz
customer10123 0:5007c2a9b9d1 80 }
customer10123 0:5007c2a9b9d1 81 led = 0; //roznuti cervene led
customer10123 0:5007c2a9b9d1 82
customer10123 0:5007c2a9b9d1 83 for(int i=0;i<N-M;i++){ //smycka pro vypocet obalky emg4 ucici faze
customer10123 0:5007c2a9b9d1 84 for(int j=0;j<M;j++){
customer10123 0:5007c2a9b9d1 85 Maktual[j]=emg4abs[i+j]; //nacteni do okna M vzorku (default 100)
customer10123 0:5007c2a9b9d1 86 }
customer10123 0:5007c2a9b9d1 87 qsort(Maktual,M,sizeof(float),compare); //serazeni od nejmensiho vzorku v okne po nejvetsi
customer10123 0:5007c2a9b9d1 88 for(int j=(M/2);j<M;j++){
customer10123 0:5007c2a9b9d1 89 sum4=sum4+Maktual[j]; //secteni hornich defaultne 50 vzorku okna
customer10123 0:5007c2a9b9d1 90 }
customer10123 0:5007c2a9b9d1 91 emg4obal[i]=sum4/(M/2); //vydeleni poctem defaultne 50 vzorku okna
customer10123 0:5007c2a9b9d1 92 sum4=0; //tj. prumerna hodnota horni poloviny hodnot okna tvori obalku
customer10123 0:5007c2a9b9d1 93 }
customer10123 0:5007c2a9b9d1 94
customer10123 0:5007c2a9b9d1 95 for (int j=0;j<N-M;j++){
customer10123 0:5007c2a9b9d1 96 emg4obal[j]=emg4obal[j]+1; //pricteni 1 pro omezeni hodnot mensich nez 1
customer10123 0:5007c2a9b9d1 97 emg4obal[j]=emg4obal[j]*emg4obal[j]; //umocneni obalky (vsechny hodnoty se tak zvysi ptze jsou vetsi nez 1)
customer10123 0:5007c2a9b9d1 98 }
customer10123 0:5007c2a9b9d1 99 qsort(emg4obal,N-M,sizeof(float),compare); //serazeni od min po max
customer10123 0:5007c2a9b9d1 100
customer10123 0:5007c2a9b9d1 101 meze2=0; //inicializace meze pro biceps
customer10123 0:5007c2a9b9d1 102 for(int j=20;j<220;j++){ //vyber 20. az 220. maxima
customer10123 0:5007c2a9b9d1 103 maxs1[j-20]=emg4obal[(N-M)-(1*j)-1]; //a ulozeni do maxs1
customer10123 0:5007c2a9b9d1 104 meze2=meze2+(maxs1[j-20]); //suma 200 submaxim
customer10123 0:5007c2a9b9d1 105 }
customer10123 0:5007c2a9b9d1 106 meze2=meze2/200/4; //podeleni poctem maxim a snizeni na ctvrtinu = definice prahu
customer10123 0:5007c2a9b9d1 107
customer10123 0:5007c2a9b9d1 108 //######################################################
customer10123 0:5007c2a9b9d1 109 //## ucici phase emg3 - otoc 8 vterin ... cca 4 flexy ##
customer10123 0:5007c2a9b9d1 110 //######################################################
customer10123 0:5007c2a9b9d1 111 led = 0; //led signalizujici konec ucici faze pro biceps
customer10123 0:5007c2a9b9d1 112 wait(1);
customer10123 0:5007c2a9b9d1 113 led = 1; //a start ucici faze pro predlokti (led zhasne)
customer10123 0:5007c2a9b9d1 114
customer10123 0:5007c2a9b9d1 115 for(int i=0;i<(N);i++){
customer10123 0:5007c2a9b9d1 116 vzorek[0] = otoc.read_u16();
customer10123 0:5007c2a9b9d1 117 nProcessedSamples = filter1_filterBlock( filter, vzorek, outputBuffer, inputBufferSize );
customer10123 0:5007c2a9b9d1 118 emg3abs[i]=abs(outputBuffer[0]);
customer10123 0:5007c2a9b9d1 119 wait(0.001f);
customer10123 0:5007c2a9b9d1 120 }
customer10123 0:5007c2a9b9d1 121 led = 0; //konec nacitani pro prodlokti
customer10123 0:5007c2a9b9d1 122
customer10123 0:5007c2a9b9d1 123 for(int i=0;i<N-M;i++){
customer10123 0:5007c2a9b9d1 124 for(int j=0;j<M;j++){
customer10123 0:5007c2a9b9d1 125 Maktual[j]=emg3abs[i+j];
customer10123 0:5007c2a9b9d1 126 }
customer10123 0:5007c2a9b9d1 127 qsort(Maktual,M,sizeof(float),compare);
customer10123 0:5007c2a9b9d1 128 for(int j=(M/2);j<M;j++){
customer10123 0:5007c2a9b9d1 129 sum3=sum3+Maktual[j];
customer10123 0:5007c2a9b9d1 130 }
customer10123 0:5007c2a9b9d1 131 emg3obal[i]=sum3/(M/2);
customer10123 0:5007c2a9b9d1 132 sum3=0;
customer10123 0:5007c2a9b9d1 133 }
customer10123 0:5007c2a9b9d1 134
customer10123 0:5007c2a9b9d1 135 for (int j=0;j<N-M;j++){
customer10123 0:5007c2a9b9d1 136 emg3obal[j]=emg3obal[j]+1;
customer10123 0:5007c2a9b9d1 137 emg3obal[j]=emg3obal[j]*emg3obal[j];
customer10123 0:5007c2a9b9d1 138 }
customer10123 0:5007c2a9b9d1 139 qsort(emg3obal,N-M,sizeof(float),compare);
customer10123 0:5007c2a9b9d1 140 meze1=0;
customer10123 0:5007c2a9b9d1 141 for(int j=20;j<220;j++){
customer10123 0:5007c2a9b9d1 142 maxs1[j-20]=emg3obal[(N-M)-(1*j)-1];
customer10123 0:5007c2a9b9d1 143 meze1=meze1+(maxs1[j-20]);
customer10123 0:5007c2a9b9d1 144 }
customer10123 2:d59911284528 145 meze1=meze1/200/4;
customer10123 0:5007c2a9b9d1 146
customer10123 0:5007c2a9b9d1 147 led=1; //zhasnuti led indikujici start mereni
customer10123 0:5007c2a9b9d1 148 //####################################################
customer10123 0:5007c2a9b9d1 149 //## kriterium emg4 - biceps, kriterium emg3 - otoc ##
customer10123 0:5007c2a9b9d1 150 //####################################################
customer10123 0:5007c2a9b9d1 151
customer10123 0:5007c2a9b9d1 152 for(int i=0;i<M;i++){
customer10123 0:5007c2a9b9d1 153 vzorek[0]=ain.read_u16();
customer10123 0:5007c2a9b9d1 154 nProcessedSamples = filter1_filterBlock( filter, vzorek, outputBuffer, inputBufferSize );
customer10123 0:5007c2a9b9d1 155 emg4[i]=abs(outputBuffer[0]);
customer10123 0:5007c2a9b9d1 156
customer10123 0:5007c2a9b9d1 157 vzorek[0]=otoc.read_u16();
customer10123 0:5007c2a9b9d1 158 nProcessedSamples = filter1_filterBlock( filter, vzorek, outputBuffer, inputBufferSize );
customer10123 0:5007c2a9b9d1 159 emg3[i]=abs(outputBuffer[0]);
customer10123 0:5007c2a9b9d1 160 wait(0.001f);
customer10123 0:5007c2a9b9d1 161
customer10123 0:5007c2a9b9d1 162 }
customer10123 0:5007c2a9b9d1 163 for (int i=0;i<M;i++){
customer10123 0:5007c2a9b9d1 164 data4[i]=emg4[i]; //ulozeni do data4 a data3 ktere se quicksortuje
customer10123 0:5007c2a9b9d1 165 data3[i]=emg3[i];
customer10123 0:5007c2a9b9d1 166 }
customer10123 0:5007c2a9b9d1 167 while(1) //nekonecna smycka mereni
customer10123 0:5007c2a9b9d1 168 {
customer10123 0:5007c2a9b9d1 169 qsort(data4,M,sizeof(float),compare); //serazeni data4 od min po max
customer10123 0:5007c2a9b9d1 170 qsort(data3,M,sizeof(float),compare); //serazeni data3 od min po max
customer10123 0:5007c2a9b9d1 171 for(int j=(M/2);j<M;j++){
customer10123 0:5007c2a9b9d1 172 sum4=sum4+data4[j]; //secteni horni poloviny dat
customer10123 0:5007c2a9b9d1 173 sum3=sum3+data3[j];
customer10123 0:5007c2a9b9d1 174 }
customer10123 0:5007c2a9b9d1 175 obal4=sum4/(M/2); //prumer horni poloviny dat
customer10123 0:5007c2a9b9d1 176 obal4=obal4+1;
customer10123 0:5007c2a9b9d1 177 obal4=obal4*obal4; //mocninne zvyseni dat
customer10123 0:5007c2a9b9d1 178 sum4=0; //reset sumy
customer10123 0:5007c2a9b9d1 179
customer10123 0:5007c2a9b9d1 180 obal3=sum3/(M/2); //to same s predloktim
customer10123 0:5007c2a9b9d1 181 obal3=obal3+1;
customer10123 0:5007c2a9b9d1 182 obal3=obal3*obal3;
customer10123 0:5007c2a9b9d1 183 sum3=0;
customer10123 0:5007c2a9b9d1 184
customer10123 0:5007c2a9b9d1 185 if(meze2<obal4){ //jestlize bude hodnota obalky vetsi nez meze2
customer10123 0:5007c2a9b9d1 186 emg4krit=1; //kriterium pro biceps = 1 ... ruka se zveda
customer10123 0:5007c2a9b9d1 187 if(procenta<400){ //jestlize neni na max hodnote 400 pak se zvedne
customer10123 0:5007c2a9b9d1 188 procenta++;
customer10123 0:5007c2a9b9d1 189 }
customer10123 0:5007c2a9b9d1 190 led2=0; //zelena led signalizujici zvedani
customer10123 0:5007c2a9b9d1 191 }else{
customer10123 0:5007c2a9b9d1 192 emg4krit=0;
customer10123 0:5007c2a9b9d1 193 if(procenta>0){ //jestlize neni na min hodnote 0 pak klesne
customer10123 0:5007c2a9b9d1 194 procenta--;
customer10123 0:5007c2a9b9d1 195 }
customer10123 0:5007c2a9b9d1 196 led2=1; //zelena led signalizuje pokles
customer10123 0:5007c2a9b9d1 197 }
customer10123 0:5007c2a9b9d1 198
customer10123 0:5007c2a9b9d1 199 meze1zaloha=meze1; //zalohuje se puvodni meze
customer10123 0:5007c2a9b9d1 200 if(emg4krit==1){ //jestlize se ruka zveda
customer10123 0:5007c2a9b9d1 201 meze1=meze1*zvysenimeze;//a zvysi se prah (defaultne 3*)
customer10123 0:5007c2a9b9d1 202 }
customer10123 0:5007c2a9b9d1 203
customer10123 0:5007c2a9b9d1 204 if(meze1<obal3){ //jestlize bude hodnota obalky vetsi nez meze1
customer10123 0:5007c2a9b9d1 205 if(procenta<400400){ //jestlize neni na max hodnote
customer10123 0:5007c2a9b9d1 206 procenta=procenta+1000; //tak se zvysi (mod 1000)
customer10123 0:5007c2a9b9d1 207 }
customer10123 0:5007c2a9b9d1 208 led3=0; //modra led signalizujici otoceni
customer10123 0:5007c2a9b9d1 209 }else{
customer10123 0:5007c2a9b9d1 210 led3=1;
customer10123 0:5007c2a9b9d1 211 if(procenta>400){ //jestlize neni na min hodnote mod 1000 0 pak klesne
customer10123 0:5007c2a9b9d1 212 procenta=procenta-1000;
customer10123 0:5007c2a9b9d1 213 }
customer10123 0:5007c2a9b9d1 214 }
customer10123 0:5007c2a9b9d1 215 meze1=meze1zaloha; //navrat na puvodni mez
customer10123 0:5007c2a9b9d1 216
customer10123 0:5007c2a9b9d1 217 if (cyklus % 10 == 1){ //po seriove lince se posila kazda 10. hodnota
customer10123 0:5007c2a9b9d1 218 pc.printf("%d\n",procenta); //vypis procent po seriove lince
customer10123 0:5007c2a9b9d1 219 }
customer10123 0:5007c2a9b9d1 220
customer10123 0:5007c2a9b9d1 221 for (int i=0;i<M-1;i++){ //posun v okne o 1 dozadu
customer10123 0:5007c2a9b9d1 222 emg4[i]=emg4[i+1];
customer10123 0:5007c2a9b9d1 223 emg3[i]=emg3[i+1];
customer10123 0:5007c2a9b9d1 224 }
customer10123 0:5007c2a9b9d1 225
customer10123 0:5007c2a9b9d1 226 vzorek[0]=ain.read_u16(); //ulozeni vyfiltrovane hodnoty na posledni index okna
customer10123 0:5007c2a9b9d1 227 nProcessedSamples = filter1_filterBlock( filter, vzorek, outputBuffer, inputBufferSize );
customer10123 0:5007c2a9b9d1 228 emg4[M-1]=abs(outputBuffer[0]);
customer10123 0:5007c2a9b9d1 229 vzorek[0]=otoc.read_u16();
customer10123 0:5007c2a9b9d1 230 nProcessedSamples = filter1_filterBlock( filter, vzorek, outputBuffer, inputBufferSize );
customer10123 0:5007c2a9b9d1 231 emg3[M-1]=abs(outputBuffer[0]);
customer10123 0:5007c2a9b9d1 232 wait(0.001f);
customer10123 0:5007c2a9b9d1 233
customer10123 0:5007c2a9b9d1 234 cyklus++; //dalsi cyklus
customer10123 0:5007c2a9b9d1 235
customer10123 0:5007c2a9b9d1 236 for (int i=0;i<M;i++){ //ulozeni do data4 a data3 ktere se quicksortuje
customer10123 0:5007c2a9b9d1 237 data4[i]=emg4[i];
customer10123 0:5007c2a9b9d1 238 data3[i]=emg3[i];
customer10123 0:5007c2a9b9d1 239 }
customer10123 0:5007c2a9b9d1 240 }
customer10123 0:5007c2a9b9d1 241 }
customer10123 0:5007c2a9b9d1 242
customer10123 0:5007c2a9b9d1 243 int compare (const void * a, const void * b) //definice fce pro quicksort
customer10123 0:5007c2a9b9d1 244 {
customer10123 0:5007c2a9b9d1 245 return ( *(int*)a - *(int*)b );
customer10123 0:5007c2a9b9d1 246 }
customer10123 0:5007c2a9b9d1 247 //fce pro definice filtru s iir
customer10123 0:5007c2a9b9d1 248 filter1Type *filter1_create( void )
customer10123 0:5007c2a9b9d1 249 {
customer10123 0:5007c2a9b9d1 250 filter1Type *result = (filter1Type *)malloc( sizeof( filter1Type ) ); // Allocate memory for the object
customer10123 0:5007c2a9b9d1 251 filter1_init( result ); // Initialize it
customer10123 0:5007c2a9b9d1 252 return result; // Return the result
customer10123 0:5007c2a9b9d1 253 }
customer10123 0:5007c2a9b9d1 254
customer10123 0:5007c2a9b9d1 255 void filter1_destroy( filter1Type *pObject )
customer10123 0:5007c2a9b9d1 256 {
customer10123 0:5007c2a9b9d1 257 free( pObject );
customer10123 0:5007c2a9b9d1 258 }
customer10123 0:5007c2a9b9d1 259
customer10123 0:5007c2a9b9d1 260 void filter1_init( filter1Type * pThis )
customer10123 0:5007c2a9b9d1 261 {
customer10123 0:5007c2a9b9d1 262 filter1_reset( pThis );
customer10123 0:5007c2a9b9d1 263
customer10123 0:5007c2a9b9d1 264 }
customer10123 0:5007c2a9b9d1 265
customer10123 0:5007c2a9b9d1 266 void filter1_reset( filter1Type * pThis )
customer10123 0:5007c2a9b9d1 267 {
customer10123 0:5007c2a9b9d1 268 memset( &pThis->state, 0, sizeof( pThis->state ) ); // Reset state to 0
customer10123 0:5007c2a9b9d1 269 pThis->output = 0; // Reset output
customer10123 0:5007c2a9b9d1 270
customer10123 0:5007c2a9b9d1 271 }
customer10123 0:5007c2a9b9d1 272
customer10123 0:5007c2a9b9d1 273 int filter1_filterBlock( filter1Type * pThis, float * pInput, float * pOutput, unsigned int count )
customer10123 0:5007c2a9b9d1 274 {
customer10123 0:5007c2a9b9d1 275 filter1_executionState executionState; // The executionState structure holds call data, minimizing stack reads and writes
customer10123 0:5007c2a9b9d1 276 if( ! count ) return 0; // If there are no input samples, return immediately
customer10123 0:5007c2a9b9d1 277 executionState.pInput = pInput; // Pointers to the input and output buffers that each call to filterBiquad() will use
customer10123 0:5007c2a9b9d1 278 executionState.pOutput = pOutput; // - pInput and pOutput can be equal, allowing reuse of the same memory.
customer10123 0:5007c2a9b9d1 279 executionState.count = count; // The number of samples to be processed
customer10123 0:5007c2a9b9d1 280 executionState.pState = pThis->state; // Pointer to the biquad's internal state and coefficients.
customer10123 0:5007c2a9b9d1 281 executionState.pCoefficients = filter1_coefficients; // Each call to filterBiquad() will advance pState and pCoefficients to the next biquad
customer10123 0:5007c2a9b9d1 282
customer10123 0:5007c2a9b9d1 283 // The 1st call to filter1_filterBiquad() reads from the caller supplied input buffer and writes to the output buffer.
customer10123 0:5007c2a9b9d1 284 // The remaining calls to filterBiquad() recycle the same output buffer, so that multiple intermediate buffers are not required.
customer10123 0:5007c2a9b9d1 285
customer10123 0:5007c2a9b9d1 286 filter1_filterBiquad( &executionState ); // Run biquad #0
customer10123 0:5007c2a9b9d1 287 executionState.pInput = executionState.pOutput; // The remaining biquads will now re-use the same output buffer.
customer10123 0:5007c2a9b9d1 288
customer10123 0:5007c2a9b9d1 289 filter1_filterBiquad( &executionState ); // Run biquad #1
customer10123 0:5007c2a9b9d1 290
customer10123 0:5007c2a9b9d1 291 filter1_filterBiquad( &executionState ); // Run biquad #2
customer10123 0:5007c2a9b9d1 292
customer10123 0:5007c2a9b9d1 293 filter1_filterBiquad( &executionState ); // Run biquad #3
customer10123 0:5007c2a9b9d1 294
customer10123 0:5007c2a9b9d1 295 // At this point, the caller-supplied output buffer will contain the filtered samples and the input buffer will contain the unmodified input samples.
customer10123 0:5007c2a9b9d1 296 return count; // Return the number of samples processed, the same as the number of input samples
customer10123 0:5007c2a9b9d1 297
customer10123 0:5007c2a9b9d1 298 }
customer10123 0:5007c2a9b9d1 299
customer10123 0:5007c2a9b9d1 300 void filter1_filterBiquad( filter1_executionState * pExecState )
customer10123 0:5007c2a9b9d1 301 {
customer10123 0:5007c2a9b9d1 302 // Read state variables
customer10123 0:5007c2a9b9d1 303 float w0, x0;
customer10123 0:5007c2a9b9d1 304 float w1 = pExecState->pState[0];
customer10123 0:5007c2a9b9d1 305 float w2 = pExecState->pState[1];
customer10123 0:5007c2a9b9d1 306
customer10123 0:5007c2a9b9d1 307 // Read coefficients into work registers
customer10123 0:5007c2a9b9d1 308 float b0 = *(pExecState->pCoefficients++);
customer10123 0:5007c2a9b9d1 309 float b1 = *(pExecState->pCoefficients++);
customer10123 0:5007c2a9b9d1 310 float b2 = *(pExecState->pCoefficients++);
customer10123 0:5007c2a9b9d1 311 float a1 = *(pExecState->pCoefficients++);
customer10123 0:5007c2a9b9d1 312 float a2 = *(pExecState->pCoefficients++);
customer10123 0:5007c2a9b9d1 313
customer10123 0:5007c2a9b9d1 314 // Read source and target pointers
customer10123 0:5007c2a9b9d1 315 float *pInput = pExecState->pInput;
customer10123 0:5007c2a9b9d1 316 float *pOutput = pExecState->pOutput;
customer10123 0:5007c2a9b9d1 317 short count = pExecState->count;
customer10123 0:5007c2a9b9d1 318 float accumulator;
customer10123 0:5007c2a9b9d1 319
customer10123 0:5007c2a9b9d1 320 // Loop for all samples in the input buffer
customer10123 0:5007c2a9b9d1 321 while( count-- )
customer10123 0:5007c2a9b9d1 322 {
customer10123 0:5007c2a9b9d1 323 // Read input sample
customer10123 0:5007c2a9b9d1 324 x0 = *(pInput++);
customer10123 0:5007c2a9b9d1 325
customer10123 0:5007c2a9b9d1 326 // Run feedback part of filter
customer10123 0:5007c2a9b9d1 327 accumulator = w2 * a2;
customer10123 0:5007c2a9b9d1 328 accumulator += w1 * a1;
customer10123 0:5007c2a9b9d1 329 accumulator += x0 ;
customer10123 0:5007c2a9b9d1 330
customer10123 0:5007c2a9b9d1 331 w0 = accumulator ;
customer10123 0:5007c2a9b9d1 332
customer10123 0:5007c2a9b9d1 333 // Run feedforward part of filter
customer10123 0:5007c2a9b9d1 334 accumulator = w0 * b0;
customer10123 0:5007c2a9b9d1 335 accumulator += w1 * b1;
customer10123 0:5007c2a9b9d1 336 accumulator += w2 * b2;
customer10123 0:5007c2a9b9d1 337
customer10123 0:5007c2a9b9d1 338 w2 = w1; // Shuffle history buffer
customer10123 0:5007c2a9b9d1 339 w1 = w0;
customer10123 0:5007c2a9b9d1 340
customer10123 0:5007c2a9b9d1 341 // Write output
customer10123 0:5007c2a9b9d1 342 *(pOutput++) = accumulator ;
customer10123 0:5007c2a9b9d1 343 }
customer10123 0:5007c2a9b9d1 344
customer10123 0:5007c2a9b9d1 345 // Write state variables
customer10123 0:5007c2a9b9d1 346 *(pExecState->pState++) = w1;
customer10123 0:5007c2a9b9d1 347 *(pExecState->pState++) = w2;
customer10123 0:5007c2a9b9d1 348 }