feat(FFT): добавлено FFT, выделение основной частоты

2025-12-26 00:13:57 +03:00
parent 063cced2a5
commit 97c59cdda2
9 changed files with 291 additions and 83 deletions
--- a/.gitmodules
+++ b/.gitmodules
@@ -10,3 +10,6 @@
 [submodule "firmware/Drivers/CMSIS/Core"]
 	path = firmware/Drivers/CMSIS/Core
 	url = https://github.com/STMicroelectronics/cmsis_core.git
+[submodule "firmware/Middlewares/CMSIS-DSP"]
+	path = firmware/Middlewares/CMSIS-DSP
+	url = https://github.com/ARM-software/CMSIS-DSP.git
--- a/client/src/reveier.py
+++ b/client/src/reveier.py
@@ -32,6 +32,8 @@ def read_serial_data():
        )
    except KeyboardInterrupt:
        print("\nExiting...")
+    except:
+        print("\nAn error occure!")
    finally:
        if "ser" in locals() and ser.is_open:
            ser.close()
--- a/firmware/App/Inc/FreeRTOSConfig.h
+++ b/firmware/App/Inc/FreeRTOSConfig.h
@@ -10,7 +10,7 @@
 #define configTICK_RATE_HZ ((TickType_t)1000)
 #define configMAX_PRIORITIES (5)
 #define configMINIMAL_STACK_SIZE ((unsigned short)128)
-#define configTOTAL_HEAP_SIZE ((size_t)(10 * 1024))
+#define configTOTAL_HEAP_SIZE ((size_t)(8 * 1024))
 #define configMAX_TASK_NAME_LEN (16)
 #define configUSE_16_BIT_TICKS 0
 #define configIDLE_SHOULD_YIELD 1
