You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('') and can be up to 35 characters long.
189 lines
25 KiB
189 lines
25 KiB
6 years ago

/**



* Spectral peak detection algorithm with interpolation



*



* Written by Ondřej Hruška "MightyPork", Dec 30, 2017



*



* This source code is hereby released to the public domain



* for anyone to use for anything they want.



*/






#include <stdlib.h>



#include <stdio.h>



#include <string.h>



#include <stdint.h>



#include <stdbool.h>



#include <string.h>



#include <math.h>






/**



* Rolling average centered at a bin



*



* @param arr  array of bins



* @param count  number of bins



* @param pos  position we're intersted in



* @param len  size of the rolling window, centered around the position



* @return average within the window, excluding bin at position 'pos'



*/



static float ravg(float *arr, uint32_t count, uint32_t pos, uint32_t len)



{



// XXX this needs some adjustments, it's not perfectly centered






// if we're at the end or beginning, use only bins we have available



uint32_t from = (pos > len/2 ? poslen/2 : 0);



uint32_t to = (pos < countlen/2 ? pos+len/2 : count1);






float acu = 0;



for (uint32_t i = from; i <= to; i++) {



if (i == pos) continue;



acu += arr[i];



}



acu /= (to  from); // not +1 because we skip the middle






return acu;



}






/** A detected peak struct */



struct peak {



float position; // precise position, unit is 1 bin



float magnitude; // precise magnitude



float weight; // sorting weight (internal use)



};






/**



* Quadratic interpolation to find the real peak position and magnitude



*



* @param pk  peak struct to store the results in



* @param values  the bins array



* @param vcount  size of the bins array



* @param pos  position of the peak we're triyng to analyze



*/



static void qinterp(struct peak *pk, const float *values, uint32_t vcount, uint32_t pos)



{



float a = (pos>0?values[pos1]:values[pos]);



float b = values[pos];



float c = (pos<vcount2?values[pos+1]:values[pos]);






float p = 0.5 * (ac)/(a2*b+c);






pk>position = pos + p;



pk>magnitude = b  0.25 * (a  c) * p;



}






/** Copy a peak */



static void cpy_pk(struct peak *dest, const struct peak *src)



{



dest>position = src>position;



dest>magnitude = src>magnitude;



dest>weight = src>weight;



}






/**



* Detect peaks in a real float spectrum



*



* @param peaks  destination for the peak detect algorithm, peaks are sorted from the most important



* @param pcount  number of peaks to detect



* @param values  the spectrum as an array of bin magnitudes



* @param vcount  number of bins in the spectrum



* @return average level (excluding the peaks)



*/



float pkdetect(struct peak *peaks, uint32_t pcount, float *values, uint32_t vcount)



{



uint32_t used_peaks = 0;






// clear the table



for (uint32_t i = 0; i < pcount; i++) {



peaks[i].position



= peaks[i].magnitude



= peaks[i].weight = 0;



}






struct peak pk; // scratch peak



float prev = 0;



float sum = 0;



for (uint32_t i = 0; i < vcount; i++) {



float base = ravg(values, vcount, i, 32);



float raw = values[i];



float normed = raw / base;



if (i > 0) {



// difference from the previous bin (this serves as the primary peak detection factor)



float diff = normed  prev;






if (diff > 0) {



// find the precise position and magnitude



qinterp(&pk, values, vcount, i);



// weight for sorting the peaks



pk.weight = diff * pk.magnitude;






// try to fit it in the peak list



for (uint32_t j = 0; j < pcount; j++) {



if (pk.weight > peaks[j].weight) {



for (uint32_t k = used_peaks; k > j; k) {



cpy_pk(&peaks[k], &peaks[k1]);



}



cpy_pk(&peaks[j], &pk);



used_peaks++;



break;



}



}



}



}



prev = normed;



sum += raw; // this is rms



}






// now remove some area around the found peaks (NOTE: this will cause double removal if two peaks happened to be very close together)



float pksum = 0;



const uint32_t pkexpand = 4;



for (uint32_t i = 0; i < pcount; i++) {



uint32_t pos = (uint32_t)roundf(peaks[i].position);



uint32_t from = (pos > pkexpand/2 ? pospkexpand/2 : 0);



uint32_t to = (pos < vcountpkexpand/2 ? pos+pkexpand/2 : vcount1);



for (uint32_t j = from; j <= to; j++) {



pksum += values[j];



}



}






float noise = (sum  pksum);



if (noise < 0) noise = 0;



return noise / vcount;



}












// this is just for the demo



union smp_union {



uint8_t bytes[4096];



float floats[1024];



};






union smp_union smp;






int main (void)



