spd-lorabees/Src/audio.c

/*
 * audio.c
 *
 *  Created on: Dec 31, 2017
 *      Author: ondra
 */

#include "audio.h"
#include <stdint.h>
#include <stdbool.h>
#include "main.h"
#include "tim.h"
#include "dma.h"
#include "adc.h"
#include "stm32l0xx_hal.h"
#include <string.h>
#include "arm_math.h"
#include <math.h>
#include <float.h>
#include "arm_const_structs.h"
#include "debug.h"

#define print PRINTF

extern DMA_HandleTypeDef hdma_adc;
extern ADC_HandleTypeDef hadc;

void ftoa(float n, char *res, int afterpoint);

//static const float win_hamming_2048[] = {0.08000000f, 0.08000217f, 0.08000867f, 0.08001950f, 0.08003467f, 0.08005417f, 0.08007801f, 0.08010618f, 0.08013868f, 0.08017551f, 0.08021668f, 0.08026218f, 0.08031201f, 0.08036617f, 0.08042466f, 0.08048748f, 0.08055463f, 0.08062611f, 0.08070192f, 0.08078205f, 0.08086651f, 0.08095530f, 0.08104841f, 0.08114585f, 0.08124761f, 0.08135369f, 0.08146409f, 0.08157881f, 0.08169786f, 0.08182121f, 0.08194889f, 0.08208088f, 0.08221719f, 0.08235781f, 0.08250274f, 0.08265198f, 0.08280553f, 0.08296339f, 0.08312555f, 0.08329202f, 0.08346279f, 0.08363786f, 0.08381724f, 0.08400090f, 0.08418887f, 0.08438113f, 0.08457769f, 0.08477853f, 0.08498366f, 0.08519308f, 0.08540679f, 0.08562478f, 0.08584704f, 0.08607359f, 0.08630442f, 0.08653952f, 0.08677889f, 0.08702253f, 0.08727044f, 0.08752261f, 0.08777905f, 0.08803975f, 0.08830470f, 0.08857392f, 0.08884738f, 0.08912510f, 0.08940706f, 0.08969327f, 0.08998372f, 0.09027841f, 0.09057734f, 0.09088050f, 0.09118790f, 0.09149952f, 0.09181537f, 0.09213544f, 0.09245973f, 0.09278824f, 0.09312096f, 0.09345789f, 0.09379903f, 0.09414437f, 0.09449391f, 0.09484765f, 0.09520559f, 0.09556771f, 0.09593402f, 0.09630452f, 0.09667920f, 0.09705805f, 0.09744107f, 0.09782827f, 0.09821963f, 0.09861515f, 0.09901484f, 0.09941867f, 0.09982666f, 0.10023880f, 0.10065508f, 0.10107549f, 0.10150005f, 0.10192873f, 0.10236154f, 0.10279848f, 0.10323953f, 0.10368470f, 0.10413398f, 0.10458737f, 0.10504486f, 0.10550644f, 0.10597213f, 0.10644189f, 0.10691575f, 0.10739368f, 0.10787569f, 0.10836178f, 0.10885193f, 0.10934614f, 0.10984440f, 0.11034673f, 0.11085309f, 0.11136351f, 0.11187796f, 0.11239644f, 0.11291895f, 0.11344549f, 0.11397605f, 0.11451062f, 0.11504919f, 0.11559178f, 0.11613836f, 0.11668893f, 0.11724349f, 0.11780204f, 0.11836456f, 0.11893106f, 0.11950152f, 0.12007594f, 0.12065432f, 0.12123665f, 0.12182293f, 0.12241315f, 0.12300730f, 0.12360538f, 0.12420738f, 0.12481330f, 0.12542313f, 0.12603687f, 0.12665451f, 0.12727604f, 0.12790146f, 0.12853077f, 0.12916395f, 0.12980100f, 0.13044192f, 0.13108669f, 0.13173532f, 0.13238779f, 0.13304410f, 0.13370425f, 0.13436823f, 0.13503603f, 0.13570764f, 0.13638306f, 0.13706229f, 0.13774531f, 0.13843212f, 0.13912271f, 0.13981709f, 0.14051523f, 0.14121713f, 0.14192280f, 0.14263221f, 0.14334537f, 0.14406226f, 0.14478289f, 0.14550723f, 0.14623530f, 0.14696707f, 0.14770255f, 0.14844173f, 0.14918459f, 0.14993113f, 0.15068135f, 0.15143524f, 0.15219279f, 0.15295399f, 0.15371884f, 0.15448733f, 0.15525945f, 0.15603520f, 0.15681456f, 0.15759754f, 0.15838411f, 0.15917428f, 0.15996804f, 0.16076538f, 0.16156630f, 0.16237078f, 0.16317881f, 0.16399040f, 0.16480553f, 0.16562419f, 0.16644638f, 0.16727209f, 0.16810132f, 0.16893404f, 0.16977027f, 0.17060998f, 0.17145317f, 0.17229983f, 0.17314996f, 0.17400354f, 0.17486058f, 0.17572105f, 0.17658495f, 0.17745228f, 0.17832302f, 0.17919718f, 0.18007473f, 0.18095567f, 0.18183999f, 0.18272769f, 0.18361876f, 0.18451318f, 0.18541095f, 0.18631207f, 0.18721651f, 0.18812428f, 0.18903537f, 0.18994976f, 0.19086745f, 0.19178843f, 0.19271268f, 0.19364022f, 0.19457101f, 0.19550506f, 0.19644235f, 0.19738288f, 0.19832664f, 0.19927362f, 0.20022381f, 0.20117720f, 0.20213378f, 0.20309355f, 0.20405649f, 0.20502259f, 0.20599185f, 0.20696426f, 0.20793981f, 0.20891848f, 0.20990027f, 0.21088518f, 0.21187318f, 0.21286428f, 0.21385845f, 0.21485570f, 0.21585602f, 0.21685939f, 0.21786580f, 0.21887525f, 0.21988772f, 0.22090321f, 0.22192170f, 0.22294319f, 0.22396767f, 0.22499513f, 0.22602555f, 0.22705894f, 0.22809527f, 0.22913454f, 0.23017674f, 0.23122185f, 0.23226988f, 0.23332081f, 0.23437462f, 0.23543132f, 0.23649088f, 0.23755330f, 0.23861858f, 0.23968669f, 0.24075763f, 0.24183139f, 0.24290797f, 0.24398734f, 0.24506949f, 0.24615443f, 0.24724214f, 0.24833260f, 0.24942581f, 0.25052176f, 0.25162044f, 0.25272184f, 0.25382594f, 0.25493273f, 0.25604222f, 0.25715437f, 0.25826920f, 0.25938668f, 0.26050680f, 0.26162955f, 0.26275493f, 0.26388292f, 0.26501351f, 0.26614669f, 0.26728245f, 0.26842078f, 0.26956167f, 0.27070511f, 0.27185109f, 0.27299959f, 0.2741506