--- a/firmware/App/Inc/audio_processor.h
+++ b/firmware/App/Inc/audio_processor.h
@@ -0,0 +1,20 @@
+#pragma once
+
+#include <stdbool.h>
+#include <stdint.h>
+#include "audio_config.h"
+
+typedef struct {
+    // dBFS (потом можно добавить калибровочный оффсет до dB SPL)
+    float rms_dbfs;
+    float peak_hz;    // доминантная частота
+    float peak_mag;   // амплитуда бина (относительная)
+    uint8_t clipped;  // 1 если был клиппинг (0 или 4095)
+} audio_metrics_t;
+
+bool audio_processor_init(void);
+
+// process ровно 512 сэмплов
+bool audio_processor_process_512(
+    const audio_sample_t* samples,
+    audio_metrics_t* out);
--- a/firmware/App/Src/audio_processor.c
+++ b/firmware/App/Src/audio_processor.c
@@ -0,0 +1,112 @@
+#include "audio_processor.h"
+#include <math.h>
+#include <string.h>
+#include "arm_math.h"
+#include "stm32f1xx.h"
+
+#ifndef AUDIO_FFT_SIZE
+#define AUDIO_FFT_SIZE 512U
+#endif
+
+#if (AUDIO_FFT_SIZE != 512U)
+#error "This module currently expects AUDIO_FFT_SIZE == 512"
+#endif
+
+#define ADC_FULL_SCALE 4095.0f
+#define ADC_MID_SCALE 2048.0f
+#define EPS_RMS 1e-12f
+
+static arm_rfft_fast_instance_f32 rfft;
+
+// ОПТИМИЗАЦИЯ: используем один буфер для in/out FFT
+static float32_t fft_buffer[AUDIO_FFT_SIZE];  // 2KB
+static float32_t mag[AUDIO_FFT_SIZE / 2U];    // 1KB (bins 0..N/2-1)
+
+static uint32_t bin_min = 0;
+static uint32_t bin_max = 0;
+static float32_t hz_per_bin = 0.0f;
+
+// Inline Hann window (без хранения коэффициентов)
+static inline float32_t hann_coeff(uint32_t i, uint32_t n) {
+    const float32_t two_pi = 6.28318530717958647693f;
+    const float32_t denom = (float32_t)(n - 1U);
+    return 0.5f - 0.5f * arm_cos_f32(two_pi * (float32_t)i / denom);
+}
+
+bool audio_processor_init(void) {
+    // FFT init
+    if (arm_rfft_fast_init_512_f32(&rfft) != ARM_MATH_SUCCESS) { return false; }
+
+    hz_per_bin = ((float32_t)AUDIO_SAMPLE_RATE) / ((float32_t)AUDIO_FFT_SIZE);
+
+    // Диапазон поиска пика 100..8000 Hz
+    bin_min = (uint32_t)ceilf(100.0f / hz_per_bin);
+    bin_max = (uint32_t)floorf(8000.0f / hz_per_bin);
+
+    // safety
+    if (bin_min < 1U) bin_min = 1U;
+    const uint32_t last_bin = (AUDIO_FFT_SIZE / 2U) - 1U;
+    if (bin_max > last_bin) bin_max = last_bin;
+
+    return true;
+}
+
+bool audio_processor_process_512(
+    const audio_sample_t* samples,
+    audio_metrics_t* out) {
+    if (!samples || !out) return false;
+
+    // 1) Mean + clipping detect
+    uint32_t sum = 0;
+    uint8_t clipped = 0;
+
+    for (uint32_t i = 0; i < AUDIO_FFT_SIZE; i++) {
+        const uint16_t s = samples[i];
+        sum += s;
+        if (s == 0U || s == 4095U) clipped = 1;
+    }
+
+    const float32_t mean = (float32_t)sum / (float32_t)AUDIO_FFT_SIZE;
+
+    // 2) RMS of AC component + prepare FFT input (normalized, windowed)
+    float32_t acc = 0.0f;
+
+    for (uint32_t i = 0; i < AUDIO_FFT_SIZE; i++) {
+        // centered around 0, normalized to roughly [-1..1]
+        float32_t x = ((float32_t)samples[i] - mean) / ADC_MID_SCALE;
+        acc += x * x;
+
+        // apply window inline (saves 2KB RAM)
+        fft_buffer[i] = x * hann_coeff(i, AUDIO_FFT_SIZE);
+    }
+
+    const float32_t rms = sqrtf(acc / (float32_t)AUDIO_FFT_SIZE);
+    const float32_t rms_dbfs = 20.0f * log10f(rms + EPS_RMS);
+
+    // 3) FFT (in-place: fft_buffer используется для in/out)
+    arm_rfft_fast_f32(&rfft, fft_buffer, fft_buffer, 0);
+
+    // 4) Magnitudes for bins 0..N/2-1
+    // CMSIS layout: [Re(0), Im(0)=0, Re(1), Im(1), ..., Re(N/2), Im(N/2)=0]
+    // Мы берём bin 1..N/2-1 для поиска пика
+    arm_cmplx_mag_f32(fft_buffer, mag, AUDIO_FFT_SIZE / 2U);
+
+    // 5) Peak search in desired band (skip DC bin 0)
+    uint32_t best_bin = bin_min;
+    float32_t best_mag = 0.0f;
+
+    for (uint32_t k = bin_min; k <= bin_max; k++) {
+        const float32_t m = mag[k];
+        if (m > best_mag) {
+            best_mag = m;
+            best_bin = k;
+        }
+    }
+
+    out->rms_dbfs = rms_dbfs;
+    out->peak_mag = best_mag;
+    out->peak_hz = (float32_t)best_bin * hz_per_bin;
+    out->clipped = clipped;
+
+    return true;
+}
--- a/firmware/App/Src/main.c
+++ b/firmware/App/Src/main.c
@@ -2,6 +2,7 @@
 #include <string.h>
 #include "FreeRTOS.h"
 #include "audio_adc.h"
+#include "audio_processor.h"  // НОВОЕ
 #include "queue.h"
 #include "stm32f1xx.h"
 #include "task.h"
@@ -10,23 +11,25 @@
 void vApplicationStackOverflowHook(TaskHandle_t xTask, char *pcTaskName) {
    (void)xTask;
    (void)pcTaskName;
-    // Мигаем LED очень быстро при переполнении стека
    while (1) {
        GPIOC->ODR ^= GPIO_ODR_ODR13;
        for (volatile int i = 0; i < 50000; i++);
    }
 }

-// === Структура данных для очереди ===
+// === Структуры данных ===

