//КИХ фильтр работающий по DEV_PeriodSamples количеству выборок. Коэффициенты предварительно рассчитываются в calcFiltCoef
//errCode 0x3302, 0x3303
float filterDSP(int sample, Filt *filt)
{
float inp;
//float in;
//int index=0;
fifo_int_push(&(filt->samples), sample); //signalShiftInt(filt->samples , sample, DEV_PeriodSamples);
// if (getErr()!=0)
// {
// setErr_str("filter");
// return 0.0;
// }
// filt->result =0;
// for (index = 0; index < DEV_PeriodSamples; index++) //TODO: переписать под CMSIS в Keil
// {
// filt->result += filt->filtCoef[index] * fifo_int_oldnumber(&(filt->samples), DEV_PeriodSamples - 1 - index); //filt->samples[DEV_PeriodSamples - 1 - index];
// if (getErr()!=0)
// {
// setErr_str("filter");
// return 0.0;
// }
// }
inp = (float)sample;
arm_fir_f32(&S, &inp, &(filt->result), 1);
filt->result *= 2.0 / DEV_PeriodSamples;//Множитель для алгоритма Фурье
return filt->result;
}
/**
* @brief [calculate equalized output samples]
* @details [this routine uses fir lowpass filters and delays to split off the spectrum
* into the specified bands.
* The input signal is low pass filtered to split the spectrum, and also separately delayed to
* keep the input in phase with the output of the filters. This delayed input is used to find the
* rest of the spectrum not being filtered, by subtracting the input by the filtered samples. So using
* two filters allows us to split off 3 different bands: the band below and the band above the first lowpass
* filter, and the band below and the band above the second lowpass filter. The band above the first lowpass
* and the band below the second lowpass is the same band.]
*
* @param D1 [pointer to the delay struct]
* @param D2 [pointer to the delay struct]
* @param D3 [pointer to the delay struct]
* @param Q [pointer to the eq struct]
* @param input [buffer containing samples to work on]
*/
void calc_eq(DELAY_T * D1, DELAY_T * D2, DELAY_T * D3, EQ_T * Q, float * input) {
int i, j, k;
float mid_input[Q->block_size];
// LOW BAND ------------------------------------------------------------------------------------------------
// calculate low band output with no gain
// lowpass with cutoff of 350Hz
arm_fir_f32(&(Q->S_low), input, Q->low_band_out, Q->block_size);
// delay filter output to stay in phase with mid and high band for reconstructing output
calc_delay(0, D1, Q->low_band_out); // D1->output is now the final low band output to be reconstructed
// MID BAND ------------------------------------------------------------------------------------------------
// delay input signal to stay in phase for mid band calculation
calc_delay(0, D2, input); // 0 is to output just the delayed signal
// input for mid band is the delayed signal minus the low band
// this gives the samples for the rest of the spectrum that the low band doesn't cover
for(j = 0; j < Q->block_size; j++) {
mid_input[j] = D2->output[j] - Q->low_band_out[j];
}
// lowpass with cutoff of 1050Hz
// this contains the band from the cutoff of the low band, to 1050Hz
arm_fir_f32(&(Q->S_mid), mid_input, Q->mid_band_out, Q->block_size);
// HIGH BAND -----------------------------------------------------------------------------------------------
// delay signal to stay in phase for high band calculation (note the input was already delayed once for the mid band calc)
calc_delay(0, D3, mid_input); // delay the input to the mid filter once more
// the high band is the input going into the mid filter delayed, and then subtracted from the mid filter output
// this gives the samples for the rest of the spectrum that the low and mid band doesn't cover
for(k = 0; k < Q->block_size; k++) {
Q->high_band_out[k] = D3->output[k] - Q->mid_band_out[k];
}
// calculate block of equalized output samples -------------------------------------------------------------
for(i = 0; i < Q->block_size; i++) {
// output is the output of each band scaled by the band gain and added together
Q->output[i] = 0.6 * ((Q->low_scale * D1->output[i]) + (Q->mid_scale * Q->mid_band_out[i]) + (Q->high_scale * Q->high_band_out[i]));
}
}
int32_t main(void)
{
uint32_t i;
arm_fir_instance_f32 S;
arm_status status;
float32_t *inputF32, *outputF32;
/* Initialize input and output buffer pointers */
inputF32 = &testInput_f32_1kHz_15kHz[0];
outputF32 = &testOutput[0];
/* Call FIR init function to initialize the instance structure. */
arm_fir_init_f32(&S, NUM_TAPS, (float32_t *)&firCoeffs32[0], &firStateF32[0], blockSize);
/* ----------------------------------------------------------------------
** Call the FIR process function for every blockSize samples
** ------------------------------------------------------------------- */
for(i=0; i < numBlocks; i++)
{
Enable_Performance_Monitor(0); // Init PMU
Performance_Monitor_Start(0); // start PMU counters
arm_fir_f32(&S, inputF32 + (i * blockSize), outputF32 + (i * blockSize), blockSize);
Performance_Monitor_Stop(0); // stop PMU counters
PMU_counter0_result[i] = Performance_Monitor_Read_Counter0(0);
PMU_counter1_result[i] = Performance_Monitor_Read_Counter1(0);
PMU_counter2_result[i] = Performance_Monitor_Read_Counter2(0);
PMU_cycle_count[i] = Performance_Monitor_Read_CycleCount(0);
}
/* ----------------------------------------------------------------------
** Compare the generated output against the reference output computed
** in MATLAB.
** ------------------------------------------------------------------- */
snr = arm_snr_f32(&refOutput[0], &testOutput[0], TEST_LENGTH_SAMPLES);
if (snr < SNR_THRESHOLD_F32)
{
status = ARM_MATH_TEST_FAILURE;
}
else
{
status = ARM_MATH_SUCCESS;
}
/* ----------------------------------------------------------------------
** Loop here if the signal does not match the reference output.
** ------------------------------------------------------------------- */
if( status != ARM_MATH_SUCCESS)
{
while(1);
}
}
int main(void)
{
uint32_t i;
arm_fir_instance_f32 S;
arm_status status;
float32_t *inputF32, *outputF32;
/* Initialize input and output buffer pointers */
inputF32 = &testInput_f32_1kHz_15kHz[0];
outputF32 = &testOutput[0];
/* Call FIR init function to initialize the instance structure. */
arm_fir_init_f32(&S, NUM_TAPS, (float32_t *)&firCoeffs32[0], &firStateF32[0], blockSize);
/* ----------------------------------------------------------------------
** Call the FIR process function for every blockSize samples
** ------------------------------------------------------------------- */
for(i=0; i < numBlocks; i++)
{
arm_fir_f32(&S, inputF32 + (i * blockSize), outputF32 + (i * blockSize), blockSize);
}
/* ----------------------------------------------------------------------
** Compare the generated output against the reference output computed
** in MATLAB.
** ------------------------------------------------------------------- */
snr = arm_snr_f32(&refOutput[0], &testOutput[0], TEST_LENGTH_SAMPLES);
if (snr < SNR_THRESHOLD_F32)
{
status = ARM_MATH_TEST_FAILURE;
}
else
{
status = ARM_MATH_SUCCESS;
}
/* ----------------------------------------------------------------------
** Loop here if the signal does not match the reference output.
** ------------------------------------------------------------------- */
if( status != ARM_MATH_SUCCESS)
{
while(1);
}
while(1); /* main function does not return */
}
void FIRprocessing(uint32_t* input, float32_t* output) {
arm_fir_instance_f32 S;
input = &g_ulADCValues[0];
output = &g_fFIRResult[0];
arm_fir_init_f32(&S, NUM_TAPS, (float32_t*) &firCoeffs32[0],
&firStateF32[0], FilterNode[0].blockSize);
uint32_t i = 0;
for (i = 0; i < FilterNode[0].blockSize; i++)
arm_fir_f32(&S, (float32_t*) input + (i * FilterNode[0].blockSize),
output + (i * FilterNode[0].blockSize),
FilterNode[0].blockSize);
setAgainSampling();
}
void PPG_Filter(uint16_t *Input, uint16_t *Output)
{
uint8_t i;
float32_t input[s_PPG__FIR_BLOCK_SIZE], output[s_PPG__FIR_BLOCK_SIZE];
for(i = 0; i < s_PPG__FIR_BLOCK_SIZE; i++) {
input[i] = (float32_t)Input[i];
}
arm_fir_f32(&g_Ppg_FirInst, input, output, s_PPG__FIR_BLOCK_SIZE);
for(i = 0; i < s_PPG__FIR_BLOCK_SIZE; i++) {
Output[i] = (uint16_t)output[i];
}
}
void proces_buffer(void)
{
int ii;
uint32_t *txbuf, *rxbuf;
if(tx_proc_buffer == PING) txbuf = dma_tx_buffer_ping;
else txbuf = dma_tx_buffer_pong;
if(rx_proc_buffer == PING) rxbuf = dma_rx_buffer_ping;
else rxbuf = dma_rx_buffer_pong;
for (ii=0 ; ii<(DMA_BUFFER_SIZE) ; ii++){
x[ii] = (float32_t)(prbs());
}
arm_fir_f32(&S,x,y,DMA_BUFFER_SIZE);
for (ii=0 ; ii<(DMA_BUFFER_SIZE) ; ii++){
*txbuf++ = (((short)(y[ii])<<16 & 0xFFFF0000)) + ((short)(y[ii]) & 0x0000FFFF);
}
tx_buffer_empty = 0;
rx_buffer_full = 0;
}
int32_t main(void)
{
uint32_t i;
arm_status status;
uint32_t index;
float32_t minValue;
/* Initialize the LMSNorm data structure */
arm_lms_norm_init_f32(&lmsNorm_instance, NUMTAPS, lmsNormCoeff_f32, lmsStateF32, MU, BLOCKSIZE);
/* Initialize the FIR data structure */
arm_fir_init_f32(&LPF_instance, NUMTAPS, (float32_t *)FIRCoeff_f32, firStateF32, BLOCKSIZE);
/* ----------------------------------------------------------------------
* Loop over the frames of data and execute each of the processing
* functions in the system.
* ------------------------------------------------------------------- */
for(i=0; i < NUMFRAMES; i++)
{
/* Read the input data - uniformly distributed random noise - into wire1 */
arm_copy_f32(testInput_f32 + (i * BLOCKSIZE), wire1, BLOCKSIZE);
/* Execute the FIR processing function. Input wire1 and output wire2 */
arm_fir_f32(&LPF_instance, wire1, wire2, BLOCKSIZE);
/* Execute the LMS Norm processing function*/
arm_lms_norm_f32(&lmsNorm_instance, /* LMSNorm instance */
wire1, /* Input signal */
wire2, /* Reference Signal */
wire3, /* Converged Signal */
err_signal, /* Error Signal, this will become small as the signal converges */
BLOCKSIZE); /* BlockSize */
/* apply overall gain */
arm_scale_f32(wire3, 5, wire3, BLOCKSIZE); /* in-place buffer */
}
status = ARM_MATH_SUCCESS;
/* -------------------------------------------------------------------------------
* Test whether the error signal has reached towards 0.
* ----------------------------------------------------------------------------- */
arm_abs_f32(err_signal, err_signal, BLOCKSIZE);
arm_min_f32(err_signal, BLOCKSIZE, &minValue, &index);
if (minValue > DELTA_ERROR)
{
status = ARM_MATH_TEST_FAILURE;
}
/* ----------------------------------------------------------------------
* Test whether the filter coefficients have converged.
* ------------------------------------------------------------------- */
arm_sub_f32((float32_t *)FIRCoeff_f32, lmsNormCoeff_f32, lmsNormCoeff_f32, NUMTAPS);
arm_abs_f32(lmsNormCoeff_f32, lmsNormCoeff_f32, NUMTAPS);
arm_min_f32(lmsNormCoeff_f32, NUMTAPS, &minValue, &index);
if (minValue > DELTA_COEFF)
{
status = ARM_MATH_TEST_FAILURE;
}
/* ----------------------------------------------------------------------
* Loop here if the signals did not pass the convergence check.
* This denotes a test failure
* ------------------------------------------------------------------- */
if( status != ARM_MATH_SUCCESS)
{
while(1);
}
}
void main (void)
{
/*开硬件浮点*/
SCB->CPACR |=((3UL << 10*2)|(3UL << 11*2)); /* set CP10 and CP11 Full Access */
uint16 flag;
uint16 i,j;
DisableInterrupts;
LCD_init(1);
Disp_single_colour(Black);
LCD_PutString(10, 50,"Frequency: ", White, Black);
LCD_PutString(145, 50," KHz", White, Black);
LCD_PutString(10, 80,"Power: ", White, Black);
LCD_PutString(145, 80," W", White, Black);
LCD_PutString(10, 110,"Amplify: ", White, Black);
LCD_PutString(165, 110,"Restrain: ", White, Black);
init_ADC();
init_DAC();
init_DMA();
init_PDB();
init_PIT();
init_gpio_PE24();
EnableInterrupts;
LPLD_LPTMR_DelayMs(100);
flag = Result_flag;
uint16 ShowAFlag = 0;
uint16 ShowBFlag = 0;
uint16 ShowCFlag = 0;
arm_fir_init_f32(&S, NUM_TAPS, (float32_t *)&firCoeffs32[0], &firStateF32[0], blockSize);
while(1)
{
if( flag==Result_flag && Result_flag == 0)
{
if(++ShowAFlag<10)
{
for(j = 0;j<LENGTH;j++)
testInput_x[j*2] = Result_A[j];
for(j = 0;j<LENGTH;j++)
testInput_x[j*2+1] = 0;
arm_cfft_f32(&arm_cfft_sR_f32_len2048, testInput_x, ifftFlag, doBitReverse);
/* Process the data through the Complex Magnitude Module for
calculating the magnitude at each bin */
arm_cmplx_mag_f32(testInput_x, testOutput, fftSize);
testOutput[0] = 0;
/* Calculates maxValue and returns corresponding BIN value */
arm_max_f32(testOutput, fftSize, &maxValue, &testIndex);
}
else
{
ShowAFlag = 0;
if(starfir !=2 )
LCD_Put_Float(100, 50,"",testIndex*40.0/2048, White, Black);
}
if(starfir == 1)
{
PTE24_O = 1;
for(j = 0;j<LENGTH;j++)
firInput[j] = Result_A[j];
inputF32 = &firInput[0];
outputF32 = &firOutput[0];
for(i=0; i < numBlocks; i++)
arm_fir_f32(&S, inputF32 + (i * blockSize), outputF32 + (i * blockSize), blockSize);
for(j = 0;j<LENGTH;j++)
Result_A[j] = firOutput[j];
PTE24_O = 0;
}
flag = 1;
}
else if(flag==Result_flag && Result_flag == 1)
{
if(starfir !=2 )
{
if(++ShowBFlag<10)
{
power = 0;
for(i=0;i<LENGTH;i++)
power+=((Result_B[i] - OFFEST)/1241.0)*((Result_B[i] - OFFEST)/1241.0)*90*MyDb/8.0;
power = power/LENGTH;
}
else
{
ShowBFlag = 0;
LCD_Put_Float(100, 80,"",power, White, Black);
}
}
else
{
for(i = 0;i<160;i++)
{
FFT_RESULT_NEW[i] = testOutput[i*6]/FFT_VALUE;
if(FFT_RESULT_NEW[i]>239) FFT_RESULT_NEW[i] = 239;
}
}
// {
// for(j = 0;j<LENGTH;j++)
// testInput_x[j*2] = Result_B[j];
// for(j = 0;j<LENGTH;j++)
// testInput_x[j*2+1] = 0;
//
// arm_cfft_f32(&arm_cfft_sR_f32_len2048, testInput_x, ifftFlag, doBitReverse);
//
// /* Process the data through the Complex Magnitude Module for
// calculating the magnitude at each bin */
// arm_cmplx_mag_f32(testInput_x, testOutput, fftSize);
//
// testOutput[0] = 0;
// /* Calculates maxValue and returns corresponding BIN value */
// arm_max_f32(testOutput, fftSize, &maxValue, &testIndex);
// }
if(starfir == 1)
{
PTE24_O = 1;
for(j = 0;j<LENGTH;j++)
firInput[j] = Result_B[j];
inputF32 = &firInput[0];
outputF32 = &firOutput[0];
for(i=0; i < numBlocks; i++)
arm_fir_f32(&S, inputF32 + (i * blockSize), outputF32 + (i * blockSize), blockSize);
for(j = 0;j<LENGTH;j++)
Result_B[j] = firOutput[j];
PTE24_O = 0;
}
flag = 2;
}
else if(flag==Result_flag && Result_flag == 2)
{
//
// {
// for(j = 0;j<LENGTH;j++)
// testInput_x[j*2] = Result_C[j];
// for(j = 0;j<LENGTH;j++)
// testInput_x[j*2+1] = 0;
//
// arm_cfft_f32(&arm_cfft_sR_f32_len2048, testInput_x, ifftFlag, doBitReverse);
//
// /* Process the data through the Complex Magnitude Module for
// calculating the magnitude at each bin */
// arm_cmplx_mag_f32(testInput_x, testOutput, fftSize);
//
// testOutput[0] = 0;
// /* Calculates maxValue and returns corresponding BIN value */
// arm_max_f32(testOutput, fftSize, &maxValue, &testIndex);
// }
if(starfir == 1)
{
// PTE24_O = 1;
for(j = 0;j<LENGTH;j++)
firInput[j] = Result_C[j];
inputF32 = &firInput[0];
outputF32 = &firOutput[0];
for(i=0; i < numBlocks; i++)
arm_fir_f32(&S, inputF32 + (i * blockSize), outputF32 + (i * blockSize), blockSize);
for(j = 0;j<LENGTH;j++)
Result_C[j] = firOutput[j];
// PTE24_O = 0;
}
if(starfir != 2)
{
if(++ShowCFlag<5)
{
}
else
{
if(ShowMenu)
{
Disp_single_colour(Black);
LCD_PutString(10, 50,"Frequency: ", White, Black);
LCD_PutString(145, 50," KHz", White, Black);
LCD_PutString(10, 80,"Power: ", White, Black);
LCD_PutString(145, 80," W", White, Black);
LCD_PutString(10, 110,"Amplify: ", White, Black);
LCD_PutString(165, 110,"Restrain: ", White, Black);
ShowMenu = 0;
}
LCD_Put_Float(100, 110,"",MyDb/0.5, White, Black);
if(starfir)
LCD_PutString(260, 110,"On ", White, Black);
else
LCD_PutString(260, 110,"Off", White, Black);
}
}
else
{
if(ShowMenu)
{
Disp_single_colour(Black);
ShowMenu = 0;
}
draw_fft();
}
flag = 0;
}
}
}
int main(void)
{
int nsamp, i;
float32_t *input, *output1, *output2;
initialize(FS_48K, MONO_IN, STEREO_OUT); // Set up the DAC/ADC interface
// Allocate Required Memory
nsamp = getblocksize();
input = (float32_t *)malloc(sizeof(float)*nsamp);
output1 = (float32_t *)malloc(sizeof(float)*nsamp);
output2 = (float32_t *)malloc(sizeof(float)*nsamp);
if (input==NULL || output1==NULL || output2==NULL) {
flagerror(MEMORY_ALLOCATION_ERROR);
while(1);
}
// Impulse response/FIR coefficient array
float32_t h[20] = {0.000168,
-0.000883,
-0.004735,
-0.010728,
-0.013639,
-0.004205,
0.025992,
0.077462,
0.138625,
0.188861,
0.208333,
0.188861,
0.138625,
0.077462,
0.025992,
-0.004205,
-0.013639,
-0.010728,
-0.004735,
-0.000883};
// state buffer used by arm routine of size NUM_TAPS + BLOCKSIZE -1
float32_t *state = (float32_t *) malloc(sizeof(float)*(20+nsamp-1));
// arm FIR filter struct initialization
arm_fir_instance_f32 f1;
arm_fir_init_f32(&f1,20,&h[0],state,nsamp);
// Infinite Loop to process the data stream, "nsamp" samples at a time
while(1){
/*
* Ask for a block of ADC samples to be put into the working buffer
* getblock() will wait until the input buffer is filled... On return
* we work on the new data buffer.
*/
getblock(input); // Wait here until the input buffer is filled... Then process
// signal processing code to calculate the required output buffers
// copy input to output2 for reference
for(i=0;i<nsamp;i++) {
output2[i] = input[i];
}
DIGITAL_IO_SET(); // Use a scope on PC4 to measure execution time
// Call the arm provided FIR filter routine
arm_fir_f32(&f1, input, output1, nsamp);
DIGITAL_IO_RESET(); // (falling edge.... done processing data )
// pass the processed working buffer back for DAC output
putblockstereo(output1, output2);
}
}
