329 lines
8.2 KiB
C
329 lines
8.2 KiB
C
|
|
#include "dft.h"
|
|
#include <math.h>
|
|
#include <stdio.h>
|
|
#include <stdint.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
|
|
void DoDFT( float * outbins, float * frequencies, int bins, float * databuffer, int place_in_data_buffer, int size_of_data_buffer, float q )
|
|
{
|
|
int i, j;
|
|
for( i = 0; i < bins; i++ )
|
|
{
|
|
float freq = frequencies[i];
|
|
float phi = 0;
|
|
int sampleplace = place_in_data_buffer;
|
|
float advance = 3.14159*2.0/freq;
|
|
|
|
float binqtys = 0;
|
|
float binqtyc = 0;
|
|
|
|
for( j = 0; j <= freq * q; j++ )
|
|
{
|
|
float sample = databuffer[sampleplace];
|
|
sampleplace = (sampleplace-1+size_of_data_buffer)%size_of_data_buffer;
|
|
//printf( "%d\n", sampleplace );
|
|
float sv = sin( phi ) * sample;
|
|
float cv = cos( phi ) * sample;
|
|
|
|
binqtys += sv;
|
|
binqtyc += cv;
|
|
|
|
phi += advance;
|
|
}
|
|
|
|
float amp = sqrtf( binqtys * binqtys + binqtyc * binqtyc );
|
|
outbins[i] = amp / freq / q;
|
|
}
|
|
}
|
|
|
|
void DoDFTQuick( float * outbins, float * frequencies, int bins, const float * databuffer, int place_in_data_buffer, int size_of_data_buffer, float q, float speedup )
|
|
{
|
|
int i, j;
|
|
|
|
for( i = 0; i < bins; i++ )
|
|
{
|
|
int flirts = 0;
|
|
|
|
float freq = frequencies[i];
|
|
float phi = 0;
|
|
int ftq = freq * q;
|
|
int sampleplace = place_in_data_buffer;
|
|
float advance = 3.14159*2.0/freq;
|
|
|
|
float binqtys = 0;
|
|
float binqtyc = 0;
|
|
|
|
int skip = floor( ftq / speedup );
|
|
if( skip == 0 ) skip = 1;
|
|
advance *= skip;
|
|
|
|
for( j = 0; j <= ftq; j += skip )
|
|
{
|
|
float sample = databuffer[sampleplace];
|
|
sampleplace = (sampleplace-skip+size_of_data_buffer)%size_of_data_buffer;
|
|
//printf( "%d\n", sampleplace );
|
|
float sv = sinf( phi ) * sample;
|
|
float cv = cosf( phi ) * sample;
|
|
|
|
binqtys += sv;
|
|
binqtyc += cv;
|
|
|
|
phi += advance;
|
|
flirts++;
|
|
}
|
|
|
|
float amp = sqrtf( binqtys * binqtys + binqtyc * binqtyc );
|
|
outbins[i] = amp / freq / q * skip;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
////////////////////////////DFT Progressive is for embedded systems, primarily.
|
|
|
|
|
|
static float * gbinqtys;
|
|
static float * gbinqtyc;
|
|
static float * phis;
|
|
static float * gfrequencies;
|
|
static float * goutbins;
|
|
static float * lastbins;
|
|
static float * advances;
|
|
|
|
static int gbins;
|
|
static float gq;
|
|
static float gspeedup;
|
|
|
|
#define PROGIIR .005
|
|
|
|
void HandleProgressive( float sample )
|
|
{
|
|
int i;
|
|
|
|
for( i = 0; i < gbins; i++ )
|
|
{
|
|
float thiss = sinf( phis[i] ) * sample;
|
|
float thisc = cosf( phis[i] ) * sample;
|
|
|
|
float s = gbinqtys[i] = gbinqtys[i] * (1.-PROGIIR) + thiss * PROGIIR;
|
|
float c = gbinqtyc[i] = gbinqtyc[i] * (1.-PROGIIR) + thisc * PROGIIR;
|
|
|
|
phis[i] += advances[i];
|
|
if( phis[i] > 6.283 ) phis[i]-=6.283;
|
|
|
|
goutbins[i] = sqrtf( s * s + c * c );
|
|
}
|
|
}
|
|
|
|
|
|
void DoDFTProgressive( float * outbins, float * frequencies, int bins, const float * databuffer, int place_in_data_buffer, int size_of_data_buffer, float q, float speedup )
|
|
{
|
|
int i;
|
|
static int last_place;
|
|
|
|
if( gbins != bins )
|
|
{
|
|
if( gbinqtys ) free( gbinqtys );
|
|
if( gbinqtyc ) free( gbinqtyc );
|
|
if( phis ) free( phis );
|
|
if( lastbins ) free( lastbins );
|
|
if( advances ) free( advances );
|
|
|
|
gbinqtys = malloc( sizeof(float)*bins );
|
|
gbinqtyc = malloc( sizeof(float)*bins );
|
|
phis = malloc( sizeof(float)*bins );
|
|
lastbins = malloc( sizeof(float)*bins );
|
|
advances = malloc( sizeof(float)*bins );
|
|
|
|
memset( gbinqtys, 0, sizeof(float)*bins );
|
|
memset( gbinqtyc, 0, sizeof(float)*bins );
|
|
memset( phis, 0, sizeof(float)*bins );
|
|
memset( lastbins, 0, sizeof(float)*bins );
|
|
|
|
}
|
|
memcpy( outbins, lastbins, sizeof(float)*bins );
|
|
|
|
for( i = 0; i < bins; i++ )
|
|
{
|
|
float freq = frequencies[i];
|
|
advances[i] = 3.14159*2.0/freq;
|
|
}
|
|
|
|
gbins = bins;
|
|
gfrequencies = frequencies;
|
|
goutbins = outbins;
|
|
gspeedup = speedup;
|
|
gq = q;
|
|
|
|
place_in_data_buffer = (place_in_data_buffer+1)%size_of_data_buffer;
|
|
|
|
int didrun = 0;
|
|
for( i = last_place; i != place_in_data_buffer; i = (i+1)%size_of_data_buffer )
|
|
{
|
|
float fin = ((float)((int)(databuffer[i] * 127))) / 127.0; //simulate 8-bit input (it looks FINE!)
|
|
HandleProgressive( fin );
|
|
didrun = 1;
|
|
}
|
|
last_place = place_in_data_buffer;
|
|
|
|
if( didrun )
|
|
{
|
|
memcpy( lastbins, outbins, sizeof(float)*bins );
|
|
}
|
|
|
|
/* for( i = 0; i < bins; i++ )
|
|
{
|
|
printf( "%0.2f ", outbins[i]*100 );
|
|
}
|
|
printf( "\n" );*/
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/////////////////////////////INTEGER DFT
|
|
|
|
|
|
|
|
|
|
#define PROGIIR .005
|
|
|
|
//NOTES to self:
|
|
//
|
|
// Let's say we want to try this on an AVR.
|
|
// 24 bins, 5 octaves = 120 bins.
|
|
// 20 MHz clock / 4.8k sps = 4096 IPS = 34 clocks per bin = :(
|
|
// We can do two at the same time, this frees us up some
|
|
|
|
static uint8_t donefirstrun;
|
|
static int8_t sintable[512]; //Actually [sin][cos] pairs.
|
|
|
|
|
|
//LDD instruction on AVR can read with constant offset. We can set Y to be the place in the buffer, and read with offset.
|
|
static uint16_t * datspace; //(advances,places,isses,icses)
|
|
|
|
//
|
|
void HandleProgressiveInt( int8_t sample1, int8_t sample2 )
|
|
{
|
|
int i;
|
|
uint16_t startpl = 0;
|
|
int16_t ts, tc;
|
|
int16_t tmp1;
|
|
int8_t s1, c1;
|
|
uint16_t ipl, localipl, adv;
|
|
|
|
|
|
//startpl maps to 'Y'
|
|
//
|
|
|
|
|
|
//Estimated 68 minimum instructions... So for two pairs each... just under 5ksps, theoretical.
|
|
//Running overall at ~2kHz.
|
|
for( i = 0; i < gbins; i++ ) //Loop, fixed size = 3 + 2 cycles 5
|
|
{
|
|
//12 cycles MIN
|
|
adv = datspace[startpl++]; //Read, indirect from RAM (and increment) 2+2 cycles 4
|
|
ipl = datspace[startpl++]; //Read, indirect from RAM (and increment) 2+2 cycles 4
|
|
|
|
//13 cycles MIN
|
|
ipl += adv; //Advance, 16bit += 16bit, 1 + 1 cycles 2
|
|
localipl = (ipl>>8)<<1; //Select upper 8 bits 1 cycles 1
|
|
|
|
// need to load Z with 'sintable' and add localipl 2
|
|
s1 = sintable[localipl++]; //Read s1 component out of table. 2+2 cycles 2
|
|
c1 = sintable[localipl++]; //Read c1 component out of table. 2 cycles 2
|
|
|
|
ts = (s1 * sample1); // 8 x 8 multiply signed + copy R1 out. zero MSB ts 2
|
|
tc = (c1 * sample1); // 8 x 8 multiply signed + copy R1 out. zero MSB tc 2
|
|
|
|
|
|
//15 cycles MIN
|
|
ipl += adv; //Advance, 16bit += 16bit, 1 + 1 cycles 2
|
|
localipl = (ipl>>8)<<1; //Select upper 8 bits 1 cycles 1
|
|
|
|
// need to load Z with 'sintable' and add localipl 2
|
|
s1 = sintable[localipl++]; //Read s1 component out of table. 2 cycles 2
|
|
c1 = sintable[localipl++]; //Read c1 component out of table. 2 cycles 2
|
|
|
|
ts += (s1 * sample2); // 8 x 8 multiply signed + add R1 out. 3
|
|
tc += (c1 * sample2); // 8 x 8 multiply signed + add R1 out. 3
|
|
|
|
|
|
//Add TS and TC to the datspace stuff. (24 instructions)
|
|
tmp1 = datspace[startpl]; //Read out, sin component. 4
|
|
tmp1 -= tmp1>>6; //Subtract from the MSB (with carry) 2
|
|
tmp1 += ts>>6; //Add MSBs with carry 2
|
|
|
|
datspace[startpl++] = tmp1; //Store values back 4
|
|
|
|
tmp1 = datspace[startpl]; //Read out, sin component. 4
|
|
tmp1 -= tmp1>>6; //Subtract from the MSB (with carry) 2
|
|
tmp1 += tc>>6; //Add MSBs with carry 2
|
|
|
|
datspace[startpl++] = tmp1; //Store values back 4
|
|
|
|
datspace[startpl-3] = ipl; //Store values back 4
|
|
}
|
|
}
|
|
|
|
|
|
void DoDFTProgressiveInteger( float * outbins, float * frequencies, int bins, const float * databuffer, int place_in_data_buffer, int size_of_data_buffer, float q, float speedup )
|
|
{
|
|
int i;
|
|
static int last_place;
|
|
|
|
if( !donefirstrun )
|
|
{
|
|
donefirstrun = 1;
|
|
for( i = 0; i < 256; i++ )
|
|
{
|
|
sintable[i*2+0] = (int8_t)((sinf( i / 256.0 * 6.283 ) * 127.0));
|
|
sintable[i*2+1] = (int8_t)((cosf( i / 256.0 * 6.283 ) * 127.0));
|
|
}
|
|
}
|
|
|
|
if( gbins != bins )
|
|
{
|
|
gbins = bins;
|
|
if( datspace ) free( datspace );
|
|
datspace = malloc( bins * 2 * 4 );
|
|
}
|
|
|
|
|
|
for( i = 0; i < bins; i++ )
|
|
{
|
|
float freq = frequencies[i];
|
|
datspace[i*4] = 65536.0/freq;
|
|
}
|
|
|
|
|
|
for( i = last_place; i != ( place_in_data_buffer&0xffffe ); i = (i+2)%size_of_data_buffer )
|
|
{
|
|
int8_t ifr1 = (int8_t)( ((databuffer[i+0]) ) * 127 );
|
|
int8_t ifr2 = (int8_t)( ((databuffer[i+1]) ) * 127 );
|
|
// printf( "%d %d\n", i, place_in_data_buffer&0xffffe );
|
|
HandleProgressiveInt( ifr1, ifr2 );
|
|
}
|
|
|
|
last_place = place_in_data_buffer&0xfffe;
|
|
|
|
//Extract bins.
|
|
for( i = 0; i < bins; i++ )
|
|
{
|
|
int16_t isps = datspace[i*4+2];
|
|
int16_t ispc = datspace[i*4+3];
|
|
int16_t mux = ( (isps/256) * (isps/256)) + ((ispc/256) * (ispc/256));
|
|
// printf( "%d (%d %d)\n", mux, isps, ispc );
|
|
outbins[i] = sqrt( mux )/100.0;
|
|
}
|
|
// printf( "\n");
|
|
}
|
|
|
|
|