FIR фильтр на Си

FreeRTOS, CMSIS DSP (Спасибо Геннадию Завидовскому)
ПРИМЕР:

//заполнение массива тремя sin-усоидами.
// FIR фильтр на 169 taps
// FFT
// получение амплитуд pDst[n] = sqrt(pSrc[(2*n)+0]^2 + pSrc[(2*n)+1]^2);
// прорисовка

__IO float32_t p[FRAME_SIZE * 2];
__IO float32_t pFirOut[FRAME_SIZE];
__IO float32_t pOut[FRAME_SIZE];
__IO float32_t win[FRAME_SIZE];
__IO float32_t pMag[FRAME_SIZE];

float32_t firStateF32[FRAME_SIZE + SAMPLEFILTER_TAP_NUM - 1];
float32_t firCoeff32[SAMPLEFILTER_TAP_NUM];
..
..
..
..

void StartDefaultTask(void const *argument) {
/* init code for USB_HOST */
MX_USB_HOST_Init();
/* USER CODE BEGIN 5 */
unsigned char buff[20];
uint32_t cou = 0;
float32_t pp = (float32_t) (2 * 3.14159265359);//3.14159265359
int freq = 6700;
int bit = 24000;//битрэйд

//ini log2 table
init_log_table();

//весовое окно
for (int i = 0; i < FRAME_SIZE; i++) {
win[ i] = (float32_t) (1.0
- 1.93293488969227 * cos((double) i * pp / (FRAME_SIZE - 1))
+ 1.28349769674027 * cos((double) i * pp * 2.0 / (FRAME_SIZE - 1))
- 0.38130801681619 * cos((double) i * pp * 3.0 / (FRAME_SIZE - 1))
+ 0.02929730258511 * cos((double) i * pp * 4.0 / (FRAME_SIZE - 1)));
}

HAL_RNG_Init(&hrng);
//ini fft
arm_rfft_fast_instance_f32 fftInstance;
arm_rfft_fast_init_f32(&fftInstance, 1024);
//ini fir filtra
for (int i = 0; i < SAMPLEFILTER_TAP_NUM; i++) {
firCoeff32[ i] = (float32_t) filter_tap[ i];
}

arm_fir_instance_f32 S;
arm_fir_init_f32(&S, SAMPLEFILTER_TAP_NUM, (float32_t *) firCoeff32, &firStateF32[0], FRAME_SIZE);
/* Infinite loop */
while (1) {

if (freq > 11000)freq = 6700;

for (int i = 0; i < FRAME_SIZE; i++) {
float32_t sin1 = arm_sin_f32(pp * 1456 * i / bit);
float32_t sin2 = arm_sin_f32(pp * 10976 * i / bit);
float32_t sin3 = arm_sin_f32(pp * freq * i / bit);
float32_t sinn = (sin1 + 3 * sin2 + 6 * sin3 + HAL_RNG_GetRandomNumber(&hrng) / 0xdfffffff);
p[ i] = sinn * win[ i];//wtFLATTOP

p[i + FRAME_SIZE] = 0; // заполняем мнимую часть сигнала
}
p[0] = 0;
p[FRAME_SIZE] = 0;

arm_fir_f32(&S, (float *) p, (float32_t *) pFirOut, FRAME_SIZE);
// arm_rfft_fast_f32(&fftInstance, (float32_t *) p, (float32_t *) pOut, 0);
arm_rfft_fast_f32(&fftInstance, (float32_t *) pFirOut, (float32_t *) pOut, 0);

//pDst[n] = sqrt(pSrc[(2*n)+0]^2 + pSrc[(2*n)+1]^2);
arm_cmplx_mag_f32((float32_t *)pOut,(float32_t *)pMag,FRAME_SIZE);

fft();

freq += 100;
cou++;
if (cou >= 999999)cou = 0;
// osDelay(1);
}
/* USER CODE END 5 */
}

Ну и сами процедуры.

//
// Created by Сергей Туров on 21.12.2018.
//
#include "math.h"

#include "fft.h"
#include "stdlib.h"
#include "stdio.h"

#include <../Drivers/BSP/STM32F429I-Discovery/stm32f429i_discovery_lcd.h>
#include "main.h"
#include <../Drivers/CMSIS/DSP/Include/arm_math.h>

//extern __IO float Re[FRAME_SIZE];
//extern __IO float Im[FRAME_SIZE];
extern __IO float32_t pOut[FRAME_SIZE];
extern __IO float32_t p[FRAME_SIZE * 2];
extern __IO float32_t pMag[FRAME_SIZE];

extern LTDC_HandleTypeDef LtdcHandler;

//log2
#define LogPrecisionLevel 5
#define LogTableSize 0b100000 //1<<LogPrecisionLevel
double log_table[LogTableSize];
//

