added kiss_fft_framework.c and .h. They contain ChatGPT-generated functions to init and perform real-FFT

.gitmodules

@@ -1,3 +1,3 @@
-[submodule "FFT_and_FP_math"]
-	path = FFT_and_FP_math
-	url = https://git.radiophotonics.ru/ChTheo/FFT_and_FP_math.git
+[submodule "kissfft"]
+	path = kissfft
+	url = https://github.com/mborgerding/kissfft.git

makefile

@@ -76,8 +76,7 @@ SRC  = src/main.c \
        src/l502_hdma.c \
        src/l502_params.c \
        src/l502_tests.c \
-       src/l502_user_process.c \
-       Theta_fft\
+       src/l502_user_process.c
 
 
 

src/kiss_fft_framework.c

@@ -0,0 +1,191 @@
+#include "kiss_fft_framework.h"
+#include <string.h>   // 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;
+}

src/kiss_fft_framework.h

@@ -0,0 +1,54 @@
+#pragma once
+#include <stdint.h>
+#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

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-битных словах */

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

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