#include #include #include #include // Accuracy here matters. // With a max 25k TDS (i.e. 16 bits input signal signed) // this means 6 + 16 == 22. // This fits within a 32 bit int // and leaves ten bits of head room. // // For two pole the y0, y1, y2 registers must be 32 bit at least. // The new rpi is 64 bit so these have had to have been deliberately set // as int32_t to simulate the pic18 `long' type. // The short int type on the PIC18 is `int' // #define BIN_FRACS 6 // this is a 64 bit machine (the pi) // int16_t /* squared version of LAG_7_8 */ two_pole_7_8 ( int16_t input ) { static int32_t y1=0,y2=0, x1,x2; int32_t * res; int32_t y0; int32_t x0 = input; // x0 times 0.125 // the minus 3 divides by 8 : DOUBLE POLE x0 <<= (BIN_FRACS-3) ; // now all calculations are done times BIN_FRACS^2 // this works stabley but not well WHY? //y0 = x0 + y1-(y1>>3) - ((y2>>1) + (y2>>2) + (y2>>6)); // should be //y0 = x0 + y1+y1-(y1>>2) - (y2>>1 + y2>>2 + y2>>6); // // // OK this works well // y0 = x0 + (14.0 / 8.0) * (double) y1 - (49.0/64.0) * (double) y2; y0 = x0 + // (14.0 / 8.0) * (double) y1 // this is 7/4 y1 + y1 - (y1>>2) // // // - (49.0/64.0) * (double) y2; // - ( (y2>>1) + // half (y2>>2) + // quarter (y2>>6) ); // 64th // // y0 = x0/64 + (y1 * 7) / 4 + (y2*49)/64; // see what happens when y2>>6 is left out. I theory d.c goes unstable // and yes it does! 25AUG2019 // y0 = x0 + y1-(y1>>3) - (y2>>1 + y2>>2 /* + y2>>6*/); y2 = y1; y1 = y0; x2 = x1; x1 = x0; //res = (short int *)&y0; //res++; //printf("y0=%X res should be %X res=%x\n",y0, y0>>16, res); // // the plus 3 divides by 8 return y0>>(BIN_FRACS+3); // divide back down for scaling and then divide by 8 filter gain // *res; } // Added zero guard. tried a pair of zeros near unit circle // 7/8 in ADJ, i.e hyp = 1 so on unit circle. // int16_t /* squared version of LAG_7_8 */ two_pole_7_8_zg ( int16_t input ) { // with zero guard static int32_t y1=0,y2=0, x1,x2,x3,x4,x5,x6,x7,x8; int32_t * res; int32_t y0; int32_t x0 = input; // x0 times 0.125 // the minus 3 divides by 8 : DOUBLE POLE x0 <<= (BIN_FRACS-3) ; // now all calculations are done times BIN_FRACS^2 // this works stabley but not well WHY? //y0 = x0 + y1-(y1>>3) - ((y2>>1) + (y2>>2) + (y2>>6)); // should be //y0 = x0 + y1+y1-(y1>>2) - (y2>>1 + y2>>2 + y2>>6); // // // OK this works well // y0 = x0 + (14.0 / 8.0) * (double) y1 - (49.0/64.0) * (double) y2; // (z-j)(z+j) zeros at half nyquist (Z^2 + 1) x0 + x2 y0 = //(x0>>1) + (x2>>1) + (x0+x1+x2+x3) + //(x0>>1) + (x0>>2) + (x0>>6) + (x2>>1) + // 49/64 * x0 + 1/2 * x2 // (14.0 / 8.0) * (double) y1 // this is 7/4 y1 + y1 - (y1>>2) // // // - (49.0/64.0) * (double) y2; // - ( (y2>>1) + // half (y2>>2) + // quarter (y2>>6) ); // 64th // // y0 = x0/64 + (y1 * 7) / 4 + (y2*49)/64; // see what happens when y2>>6 is left out. I theory d.c goes unstable // and yes it does! 25AUG2019 // y0 = x0 + y1-(y1>>3) - (y2>>1 + y2>>2 /* + y2>>6*/); y2 = y1; y1 = y0; x4 = x3; x3 = x2; x2 = x1; x1 = x0; //res = (short int *)&y0; //res++; //printf("y0=%X res should be %X res=%x\n",y0, y0>>16, res); // // the plus 3 divides by 8 return y0>>(BIN_FRACS+3); // divide back down for scaling and then divide by 8 filter gain // *res; } // // At 20,000 TDS with 1500 noise this caused instability in the filter // when BINFRACS was set to 9. As the input is not divided // its already 8 bits (*256) high. Try BINFRACS at 2. // #define BIN_FRACS_15_16 2 // this is a 64 bit machine (the pi) // int16_t /* squared version of LAG_15_16 */ two_pole_15_16 ( int16_t input ) { static int32_t y1=0,y2=0, x1, x2; int32_t * res; int32_t y0; int32_t x0 = input; x0 <<= (BIN_FRACS_15_16) ; // now all calculations are done times BIN_FRACS^2 y0 = x0 + // // this is 30/16 == 15/8 y1 + y1 - (y1>>3) // // // - (49.0/64.0) * (double) y2; // ; //- ( (y2>>1) + // half // (y2>>2) + // quarter // (y2>>3) + // eighth // (y2>>8) // 256th // ); // // try to do the shfts in a way effecient for the HYTEC PIC18 compiler // y2>>=1; y0 -= y2; y2>>=1; y0 -= y2; y2>>=1; y0 -= y2; y2>>=5; y0 -= y2; y2 = y1; y1 = y0; // gain of the filter is 256 ((1/16)^2) if x0 is allowed in without pre-dividing // y0 >>= 8; return y0>>(BIN_FRACS_15_16); // divide back down for scaling and then divide by 8 filter gain // *res; } int zero_only_2 ( int input ) { static int x0,x1,x2,x3,x4; int res=1500*4; // (z^2+1)^2 ZEROS AT HALF NYQUIST // z^4 z^2 1 //res = input + 2*x2 + x4; x4 = x3; x3 = x2; x2 = x1; x1 = input; return res/4; } int zero_only_3 ( int input ) { static int x0,x1,x2,x3,x4; static int x5,x6,x7,x8,x9; int res=1500;; // (Z+j)(Z-j) == (Z^2+1) // zero at nyquist // (z^2+1)^3 ZEROS AT HALF NYQUIST // z^6 z^4 2z^3 z^2 2*z 1 //res = input + x2 +2*x3 + x4 +2*x2 + x6; // (Z^2 - 1)(Z+1) is zero at half nyquist and zero at nyquist // z^3 +z^2 + z + 1 res = input + x1 + x2 + x3; x8 = x7; x7 = x6; x6 = x5; x5 = x4; x4 = x3; x3 = x2; x2 = x1; x1 = input; return res/4; } int zero_only_4 ( int input ) { static int x0,x1,x2,x3,x4; static int x5,x6,x7,x8,x9; int res=1500*16; // (Z^2+1)^4 ZEROS AT HALF NYQUIST // z^8 2z^6 2z^5 2z^4 4z^3 2z^2 2z^1 + 1 //res = input + 2*x2 +2*x3 + x4 + 4*x5 + 2*x6 + 2*x7 + x8; // (Z+1)(Z-1 // zero at nyquist zero at half nyquist // (Z^2 - 1) // x8 = x7; x7 = x6; x6 = x5; x5 = x4; x4 = x3; x3 = x2; x2 = x1; x1 = input; return res/16; } // 1/16 is 0.0625 // // A double precision two pole filter could be used in a higher level // to further smooth readings #define DOUBLE_FILTER_PARAM 0.01 #define DOUBLE_FILTER_PARAM_LAG (1.0 - DOUBLE_FILTER_PARAM) int16_t /* squared version of LAG 0.01 0.99 */ two_pole_double_low_pass ( int16_t input ) { static double x0, y1, y2; static double res; x0 = input; res = x0 * (DOUBLE_FILTER_PARAM * DOUBLE_FILTER_PARAM); res += y1 * 2.0 * DOUBLE_FILTER_PARAM_LAG; res -= y2 * (DOUBLE_FILTER_PARAM_LAG * DOUBLE_FILTER_PARAM_LAG); y2 = y1; y1 = res; return (int16_t) res + 0.5; } // This was initially tested at 1500 TDS with \pm 500 noise // Tests at 15000 with \pm 1500 noise at BINFRACS 9 caused instability in the two pole 15 16 // BINFRACS down to 2 for that filter. // #define RAND_RANGE 500 // #define DC_TERM 1500 #define RAND_RANGE 500 #define DC_TERM 1000 int main () { int i, zo2,zo3,zo4; int16_t val,res, res34, res78, res78_2, rr, res_1516, tpdlp, tpdlp_1516; for (i=0;i<1000;i++) { val = sin ( (double)i*10.0 / (3.142 * 2.0) ) * 10.0 + DC_TERM ; rr = rand() % (RAND_RANGE*2); rr -= RAND_RANGE; val += rr; // fast sine //val = sin ( ((double)i*10.0) / (3.142 * 2.0) ) * 10000; res = two_pole_7_8 ( val ); res_1516 = two_pole_15_16 ( val ); zo2 = two_pole_7_8_zg(val); // zero_only_2 (val); zo3 = zero_only_3 (val); zo4 = zero_only_4 (val); tpdlp = two_pole_double_low_pass ( val ); tpdlp_1516 = two_pole_double_low_pass (res_1516); // as fed by the yellow trace res34 = (((res34<<2) - res34)>>2) + (val>>2); res78 = (((res78<<3) - res78)>>3) + (val>>3); res78_2 = (((res78_2<<3) - res78_2)>>3) + (res78>>3); // feed res78 into another should be the same as two pole // 1 3 5 7 9 11 13 15 17 19 21 printf ("%d val %d res %d res34 %d res78 %d res78_2 %d res_1516 %d zo2 %d zo3 %d zo4 %d tpdlp_1516 %dEOL\n", i, val,res, res34, res78, res78_2, res_1516, zo2, zo3, zo4, tpdlp_1516); } return 0; }