ゼロからのロボット用シンセサイザー

#include <math.h> #include <stdio.h> /* speesy_Entropy_f -    (  ) speesy_GetBasicTone -     speesy_GetAllCoincidence -         speesy_GetBasicToneEntropy -        speesy_SetFreqMeander -    speesy_Meander -    */ #define SPEESY_SAMPLE_FREQ8 8000 #define SPEESY_SAMPLE_FREQ16 16000 #define SPEESY_SAMPLE_FREQ SPEESY_SAMPLE_FREQ16 #define SPEESY_MEANDER_MAX 100 #define FOR_FLOAT_EPSILON 1.19209e-007 static float speesy_all_coincidence = 0; //        static float speesy_fliter600_Entropy = 1.0; static float speesy_meander_period = 0.01;//   float speesy_Entropy_f(const float* source, unsigned long int start, unsigned long int finish, unsigned char binsCount, float minRaw, float maxRaw) { float entropy = 0; float binSize = fabs(maxRaw - minRaw) / (float)binsCount; //FOR_FLOAT_EPSILON == numeric_limits<float>::epsilon() if (fabs(binSize) < FOR_FLOAT_EPSILON) { return 0; } //float* p = new float[binsCount]; float p[256]; for (unsigned char i = 0; i < binsCount; i++) { p[i] = 0.0; } // Calculate probabilities unsigned char index; float value; for (unsigned long int i = start; i <= finish; i++) { value = source[i]; //for 8-bit data index = floor((value - minRaw) / binSize); if (index >= binsCount) { index = binsCount - 1; } p[index] += 1.0; } unsigned char Normalize_size = finish - start + 1; for (unsigned char i = 0; i < binsCount; i++) { p[i] /= Normalize_size; } for (unsigned char i = 0; i < binsCount; i++) { if (p[i] > FOR_FLOAT_EPSILON) { entropy += p[i] * log2(p[i]); } } entropy = -entropy; return entropy; } float speesy_GetBasicTone(float source) { static float matrix[6][6] ={0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0}; const float max_detector_p = 0.0255; const float min_detector_p = 0.0016; static float detector_p[6] = {min_detector_p}; static float detector_old_p[6] = {min_detector_p}; static float detector_t[6] = {0}; static float detector_tay[6] = {0.016}; static float detector_t_end = 0; //static float detector_beta[6] = {0}; static float detector_value[6] = {0}; float f_data = 0; //char detector_p_t0_f = 0; static float sig_old = 0; static char sig_p = 0; static char sig_m = 0; static unsigned short tim160 = 0; float detector_m1; float detector_m2; float detector_m3; float detector_m4; float detector_m5; float detector_m6; int detector_data = 0; static float detector_old_m1; static float detector_old_m4 = 0; static char speesy_tone_i = 0; static char speesy_tone_x = 0; static char speesy_tone_y = 0; static char speesy_tone_inter = 0; //char n_coincidence[4] ={0}; char n_coincidence_matrix[6][4] ={0}; static float out_t; #if SPEESY_SAMPLE_FREQ == SPEESY_SAMPLE_FREQ8 static float source_data[16] = {0}; const int max_source_data = 16; const float p_conts = 0.000125; #endif // SPEESY_SAMPLE_FREQ #if SPEESY_SAMPLE_FREQ == SPEESY_SAMPLE_FREQ16 static float source_data[32] = {0}; const int max_source_data = 32; const float p_conts = 0.0000625; #endif // SPEESY_SAMPLE_FREQ /*************Filter 600 Hz**********************/ #if SPEESY_SAMPLE_FREQ == SPEESY_SAMPLE_FREQ8 const float filter600_ACoef1[5] = { 0.00161978575856732190, 0.00647914303426928760, 0.00971871455140393280, 0.00647914303426928760, 0.00161978575856732190 }; const float filter600_BCoef1[4] = { -2.63228606617055720000, 2.68865140959361830000, -1.25580694576241330000, 0.22536111137571077000 }; #endif // SPEESY_SAMPLE_FREQ #if SPEESY_SAMPLE_FREQ == SPEESY_SAMPLE_FREQ16 const float filter600_ACoef1[5] = { 0.00013538805748957640, 0.00054155222995830559, 0.00081232834493745844, 0.00054155222995830559, 0.00013538805748957640 }; const float filter600_BCoef1[4] = { -3.29078386336302660000, 4.09122986596582550000, -2.27618508727807440000, 0.47792443748067198000 }; #endif // SPEESY_SAMPLE_FREQ static float filter600_y[5] = {0}; //output samples static float filter600_x[5] = {0}; //input samples static float out_filter600[240] = {0}; short out_i = 0; filter600_x[4] = filter600_x[3]; filter600_y[4] = filter600_y[3]; filter600_x[3] = filter600_x[2]; filter600_y[3] = filter600_y[2]; filter600_x[2] = filter600_x[1]; filter600_y[2] = filter600_y[1]; filter600_x[1] = filter600_x[0]; filter600_y[1] = filter600_y[0]; filter600_x[0] = source; filter600_y[0] = filter600_ACoef1[0] * filter600_x[0]; filter600_y[0] += filter600_ACoef1[1] * filter600_x[1] - filter600_BCoef1[0] * filter600_y[1]; filter600_y[0] += filter600_ACoef1[2] * filter600_x[2] - filter600_BCoef1[1] * filter600_y[2]; filter600_y[0] += filter600_ACoef1[3] * filter600_x[3] - filter600_BCoef1[2] * filter600_y[3]; filter600_y[0] += filter600_ACoef1[4] * filter600_x[4] - filter600_BCoef1[3] * filter600_y[4]; /***************End Filter 600 Hz*********************/ for (out_i = 239;out_i>0; out_i--) { out_filter600[out_i] = out_filter600[out_i - 1]; } out_filter600[0] = filter600_y[0]; if (tim160 < 160) {tim160 = tim160 + 1;} else { tim160 = 0; speesy_fliter600_Entropy = speesy_Entropy_f(out_filter600,0,159,255,-32768,32768); speesy_fliter600_Entropy = speesy_Entropy_f(out_filter600,160,239,255,-32768,32768); } detector_m1 = 0; detector_m2 = 0; detector_m3 = 0; detector_m4 = 0; detector_m5 = 0; detector_m6 = 0; //printf("\nSpeesy max_source_data = %d\n",max_source_data); if (filter600_y[0] >= 0) { if (filter600_y[0] > sig_old) {sig_p = 1;} else { if (sig_p == 1) { sig_p = 0; detector_m1 = filter600_y[0]; if (detector_m1 > detector_old_m1) { detector_m3 = detector_m1 - detector_old_m1; } else detector_m3 = 0; detector_m2 = detector_m1 + detector_old_m4; detector_old_m1 = detector_m1; } } sig_old = filter600_y[0]; } else { if ((-filter600_y[0]) > sig_old) {sig_m = 1;} else { if (sig_m == 1) { sig_m = 0; detector_m4 = -filter600_y[0]; if (detector_m4 > detector_old_m4) { detector_m6 = detector_m4 - detector_old_m4; } else detector_m6 = 0; detector_m5 = detector_m4 + detector_old_m1; detector_old_m4 = detector_m4; } } sig_old = -filter600_y[0]; } /*****************************************************/ //6 if (detector_t[5] > detector_tay[5]) { //   tay f_data = detector_value[5]*(exp(-(detector_t[5] - detector_tay[5])/(detector_p[5]/0.695))); //  if (detector_m6 > f_data) { //  detector_value[5] = detector_m6; //  detector_p[5] = (detector_t[5] + detector_p[5])/2; //   if (detector_p[5] > max_detector_p) detector_p[5] = max_detector_p; if (detector_p[5] < min_detector_p) detector_p[5] = min_detector_p; detector_tay[5] = 0.4*detector_p[5]; //  detector_t[5] = 0; //detector_p_t0_f = 1; matrix[5][2] = matrix[5][1]; matrix[5][1] = matrix[5][0]; matrix[5][0] = detector_p[5]; } else { detector_t[5] = detector_t[5] + p_conts; //  } } else { detector_t[5] = detector_t[5] + p_conts; } //5 if (detector_t[4] > detector_tay[4]) { //   tay f_data = detector_value[4]*(exp(-(detector_t[4] - detector_tay[4])/(detector_p[4]/0.695))); //  if (detector_m5 > f_data) { //  detector_value[4] = detector_m5; //  detector_p[4] = (detector_t[4] + detector_p[4])/2; //   if (detector_p[4] > max_detector_p) detector_p[4] = max_detector_p; if (detector_p[4] < min_detector_p) detector_p[4] = min_detector_p; detector_tay[4] = 0.4*detector_p[4]; //  detector_t[4] = 0; //detector_p_t0_f = 1; matrix[4][2] = matrix[4][1]; matrix[4][1] = matrix[4][0]; matrix[4][0] = detector_p[4]; } else { detector_t[4] = detector_t[4] + p_conts; //  } } else { detector_t[4] = detector_t[4] + p_conts; } //4 if (detector_t[3] > detector_tay[3]) { //   tay f_data = detector_value[3]*(exp(-(detector_t[3] - detector_tay[3])/(detector_p[3]/0.695))); //  if (detector_m4 > f_data) { //  detector_value[3] = detector_m4; //  detector_p[3] = (detector_t[3] + detector_p[3])/2; //   if (detector_p[3] > max_detector_p) detector_p[3] = max_detector_p; if (detector_p[3] < min_detector_p) detector_p[3] = min_detector_p; detector_tay[3] = 0.4*detector_p[3]; //  detector_t[3] = 0; //detector_p_t0_f = 1; matrix[3][2] = matrix[3][1]; matrix[3][1] = matrix[3][0]; matrix[3][0] = detector_p[3]; } else { detector_t[3] = detector_t[3] + p_conts; //  } } else { detector_t[3] = detector_t[3] + p_conts; } //3 if (detector_t[2] > detector_tay[2]) { //   tay f_data = detector_value[2]*(exp(-(detector_t[2] - detector_tay[2])/(detector_p[2]/0.695))); //  if (detector_m3 > f_data) { //  detector_value[2] = detector_m3; //  detector_p[2] = (detector_t[2] + detector_p[2])/2; //   if (detector_p[2] > max_detector_p) detector_p[2] = max_detector_p; if (detector_p[2] < min_detector_p) detector_p[2] = min_detector_p; detector_tay[2] = 0.4*detector_p[2]; //  detector_t[2] = 0; //detector_p_t0_f = 1; matrix[2][2] = matrix[2][1]; matrix[2][1] = matrix[2][0]; matrix[2][0] = detector_p[2]; } else { detector_t[2] = detector_t[2] + p_conts; //  } } else { detector_t[2] = detector_t[2] + p_conts; } //2 if (detector_t[1] > detector_tay[1]) { //   tay f_data = detector_value[1]*(exp(-(detector_t[1] - detector_tay[1])/(detector_p[1]/0.695))); //  if (detector_m2 > f_data) { //  detector_value[1] = detector_m2; //  detector_p[1] = (detector_t[1] + detector_p[1])/2; //   if (detector_p[1] > max_detector_p) detector_p[1] = max_detector_p; if (detector_p[1] < min_detector_p) detector_p[1] = min_detector_p; detector_tay[1] = 0.4*detector_p[1]; //  detector_t[1] = 0; //detector_p_t0_f = 1; matrix[1][2] = matrix[1][1]; matrix[1][1] = matrix[1][0]; matrix[1][0] = detector_p[1]; } else { detector_t[1] = detector_t[1] + p_conts; //  } } else { detector_t[1] = detector_t[1] + p_conts; } //1 if (detector_t[0] > detector_tay[0]) { //   tay f_data = detector_value[0]*(exp(-(detector_t[0] - detector_tay[0])/(detector_p[0]/0.695))); //  if (detector_m1 > f_data) { //  detector_value[0] = detector_m1; //  detector_p[0] = (detector_t[0] + detector_p[0])/2; //   if (detector_p[0] > max_detector_p) detector_p[0] = max_detector_p; if (detector_p[0] < min_detector_p) detector_p[0] = min_detector_p; detector_tay[0] = 0.4*detector_p[0]; //  detector_t[0] = 0; //detector_p_t0_f = 1; matrix[0][2] = matrix[0][1]; matrix[0][1] = matrix[0][0]; matrix[0][0] = detector_p[0]; } else { detector_t[0] = detector_t[0] + p_conts; //  } } else { detector_t[0] = detector_t[0] + p_conts; } /************************************************************************/ if (detector_t_end == 0) { for (speesy_tone_x = 0;speesy_tone_x<6;speesy_tone_x++) { matrix[speesy_tone_x][3] = matrix[speesy_tone_x][0] + matrix[speesy_tone_x][1]; matrix[speesy_tone_x][4] = matrix[speesy_tone_x][1] + matrix[speesy_tone_x][2]; matrix[speesy_tone_x][5] = matrix[speesy_tone_x][1] + matrix[speesy_tone_x][2] + matrix[speesy_tone_x][0]; } for (speesy_tone_inter = 0; speesy_tone_inter<4;speesy_tone_inter++) { n_coincidence_matrix[0][speesy_tone_inter] = 0; n_coincidence_matrix[1][speesy_tone_inter] = 0; n_coincidence_matrix[2][speesy_tone_inter] = 0; n_coincidence_matrix[3][speesy_tone_inter] = 0; n_coincidence_matrix[4][speesy_tone_inter] = 0; n_coincidence_matrix[5][speesy_tone_inter] = 0; for (speesy_tone_x = 0;speesy_tone_x<6;speesy_tone_x++) { for (speesy_tone_y = 0;speesy_tone_y<6;speesy_tone_y++) { //printf("\nValue_matrix %f",matrix[speesy_tone_x][speesy_tone_y]); //printf("\nmatrix %f",(float)matrix[speesy_tone_x][speesy_tone_y]); for (speesy_tone_i = 0;speesy_tone_i<6;speesy_tone_i++) { //printf("\nmatrix %f",(float)matrix[speesy_tone_x][speesy_tone_y]); //printf("\nspeesy_tone_i %d",speesy_tone_i); //printf("\nsr matrix %f",(float)matrix[speesy_tone_i][0]); if (((speesy_tone_y != 0)&(speesy_tone_x!=speesy_tone_i))|(speesy_tone_y > 0)) if ((matrix[speesy_tone_i][0] >= 0.0016)&(matrix[speesy_tone_i][0] <= 0.0031)) { f_data = 0.0001*((float)speesy_tone_inter + 1.0); if ((((float)matrix[speesy_tone_i][0] + (float)f_data) >= (float)matrix[speesy_tone_x][speesy_tone_y])& (((float)matrix[speesy_tone_i][0] - (float)f_data) <= (float)matrix[speesy_tone_x][speesy_tone_y])) { n_coincidence_matrix[speesy_tone_i][speesy_tone_inter] = n_coincidence_matrix[speesy_tone_i][speesy_tone_inter] + 1; //printf("\ncoincidence 0.0016 - 0.0031"); } } else if ((matrix[speesy_tone_i][0] > 0.0031)&(matrix[speesy_tone_i][0] <= 0.0063)) { f_data = 0.0002*((float)speesy_tone_inter + 1.0); if ((((float)matrix[speesy_tone_i][0] + (float)f_data) >= (float)matrix[speesy_tone_x][speesy_tone_y])& (((float)matrix[speesy_tone_i][0] - (float)f_data) <= (float)matrix[speesy_tone_x][speesy_tone_y])) { n_coincidence_matrix[speesy_tone_i][speesy_tone_inter] = n_coincidence_matrix[speesy_tone_i][speesy_tone_inter] + 1; //printf("\ncoincidence 0.0031 - 0.0063"); } } else if ((matrix[speesy_tone_i][0] > 0.0063)&(matrix[speesy_tone_i][0] <= 0.0127)) { f_data = 0.0004*((float)speesy_tone_inter + 1.0); if ((((float)matrix[speesy_tone_i][0] + (float)f_data) >= (float)matrix[speesy_tone_x][speesy_tone_y])& (((float)matrix[speesy_tone_i][0] - (float)f_data) <= (float)matrix[speesy_tone_x][speesy_tone_y])) { n_coincidence_matrix[speesy_tone_i][speesy_tone_inter] = n_coincidence_matrix[speesy_tone_i][speesy_tone_inter] + 1; //printf("\ncoincidence 0.0063 - 0.0127"); } } else if ((matrix[speesy_tone_i][0] > 0.0127)&(matrix[speesy_tone_i][0] <= 0.0255)) { f_data = 0.0008*((float)speesy_tone_inter + 1.0); if (((matrix[speesy_tone_i][0] + f_data) >= matrix[speesy_tone_x][speesy_tone_y])& ((matrix[speesy_tone_i][0] - f_data) <= matrix[speesy_tone_x][speesy_tone_y])) { n_coincidence_matrix[speesy_tone_i][speesy_tone_inter] = n_coincidence_matrix[speesy_tone_i][speesy_tone_inter] + 1; //printf("\ncoincidence 0.0127 - 0.0255"); } } else { //printf("\nNO coincidence"); } //printf("\ncoincidence %d",n_coincidence_matrix[speesy_tone_i][speesy_tone_inter]); } //end for } //end for for (speesy_tone_inter = 0; speesy_tone_inter<4;speesy_tone_inter++) { for (speesy_tone_i = 0;speesy_tone_i<6;speesy_tone_i++) { //printf("\nDo mat_ton %d",(int)n_coincidence_matrix[speesy_tone_i][speesy_tone_inter]); } } if (speesy_tone_inter == 0) { for (speesy_tone_i = 0;speesy_tone_i<6;speesy_tone_i++) { if (n_coincidence_matrix[speesy_tone_i][speesy_tone_inter] >= 1) { n_coincidence_matrix[speesy_tone_i][speesy_tone_inter] -= 1; } else { n_coincidence_matrix[speesy_tone_i][speesy_tone_inter] = 0; } } } else if (speesy_tone_inter == 1) { for (speesy_tone_i = 0;speesy_tone_i<6;speesy_tone_i++) { if (n_coincidence_matrix[speesy_tone_i][speesy_tone_inter] >= 2) { n_coincidence_matrix[speesy_tone_i][speesy_tone_inter] -= 2; } else { n_coincidence_matrix[speesy_tone_i][speesy_tone_inter] = 0; } } } else if (speesy_tone_inter == 2) { for (speesy_tone_i = 0;speesy_tone_i<6;speesy_tone_i++) { if (n_coincidence_matrix[speesy_tone_i][speesy_tone_inter] >= 5) { n_coincidence_matrix[speesy_tone_i][speesy_tone_inter] -= 5; } else { n_coincidence_matrix[speesy_tone_i][speesy_tone_inter] = 0; } } } else if (speesy_tone_inter == 3) { for (speesy_tone_i = 0;speesy_tone_i<6;speesy_tone_i++) { if (n_coincidence_matrix[speesy_tone_i][speesy_tone_inter] >= 7) { n_coincidence_matrix[speesy_tone_i][speesy_tone_inter] -= 7; } else { n_coincidence_matrix[speesy_tone_i][speesy_tone_inter] = 0; } } } } //end for } //end for () for inter out_t = 0; speesy_tone_x = 0; for (speesy_tone_inter = 0; speesy_tone_inter<4;speesy_tone_inter++) { for (speesy_tone_i = 0;speesy_tone_i<6;speesy_tone_i++) { //printf("\n mat_ton %d",(int)n_coincidence_matrix[speesy_tone_i][speesy_tone_inter]); if (speesy_tone_x < n_coincidence_matrix[speesy_tone_i][speesy_tone_inter]) { speesy_tone_x = n_coincidence_matrix[speesy_tone_i][speesy_tone_inter]; out_t = matrix[speesy_tone_i][0]; } } } speesy_all_coincidence = speesy_tone_x; } // end if detector_t_end = detector_t_end + p_conts; if (detector_t_end > 0.01) detector_t_end = 0; return out_t; } float speesy_GetAllCoincidence(void) { return speesy_all_coincidence; } float speesy_GetBasicToneEntropy(void) { return speesy_fliter600_Entropy; } void speesy_SetFreqMeander(float freq) { if (freq > 0) { speesy_meander_period = (1.0/freq)/2; } } signed char speesy_Meander(void) { static float tim = 0; static signed char out = SPEESY_MEANDER_MAX; if (tim < speesy_meander_period) { tim = tim + 0.0000625; } else { tim = 0; out = -out; } return out; } 

 #define SPEESY_NCOEF 4 #define SPEESY_MAXCAN 16 #define SPEESY_SAMPLE_FREQ8 8000 #define SPEESY_SAMPLE_FREQ16 16000 #define SPEESY_SAMPLE_FREQ SPEESY_SAMPLE_FREQ16 #if SPEESY_SAMPLE_FREQ == SPEESY_SAMPLE_FREQ16 //const float speesy_gain_correction_factor[16] = {1}; //for 0-88 Hz const float ACoef1[SPEESY_NCOEF+1] = { 0.00000010368236408362, 0.00000041472945633450, 0.00000062209418450175, 0.00000041472945633450, 0.00000010368236408362 }; const float BCoef1[SPEESY_NCOEF] = { -3.89262720221970990000, 5.68304206565440850000, -3.68807460061232460000, 0.89766108913372833000 }; //for 88-188 Hz --- const float ACoef2[SPEESY_NCOEF+1] = { 0.00046664702180067459, 0.00000000000000000000, -0.00093329404360134919, 0.00000000000000000000, 0.00046664702180067459 }; const float BCoef2[SPEESY_NCOEF] = { -3.92772838537582160000, 5.78986681239677910000, -3.79636811635581890000, 0.93423598091247251000 }; //for 188-300 Hz const float ACoef3[SPEESY_NCOEF+1] = { 0.00068858979234044106, 0.00000000000000000000, -0.00137717958468088210, 0.00000000000000000000, 0.00068858979234044106 }; const float BCoef3[SPEESY_NCOEF] = { -3.90771680345434060000, 5.74278248379999570000, -3.76163133600848760000, 0.92663841158343707000 }; //for 300-426 Hz const float ACoef4[SPEESY_NCOEF+1] = { 0.00088886359477686550, 0.00000000000000000000, -0.00177772718955373100, 0.00000000000000000000, 0.00088886359477686550 }; const float BCoef4[SPEESY_NCOEF] = { -3.87697578244056950000, 5.67379106513293690000, -3.71429576416733910000, 0.91785164973031175000 }; //for 426-569 Hz const float ACoef5[SPEESY_NCOEF+1] = { 0.00089752885269638212, 0.00000000000000000000, -0.00179505770539276420, 0.00000000000000000000, 0.00089752885269638212 }; const float BCoef5[SPEESY_NCOEF] = { -3.83152224876138180000, 5.57511629406004070000, -3.64956251837243120000, 0.90729235654450602000 }; //for 569-729 Hz const float ACoef6[SPEESY_NCOEF+1] = { 0.00117165116363920890, 0.00000000000000000000, -0.00234330232727841770, 0.00000000000000000000, 0.00117165116363920890 }; const float BCoef6[SPEESY_NCOEF] = { -3.76921491262598970000, 5.44568593950509160000, -3.56948525439078870000, 0.89685259841470821000 }; //for 729-910 Hz const float ACoef7[SPEESY_NCOEF+1] = { 0.00148423763165421900, 0.00000000000000000000, -0.00296847526330843790, 0.00000000000000000000, 0.00148423763165421900 }; const float BCoef7[SPEESY_NCOEF] = { -3.68252961240084180000, 5.27062615397503010000, -3.46252708324253880000, 0.88411914122102575000 }; //for 910-1113 Hz const float ACoef8[SPEESY_NCOEF+1] = { 0.00200280660037213200, 0.00000000000000000000, -0.00400561320074426400, 0.00000000000000000000, 0.00200280660037213200 }; const float BCoef8[SPEESY_NCOEF] = { -3.56693687706466770000, 5.04696879760847760000, -3.32877844509510410000, 0.87096927786413736000 }; //for 1113-1343 Hz const float ACoef9[SPEESY_NCOEF+1] = { 0.00211587349387137120, 0.00000000000000000000, -0.00423174698774274240, 0.00000000000000000000, 0.00211587349387137120 }; const float BCoef9[SPEESY_NCOEF] = { -3.41224013209053600000, 4.75975212198515950000, -3.15521200191456290000, 0.85509175287783645000 }; //for 1343-1601 Hz const float ACoef10[SPEESY_NCOEF+1] = { 0.00297423038464923910, 0.00000000000000000000, -0.00594846076929847830, 0.00000000000000000000, 0.00297423038464923910 }; const float BCoef10[SPEESY_NCOEF] = { -3.21142823810671900000, 4.40933217825630660000, -2.94127038526055400000, 0.83892403143238325000 }; //for 1601-1892 Hz const float ACoef11[SPEESY_NCOEF+1] = { 0.00355749937553949790, 0.00000000000000000000, -0.00711499875107899590, 0.00000000000000000000, 0.00355749937553949790 }; const float BCoef11[SPEESY_NCOEF] = { -2.95205117909921370000, 3.98864705820180850000, -2.67338596187190400000, 0.82024999905406459000 }; //for 1892-2219 Hz const float ACoef12[SPEESY_NCOEF+1] = { 0.00647978043392210490, 0.00000000000000000000, -0.01295956086784421000, 0.00000000000000000000, 0.00647978043392210490 }; const float BCoef12[SPEESY_NCOEF] = { -2.62319693441575370000, 3.50739946700623410000, -2.34648117429591170000, 0.80033689362210914000 }; //for 2219-2588 Hz const float ACoef13[SPEESY_NCOEF+1] = { 0.00856598928083165260, 0.00000000000000000000, -0.01713197856166330500, 0.00000000000000000000, 0.00856598928083165260 }; const float BCoef13[SPEESY_NCOEF] = { -2.21114034361129000000, 2.98277240977037210000, -1.94961151450908200000, 0.77769319296960093000 }; //for 2588-3004 Hz const float ACoef14[SPEESY_NCOEF+1] = { 0.02526264992554181800, 0.00000000000000000000, -0.05052529985108363700, 0.00000000000000000000, 0.02526264992554181800 }; const float BCoef14[SPEESY_NCOEF] = { -1.70416700060032110000, 2.45684840140417120000, -1.47852699918255030000, 0.75308204601400430000 }; //for 3004-3472 Hz const float ACoef15[SPEESY_NCOEF+1] = { 0.03942139343875778600, 0.00000000000000000000, -0.07884278687751557200, 0.00000000000000000000, 0.03942139343875778600 }; const float BCoef15[SPEESY_NCOEF] = { -1.09464887950984990000, 1.99774885820253490000, -0.93284437413716226000, 0.72671843772403244000 }; //for 3472-4000 Hz const float ACoef16[SPEESY_NCOEF+1] = { 0.11014553344131538000, 0.00000000000000000000, -0.22029106688263075000, 0.00000000000000000000, 0.11014553344131538000 }; const float BCoef16[SPEESY_NCOEF] = { -0.38091397871674004000, 1.69738617536193790000, -0.31794271360590415000, 0.69738617534660274000 }; #endif // SPEESY_SAMPLE_FREQ static float speesy_y1[SPEESY_NCOEF+1]; static float speesy_x1[SPEESY_NCOEF+1]; static float speesy_y2[SPEESY_NCOEF+1]; static float speesy_x2[SPEESY_NCOEF+1]; static float speesy_y3[SPEESY_NCOEF+1]; static float speesy_x3[SPEESY_NCOEF+1]; static float speesy_y4[SPEESY_NCOEF+1]; static float speesy_x4[SPEESY_NCOEF+1]; static float speesy_y5[SPEESY_NCOEF+1]; static float speesy_x5[SPEESY_NCOEF+1]; static float speesy_y6[SPEESY_NCOEF+1]; static float speesy_x6[SPEESY_NCOEF+1]; static float speesy_y7[SPEESY_NCOEF+1]; static float speesy_x7[SPEESY_NCOEF+1]; static float speesy_y8[SPEESY_NCOEF+1]; static float speesy_x8[SPEESY_NCOEF+1]; static float speesy_y9[SPEESY_NCOEF+1]; static float speesy_x9[SPEESY_NCOEF+1]; static float speesy_y10[SPEESY_NCOEF+1]; static float speesy_x10[SPEESY_NCOEF+1]; static float speesy_y11[SPEESY_NCOEF+1]; static float speesy_x11[SPEESY_NCOEF+1]; static float speesy_y12[SPEESY_NCOEF+1]; static float speesy_x12[SPEESY_NCOEF+1]; static float speesy_y13[SPEESY_NCOEF+1]; static float speesy_x13[SPEESY_NCOEF+1]; static float speesy_y14[SPEESY_NCOEF+1]; static float speesy_x14[SPEESY_NCOEF+1]; static float speesy_y15[SPEESY_NCOEF+1]; static float speesy_x15[SPEESY_NCOEF+1]; static float speesy_y16[SPEESY_NCOEF+1]; static float speesy_x16[SPEESY_NCOEF+1]; static float speesy_cannel[SPEESY_MAXCAN] ={0}; static float speesy_value[SPEESY_MAXCAN] ={0}; static int speesy_i = 0; const float speesy_gain_correction_factor[16] = {0.95,0.79,0.66,0.66,0.8,0.79,0.8,0.74,0.85,0.79,0.834,0.57,0.54,0.23,0.18,0.082}; void speesy_set_cannel(float * cannel) { for (speesy_i = 0; speesy_i < SPEESY_MAXCAN; speesy_i++) { speesy_value[speesy_i] = cannel[speesy_i]; } } void speesy_get_value(float * value) { for (speesy_i = 0; speesy_i < SPEESY_MAXCAN; speesy_i++) { value[speesy_i] = speesy_cannel[speesy_i] * speesy_gain_correction_factor[speesy_i]; } } void speesy_update_filter(float NewSample) { /*************1***********/ speesy_x1[4] = speesy_x1[3]; speesy_y1[4] = speesy_y1[3]; speesy_x1[3] = speesy_x1[2]; speesy_y1[3] = speesy_y1[2]; speesy_x1[2] = speesy_x1[1]; speesy_y1[2] = speesy_y1[1]; speesy_x1[1] = speesy_x1[0]; speesy_y1[1] = speesy_y1[0]; speesy_x1[0] = speesy_value[0]*NewSample; speesy_y1[0] = ACoef1[0] * speesy_x1[0]; speesy_y1[0] += ACoef1[1] * speesy_x1[1] - BCoef1[0] * speesy_y1[1]; speesy_y1[0] += ACoef1[2] * speesy_x1[2] - BCoef1[1] * speesy_y1[2]; speesy_y1[0] += ACoef1[3] * speesy_x1[3] - BCoef1[2] * speesy_y1[3]; speesy_y1[0] += ACoef1[4] * speesy_x1[4] - BCoef1[3] * speesy_y1[4]; speesy_cannel[0] = speesy_y1[0]; /*************************/ /*************2***********/ speesy_x2[4] = speesy_x2[3]; speesy_y2[4] = speesy_y2[3]; speesy_x2[3] = speesy_x2[2]; speesy_y2[3] = speesy_y2[2]; speesy_x2[2] = speesy_x2[1]; speesy_y2[2] = speesy_y2[1]; speesy_x2[1] = speesy_x2[0]; speesy_y2[1] = speesy_y2[0]; speesy_x2[0] = speesy_value[1]*NewSample; speesy_y2[0] = ACoef2[0] * speesy_x2[0]; speesy_y2[0] += ACoef2[1] * speesy_x2[1] - BCoef2[0] * speesy_y2[1]; speesy_y2[0] += ACoef2[2] * speesy_x2[2] - BCoef2[1] * speesy_y2[2]; speesy_y2[0] += ACoef2[3] * speesy_x2[3] - BCoef2[2] * speesy_y2[3]; speesy_y2[0] += ACoef2[4] * speesy_x2[4] - BCoef2[3] * speesy_y2[4]; speesy_cannel[1] = speesy_y2[0]; /*************************/ /*************3***********/ speesy_x3[4] = speesy_x3[3]; speesy_y3[4] = speesy_y3[3]; speesy_x3[3] = speesy_x3[2]; speesy_y3[3] = speesy_y3[2]; speesy_x3[2] = speesy_x3[1]; speesy_y3[2] = speesy_y3[1]; speesy_x3[1] = speesy_x3[0]; speesy_y3[1] = speesy_y3[0]; speesy_x3[0] = speesy_value[2]*NewSample; speesy_y3[0] = ACoef3[0] * speesy_x3[0]; speesy_y3[0] += ACoef3[1] * speesy_x3[1] - BCoef3[0] * speesy_y3[1]; speesy_y3[0] += ACoef3[2] * speesy_x3[2] - BCoef3[1] * speesy_y3[2]; speesy_y3[0] += ACoef3[3] * speesy_x3[3] - BCoef3[2] * speesy_y3[3]; speesy_y3[0] += ACoef3[4] * speesy_x3[4] - BCoef3[3] * speesy_y3[4]; speesy_cannel[2] = speesy_y3[0]; /*************************/ /*************4***********/ speesy_x4[4] = speesy_x4[3]; speesy_y4[4] = speesy_y4[3]; speesy_x4[3] = speesy_x4[2]; speesy_y4[3] = speesy_y4[2]; speesy_x4[2] = speesy_x4[1]; speesy_y4[2] = speesy_y4[1]; speesy_x4[1] = speesy_x4[0]; speesy_y4[1] = speesy_y4[0]; speesy_x4[0] = speesy_value[3]*NewSample; speesy_y4[0] = ACoef4[0] * speesy_x4[0]; speesy_y4[0] += ACoef4[1] * speesy_x4[1] - BCoef4[0] * speesy_y4[1]; speesy_y4[0] += ACoef4[2] * speesy_x4[2] - BCoef4[1] * speesy_y4[2]; speesy_y4[0] += ACoef4[3] * speesy_x4[3] - BCoef4[2] * speesy_y4[3]; speesy_y4[0] += ACoef4[4] * speesy_x4[4] - BCoef4[3] * speesy_y4[4]; speesy_cannel[3] = speesy_y4[0]; /*************************/ /*************5***********/ speesy_x5[4] = speesy_x5[3]; speesy_y5[4] = speesy_y5[3]; speesy_x5[3] = speesy_x5[2]; speesy_y5[3] = speesy_y5[2]; speesy_x5[2] = speesy_x5[1]; speesy_y5[2] = speesy_y5[1]; speesy_x5[1] = speesy_x5[0]; speesy_y5[1] = speesy_y5[0]; speesy_x5[0] = speesy_value[4]*NewSample; speesy_y5[0] = ACoef5[0] * speesy_x5[0]; speesy_y5[0] += ACoef5[1] * speesy_x5[1] - BCoef5[0] * speesy_y5[1]; speesy_y5[0] += ACoef5[2] * speesy_x5[2] - BCoef5[1] * speesy_y5[2]; speesy_y5[0] += ACoef5[3] * speesy_x5[3] - BCoef5[2] * speesy_y5[3]; speesy_y5[0] += ACoef5[4] * speesy_x5[4] - BCoef5[3] * speesy_y5[4]; speesy_cannel[4] = speesy_y5[0]; /*************************/ /*************6***********/ speesy_x6[4] = speesy_x6[3]; speesy_y6[4] = speesy_y6[3]; speesy_x6[3] = speesy_x6[2]; speesy_y6[3] = speesy_y6[2]; speesy_x6[2] = speesy_x6[1]; speesy_y6[2] = speesy_y6[1]; speesy_x6[1] = speesy_x6[0]; speesy_y6[1] = speesy_y6[0]; speesy_x6[0] = speesy_value[5]*NewSample; speesy_y6[0] = ACoef6[0] * speesy_x6[0]; speesy_y6[0] += ACoef6[1] * speesy_x6[1] - BCoef6[0] * speesy_y6[1]; speesy_y6[0] += ACoef6[2] * speesy_x6[2] - BCoef6[1] * speesy_y6[2]; speesy_y6[0] += ACoef6[3] * speesy_x6[3] - BCoef6[2] * speesy_y6[3]; speesy_y6[0] += ACoef6[4] * speesy_x6[4] - BCoef6[3] * speesy_y6[4]; speesy_cannel[5] = speesy_y6[0]; /*************************/ /*************7***********/ speesy_x7[4] = speesy_x7[3]; speesy_y7[4] = speesy_y7[3]; speesy_x7[3] = speesy_x7[2]; speesy_y7[3] = speesy_y7[2]; speesy_x7[2] = speesy_x7[1]; speesy_y7[2] = speesy_y7[1]; speesy_x7[1] = speesy_x7[0]; speesy_y7[1] = speesy_y7[0]; speesy_x7[0] = speesy_value[6]*NewSample; speesy_y7[0] = ACoef7[0] * speesy_x7[0]; speesy_y7[0] += ACoef7[1] * speesy_x7[1] - BCoef7[0] * speesy_y7[1]; speesy_y7[0] += ACoef7[2] * speesy_x7[2] - BCoef7[1] * speesy_y7[2]; speesy_y7[0] += ACoef7[3] * speesy_x7[3] - BCoef7[2] * speesy_y7[3]; speesy_y7[0] += ACoef7[4] * speesy_x7[4] - BCoef7[3] * speesy_y7[4]; speesy_cannel[6] = speesy_y7[0]; /*************************/ /*************8***********/ speesy_x8[4] = speesy_x8[3]; speesy_y8[4] = speesy_y8[3]; speesy_x8[3] = speesy_x8[2]; speesy_y8[3] = speesy_y8[2]; speesy_x8[2] = speesy_x8[1]; speesy_y8[2] = speesy_y8[1]; speesy_x8[1] = speesy_x8[0]; speesy_y8[1] = speesy_y8[0]; speesy_x8[0] = speesy_value[7]*NewSample; speesy_y8[0] = ACoef8[0] * speesy_x8[0]; speesy_y8[0] += ACoef8[1] * speesy_x8[1] - BCoef8[0] * speesy_y8[1]; speesy_y8[0] += ACoef8[2] * speesy_x8[2] - BCoef8[1] * speesy_y8[2]; speesy_y8[0] += ACoef8[3] * speesy_x8[3] - BCoef8[2] * speesy_y8[3]; speesy_y8[0] += ACoef8[4] * speesy_x8[4] - BCoef8[3] * speesy_y8[4]; speesy_cannel[7] = speesy_y8[0]; /*************************/ /*************9***********/ speesy_x9[4] = speesy_x9[3]; speesy_y9[4] = speesy_y9[3]; speesy_x9[3] = speesy_x9[2]; speesy_y9[3] = speesy_y9[2]; speesy_x9[2] = speesy_x9[1]; speesy_y9[2] = speesy_y9[1]; speesy_x9[1] = speesy_x9[0]; speesy_y9[1] = speesy_y9[0]; speesy_x9[0] = speesy_value[8]*NewSample; speesy_y9[0] = ACoef9[0] * speesy_x9[0]; speesy_y9[0] += ACoef9[1] * speesy_x9[1] - BCoef9[0] * speesy_y9[1]; speesy_y9[0] += ACoef9[2] * speesy_x9[2] - BCoef9[1] * speesy_y9[2]; speesy_y9[0] += ACoef9[3] * speesy_x9[3] - BCoef9[2] * speesy_y9[3]; speesy_y9[0] += ACoef9[4] * speesy_x9[4] - BCoef9[3] * speesy_y9[4]; speesy_cannel[8] = speesy_y9[0]; /*************************/ /*************10***********/ speesy_x10[4] = speesy_x10[3]; speesy_y10[4] = speesy_y10[3]; speesy_x10[3] = speesy_x10[2]; speesy_y10[3] = speesy_y10[2]; speesy_x10[2] = speesy_x10[1]; speesy_y10[2] = speesy_y10[1]; speesy_x10[1] = speesy_x10[0]; speesy_y10[1] = speesy_y10[0]; speesy_x10[0] = speesy_value[9]*NewSample; speesy_y10[0] = ACoef10[0] * speesy_x10[0]; speesy_y10[0] += ACoef10[1] * speesy_x10[1] - BCoef10[0] * speesy_y10[1]; speesy_y10[0] += ACoef10[2] * speesy_x10[2] - BCoef10[1] * speesy_y10[2]; speesy_y10[0] += ACoef10[3] * speesy_x10[3] - BCoef10[2] * speesy_y10[3]; speesy_y10[0] += ACoef10[4] * speesy_x10[4] - BCoef10[3] * speesy_y10[4]; speesy_cannel[9] = speesy_y10[0]; /*************************/ /*************11***********/ speesy_x11[4] = speesy_x11[3]; speesy_y11[4] = speesy_y11[3]; speesy_x11[3] = speesy_x11[2]; speesy_y11[3] = speesy_y11[2]; speesy_x11[2] = speesy_x11[1]; speesy_y11[2] = speesy_y11[1]; speesy_x11[1] = speesy_x11[0]; speesy_y11[1] = speesy_y11[0]; speesy_x11[0] = speesy_value[10]*NewSample; speesy_y11[0] = ACoef11[0] * speesy_x11[0]; speesy_y11[0] += ACoef11[1] * speesy_x11[1] - BCoef11[0] * speesy_y11[1]; speesy_y11[0] += ACoef11[2] * speesy_x11[2] - BCoef11[1] * speesy_y11[2]; speesy_y11[0] += ACoef11[3] * speesy_x11[3] - BCoef11[2] * speesy_y11[3]; speesy_y11[0] += ACoef11[4] * speesy_x11[4] - BCoef11[3] * speesy_y11[4]; speesy_cannel[10] = speesy_y11[0]; /*************************/ /*************12***********/ speesy_x12[4] = speesy_x12[3]; speesy_y12[4] = speesy_y12[3]; speesy_x12[3] = speesy_x12[2]; speesy_y12[3] = speesy_y12[2]; speesy_x12[2] = speesy_x12[1]; speesy_y12[2] = speesy_y12[1]; speesy_x12[1] = speesy_x12[0]; speesy_y12[1] = speesy_y12[0]; speesy_x12[0] = speesy_value[11]*NewSample; speesy_y12[0] = ACoef12[0] * speesy_x12[0]; speesy_y12[0] += ACoef12[1] * speesy_x12[1] - BCoef12[0] * speesy_y12[1]; speesy_y12[0] += ACoef12[2] * speesy_x12[2] - BCoef12[1] * speesy_y12[2]; speesy_y12[0] += ACoef12[3] * speesy_x12[3] - BCoef12[2] * speesy_y12[3]; speesy_y12[0] += ACoef12[4] * speesy_x12[4] - BCoef12[3] * speesy_y12[4]; speesy_cannel[11] = speesy_y12[0]; /*************************/ /*************13***********/ speesy_x13[4] = speesy_x13[3]; speesy_y13[4] = speesy_y13[3]; speesy_x13[3] = speesy_x13[2]; speesy_y13[3] = speesy_y13[2]; speesy_x13[2] = speesy_x13[1]; speesy_y13[2] = speesy_y13[1]; speesy_x13[1] = speesy_x13[0]; speesy_y13[1] = speesy_y13[0]; speesy_x13[0] = speesy_value[12]*NewSample; speesy_y13[0] = ACoef13[0] * speesy_x13[0]; speesy_y13[0] += ACoef13[1] * speesy_x13[1] - BCoef13[0] * speesy_y13[1]; speesy_y13[0] += ACoef13[2] * speesy_x13[2] - BCoef13[1] * speesy_y13[2]; speesy_y13[0] += ACoef13[3] * speesy_x13[3] - BCoef13[2] * speesy_y13[3]; speesy_y13[0] += ACoef13[4] * speesy_x13[4] - BCoef13[3] * speesy_y13[4]; speesy_cannel[12] = speesy_y13[0]; /*************************/ /*************14***********/ speesy_x14[4] = speesy_x14[3]; speesy_y14[4] = speesy_y14[3]; speesy_x14[3] = speesy_x14[2]; speesy_y14[3] = speesy_y14[2]; speesy_x14[2] = speesy_x14[1]; speesy_y14[2] = speesy_y14[1]; speesy_x14[1] = speesy_x14[0]; speesy_y14[1] = speesy_y14[0]; speesy_x14[0] = speesy_value[13]*NewSample; speesy_y14[0] = ACoef14[0] * speesy_x14[0]; speesy_y14[0] += ACoef14[1] * speesy_x14[1] - BCoef14[0] * speesy_y14[1]; speesy_y14[0] += ACoef14[2] * speesy_x14[2] - BCoef14[1] * speesy_y14[2]; speesy_y14[0] += ACoef14[3] * speesy_x14[3] - BCoef14[2] * speesy_y14[3]; speesy_y14[0] += ACoef14[4] * speesy_x14[4] - BCoef14[3] * speesy_y14[4]; speesy_cannel[13] = speesy_y14[0]; /*************************/ /*************15***********/ speesy_x15[4] = speesy_x15[3]; speesy_y15[4] = speesy_y15[3]; speesy_x15[3] = speesy_x15[2]; speesy_y15[3] = speesy_y15[2]; speesy_x15[2] = speesy_x15[1]; speesy_y15[2] = speesy_y15[1]; speesy_x15[1] = speesy_x15[0]; speesy_y15[1] = speesy_y15[0]; speesy_x15[0] = speesy_value[14]*NewSample; speesy_y15[0] = ACoef15[0] * speesy_x15[0]; speesy_y15[0] += ACoef15[1] * speesy_x15[1] - BCoef15[0] * speesy_y15[1]; speesy_y15[0] += ACoef15[2] * speesy_x15[2] - BCoef15[1] * speesy_y15[2]; speesy_y15[0] += ACoef15[3] * speesy_x15[3] - BCoef15[2] * speesy_y15[3]; speesy_y15[0] += ACoef15[4] * speesy_x15[4] - BCoef15[3] * speesy_y15[4]; speesy_cannel[14] = speesy_y15[0]; /*************************/ /*************16***********/ speesy_x16[4] = speesy_x16[3]; speesy_y16[4] = speesy_y16[3]; speesy_x16[3] = speesy_x16[2]; speesy_y16[3] = speesy_y16[2]; speesy_x16[2] = speesy_x16[1]; speesy_y16[2] = speesy_y16[1]; speesy_x16[1] = speesy_x16[0]; speesy_y16[1] = speesy_y16[0]; speesy_x16[0] = speesy_value[15]*NewSample; speesy_y16[0] = ACoef16[0] * speesy_x16[0]; speesy_y16[0] += ACoef16[1] * speesy_x16[1] - BCoef16[0] * speesy_y16[1]; speesy_y16[0] += ACoef16[2] * speesy_x16[2] - BCoef16[1] * speesy_y16[2]; speesy_y16[0] += ACoef16[3] * speesy_x16[3] - BCoef16[2] * speesy_y16[3]; speesy_y16[0] += ACoef16[4] * speesy_x16[4] - BCoef16[3] * speesy_y16[4]; speesy_cannel[15] = speesy_y16[0]; /*************************/ }