void fft(void) {

uint16_t z = 0;
uint16_t x = 0;
uint16_t yOld = 0;

uint16_t nak = 0;
uint32_t b = BSP_LCD_GetBackColor();

for (int i = 0; i < FRAME_SIZE/2; i += 1) {

// if(Re<Re[i+1] && Re[i+1]>Re[i+2] ){//Квадратичная интерполяция по трём точкам
//
// int x1=i;
// int x2=i+1;
// int x3=i+2;
//
// float y1=Re;
// float y2=Re[i+1];
// float y3=Re[i+2];
//
// float a = ((y3 - y1)*(x2 - x1) - (y2 - y1)*(x3 - x1)) / ((x3*x3 - x1*x1)*(x2 - x1) - (x2*x2 - x1*x1)*(x3 - x1));
//
// float b = (y2 - y1 - a*(x2*x2 - x1*x1)) / (x2 - x1);
//
// float c = y1 - (a*x1*x1 + b*x1);
//
// Re[i+1]=c-(b*b)/(4*a);
//
// }

//arm_sqrt_f32(pOut * pOut + pOut[i + 1] * pOut[i + 1], &pO);
float32_t lo=(float32_t)(10 * fast_log2(pMag[i ]) );
uint16_t y = (uint16_t) lo; //*0.43429448190325

if (y > nak) nak = y;

if (z > 0) {
x++;
z = 0;

// if (!nak)nak = 1;
if (nak > 130)nak = 130;
if (nak < 0)nak = 0;
BSP_LCD_SetTextColor(LCD_COLOR_BLACK);

BSP_LCD_DrawHLine(100, x, 131);

BSP_LCD_SetTextColor(LCD_COLOR_YELLOW);

BSP_LCD_DrawLine(100 + yOld, x - 1, 100 + nak, x);
// BSP_LCD_DrawHLine(100, x, nak);
// BSP_LCD_DrawHLine(100 + nak, x, 131 - nak);

// BSP_LCD_DrawPixel(105+nak, x, LCD_COLOR_YELLOW);

// for (int p = 50; p > 0; p--) {
// uint32_t ret = *(__IO uint32_t *) (LtdcHandler.LayerCfg[0].FBStartAdress +
// (4 * (x * BSP_LCD_GetXSize() + p - 1)));//pixelRead new
// BSP_LCD_DrawPixel(p, x, ret);
//
// }
// uint32_t col = nak * 0xffffff;
// BSP_LCD_DrawPixel(0, x, col);
yOld = nak;
nak = 0;

} else {
z++;
}

}
BSP_LCD_SetTextColor(b);

}

void init_log_table(void) {
for (int i = 0; i < LogTableSize; i++) {
log_table[ i] = log2(1 + (double)i /LogTableSize);
}
}

double fast_log2(double x) { // x>0
unsigned long long t = *(unsigned long long*)&x;
int exp = (t >> 52) - 0x3ff;
int mantissa = (t >> (52 - LogPrecisionLevel)) & (LogTableSize - 1);
return exp + log_table[mantissa];
}

Результат

Цитата: ra0ahc от Сентябрь 18, 2020, 12:19:05 pm

//весовое окно
for (int i = 0; i < FRAME_SIZE; i++) {
win = (float32_t) (1.0
- 1.93293488969227 * cos((double) i * pp / (FRAME_SIZE - 1))
+ 1.28349769674027 * cos((double) i * pp * 2.0 / (FRAME_SIZE - 1))
- 0.38130801681619 * cos((double) i * pp * 3.0 / (FRAME_SIZE - 1))
+ 0.02929730258511 * cos((double) i * pp * 4.0 / (FRAME_SIZE - 1)));
}

может тут надо win[ i ] ?

Цитата: ra0ahc от Сентябрь 18, 2020, 12:19:05 pm

for (int i = 0; i < FRAME_SIZE; i++) {
float32_t sin1 = arm_sin_f32(pp * 1456 * i / bit);
float32_t sin2 = arm_sin_f32(pp * 10976 * i / bit);
float32_t sin3 = arm_sin_f32(pp * freq * i / bit);
float32_t sinn = (sin1 + 3 * sin2 + 6 * sin3 + HAL_RNG_GetRandomNumber(&hrng) / 0xdfffffff);
p = sinn * win;//wtFLATTOP

p[i + FRAME_SIZE] = 0; // заполняем мнимую часть сигнала
}

а где вещественная заполняется?

Да это косяк данного блога
[i ] он считает наклонным шрифтом и не показывает
Правильно p[ i ]

Не проще ли ффт посчитать и из него спектр нарисовать? Зачем весь этот головняк с FIR да еще и такого высокого порядка?

Это заготовка для будущего проекта. Дсп в процессоре будет. Это тренировка на скорость. Проц медленный , фиг знает как он в потоке поведёт себя. Жаль видосы сюда не льются. Так бы показал что было на стандартных сишных функциях и что стало когда Ассемблерные функции подключил от cmsis dsp. Раз в двадцать все быстрее зашевелилось. Особенно замена штатных sin() на дспишные .. ух как всё залипало.

На ютуб выложите и сюда ссылку

А в cmsis не ассемблерное... основное что там это врвчную циклы разворачивали.
Если что они на гитхабе своем живут.

Еще... у всех функций из math.h есть для одинарной точности варианты с добавленной f в конце.

Цитата: GenaSPB от Сентябрь 18, 2020, 02:43:29 pm

А в cmsis не ассемблерное... основное что там это врвчную циклы разворачивали.
Если что они на гитхабе своем живут.

Я когда мониторил тему, то где то проскакивали (вспомнил ... форум ёхный, и они там ответ давали кому-то, что у них ассемблерная оптимизация сделана)

https://youtu.be/mWyAFBeWwLs

залил на ютуб

Максимальная скорость, которую удалось достичь

А вот так было до оптимизации

https://youtu.be/gxQrmVq5zrg

Здорово шурует.
Я аж прям передумал внешний контроллер LCD использовать.

Поставил на "поток" realTime ...еще быстрее все заработало.

Сейчас : один поток только считает, а второй поток только на экран выводит.
Терпимо!

Настройки freeRTOS потока