B3M38SPD seminar project - beehive monitor with LoRa reporting
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 Raw Permalink Normal View History Unescape Escape

 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 ``` ```#include ``` ```#include ``` ```#include ``` ```#include ``` ```#include ``` ```#include ``` ``` ``` ```/** ``` ``` * 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 ? pos-len/2 : 0); ``` ``` uint32_t to = (pos < count-len/2 ? pos+len/2 : count-1); ``` ``` ``` ``` 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[pos-1]:values[pos]); ``` ``` float b = values[pos]; ``` ``` float c = (posposition = 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[k-1]); ``` ``` } ``` ``` 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 ? pos-pkexpand/2 : 0); ``` ``` uint32_t to = (pos < vcount-pkexpand/2 ? pos+pkexpand/2 : vcount-1); ``` ``` 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; ``` ```} ```