And this is the double math version:
#define nelems(x) sizeof(x)/sizeof(*x)
const int Fs = 5000; // Sample Rate
const int D = 5; // Down sampling factor
float T = 1.0; // Acquisition Time sec
double meanValue = 0.0;
double stdValue = 0.0;
int volatile sampleInd = 0; // Global Buffer Location
short int * volatile rawSignal; // Acquisition RAM Buffer
double * polySignal[D]; // PolySignal branches
double * polyIQ[2]; // Quadrature filtered, downsampled, demodulated signals
double * polyResult; // Reconstructed, LP filtered signal
// I-Q quadrature oscillations
const double I[D] = { 2.000000000000000, // Cosine
0.618033988749895,
-1.618033988749895,
-1.618033988749895,
0.618033988749894};
const double Q[D] = {0.000000000000000, // Sine
1.902113032590307,
1.175570504584947,
-1.175570504584946,
-1.902113032590307};
// AntiAliasing filter coefficients :: FIR window Kaiser, Fs=5000, Fc=300
const double AAF[15] = {0.017862400522538,
0.033885458153087,
0.050891294195008,
0.067512846793389,
0.082326310000912,
0.094008030100344,
0.101484759026756,
0.104057802415933,
0.101484759026756,
0.094008030100344,
0.082326310000912,
0.067512846793389,
0.050891294195008,
0.033885458153087,
0.017862400522538};
double * polyAAF[D]; // Polyphase downsampled impulse responses
// LP filter coefficients :: FIR window Kaiser, Fs=1000, Fc=15
const double LP[21] = { 0.041447420224253,
0.043208077329578,
0.044830173046221,
0.046298913638535,
0.047600786034868,
0.048723722606653,
0.049657248993218,
0.050392612896925,
0.050922892035903,
0.051243079731859,
0.051350146923975,
0.051243079731859,
0.050922892035903,
0.050392612896925,
0.049657248993218,
0.048723722606653,
0.047600786034868,
0.046298913638535,
0.044830173046221,
0.043208077329578,
0.041447420224253};
bool volatile BUSY_STATE = false; // Internal state
unsigned long int timemicros;
void computeOutput(){
timemicros = micros();
rawSignal[0] = rawSignal[5]; // First value adjust
short int offset = 0; // Offset
for(int h=0;h<Fs*T;h++)
offset+=rawSignal[h];
offset/=Fs*T;
for (int i=0;i<Fs*T/D;i++) // Initialize arrays
{
polyIQ[0][i] = 0;
polyIQ[1][i] = 0;
}
for (int j=0;j<D;j++)
{
// DownSampling
for (int i=j;i<Fs*T;i+=5)
polySignal[j][i/5] = (double)(rawSignal[i]-offset); // Downsampling and offset removal
for (int i=j;i<nelems(AAF);i+=5)
polyAAF[j][i/5] = AAF[i];
// AntiAlias Filtering
filter(polyAAF[j],nelems(AAF)/D,polySignal[j],(int) Fs*T/D);
// Quadrature Demodulation
for (int i=0;i<Fs*T/D;i++)
{
polyIQ[0][i] += I[j]*(polySignal[j][i]);
polyIQ[1][i] += Q[j]*(polySignal[j][i]);
}
}
// Norm calculation :: sqrt(I^2+Q^2)
for (int i=0;i<Fs*T/D;i++)
polyResult[i] = (double) sqrt(polyIQ[0][i]*polyIQ[0][i] + polyIQ[1][i]*polyIQ[1][i]);
// LP filtering
filter((double*) LP,nelems(LP),(double*) polyResult,(int) Fs*T/D);
// Elaborate stats
statistics();
double convFactor = 2.0*4.0/65535.0; // 2 * FullScale / (2^16-1)
meanValue *= convFactor;
stdValue *= convFactor;
printResults();
polyFreeAlloc();
timemicros=micros()-timemicros;
Serial.println(timemicros);
}
bool polyAlloc()
{
if(NULL != rawSignal)
{
free(rawSignal); // Deallocate buffer memory
free(polyResult);
rawSignal = NULL;
polyResult= NULL;
}
rawSignal = (short int *)malloc(Fs*T*sizeof(short int)); // Dynamic Memory Allocation
polyResult = (double *)malloc(Fs*T/D*sizeof(double));
polyIQ[0] = (double *)malloc(Fs*T/D*sizeof(double));
polyIQ[1] = (double *)malloc(Fs*T/D*sizeof(double));
bool polys = true;
for (int j=0;j<D;j++)
{
polySignal[j]= (double *)malloc(Fs*T/D*sizeof(double));
polyAAF[j] = (double *)malloc(nelems(AAF)/D*sizeof(double));
polys &= polySignal[j] != NULL && polyAAF[j] != NULL;
}
return (rawSignal[0] != NULL && polyIQ[0] != NULL && polyIQ[1] != NULL && polyResult != NULL && polys);
}
void polyFreeAlloc()
{
for (int i=0;i<D;i++)
{
free(polySignal[i]);
polySignal[i] = NULL;
}
free(polyIQ[0]);
free(polyIQ[1]);
polyIQ[0] = NULL;
polyIQ[1] = NULL;
}
void filter(double *Num, int n, double *vect, int vLen)
{
double average = 0.0; // Initial conditions calculation
for(int h=0;h<vLen;h++)
average+=vect[h];
average/=(double)vLen;
double numbuf[n]; // Initialize FIFO FIR buffer
for(int h=0;h<n;h++)
numbuf[h]=average;
for (int h=0;h<vLen;h++){ // Filtering cycle
double FIR = 0.0;
for (int k=n-1;k>0;k--) // Cycle FIR buffer elements
numbuf[k]=numbuf[k-1];
numbuf[0] = vect[h]; // Push new element inside FIR buffer
for (int j=0;j<n;j++) // FIR convolution
FIR += numbuf[j]*Num[j];
vect[h] = FIR; // Output calculation
}
}