{



uint8_t b[] = {



0x53,0x6C,0x46,0x39,0x31,0x91,0xF8,0x38,0xFA,0xF9,0x70,0x38,0xB6,0xB3,0xE8,0x38,0xB0,0xA8,0xA3,0x39,0x5C,0x98,0xAD,0x3A,0xE1,0x38,0x00,0x3B,0x4D,0x09,0x7D,0x3A,0x1A,0xD2,0x26,0x3B,0xF9,0x85,0xD7,0x3A,0x5E,0xBB,0x0B,0x3B,0xF0,0x04,0xAD,0x3A,0xDF,0x95,0xCE,0x3A,0xD1,0x94,0x2E,0x39,0x0F,0xE2,0x68,0x39,0x4C,0xE9,0x02,0x39,0xFC,0xAA,0x4B,0x3A,0xB4,0x56,0xE2,0x3A,0xF1,0xA4,0x1C,0x3A,0x7C,0x4A,0x12,0x3A,0x7A,0xE4,0xDF,0x3A,0x3B,0xD6,0x57,0x3A,0x0E,0x1E,0x35,0x38,0x66,0x52,0x32,0x39,0xD6,0x0F,0x08,0x39,0xB3,0xBC,0x6E,0x39,0x25,0x9A,0xD8,0x39,0xE0,0xC9,0xF9,0x3A,0xD0,0x09,0x66,0x3A,0xA2,0x79,0x30,0x39,0xA9,0x0F,0x6D,0x39,0xC1,0x90,0xFF,0x39,0x36,0x8B,0xE2,0x39,0x77,0x5C,0x37,0x39,0x26,0xA8,0x0F,0x3A,0x1C,0x75,0xD5,0x39,0x29,0xD1,0xA3,0x38,0x2C,0x32,0x65,0x39,0x4A,0x0F,0xC7,0x38,0x31,0xE9,0x1D,0x3A,0x31,0xB6,0x8F,0x3B,0x6F,0x8D,0x94,0x3B,0x0E,0x90,0x24,0x3A,0xAA,0x62,0x1A,0x3A,0xFB,0xF1,0x99,0x39,0x77,0xE7,0xE4,0x37,0x09,0x0D,0x1D,0x39,0x2E,0x7D,0x1F,0x38,0x86,0xC1,0x41,0x38,0x8D,0xA6,0x80,0x38,0x48,0xD5,0x25,0x38,0xB4,0xC6,0x9C,0x39,0x94,0xAE,0x1F,0x3A,0x3C,0xDF,0x4A,0x3A,0x4E,0xF6,0xE7,0x3A,0x18,0x28,0xCE,0x3A,0xB2,0xEB,0xD5,0x39,0x79,0xE3,0x7C,0x39,0xCF,0x20,0xD0,0x39,0x5C,0xD7,0x1F,0x39,0x19,0x70,0x00,0x39,0xD4,0xFD,0xA4,0x39,0x27,0x26,0x04,0x39,0x95,0xCA,0x93,0x38,0x0E,0x50,0x54,0x38,0xD6,0x89,0x32,0x38,0x3D,0x5A,0x33,0x38,0x34,0xC7,0x99,0x38,0x2F,0xF6,0x22,0x38,0x9A,0x71,0x97,0x38,0xBE,0x7A,0x82,0x38,0x7C,0x3F,0x1C,0x39,0xA8,0xFA,0x05,0x39,0x33,0xA1,0x07,0x39,0x1A,0xA6,0x39,0x39,0x83,0x9E,0x9D,0x38,0x24,0x7B,0x35,0x38,0x39,0xFA,0x28,0x38,0xF0,0x38,0x4B,0x38,0x32,0x2F,0x23,0x39,0x5D,0xBA,0x6E,0x38,0x9D,0x5D,0x7D,0x3A,0x20,0xCE,0x56,0x3A,0x3F,0xCD,0x4D,0x39,0x03,0x89,0x88,0x38,0x12,0xF9,0xB9,0x37,0x20,0xB8,0x8C,0x38,0x6F,0x9A,0x60,0x38,0x92,0x90,0x15,0x38,0xA3,0xB3,0xD9,0x38,0x3F,0x12,0x46,0x38,0x3F,0x0A,0x3F,0x38,0x3B,0x73,0xEB,0x37,0x2B,0xDC,0x46,0x38,0xED,0xA2,0xF5,0x37,0x5E,0x28,0xBE,0x38,0x1C,0x27,0xC8,0x38,0x2C,0x66,0x9C,0x37,0xCC,0x18,0x2E,0x37,0x4F,0x14,0xB9,0x37,0x19,0x72,0xBD,0x37,0xF6,0x24,0x85,0x37,0x72,0x3F,0xC9,0x37,0xEC,0x32,0xD7,0x34,0x32,0x77,0x52,0x37,0x61,0x7F,0x1A,0x37,0x7F,0x56,0xCF,0x37,0x96,0x8E,0x3F,0x38,0xE9,0xD3,0x37,0x39,0xA0,0xF5,0x60,0x39,0xE5,0x69,0x74,0x38,0xF6,0x15,0x78,0x37,0x74,0x4A,0xE2,0x37,0xEE,0x43,0x0A,0x38,0xFD,0x77,0xA6,0x37,0xDD,0xBD,0x96,0x37,0xF9,0x46,0xC1,0x37,0x1D,0x75,0x42,0x37,0xA8,0x2B,0x9E,0x36,0xC5,0xF5,0x88,0x37,0xA6,0xA6,0xE8,0x36,0x99,0x85,0x30,0x37,0x61,0x26,0x50,0x38,0x16,0x03,0xCE,0x37,0x6E,0x91,0x34,0x38,0x6F,0xC9,0x8C,0x38,0x74,0x03,0x02,0x38,0xE0,0xEF,0x8A,0x37,0x92,0xB6,0xFE,0x37,0x20,0x9E,0xE8,0x37,0x3D,0x15,0x9D,0x37,0xF7,0x03,0x9F,0x37,0x75,0xB9,0x8A,0x37,0xFD,0xE7,0xFA,0x36,0x76,0x6B,0x2B,0x37,0x36,0x09,0x84,0x36,0xC6,0x97,0x91,0x37,0x9D,0x7A,0xB4,0x37,0xFF,0xF3,0x97,0x37,0x61,0x97,0xC4,0x37,0x0C,0x42,0x39,0x37,0x65,0x10,0xBC,0x37,0x67,0xD7,0xCB,0x37,0x8A,0x7D,0xA7,0x37,0x06,0xBC,0x03,0x38,0xDA,0x35,0x7E,0x37,0x06,0x20,0x26,0x36,0x82,0x3E,0xAF,0x36,0x57,0x3E,0x14,0x37,0xED,0xFE,0x89,0x37,0x32,0xA4,0x9A,0x37,0x33,0x4E,0x05,0x37,0xDF,0x3F,0x02,0x37,0xDF,0xBE,0xD1,0x37,0xB8,0xA7,0xC2,0x37,0x7D,0x61,0xD8,0x36,0x27,0xA3,0xEC,0x36,0x6E,0x04,0x52,0x37,0x10,0xF7,0x29,0x37,0xA3,0x7E,0xD3,0x36,0xBC,0xA2,0xAC,0x37,0xD8,0x5D,0xCE,0x37,0x8F,0xD1,0xB7,0x37,0xCE,0x35,0x68,0x37,0x9D,0x14,0x82,0x37,0x86,0x54,0x4C,0x38,0x0C,0xB9,0xC3,0x37,0xE9,0xF1,0xB7,0x37,0x3C,0x7C,0xB2,0x37,0x8D,0xD4,0xD5,0x37,0x28,0xF3,0x97,0x37,0x57,0x92,0x70,0x37,0xCA,0x4E,0x68,0x36,0xED,0xC2,0x76,0x36,0xDC,0xDD,0x26,0x37,0x1A,0x70,0x91,0x37,0x86,0x64,0x65,0x37,0x9F,0xB2,0xB5,0x37,0xF5,0x6E,0xB0,0x37,0x10,0xB1,0xA4,0x37,0x7F,0x06,0x3B,0x37,0xDC,0x85,0xB9,0x37,0x3C,0xE1,0x30,0x38,0x12,0xF2,0x95,0x37,0xA8,0xB9,0x84,0x37,0x79,0xC8,0xBD,0x37,0x31,0xB9,0x95,0x37,0x92,0xFA,0x99,0x37,0x74,0x9E,0xBF,0x37,0xA0,0x52,0x2D,0x37,0x7B,0x56,0x43,0x37,0x3A,0xB3,0xBC,0x36,0x01,0xC6,0x19,0x37,0xD1,0x87,0x0E,0x38,0x77,0x9E,0x04,0x38,0x21,0xF1,0x60,0x37,0x37,0xA1,0xDB,0x37,0xE2,0xCC,0x00,0x38,0x52,0xF0,0x8C,0x37,0xCC,0x3A,0x73,0x37,0x28,0xF8,0x1A,0x37,0x19,0x73,0x80,0x37,0x36,0x79,0xBB,0x36,0x0D,0xA5,0x1F,0x37,0xAA,0x



};



memcpy(smp.bytes, b, 4096);



uint32_t sample_count = 2048;



uint32_t bins_count = 1024;






// Peaks go here



struct peak peaks[10];






// Magic * *# @ * / +



float floor = pkdetect(&peaks[0], 10, smp.floats, bins_count);






// Show the results...



float bin = 21.7;



for (int i = 0; i < 10; i++) {



printf("pk %d ... %f @ %f Hz (weight %f)\r\n", i,



peaks[i].magnitude,



peaks[i].position*bin,



peaks[i].weight);



}






printf("Noise power per bin is %f\r\n", floor);






return 0;



}
