spd-lorabees/Docs/peakdetect.c

/**
 * 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 ? 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 = (pos<vcount-2?values[pos+1]:values[pos]);

    float p = 0.5 * (a-c)/(a-2*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[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;
}