Автор Тема: FIR фильтр на Си  (Прочитано 5565 раз)

0 Пользователей и 1 Гость просматривают эту тему.

Оффлайн ra0ahc

  • Hero Member
  • *****
  • Сообщений: 4872
  • Сергей, RD6AH
FIR фильтр на Си
« : Сентябрь 18, 2020, 12:19:05 pm »
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 */
}
« Последнее редактирование: Октябрь 07, 2020, 09:38:23 pm от Сергей »
Да да, я знаю, у меня ничего не получится )))

Оффлайн ra0ahc

  • Hero Member
  • *****
  • Сообщений: 4872
  • Сергей, RD6AH
Re: FIR фильтр на С (без расчета коэф.)
« Ответ #1 : Сентябрь 18, 2020, 12:20:19 pm »
Ну и сами процедуры.

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

« Последнее редактирование: Сентябрь 18, 2020, 01:09:30 pm от ra0ahc »
Да да, я знаю, у меня ничего не получится )))

Оффлайн ra0ahc

  • Hero Member
  • *****
  • Сообщений: 4872
  • Сергей, RD6AH
Re: FIR фильтр на С (без расчета коэф.)
« Ответ #2 : Сентябрь 18, 2020, 12:24:42 pm »
Результат
Да да, я знаю, у меня ничего не получится )))

Оффлайн Relayer

  • Hero Member
  • *****
  • Сообщений: 1006
  • UR5FFR
Re: FIR фильтр на С (без расчета коэф.)
« Ответ #3 : Сентябрь 18, 2020, 01:00:41 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 ] ?
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;         // заполняем мнимую часть сигнала
        }
а где вещественная заполняется?
В любой схеме есть как минимум одна ненужная деталь :)

Оффлайн ra0ahc

  • Hero Member
  • *****
  • Сообщений: 4872
  • Сергей, RD6AH
Re: FIR фильтр на С (без расчета коэф.)
« Ответ #4 : Сентябрь 18, 2020, 01:05:15 pm »
Да это косяк данного блога
[i ] он считает наклонным шрифтом и не показывает
Правильно p[ i ]
Да да, я знаю, у меня ничего не получится )))

Оффлайн Relayer

  • Hero Member
  • *****
  • Сообщений: 1006
  • UR5FFR
Re: FIR фильтр на С (без расчета коэф.)
« Ответ #5 : Сентябрь 18, 2020, 01:09:36 pm »
Не проще ли ффт посчитать и из него спектр нарисовать? Зачем весь этот головняк с FIR да еще и такого высокого порядка?
В любой схеме есть как минимум одна ненужная деталь :)

Оффлайн ra0ahc

  • Hero Member
  • *****
  • Сообщений: 4872
  • Сергей, RD6AH
Re: FIR фильтр на С (без расчета коэф.)
« Ответ #6 : Сентябрь 18, 2020, 01:15:16 pm »
Это заготовка для будущего проекта. Дсп в процессоре будет. Это тренировка на скорость. Проц медленный , фиг знает как он в потоке поведёт себя. Жаль видосы сюда не льются. Так бы показал что было на стандартных сишных функциях и что стало когда Ассемблерные  функции подключил от cmsis dsp. Раз в двадцать все быстрее зашевелилось. Особенно замена штатных sin() на дспишные .. ух как всё залипало.
Да да, я знаю, у меня ничего не получится )))

User

  • Гость
Re: FIR фильтр на С (без расчета коэф.)
« Ответ #7 : Сентябрь 18, 2020, 02:37:00 pm »
На ютуб выложите и сюда ссылку

Оффлайн GenaSPB

  • Jr. Member
  • **
  • Сообщений: 74
Re: FIR фильтр на С (без расчета коэф.)
« Ответ #8 : Сентябрь 18, 2020, 02:43:29 pm »
А в cmsis  не ассемблерное... основное что там это врвчную циклы разворачивали.
Если что  они на гитхабе своем живут.

Оффлайн GenaSPB

  • Jr. Member
  • **
  • Сообщений: 74
Re: FIR фильтр на С (без расчета коэф.)
« Ответ #9 : Сентябрь 18, 2020, 02:46:28 pm »
Еще... у всех функций из  math.h есть для одинарной точности варианты с добавленной  f  в конце.

Оффлайн ra0ahc

  • Hero Member
  • *****
  • Сообщений: 4872
  • Сергей, RD6AH
Re: FIR фильтр на С (без расчета коэф.)
« Ответ #10 : Сентябрь 18, 2020, 03:07:13 pm »
А в cmsis  не ассемблерное... основное что там это врвчную циклы разворачивали.
Если что  они на гитхабе своем живут.
Я когда мониторил тему, то где то проскакивали (вспомнил ... форум ёхный, и они там ответ давали кому-то, что у них ассемблерная оптимизация сделана)
Да да, я знаю, у меня ничего не получится )))

Оффлайн ra0ahc

  • Hero Member
  • *****
  • Сообщений: 4872
  • Сергей, RD6AH
Re: FIR фильтр на С (без расчет коэф.)
« Ответ #11 : Сентябрь 18, 2020, 03:19:03 pm »
https://youtu.be/mWyAFBeWwLs

залил на ютуб

Максимальная скорость, которую удалось достичь
« Последнее редактирование: Сентябрь 18, 2020, 03:57:52 pm от ra0ahc »
Да да, я знаю, у меня ничего не получится )))

Оффлайн ra0ahc

  • Hero Member
  • *****
  • Сообщений: 4872
  • Сергей, RD6AH
Re: FIR фильтр на С (без расчета коэф.)
« Ответ #12 : Сентябрь 18, 2020, 03:23:29 pm »
А вот так было до оптимизации

https://youtu.be/gxQrmVq5zrg
Да да, я знаю, у меня ничего не получится )))

Оффлайн r1tx

  • Sr. Member
  • ****
  • Сообщений: 490
Re: FIR фильтр на С (без расчета коэф.)
« Ответ #13 : Сентябрь 18, 2020, 04:11:42 pm »
Здорово шурует.
Я аж прям передумал внешний контроллер LCD использовать.

Оффлайн ra0ahc

  • Hero Member
  • *****
  • Сообщений: 4872
  • Сергей, RD6AH
Re: FIR фильтр на С (без расчета коэф.)
« Ответ #14 : Сентябрь 18, 2020, 04:17:07 pm »
Поставил на "поток" realTime  ...еще быстрее все заработало.

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

Настройки freeRTOS потока
Да да, я знаю, у меня ничего не получится )))