static const float win_hamming_1024[] = {0.08000000f, 0.08000868f, 0.08003470f, 0.08007808f, 0.08013881f, 0.08021689f, 0.08031231f, 0.08042507f, 0.08055517f, 0.08070260f, 0.08086736f, 0.08104944f, 0.08124883f, 0.08146552f, 0.08169951f, 0.08195079f, 0.08221935f, 0.08250518f, 0.08280827f, 0.08312860f, 0.08346617f, 0.08382096f, 0.08419296f, 0.08458215f, 0.08498853f, 0.08541206f, 0.08585275f, 0.08631057f, 0.08678550f, 0.08727753f, 0.08778663f, 0.08831280f, 0.08885600f, 0.08941623f, 0.08999345f, 0.09058764f, 0.09119879f, 0.09182687f, 0.09247186f, 0.09313373f, 0.09381245f, 0.09450801f, 0.09522037f, 0.09594951f, 0.09669540f, 0.09745802f, 0.09823733f, 0.09903330f, 0.09984591f, 0.10067512f, 0.10152090f, 0.10238323f, 0.10326206f, 0.10415737f, 0.10506912f, 0.10599728f, 0.10694181f, 0.10790267f, 0.10887984f, 0.10987326f, 0.11088292f, 0.11190876f, 0.11295075f, 0.11400885f, 0.11508302f, 0.11617322f, 0.11727941f, 0.11840154f, 0.11953958f, 0.12069348f, 0.12186319f, 0.12304868f, 0.12424990f, 0.12546680f, 0.12669934f, 0.12794747f, 0.12921114f, 0.13049031f, 0.13178493f, 0.13309495f, 0.13442031f, 0.13576098f, 0.13711690f, 0.13848801f, 0.13987427f, 0.14127562f, 0.14269202f, 0.14412340f, 0.14556972f, 0.14703091f, 0.14850693f, 0.14999772f, 0.15150322f, 0.15302337f, 0.15455813f, 0.15610742f, 0.15767120f, 0.15924939f, 0.16084195f, 0.16244882f, 0.16406992f, 0.16570521f, 0.16735462f, 0.16901808f, 0.17069554f, 0.17238693f, 0.17409219f, 0.17581125f, 0.17754405f, 0.17929052f, 0.18105060f, 0.18282422f, 0.18461131f, 0.18641181f, 0.18822565f, 0.19005275f, 0.19189306f, 0.19374650f, 0.19561301f, 0.19749250f, 0.19938492f, 0.20129018f, 0.20320822f, 0.20513897f, 0.20708234f, 0.20903828f, 0.21100670f, 0.21298753f, 0.21498070f, 0.21698613f, 0.21900374f, 0.22103346f, 0.22307522f, 0.22512893f, 0.22719452f, 0.22927190f, 0.23136101f, 0.23346177f, 0.23557408f, 0.23769788f, 0.23983308f, 0.24197961f, 0.24413738f, 0.24630631f, 0.24848632f, 0.25067733f, 0.25287925f, 0.25509200f, 0.25731549f, 0.25954966f, 0.26179440f, 0.26404964f, 0.26631528f, 0.26859125f, 0.27087746f, 0.27317382f, 0.27548025f, 0.27779665f, 0.28012295f, 0.28245905f, 0.28480486f, 0.28716030f, 0.28952528f, 0.29189971f, 0.29428350f, 0.29667656f, 0.29907879f, 0.30149011f, 0.30391043f, 0.30633966f, 0.30877770f, 0.31122447f, 0.31367986f, 0.31614379f, 0.31861617f, 0.32109689f, 0.32358588f, 0.32608303f, 0.32858825f, 0.33110144f, 0.33362251f, 0.33615137f, 0.33868792f, 0.34123206f, 0.34378370f, 0.34634275f, 0.34890909f, 0.35148265f, 0.35406332f, 0.35665100f, 0.35924560f, 0.36184702f, 0.36445515f, 0.36706991f, 0.36969120f, 0.37231891f, 0.37495294f, 0.37759320f, 0.38023958f, 0.38289200f, 0.38555034f, 0.38821450f, 0.39088439f, 0.39355991f, 0.39624095f, 0.39892741f, 0.40161920f, 0.40431620f, 0.40701833f, 0.40972547f, 0.41243752f, 0.41515439f, 0.41787596f, 0.42060215f, 0.42333283f, 0.42606792f, 0.42880731f, 0.43155089f, 0.43429856f, 0.43705022f, 0.43980576f, 0.44256509f, 0.44532808f, 0.44809465f, 0.45086469f, 0.45363809f, 0.45641474f, 0.45919455f, 0.46197741f, 0.46476321f, 0.46755185f, 0.47034322f, 0.47313722f, 0.47593374f, 0.47873268f, 0.48153393f, 0.48433739f, 0.48714294f, 0.48995049f, 0.49275993f, 0.49557115f, 0.49838404f, 0.50119851f, 0.50401444f, 0.50683172f, 0.50965026f, 0.51246994f, 0.51529067f, 0.51811232f, 0.52093480f, 0.52375800f, 0.52658181f, 0.52940612f, 0.53223084f, 0.53505585f, 0.53788104f, 0.54070632f, 0.54353157f, 0.54635669f, 0.54918156f, 0.55200609f, 0.55483017f, 0.55765369f, 0.56047654f, 0.56329862f, 0.56611982f, 0.56894003f, 0.57175916f, 0.57457708f, 0.57739370f, 0.58020891f, 0.58302261f, 0.58583468f, 0.58864502f, 0.59145352f, 0.59426009f, 0.59706461f, 0.59986697f, 0.60266708f, 0.60546483f, 0.60826010f, 0.61105280f, 0.61384282f, 0.61663005f, 0.61941439f, 0.62219574f, 0.62497399f, 0.62774903f, 0.63052076f, 0.63328907f, 0.63605387f, 0.63881504f, 0.64157249f, 0.64432610f, 0.64707578f, 0.64982142f, 0.65256291f, 0.65530016f, 0.65803307f, 0.66076151f, 0.66348541f, 0.66620464f, 0.66891911f, 0.67162872f, 0.67433337f, 0.67703295f, 0.67972735f, 0.68241649f, 0.68510026f, 0.68777855f, 0.69045126f, 0.69311830f

#define NUM_SAMPLES 1024
#define BIN_SIZE (44444.444f / (NUM_SAMPLES/2.0f))
#define PK_MINDIST 1.5f
#define PK_MAXFREQ 20000.0f



volatile bool dma_done = false;
void done_cb(DMA_HandleTypeDef*dma)
{
	dma_done = true;
}


void graph(uint32_t width, uint32_t height, float *values, uint32_t count)
{
  float max = FLT_MIN;
  float min = FLT_MAX;
  for (int i = 0; i < count; i++) {
    float f = values[i];
    if (f < min) min = f;
    if (f > max) max = f;
  }

  int xstep = count / width;

  float ystep = 1; //(float)(max - min) / (float)height;

  // this somehow doenst work now

  char buf[100];
  sprintf(buf, "min %d max %d, ystep %d, h %d\r\n", (int)min*1000, (int)max*1000, (int)ystep*1000, height);
  print(buf);

  for (int i = height-1; i >= 0; i--) {
    float thr = i*ystep;
    for (int j = 0; j < width; j++) {
    	float acu = 0;
    	int cnt = 0;
    	for (int k = j*xstep; k < (j+1)*xstep && k < count; k++, cnt++) {
    		acu += values[k];
    	}
    	acu /= (float)cnt;

      float sample = acu;//values[j*xstep];
      if (sample >= thr) {
        print("#");
      } else {
        print(" ");
      }
    }
    print("\r\n");
  }
}












/**
 * 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;
}


/**
 * 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);
    // safeguard
    if (p>0.5 || p<-0.5) p=0;

    pk->position = pos + p;
    pk->magnitude = b - 0.25 * (a - c) * p;
}

/**
 * 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 = {0}; // 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;

                // !!!! This constant must be adjusted if bin is resized
                if (pk.position >= (PK_MAXFREQ/BIN_SIZE)) {
                	// too high, discard
                	goto pk_done;
                }

                // try to fit it in the peak list
				{
					// first we look if there's one close enough to overwrite it or use instead
					for (uint32_t j = 0; j < pcount; j++) {
						if ((pk.position > peaks[j].position - PK_MINDIST) && (pk.position < peaks[j].position + PK_MINDIST)) { // this number determines the min distance of peaks in bin units
							// replace if we have better weight
							if (peaks[j].weight < pk.weight) {
								peaks[j] = pk;
							} // else discard it
							goto pk_done; // exit the for and skip the following for loop
						}
					}

					// look for a place for this new peak, shift what is behind it
					for (uint32_t j = 0; j < pcount; j++) {
						// peaks are sorted by weight
						if (pk.weight > peaks[j].weight) {
							// shift the tail to make room
							if (used_peaks > 0) {
								for (uint32_t k = used_peaks; k > j; k--) {
									peaks[k] = peaks[k-1];
								}
							}
							peaks[j] = pk;
							if (used_peaks < pcount) used_peaks++; // increment the counter if the list wasn't full yet and grew
							break;
						}
					}
				}

				pk_done:;
            }
        }
        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;
    noise /= vcount;

    // sort the peaks by magnitude (they are now sorted by weight, which is useful for detecting prominence but confusing for practical use later)
    for (uint32_t i = 0; i < pcount-1; i++) {
    	float bestmag = peaks[i].magnitude;
    	uint32_t bmpos = i;
    	for (uint32_t j = i+1; j < pcount; j++) {
    		if (peaks[j].magnitude > bestmag) {
    			bestmag = peaks[j].magnitude;
    			bmpos = j;
    		}
		}
    	if (bmpos != i) {
    		// we found a better peak in the tail, let's swap
    		pk = peaks[i];
    		peaks[i] = peaks[bmpos];
    		peaks[bmpos] = pk;
    	}
	}

    return noise;
}















union {
	uint16_t raw[NUM_SAMPLES];
	float flt[NUM_SAMPLES*2];
} samples;


void audio_capture(struct peak *peaks, uint32_t pcount, float *noise, float *totalpower)
{
	char buf[100];

	print("AUDIO SAMPLING ...\r\n");

	print("Starting capture...\r\n");

	HAL_StatusTypeDef rv = HAL_ADC_Start_DMA(&hadc, (void*)samples.raw, (uint32_t)NUM_SAMPLES);
	assert_param(rv == HAL_OK);

	//__enable_irq();

	hdma_adc.XferCpltCallback = done_cb;
	dma_done = false;

	rv = HAL_TIM_Base_Start(&htim2); // kick it off
	assert_param(rv == HAL_OK);

	print("Capture stared.\r\n");

	// wait ...
	while(!dma_done) {
		HAL_Delay(100);
		print("waiting... ");
	}

	print("\r\nCapture completed.\r\n");


	// basic scaling
	for (int i=NUM_SAMPLES-1; i >= 0; i--) {
		samples.flt[i] = (float)samples.raw[i]/2048.0f;
	}


	// remove DC offset
	float mean;
	arm_mean_f32(samples.flt, NUM_SAMPLES, &mean);
	for (int i = 0; i < NUM_SAMPLES; i++) {
		samples.flt[i] -= mean;
	}

	// zero out the second half (this is needed because otherwise the fft is not padded by zeros and doesn't work right)
	for (int i=NUM_SAMPLES; i < NUM_SAMPLES*2; i++) {
		samples.flt[i] = 0;
	}


	uint32_t bin_count = NUM_SAMPLES/2;
	uint32_t samp_count = NUM_SAMPLES;

	for (int i = samp_count - 1; i >= 0; i--) {
		samples.flt[i * 2 + 1] = 0;                   // imaginary
		samples.flt[i * 2] = samples.flt[i] * win_hamming_1024[i]; // real
	}

	print("FFT...\r\n");
	arm_cfft_f32(&arm_cfft_sR_f32_len1024, samples.flt, 0, true); // bit reversed FFT
	print("Magnitude...\r\n");
	arm_cmplx_mag_f32(samples.flt, samples.flt, bin_count); // get magnitude (extract real values)

	// normalize
	print("Scaling...\r\n");
	arm_scale_f32(samples.flt, (1.0f/bin_count)*1000.0f, samples.flt, bin_count);

	// now we have the spectrum computed in samples.flt

	//graph(128, 22, samples.flt, bin_count/8); // doesnt work for some reason

	// Find the peaks...

	float sum = 0;
	for(uint32_t i = 0; i < bin_count; i++) {
		sum += samples.flt[i];
	}
	*totalpower = sum;

	*noise = pkdetect(peaks, pcount, samples.flt, bin_count);

	for (int i = 0; i < pcount; i++) {
		sprintf(buf, "pk %d at %d Hz, mag ", i+1, (int)roundf((peaks[i].position) * BIN_SIZE));
		print(buf);
		ftoa(peaks[i].magnitude, buf, 6);
		print(buf);
		print("\r\n");
	}

	ftoa(*noise, buf, 6);
	print("Mean noise per bin = ");
	print(buf);
	print("\r\n");

}



// ftoa stuff ---------------

// reverses a string 'str' of length 'len'
void reverse(char *str, int len)
{
    int i=0, j=len-1, temp;
    while (i<j)
    {
        temp = str[i];
        str[i] = str[j];
        str[j] = temp;
        i++; j--;
    }
}

 // Converts a given integer x to string str[].  d is the number
 // of digits required in output. If d is more than the number
 // of digits in x, then 0s are added at the beginning.
int intToStr(int x, char str[], int d)
{
    int i = 0;
    while (x)
    {
        str[i++] = (x%10) + '0';
        x = x/10;
    }

    // If number of digits required is more, then
    // add 0s at the beginning
    while (i < d)
        str[i++] = '0';

    reverse(str, i);
    str[i] = '\0';
    return i;
}

// Converts a floating point number to string.
void ftoa(float n, char *res, int afterpoint)
{
    // Extract integer part
    int ipart = (int)n;

    // Extract floating part
    float fpart = n - (float)ipart;

    // convert integer part to string
    int i = intToStr(ipart, res, 0);

    // check for display option after point
    if (afterpoint != 0)
    {
        res[i] = '.';  // add dot

        // Get the value of fraction part upto given no.
        // of points after dot. The third parameter is needed
        // to handle cases like 233.007
        fpart = fpart * pow(10, afterpoint);

        intToStr((int)fpart, res + i + 1, afterpoint);
    }
}