From ef8a2eb1742eb239d3af2677cdc707f6c496f6e9 Mon Sep 17 00:00:00 2001 From: Theodor Chikin Date: Mon, 6 Oct 2025 20:16:09 +0300 Subject: [PATCH] added kiss_fft_framework.c and .h. They contain ChatGPT-generated functions to init and perform real-FFT --- src/kiss_fft_framework.c | 191 +++++++++++++++++++++++++++++++++++++++ src/kiss_fft_framework.h | 54 +++++++++++ src/l502_stream.c | 2 + src/l502_user_process.c | 14 +++ src/l502_user_process.h | 2 + 5 files changed, 263 insertions(+) create mode 100644 src/kiss_fft_framework.c create mode 100644 src/kiss_fft_framework.h diff --git a/src/kiss_fft_framework.c b/src/kiss_fft_framework.c new file mode 100644 index 0000000..5179ed6 --- /dev/null +++ b/src/kiss_fft_framework.c @@ -0,0 +1,191 @@ +#include "kiss_fft_framework.h" +#include // memset + +// ---------------- Внутреннее состояние ---------------- +static kiss_fftr_cfg s_cfg = 0; +static int s_cfg_bytes = 0; +static kiss_fft_scalar*s_in_q15 = 0; // длина FFT_N +static kiss_fft_cpx* s_out_cpx = 0; // длина FFT_RESULT_L + +// Безопасный пресдвиг входа (uniform scaling) перед прямым БПФ, чтобы не словить переполнение. +// Для типичных окон/сигналов N=1024..4096 хорошо работает 4..6 бит. Подстройте при необходимости. +#ifndef KISS_FFT_PRESHIFT_BITS +#define KISS_FFT_PRESHIFT_BITS 5 +#endif + +// ---------------- Вспомогательные функции ---------------- +static inline int16_t sat16(int32_t x){ + if (x > 32767) return 32767; + if (x < -32768) return -32768; + return (int16_t)x; +} + +static void prescale_block_q15(kiss_fft_scalar* x, int n, int k){ + if (k <= 0) return; + for (int i = 0; i < n; ++i) x[i] = (kiss_fft_scalar)( ((int32_t)x[i]) >> k ); +} + +// Целочисленный sqrt для 32-бит (возвращает floor(sqrt(x))) +static uint32_t isqrt_u32(uint64_t x){ + // Быстрый целочисленный sqrt (битовый Ньютона-Рафсона для 64->32) + uint64_t op = x; + uint64_t res = 0; + uint64_t one = (uint64_t)1 << 62; // самая высокая чётная степень 4 + + while (one > op) one >>= 2; + while (one != 0) { + if (op >= res + one) { + op -= res + one; + res = (res >> 1) + one; + } else { + res >>= 1; + } + one >>= 2; + } + return (uint32_t)res; +} + +// Быстрый целочисленный |complex|: sqrt(r^2 + i^2) с защитой от переполнения. +// Вход: r,i в Q15 (int16). Возврат: модуль в "сырых" единицах Q15 (int16 амплитуда). +static uint32_t cpx_abs_q15(int16_t r, int16_t i){ + int32_t rr = (int32_t)r; + int32_t ii = (int32_t)i; + uint64_t mag2 = (uint64_t)(rr*rr) + (uint64_t)(ii*ii); // до ~2*(32767^2) — влезает в 32, но берём 64 для запаса + return isqrt_u32(mag2); +} + +// ЦОДИК (CORDIC) для atan2 в фиксированной точности. +// Возвращает угол в формате Q15, диапазон [-32768 .. +32767] ≈ [-π .. +π]. +// Источник: классическая вращательная CORDIC, таблица арктангенсов в Q15 (углы). +static int16_t cordic_atan2_q15(int16_t y, int16_t x){ + // Масштабирующий коэффициент CORDIC (K ≈ 0.607252) нам не нужен для угла. + // Представим x,y как 32-бит для предотвращения переполнений при сдвигах. + int32_t X = x; + int32_t Y = y; + int32_t Z = 0; + + // Нормализация квадранта + if (X < 0) { + X = -X; Y = -Y; + Z = (int32_t)32768; // +π в Q15 (переполнится до -32768, мы это учтём позже как unsigned фазу) + } + + // Таблица arctan(2^-i) в Q15 (угол), i=0..14 (достаточно для 16-бит точности) + static const int16_t atan_table_q15[15] = { + 25735, // atan(1) ≈ 0.785398163 -> 0.785398163/π * 32768 ≈ 25735 + 15192, // atan(1/2) + 8027, // atan(1/4) + 4074, // atan(1/8) + 2045, // ... + 1023, + 512, + 256, + 128, + 64, + 32, + 16, + 8, + 4, + 2 + }; + + for (int i = 0; i < 15; ++i){ + int32_t d = (Y >= 0) ? 1 : -1; + int32_t Xn = X - ((d * (Y)) >> i); + int32_t Yn = Y + ((d * (X)) >> i); + int32_t Zn = Z - d * atan_table_q15[i]; + X = Xn; Y = Yn; Z = Zn; + } + // Возврат угла в Q15, диапазон примерно [-π..+π] + return (int16_t)Z; +} + +// Преобразование фазы в беззнаковый UQ16: 0..65535 соответствует -π..+π. +static inline uint16_t phase_q15_to_uq16(int16_t ph_q15){ + return (uint16_t)((int32_t)ph_q15 + 32768); +} + +// ---------------- Публичные функции ---------------- +int kiss_fft_init(uint32_t* internals_mem, + kiss_fft_scalar* in_buf_q15, + kiss_fft_cpx* out_buf_cpx) +{ + if (!internals_mem || !in_buf_q15 || !out_buf_cpx) return KISS_FFT_FW_ERR_ALLOC; + + // Узнаём требуемый объём для конфигурации (twiddle и т.д.) + int needed = 0; + kiss_fftr_alloc(FFT_N, 0, NULL, &needed); + s_cfg_bytes = (int)(KISS_FFT_INTERNALS_SIZE_WORDS * sizeof(uint32_t)); + if (needed <= 0 || needed > s_cfg_bytes) { + s_cfg = 0; + s_in_q15 = 0; + s_out_cpx = 0; + return KISS_FFT_FW_ERR_CFGSIZE; + } + + // Инициализация в заранее выделенной памяти (без malloc) + s_cfg = kiss_fftr_alloc(FFT_N, 0, (void*)internals_mem, &s_cfg_bytes); + if (!s_cfg) return KISS_FFT_FW_ERR_ALLOC; + + s_in_q15 = in_buf_q15; + s_out_cpx = out_buf_cpx; + + // Обнулим входной буфер на всякий случай + memset(s_in_q15, 0, sizeof(kiss_fft_scalar)*FFT_N); + return KISS_FFT_FW_OK; +} + +int kiss_fft_process_data(const uint32_t* input_data, + uint32_t input_data_size, + uint32_t* fft_result_ampl, + uint32_t* fft_result_phase) +{ + if (!s_cfg || !s_in_q15 || !s_out_cpx) return KISS_FFT_FW_ERR_STATE; + if (!input_data || !fft_result_ampl || !fft_result_phase) return KISS_FFT_FW_ERR_ALLOC; + + // 1) Подготовка входа: копируем <=FFT_N с преобразованием в Q15 (берём младшие 16 бит как s16) + const uint32_t ncopy = (input_data_size < FFT_N) ? input_data_size : FFT_N; + for (uint32_t i = 0; i < ncopy; ++i){ + int16_t s16 = (int16_t)(input_data[i] & 0xFFFFu); // ожидаем уже центрированный s16 + s_in_q15[i] = s16; + } + // Остальное — нулями + for (uint32_t i = ncopy; i < FFT_N; ++i) s_in_q15[i] = 0; + + // (опционально вы можете умножить на окно здесь — Ханна/Хэмминга — в Q15) + + // 2) Пресдвиг для защиты от переполнения + prescale_block_q15(s_in_q15, FFT_N, KISS_FFT_PRESHIFT_BITS); + + // 3) FFT (real-to-complex) + kiss_fftr(s_cfg, s_in_q15, s_out_cpx); + + // 4) Постобработка: амплитуда и фаза + // Нормировка: прямое БПФ у KissFFT НЕ делит на N. Мы делали пресдвиг на 2^P. + // Приведём амплитуду примерно к шкале входа Q15 и разделим на N: + // amp ≈ (|X[k]| << P) / N + // Результат вернём как UQ15 в uint32_t (0..32767), но тип — uint32_t на будущее. + // Фаза — UQ16, 0..65535 соответствует (-π..+π). + const uint32_t N = FFT_N; + const uint32_t P = KISS_FFT_PRESHIFT_BITS; + + for (uint32_t k = 0; k < FFT_RESULT_L; ++k){ + int16_t r = s_out_cpx[k].r; + int16_t i = s_out_cpx[k].i; + // Для DC и Найквиста мнимая часть ноль, но формула общая. + + // Модуль + uint32_t mag_q15_raw = cpx_abs_q15(r, i); // ~Q15 + uint64_t num = ((uint64_t)mag_q15_raw) << P; // компенсируем пресдвиг + uint32_t amp_uq15 = (uint32_t)(num / N); // делим на N (прямое БПФ) + + if (amp_uq15 > 32767u) amp_uq15 = 32767u; // насыщение в UQ15-«видимой» шкале + fft_result_ampl[k] = amp_uq15; + + // Фаза + int16_t ph_q15 = cordic_atan2_q15(i, r); // [-32768 .. +32767] ~ [-π .. +π] + fft_result_phase[k] = (uint32_t)phase_q15_to_uq16(ph_q15); + } + + return KISS_FFT_FW_OK; +} diff --git a/src/kiss_fft_framework.h b/src/kiss_fft_framework.h new file mode 100644 index 0000000..f83f078 --- /dev/null +++ b/src/kiss_fft_framework.h @@ -0,0 +1,54 @@ +#pragma once +#include +#include "kiss_fftr.h" // из KissFFT; собирайте с -DFIXED_POINT=16 (Q15) + +#ifdef __cplusplus +extern "C" { +#endif + +// --- Настройка размера FFT --- +#ifndef FFT_N +#define FFT_N 1024 // степень двойки +#endif +#define FFT_RESULT_L ((FFT_N/2)+1) // число бинов real-FFT (включая DC и Найквист) + +// Сколько памяти (в 32-битных словах) нужно заранее выделить под внутренности KissFFT. +// Значение подобрано с запасом для типичных N до 4096–8192. +// Если окажется мало, kiss_fft_init() вернёт <0. +#define KISS_FFT_INTERNALS_SIZE_WORDS (32768u/4u) // 32 KiB в словах + +// Код возврата +enum { + KISS_FFT_FW_OK = 0, + KISS_FFT_FW_ERR_CFGSIZE = -1, + KISS_FFT_FW_ERR_ALLOC = -2, + KISS_FFT_FW_ERR_STATE = -3, +}; + +// Инициализация фреймворка. +// internals_mem — указатель на заранее выделенный массив uint32_t длиной KISS_FFT_INTERNALS_SIZE_WORDS; +// in_buf_q15 — ваш буфер входных данных Q15 длиной FFT_N (kiss_fft_scalar* == int16_t при FIXED_POINT=16); +// out_buf_cpx — ваш буфер комплексного спектра длиной FFT_RESULT_L (kiss_fft_cpx*). +// +// Возвращает 0 при успехе. +int kiss_fft_init(uint32_t* internals_mem, + kiss_fft_scalar* in_buf_q15, + kiss_fft_cpx* out_buf_cpx); + +// Обработка данных (прямое БПФ). +// input_data — входной массив длиной input_data_size. Предполагается, что в МЛС 16 бит лежит s16 семпл +// (±32768 => ±1.0 в Q15). Остальная часть слова игнорируется. +// input_data_size — может быть < FFT_N; недостающее заполняется нулями. +// fft_result_ampl — выходной массив амплитуд длиной FFT_RESULT_L (uint32_t, шкала UQ15, см. примечание ниже). +// fft_result_phase — выходной массив фаз длиной FFT_RESULT_L (uint32_t, угол в UQ16: 0..65535 соответствует -π..+π). +// +// Нормировка: амплитуды масштабируются примерно к входной шкале Q15 и делятся на FFT_N, +// при этом поправка на предсдвиг учтена. DC/Найквист корректно считаются. +int kiss_fft_process_data(const uint32_t* input_data, + uint32_t input_data_size, + uint32_t* fft_result_ampl, + uint32_t* fft_result_phase); + +#ifdef __cplusplus +} +#endif diff --git a/src/l502_stream.c b/src/l502_stream.c index a9ce0ae..83c9050 100644 --- a/src/l502_stream.c +++ b/src/l502_stream.c @@ -37,6 +37,8 @@ volatile uint32_t AVG_buff[AVG_BUFF_SIZE] __attribute__((section(".sdram_noinit" volatile uint32_t FFT_buff[FFT_BUFF_SIZE] __attribute__((section(".sdram_noinit"))); + + /** Размер буфера на прием данных по SPORT0 в 32-битных словах */ #define L502_SPORT_IN_BUF_SIZE (2048*1024) /** Размер буфера для приема данных по HostDMA на вывод в 32-битных словах */ diff --git a/src/l502_user_process.c b/src/l502_user_process.c index 9476721..76ece7d 100644 --- a/src/l502_user_process.c +++ b/src/l502_user_process.c @@ -691,6 +691,20 @@ void usr_cmd_process(t_l502_bf_cmd *cmd) { + case 0x800C:{// setup kiss_FFT + + + + +// l502_cmd_done(cmd-> param, NULL, 0); + l502_cmd_done(TX_buff_I, NULL, 0); + break; + + + } + + + case 0x8010:{ //flush HDMA TX buffer diff --git a/src/l502_user_process.h b/src/l502_user_process.h index 3032d35..3aea16b 100644 --- a/src/l502_user_process.h +++ b/src/l502_user_process.h @@ -14,6 +14,8 @@ #define AVG_BUFF_SIZE 2000 #define FFT_BUFF_SIZE 2000 +#define FFT_N 1024 + //#define L502_SPORT_IN_BUF_SIZE (2048*1024) //#define TX_BUFF_SIZE (RAW_DATA_BUFF_SIZE + AVG_BUFF_SIZE + FFT_BUFF_SIZE + L502_SPORT_IN_BUF_SIZE) //should be large enough to fit all other buffers and raw data