+// НОВОЕ: пакет с результатами FFT
 typedef struct {
-    uint16_t min_val;
-    uint16_t max_val;
-    uint16_t avg_val;
+    float rms_dbfs;
+    float peak_hz;
+    float peak_mag;
+    uint8_t clipped;
    uint32_t buffer_num;
-} audio_stats_packet_t;
+} audio_metrics_packet_t;

-static QueueHandle_t audio_stats_queue = NULL;
+static QueueHandle_t audio_metrics_queue = NULL;
+static volatile uint32_t buffer_counter = 0;

 // === System Clock ===

@@ -51,37 +54,26 @@ void SystemClock_Config(void) {
    SystemCoreClock = 72000000;
 }

-// === Audio Callback ===
+// === Audio Callback (НОВОЕ: копируем в очередь для обработки) ===
+
+// Буфер для копирования из ISR
+static audio_sample_t processing_buffer[AUDIO_BUFFER_SIZE];

 void audio_buffer_ready(audio_sample_t *buffer, uint32_t size) {
-    static uint32_t buffer_counter = 0;
    buffer_counter++;

-    // Мигаем LED при каждом вызове
+    // Мигаем LED
    if (buffer_counter % 5 == 0) { GPIOC->ODR ^= GPIO_ODR_ODR13; }

-    uint16_t min_val = 4095;
-    uint16_t max_val = 0;
-    uint32_t sum = 0;
-
-    for (uint32_t i = 0; i < size; i++) {
-        uint16_t val = buffer[i];
-        if (val < min_val) min_val = val;
-        if (val > max_val) max_val = val;
-        sum += val;
-    }
-
-    if (buffer_counter % 10 == 0) {
-        audio_stats_packet_t packet = {
-            .min_val = min_val,
-            .max_val = max_val,
-            .avg_val = (uint16_t)(max_val - min_val),
-            .buffer_num = buffer_counter};
+    // Копируем данные (ISR должен быть быстрым)
+    memcpy(processing_buffer, buffer, size * sizeof(audio_sample_t));

+    // Сигналим задаче обработки через notification
    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
-        xQueueSendFromISR(
-            audio_stats_queue,
-            &packet,
+    extern TaskHandle_t audio_process_task_handle;
+    if (audio_process_task_handle != NULL) {
+        vTaskNotifyGiveFromISR(
+            audio_process_task_handle,
            &xHigherPriorityTaskWoken);
        portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
    }
@@ -97,60 +89,110 @@ void usb_device_task(void *param) {
    }
 }

+// НОВОЕ: задача обработки FFT
+TaskHandle_t audio_process_task_handle = NULL;
+
+void audio_process_task(void *param) {
+    (void)param;
+
+    // Инициализация процессора
+    if (!audio_processor_init()) {
+        // Ошибка FFT init
+        while (1) {
+            GPIOC->ODR ^= GPIO_ODR_ODR13;
+            vTaskDelay(pdMS_TO_TICKS(100));
+        }
+    }
+
+    audio_metrics_t metrics;
+
+    while (1) {
+        // Ждём сигнала от ISR
+        ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
+
+        // Обработка 512 сэмплов
+        if (audio_processor_process_512(processing_buffer, &metrics)) {
+            // Отправляем только каждый 10-й (10 Hz)
+            if (buffer_counter % 10 == 0) {
+                audio_metrics_packet_t packet = {
+                    .rms_dbfs = metrics.rms_dbfs,
+                    .peak_hz = metrics.peak_hz,
+                    .peak_mag = metrics.peak_mag,
+                    .clipped = metrics.clipped,
+                    .buffer_num = buffer_counter};
+                xQueueSend(audio_metrics_queue, &packet, 0);
+            }
+        }
+    }
+}
+
 void cdc_task(void *param) {
    (void)param;
+
    char tx_buffer[256];
    uint32_t heartbeat_counter = 0;
-    uint32_t last_buffer_count = 0;

    while (1) {
        heartbeat_counter++;

-        // Heartbeat + ДИАГНОСТИКА регистров каждые 100 циклов
+        // Heartbeat каждые 100 циклов
        if (heartbeat_counter % 100 == 0 && tud_cdc_connected()) {
            uint32_t current_buffer_count = audio_adc_get_buffer_count();

            int len = snprintf(
                tx_buffer,
                sizeof(tx_buffer),
-                "HB:%lu Q:%u BC:%lu | "
-                "TIM3_CR1:%lX TIM3_CNT:%lu | "
-                "ADC1_CR2:%lX ADC1_SR:%lX | "
-                "DMA_CCR:%lX DMA_CNDTR:%lu\r\n",
+                "HB:%lu Q:%u BC:%lu\r\n",
                heartbeat_counter,
-                (unsigned)uxQueueMessagesWaiting(audio_stats_queue),
-                current_buffer_count,
-                TIM3->CR1,
-                TIM3->CNT,
-                ADC1->CR2,
-                ADC1->SR,
-                DMA1_Channel1->CCR,
-                DMA1_Channel1->CNDTR);
+                (unsigned)uxQueueMessagesWaiting(audio_metrics_queue),
+                current_buffer_count);

-            if (tud_cdc_write_available() >= len) {
-                tud_cdc_write(tx_buffer, len);
+            if (len > 0 && tud_cdc_write_available() >= (uint32_t)len) {
+                tud_cdc_write(tx_buffer, (uint32_t)len);
                tud_cdc_write_flush();
            }
-
-            last_buffer_count = current_buffer_count;
        }

-        // Остальной код без изменений
-        audio_stats_packet_t packet;
-        if (xQueueReceive(audio_stats_queue, &packet, pdMS_TO_TICKS(10)) ==
+        // Метрики FFT
+        audio_metrics_packet_t packet;
+        if (xQueueReceive(audio_metrics_queue, &packet, pdMS_TO_TICKS(10)) ==
            pdPASS) {
+            // fixed-point:
+            // RMS: dBFS * 10 (один знак после запятой)
+            int32_t rms_x10 = (int32_t)(packet.rms_dbfs * 10.0f);
+            int32_t rms_int = rms_x10 / 10;
+            int32_t rms_frac = rms_x10 % 10;
+            if (rms_frac < 0) rms_frac = -rms_frac;
+
+            // Freq: Hz * 10 (один знак после запятой)
+            int32_t freq_x10 = (int32_t)(packet.peak_hz * 10.0f);
+            int32_t freq_int = freq_x10 / 10;
+            int32_t freq_frac = freq_x10 % 10;
+            if (freq_frac < 0) freq_frac = -freq_frac;
+
+            // Mag: *1000 (три знака после запятой)
+            int32_t mag_x1000 = (int32_t)(packet.peak_mag * 1000.0f);
+            int32_t mag_int = mag_x1000 / 1000;
+            int32_t mag_frac = mag_x1000 % 1000;
+            if (mag_frac < 0) mag_frac = -mag_frac;
+
            int len = snprintf(
                tx_buffer,
                sizeof(tx_buffer),
-                "Buf:%lu Min:%u Max:%u Avg:%u\r\n",
+                "Buf:%lu RMS:%ld.%01ld dBFS Freq:%ld.%01ld Hz Mag:%ld.%03ld "
+                "Clip:%u\r\n",
                packet.buffer_num,
-                packet.min_val,
-                packet.max_val,
-                packet.avg_val);
+                (long)rms_int,
+                (long)rms_frac,
+                (long)freq_int,
+                (long)freq_frac,
+                (long)mag_int,
+                (long)mag_frac,
+                (unsigned)packet.clipped);

-            if (tud_cdc_connected() && len > 0 &&
-                tud_cdc_write_available() >= len) {
-                tud_cdc_write(tx_buffer, len);
+            if (len > 0 && tud_cdc_connected() &&
+                tud_cdc_write_available() >= (uint32_t)len) {
+                tud_cdc_write(tx_buffer, (uint32_t)len);
                tud_cdc_write_flush();
            }
        }
@@ -177,28 +219,24 @@ void led_task(void *param) {
    }
 }

-// === Задача инициализации аудио ===
 void audio_init_task(void *param) {
    (void)param;

-    // Индикация старта инициализации (мигнем 3 раза быстро)
+    // Индикация старта
    for (int i = 0; i < 3; i++) {
        GPIOC->ODR ^= GPIO_ODR_ODR13;
        vTaskDelay(pdMS_TO_TICKS(100));
    }

    if (!audio_adc_init(audio_buffer_ready)) {
-        // Ошибка: мигаем очень быстро
        while (1) {
            GPIOC->ODR ^= GPIO_ODR_ODR13;
            vTaskDelay(pdMS_TO_TICKS(50));
        }
    }

-    // Запускаем ADC после старта FreeRTOS
    audio_adc_start();

-    // Индикация успешного запуска (мигнем 5 раз медленно)
    for (int i = 0; i < 5; i++) {
        GPIOC->ODR ^= GPIO_ODR_ODR13;
        vTaskDelay(pdMS_TO_TICKS(200));
@@ -207,8 +245,6 @@ void audio_init_task(void *param) {
    vTaskDelete(NULL);
 }

-// === USB Reset ===
-
 void force_usb_reset(void) {
    RCC->APB2ENR |= RCC_APB2ENR_IOPAEN;
    GPIOA->CRH &= ~GPIO_CRH_CNF12;
@@ -245,9 +281,9 @@ int main(void) {

    tusb_init();

-    // Очередь создаем ДО старта планировщика
-    audio_stats_queue = xQueueCreate(10, sizeof(audio_stats_packet_t));
-    if (audio_stats_queue == NULL) {
+    // НОВОЕ: очередь для метрик FFT
+    audio_metrics_queue = xQueueCreate(10, sizeof(audio_metrics_packet_t));
+    if (audio_metrics_queue == NULL) {
        while (1) {
            GPIOC->ODR ^= GPIO_ODR_ODR13;
            for (volatile int i = 0; i < 100000; i++);
@@ -266,6 +302,15 @@ int main(void) {
    xTaskCreate(led_task, "led", 128, NULL, 1, NULL);
    xTaskCreate(audio_init_task, "audio_init", 128, NULL, 2, NULL);

+    // НОВОЕ: задача обработки FFT (высокий приоритет, большой стек для FFT)
+    xTaskCreate(
+        audio_process_task,
+        "audio_proc",
+        512,
+        NULL,
+        configMAX_PRIORITIES - 2,
+        &audio_process_task_handle);
+
    vTaskStartScheduler();

    while (1);
--- a/firmware/Makefile
+++ b/firmware/Makefile
@@ -1,15 +1,15 @@
 TARGET = stm32-usb-freertos
 BUILD_DIR = Build

-# --- Исходники ---
-# 1. Приложение
+# Приложение
 C_SOURCES =  \
 App/Src/main.c \
 App/Src/audio_adc.c \
+App/Src/audio_processor.c \
 App/Src/usb_descriptors.c \
 App/Src/system_stm32f1xx.c \

-# 2. FreeRTOS
+# FreeRTOS
 C_SOURCES += \
 Middlewares/FreeRTOS/croutine.c \
 Middlewares/FreeRTOS/event_groups.c \
@@ -20,7 +20,7 @@ Middlewares/FreeRTOS/timers.c \
 Middlewares/FreeRTOS/portable/GCC/ARM_CM3/port.c \
 Middlewares/FreeRTOS/portable/MemMang/heap_4.c

-# 3. TinyUSB
+# TinyUSB
 C_SOURCES += \
 Middlewares/TinyUSB/src/tusb.c \
 Middlewares/TinyUSB/src/common/tusb_fifo.c \
@@ -29,7 +29,23 @@ Middlewares/TinyUSB/src/device/usbd_control.c \
 Middlewares/TinyUSB/src/class/cdc/cdc_device.c \
 Middlewares/TinyUSB/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c

-# 4. Startup
+# CMSIS-DSP sources
+C_SOURCES += \
+$(CMSIS_DSP)/Source/TransformFunctions/arm_cfft_radix8_f32.c \
+$(CMSIS_DSP)/Source/TransformFunctions/arm_bitreversal2.c \
+$(CMSIS_DSP)/Source/TransformFunctions/arm_rfft_fast_f32.c \
+$(CMSIS_DSP)/Source/TransformFunctions/arm_rfft_fast_init_f32.c \
+$(CMSIS_DSP)/Source/TransformFunctions/arm_cfft_f32.c \
+$(CMSIS_DSP)/Source/TransformFunctions/arm_cfft_init_f32.c \
+$(CMSIS_DSP)/Source/ComplexMathFunctions/arm_cmplx_mag_f32.c \
+$(CMSIS_DSP)/Source/CommonTables/arm_const_structs.c \
+$(CMSIS_DSP)/Source/CommonTables/arm_common_tables.c \
+$(CMSIS_DSP)/Source/FastMathFunctions/arm_cos_f32.c
+
+# CMSIS-DSP
+CMSIS_DSP = Middlewares/CMSIS-DSP
+
+# Startup
 ASM_SOURCES = App/Src/startup_stm32f103xb.s

 # --- Настройки компилятора ---
@@ -48,19 +64,29 @@ C_INCLUDES =  \
 -IDrivers/CMSIS/Device/ST/STM32F1xx/Include \
 -IMiddlewares/FreeRTOS/include \
 -IMiddlewares/FreeRTOS/portable/GCC/ARM_CM3 \
-IMiddlewares/TinyUSB/src
+-IMiddlewares/TinyUSB/src \
+-I$(CMSIS_DSP)/Include \
+-I$(CMSIS_DSP)/PrivateInclude

 # Defines
 C_DEFS = \
 -DSTM32F103xB \
-DCFG_TUSB_MCU=OPT_MCU_STM32F1
+-DCFG_TUSB_MCU=OPT_MCU_STM32F1 \
+-DARM_MATH_CM3

-CFLAGS = $(MCU) $(C_DEFS) $(C_INCLUDES) -O2 -Wall -fdata-sections -ffunction-sections -g -gdwarf-2
+CFLAGS = $(MCU) $(C_DEFS) $(C_INCLUDES) -Os -Wall -fdata-sections -ffunction-sections -g -gdwarf-2

 # Linker
 LDSCRIPT = stm32f103c8.ld
-LIBS = -lc -lm -lnosys
-LDFLAGS = $(MCU) -T$(LDSCRIPT) $(LIBS) -Wl,-Map=$(BUILD_DIR)/$(TARGET).map,--cref -Wl,--gc-sections -Wl,--no-warn-rwx-segments
+# LIBS = -lc -lm -lnosys
+# LDFLAGS = $(MCU) -T$(LDSCRIPT) $(LIBS) -Wl,-Map=$(BUILD_DIR)/$(TARGET).map,--cref -Wl,--gc-sections -Wl,--no-warn-rwx-segments
+# LDFLAGS = $(MCU) -T$(LDSCRIPT) --specs=nano.specs --specs=nosys.specs \
+#           -Wl,-Map=$(BUILD_DIR)/$(TARGET).map,--cref -Wl,--gc-sections
+LIBS = -Wl,--start-group -lc_nano -lm -lgcc -lnosys -Wl,--end-group
+LDFLAGS = $(MCU) -T$(LDSCRIPT) $(LIBS) \
+          -Wl,-Map=$(BUILD_DIR)/$(TARGET).map,--cref \
+          -Wl,--gc-sections \
+          -Wl,--no-warn-rwx-segments

 # --- Генерация списка объектов ---
 OBJECTS = $(addprefix $(BUILD_DIR)/,$(notdir $(C_SOURCES:.c=.o)))
@@ -104,4 +130,3 @@ flash:
 	st-flash write $(BUILD_DIR)/$(TARGET).bin 0x8000000

 .PHONY: all clean flash
-
--- a/firmware/Middlewares/CMSIS-DSP
+++ b/firmware/Middlewares/CMSIS-DSP
--- a/firmware/stm32f103c8.ld
+++ b/firmware/stm32f103c8.ld
@@ -4,8 +4,8 @@ ENTRY(Reset_Handler)
 /* Highest address of the user mode stack */
 _estack = ORIGIN(RAM) + LENGTH(RAM); /* end of "RAM" Ram type memory */

-_Min_Heap_Size = 0x400;  /* required amount of heap  */
-_Min_Stack_Size = 0xa00; /* required amount of stack */
+_Min_Heap_Size = 0x000;  /* required amount of heap  */
+_Min_Stack_Size = 0x800; /* required amount of stack */

 /* Memories definition */
 MEMORY