const float sample_freq = 800; int rawData[] = {-20, 24, 10, 10, 0, 8, 16, 14, 26, 16, 6, 10, 0, 0, 16, -14, 0, 8, 18, 4, 18, 24, 8, 40, 48, 36, 18, 6, 8, 4, 0, 22, 62, 18, 16, 30, 34, 18, 8, 0, 12, 10, -2, 0, 0, 0, 18, 12, 16, 14, 10, 22, 32, 48, 0, 10, 0, -8, -2, -6, -12, 16, 0, 16, 8, 26, 16, 24, 32, 20, 0, -8, 0, -30, 4, 20, 6, 16, 6, 4, 20, 20, 24, 30, 20, 12, 40, -8, 32, 2, -48, -6, -14, 0, 16, 4, 6, 30, 8, 20, 32, 0, 6, 0, 16, -6, -12, 0, -16, 22, 22, 8, 12, 18, 10, 56, 30, 16, 32, 20, -6, 0, -8, -10, -10, 2, 0, 32}; double coefficients[30] = {0.00810278836982443983,0.00618722417339871277,0.00295303478781193656,-0.00421541657862568343,-0.01682542880714292879,-0.03391632344271969246,-0.05163869278338199115,-0.06406059641779388025,-0.06501461440422248717,-0.05037353790535013115,-0.01992880177510165940,0.02186672896355161544,0.06666310399669593545,0.10433630100933323492,0.12583433871411897620,0.12583433871411897620,0.10433630100933323492,0.06666310399669590769,0.02186672896355160850,-0.01992880177510165940,-0.05037353790535013115,-0.06501461440422247329,-0.06406059641779386638,-0.05163869278338197033,-0.03391632344271967164,-0.01682542880714291839,-0.00421541657862567649,0.00295303478781193656,0.00618722417339871277,0.00810278836982443983}; const int len = sizeof(rawData)/sizeof(rawData[0]); long sum, sum_old; int thresh = 0; byte pd_state = 0; // idea copied from: https://stackoverflow.com/questions/20917019/how-to-implement-a-filter-like-scipy-signal-lfilter int* applyFilter(int* data) { const int lenCoef = sizeof(coefficients)/sizeof(coefficients[0]); static int result[len] = {}; for (int m = 0; m < len; m++) { double y = 0; for (int n = 0; n < lenCoef; n++) { if (m < n) { break; } y += coefficients[n] * data[m-n]; } result[m] = y; } return result; } void setup() { Serial.begin(115200); int* filteredDataset = applyFilter(rawData); sum = 0; pd_state = 0; int period = 0; // Autocorrelation for(int i = 0; i < len; i++) { sum_old = sum; sum = 0; for(int k = 0; k < len - i; k++) { sum += filteredDataset[k] * filteredDataset[k+i]; } // Peak Detect State Machine if (pd_state == 2 && (sum-sum_old) <= 0) { period = i; pd_state = 3; } if (pd_state == 1 && (sum > thresh) && (sum - sum_old) > 0) { pd_state = 2; } if (i == 0) { thresh = sum * 0.5; pd_state = 1; } } // Frequency identified in Hz if (period > 0 && period < INT_MAX) { Serial.println("Wohooo, here is your frequency:"); Serial.println(sample_freq/period); } else { Serial.println("Yeah, too much noise ... "); } } void loop() { }