From 6ac4124101b3105a7127bc3215d350b063e4a090 Mon Sep 17 00:00:00 2001 From: mkuettner97 Date: Tue, 9 Dec 2025 11:14:46 +0100 Subject: [PATCH 01/16] draft: decimation filter --- src/audio/decimation_filter.c | 372 ++++++++++++++++++++++++++++++++++ src/audio/decimation_filter.h | 118 +++++++++++ 2 files changed, 490 insertions(+) create mode 100644 src/audio/decimation_filter.c create mode 100644 src/audio/decimation_filter.h diff --git a/src/audio/decimation_filter.c b/src/audio/decimation_filter.c new file mode 100644 index 00000000..ff7a95fe --- /dev/null +++ b/src/audio/decimation_filter.c @@ -0,0 +1,372 @@ +/* + * Copyright (c) 2025 + * + * SPDX-License-Identifier: LicenseRef-PCFT + */ + +#include "decimation_filter.h" +#include "arm_math.h" + +#include +LOG_MODULE_REGISTER(decimation_filter, CONFIG_AUDIO_DATAPATH_LOG_LEVEL); + +/* Maximum supported samples per block */ +#define MAX_SAMPLES_PER_BLOCK 2048 + +/* Butterworth lowpass filter coefficients for anti-aliasing + * Cutoff at Fs/2/decimation_factor with some margin + * Default: 5-stage Butterworth at 0.4 * (Fs_in / decimation_factor) + */ +#define FILTER_NUM_STAGES 2 + +/* Filter state and coefficients */ +static struct { + bool initialized; + uint32_t input_sample_rate; + uint8_t decimation_factor; + + /* CMSIS-DSP biquad filter instances for stereo */ + arm_biquad_cascade_stereo_df2T_instance_f32 biquad_stereo; + float stereo_state[4 * FILTER_NUM_STAGES]; /* 4 states per stage for stereo */ + float stereo_coeffs[5 * FILTER_NUM_STAGES]; /* 5 coeffs per stage: b0, b1, b2, -a1, -a2 */ + + /* CMSIS-DSP biquad filter instance for mono */ + arm_biquad_casd_df1_inst_f32 biquad_mono; + float mono_state[4 * FILTER_NUM_STAGES]; /* 4 states per stage for mono */ + float mono_coeffs[5 * FILTER_NUM_STAGES]; /* 5 coeffs per stage */ + + /* Working buffers for format conversion */ + float input_buf_f32[MAX_SAMPLES_PER_BLOCK * 2]; /* *2 for stereo */ + float filtered_buf_f32[MAX_SAMPLES_PER_BLOCK * 2]; +} decimation_ctx; + +/** + * @brief Design anti-aliasing filter coefficients + * + * Generates a 2-stage Butterworth lowpass filter + * Cutoff frequency: 0.4 * (input_sample_rate / decimation_factor) + * This provides adequate anti-aliasing margin + */ +static void design_antialiasing_filter(void) +{ + /* Calculate normalized cutoff frequency */ + float cutoff_hz = 0.4f * ((float)decimation_ctx.input_sample_rate / + (float)decimation_ctx.decimation_factor); + float normalized_cutoff = cutoff_hz / ((float)decimation_ctx.input_sample_rate / 2.0f); + + /* Pre-computed Butterworth coefficients for common decimation scenarios + * These are normalized for Fc = 0.4 * Fs_out at various input rates + * For 192kHz -> 48kHz (decimation = 4), Fc ≈ 19.2kHz + */ + + if (decimation_ctx.decimation_factor == 4) { + /* Butterworth 2-stage lowpass, Fc = 0.4 * 48kHz = 19.2kHz at 192kHz + * Designed with scipy.signal.butter(4, 0.4/4, 'low', analog=False) + */ + + /* Stage 1 coefficients */ + decimation_ctx.stereo_coeffs[0] = 0.00048853f; /* b0 */ + decimation_ctx.stereo_coeffs[1] = 0.00097706f; /* b1 */ + decimation_ctx.stereo_coeffs[2] = 0.00048853f; /* b2 */ + decimation_ctx.stereo_coeffs[3] = 1.95558033f; /* -a1 */ + decimation_ctx.stereo_coeffs[4] = -0.95753446f; /* -a2 */ + + /* Stage 2 coefficients */ + decimation_ctx.stereo_coeffs[5] = 1.0f; /* b0 */ + decimation_ctx.stereo_coeffs[6] = 2.0f; /* b1 */ + decimation_ctx.stereo_coeffs[7] = 1.0f; /* b2 */ + decimation_ctx.stereo_coeffs[8] = 1.95557364f; /* -a1 */ + decimation_ctx.stereo_coeffs[9] = -0.95654896f; /* -a2 */ + } else if (decimation_ctx.decimation_factor == 2) { + /* Butterworth 2-stage lowpass, Fc = 0.4 * Fs_out at 2x input rate */ + + /* Stage 1 */ + decimation_ctx.stereo_coeffs[0] = 0.00780863f; + decimation_ctx.stereo_coeffs[1] = 0.01561726f; + decimation_ctx.stereo_coeffs[2] = 0.00780863f; + decimation_ctx.stereo_coeffs[3] = 1.77863177f; + decimation_ctx.stereo_coeffs[4] = -0.80986630f; + + /* Stage 2 */ + decimation_ctx.stereo_coeffs[5] = 1.0f; + decimation_ctx.stereo_coeffs[6] = 2.0f; + decimation_ctx.stereo_coeffs[7] = 1.0f; + decimation_ctx.stereo_coeffs[8] = 1.77859908f; + decimation_ctx.stereo_coeffs[9] = -0.80872483f; + } else { + /* Generic case - simple 2-stage Butterworth approximation */ + float w0 = 3.14159265f * normalized_cutoff; + float alpha = sinf(w0) / (2.0f * 0.707f); /* Q = 0.707 for Butterworth */ + + float b0 = (1.0f - cosf(w0)) / 2.0f; + float b1 = 1.0f - cosf(w0); + float b2 = (1.0f - cosf(w0)) / 2.0f; + float a0 = 1.0f + alpha; + float a1 = -2.0f * cosf(w0); + float a2 = 1.0f - alpha; + + /* Normalize and convert to CMSIS format */ + decimation_ctx.stereo_coeffs[0] = b0 / a0; + decimation_ctx.stereo_coeffs[1] = b1 / a0; + decimation_ctx.stereo_coeffs[2] = b2 / a0; + decimation_ctx.stereo_coeffs[3] = -a1 / a0; + decimation_ctx.stereo_coeffs[4] = -a2 / a0; + + /* Duplicate for stage 2 (same coefficients) */ + for (int i = 0; i < 5; i++) { + decimation_ctx.stereo_coeffs[5 + i] = decimation_ctx.stereo_coeffs[i]; + } + } + + /* Copy coefficients for mono filter */ + memcpy(decimation_ctx.mono_coeffs, decimation_ctx.stereo_coeffs, + sizeof(decimation_ctx.stereo_coeffs)); + + LOG_DBG("Anti-aliasing filter designed: Fc=%.1f Hz (normalized=%.3f)", + cutoff_hz, normalized_cutoff); +} + +int decimation_filter_init(uint32_t input_sample_rate, uint8_t decimation_factor) +{ + if (decimation_factor == 0 || decimation_factor > 16) { + LOG_ERR("Invalid decimation factor: %d (must be 1-16)", decimation_factor); + return -EINVAL; + } + + if (input_sample_rate < 8000 || input_sample_rate > 384000) { + LOG_ERR("Invalid input sample rate: %d Hz", input_sample_rate); + return -EINVAL; + } + + decimation_ctx.input_sample_rate = input_sample_rate; + decimation_ctx.decimation_factor = decimation_factor; + + /* Design anti-aliasing filter coefficients */ + design_antialiasing_filter(); + + /* Initialize CMSIS-DSP stereo biquad cascade filter */ + arm_biquad_cascade_stereo_df2T_init_f32(&decimation_ctx.biquad_stereo, + FILTER_NUM_STAGES, + decimation_ctx.stereo_coeffs, + decimation_ctx.stereo_state); + + /* Initialize CMSIS-DSP mono biquad cascade filter */ + arm_biquad_cascade_df1_init_f32(&decimation_ctx.biquad_mono, + FILTER_NUM_STAGES, + decimation_ctx.mono_coeffs, + decimation_ctx.mono_state); + + /* Clear filter states */ + memset(decimation_ctx.stereo_state, 0, sizeof(decimation_ctx.stereo_state)); + memset(decimation_ctx.mono_state, 0, sizeof(decimation_ctx.mono_state)); + + decimation_ctx.initialized = true; + + LOG_INF("Decimation filter initialized: %d Hz -> %d Hz (factor %d)", + input_sample_rate, + input_sample_rate / decimation_factor, + decimation_factor); + + return 0; +} + +int decimation_filter_process_stereo(const int16_t *input, int16_t *output, uint32_t num_frames) +{ + if (!decimation_ctx.initialized) { + LOG_ERR("Decimation filter not initialized"); + return -EINVAL; + } + + if (input == NULL || output == NULL || num_frames == 0) { + LOG_ERR("Invalid parameters"); + return -EINVAL; + } + + if (num_frames > (MAX_SAMPLES_PER_BLOCK / 2)) { + LOG_ERR("Input buffer too large: %d frames (max %d)", + num_frames, MAX_SAMPLES_PER_BLOCK / 2); + return -EINVAL; + } + + uint32_t num_samples = num_frames * 2; /* Stereo: 2 samples per frame */ + + /* Convert int16 to float32 (scale by 1/32768.0) */ + const float scale_to_float = 1.0f / 32768.0f; + for (uint32_t i = 0; i < num_samples; i++) { + decimation_ctx.input_buf_f32[i] = (float)input[i] * scale_to_float; + } + + /* Apply anti-aliasing filter using CMSIS-DSP stereo biquad */ + arm_biquad_cascade_stereo_df2T_f32(&decimation_ctx.biquad_stereo, + decimation_ctx.input_buf_f32, + decimation_ctx.filtered_buf_f32, + num_frames); + + /* Decimate: keep every Nth sample */ + uint32_t output_frames = num_frames / decimation_ctx.decimation_factor; + const float scale_to_int16 = 32767.0f; + + for (uint32_t i = 0; i < output_frames; i++) { + uint32_t in_idx = i * decimation_ctx.decimation_factor * 2; + uint32_t out_idx = i * 2; + + /* Convert back to int16 with clamping */ + float val_l = decimation_ctx.filtered_buf_f32[in_idx] * scale_to_int16; + float val_r = decimation_ctx.filtered_buf_f32[in_idx + 1] * scale_to_int16; + + /* Clamp to int16 range */ + if (val_l > 32767.0f) val_l = 32767.0f; + if (val_l < -32768.0f) val_l = -32768.0f; + if (val_r > 32767.0f) val_r = 32767.0f; + if (val_r < -32768.0f) val_r = -32768.0f; + + output[out_idx] = (int16_t)val_l; + output[out_idx + 1] = (int16_t)val_r; + } + + return output_frames; +} + +int decimation_filter_process_stereo_f32(const float *input, float *output, uint32_t num_frames) +{ + if (!decimation_ctx.initialized) { + LOG_ERR("Decimation filter not initialized"); + return -EINVAL; + } + + if (input == NULL || output == NULL || num_frames == 0) { + LOG_ERR("Invalid parameters"); + return -EINVAL; + } + + if (num_frames > (MAX_SAMPLES_PER_BLOCK / 2)) { + LOG_ERR("Input buffer too large: %d frames (max %d)", + num_frames, MAX_SAMPLES_PER_BLOCK / 2); + return -EINVAL; + } + + /* Apply anti-aliasing filter using CMSIS-DSP stereo biquad */ + arm_biquad_cascade_stereo_df2T_f32(&decimation_ctx.biquad_stereo, + input, + decimation_ctx.filtered_buf_f32, + num_frames); + + /* Decimate: keep every Nth sample */ + uint32_t output_frames = num_frames / decimation_ctx.decimation_factor; + + for (uint32_t i = 0; i < output_frames; i++) { + uint32_t in_idx = i * decimation_ctx.decimation_factor * 2; + uint32_t out_idx = i * 2; + + output[out_idx] = decimation_ctx.filtered_buf_f32[in_idx]; + output[out_idx + 1] = decimation_ctx.filtered_buf_f32[in_idx + 1]; + } + + return output_frames; +} + +int decimation_filter_process_mono(const int16_t *input, int16_t *output, uint32_t num_samples) +{ + if (!decimation_ctx.initialized) { + LOG_ERR("Decimation filter not initialized"); + return -EINVAL; + } + + if (input == NULL || output == NULL || num_samples == 0) { + LOG_ERR("Invalid parameters"); + return -EINVAL; + } + + if (num_samples > MAX_SAMPLES_PER_BLOCK) { + LOG_ERR("Input buffer too large: %d samples (max %d)", + num_samples, MAX_SAMPLES_PER_BLOCK); + return -EINVAL; + } + + /* Convert int16 to float32 */ + const float scale_to_float = 1.0f / 32768.0f; + for (uint32_t i = 0; i < num_samples; i++) { + decimation_ctx.input_buf_f32[i] = (float)input[i] * scale_to_float; + } + + /* Apply anti-aliasing filter using CMSIS-DSP mono biquad */ + arm_biquad_cascade_df1_f32(&decimation_ctx.biquad_mono, + decimation_ctx.input_buf_f32, + decimation_ctx.filtered_buf_f32, + num_samples); + + /* Decimate: keep every Nth sample */ + uint32_t output_samples = num_samples / decimation_ctx.decimation_factor; + const float scale_to_int16 = 32767.0f; + + for (uint32_t i = 0; i < output_samples; i++) { + uint32_t in_idx = i * decimation_ctx.decimation_factor; + + /* Convert back to int16 with clamping */ + float val = decimation_ctx.filtered_buf_f32[in_idx] * scale_to_int16; + + if (val > 32767.0f) val = 32767.0f; + if (val < -32768.0f) val = -32768.0f; + + output[i] = (int16_t)val; + } + + return output_samples; +} + +int decimation_filter_process_mono_f32(const float *input, float *output, uint32_t num_samples) +{ + if (!decimation_ctx.initialized) { + LOG_ERR("Decimation filter not initialized"); + return -EINVAL; + } + + if (input == NULL || output == NULL || num_samples == 0) { + LOG_ERR("Invalid parameters"); + return -EINVAL; + } + + if (num_samples > MAX_SAMPLES_PER_BLOCK) { + LOG_ERR("Input buffer too large: %d samples (max %d)", + num_samples, MAX_SAMPLES_PER_BLOCK); + return -EINVAL; + } + + /* Apply anti-aliasing filter using CMSIS-DSP mono biquad */ + arm_biquad_cascade_df1_f32(&decimation_ctx.biquad_mono, + input, + decimation_ctx.filtered_buf_f32, + num_samples); + + /* Decimate: keep every Nth sample */ + uint32_t output_samples = num_samples / decimation_ctx.decimation_factor; + + for (uint32_t i = 0; i < output_samples; i++) { + output[i] = decimation_ctx.filtered_buf_f32[i * decimation_ctx.decimation_factor]; + } + + return output_samples; +} + +void decimation_filter_reset(void) +{ + if (!decimation_ctx.initialized) { + return; + } + + /* Clear filter states */ + memset(decimation_ctx.stereo_state, 0, sizeof(decimation_ctx.stereo_state)); + memset(decimation_ctx.mono_state, 0, sizeof(decimation_ctx.mono_state)); + + LOG_DBG("Decimation filter state reset"); +} + +uint8_t decimation_filter_get_factor(void) +{ + return decimation_ctx.initialized ? decimation_ctx.decimation_factor : 0; +} + +bool decimation_filter_is_initialized(void) +{ + return decimation_ctx.initialized; +} diff --git a/src/audio/decimation_filter.h b/src/audio/decimation_filter.h new file mode 100644 index 00000000..87c24612 --- /dev/null +++ b/src/audio/decimation_filter.h @@ -0,0 +1,118 @@ +/* + * Copyright (c) 2025 + * + * SPDX-License-Identifier: LicenseRef-PCFT + */ + +#ifndef _DECIMATION_FILTER_H_ +#define _DECIMATION_FILTER_H_ + +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * @brief Initialize the decimation filter with CMSIS-DSP + * + * This function sets up the anti-aliasing lowpass filter and decimation + * parameters. Must be called before processing any audio data. + * + * @param input_sample_rate Input sampling rate in Hz (e.g., 192000) + * @param decimation_factor Decimation factor (e.g., 4 for 192kHz -> 48kHz) + * + * @retval 0 if successful + * @retval -EINVAL if parameters are invalid + */ +int decimation_filter_init(uint32_t input_sample_rate, uint8_t decimation_factor); + +/** + * @brief Process stereo audio data with decimation filter + * + * Applies anti-aliasing lowpass filter and decimates the signal. + * Input and output buffers can be the same for in-place processing. + * + * @param input Pointer to input buffer (interleaved stereo samples) + * @param output Pointer to output buffer (interleaved stereo samples) + * @param num_frames Number of stereo frames in input buffer + * + * @retval Number of output frames produced (num_frames / decimation_factor) + * @retval -EINVAL if filter not initialized or invalid parameters + */ +int decimation_filter_process_stereo(const int16_t *input, int16_t *output, uint32_t num_frames); + +/** + * @brief Process stereo audio data with decimation filter (float version) + * + * Applies anti-aliasing lowpass filter and decimates the signal. + * Uses floating point internally for better precision. + * + * @param input Pointer to input buffer (interleaved stereo float samples) + * @param output Pointer to output buffer (interleaved stereo float samples) + * @param num_frames Number of stereo frames in input buffer + * + * @retval Number of output frames produced (num_frames / decimation_factor) + * @retval -EINVAL if filter not initialized or invalid parameters + */ +int decimation_filter_process_stereo_f32(const float *input, float *output, uint32_t num_frames); + +/** + * @brief Process mono audio data with decimation filter + * + * Applies anti-aliasing lowpass filter and decimates the signal. + * Input and output buffers can be the same for in-place processing. + * + * @param input Pointer to input buffer (mono samples) + * @param output Pointer to output buffer (mono samples) + * @param num_samples Number of samples in input buffer + * + * @retval Number of output samples produced (num_samples / decimation_factor) + * @retval -EINVAL if filter not initialized or invalid parameters + */ +int decimation_filter_process_mono(const int16_t *input, int16_t *output, uint32_t num_samples); + +/** + * @brief Process mono audio data with decimation filter (float version) + * + * Applies anti-aliasing lowpass filter and decimates the signal. + * Uses floating point internally for better precision. + * + * @param input Pointer to input buffer (mono float samples) + * @param output Pointer to output buffer (mono float samples) + * @param num_samples Number of samples in input buffer + * + * @retval Number of output samples produced (num_samples / decimation_factor) + * @retval -EINVAL if filter not initialized or invalid parameters + */ +int decimation_filter_process_mono_f32(const float *input, float *output, uint32_t num_samples); + +/** + * @brief Reset the decimation filter state + * + * Clears the filter state variables. Useful when starting a new stream + * or recovering from an error condition. + */ +void decimation_filter_reset(void); + +/** + * @brief Get current decimation factor + * + * @return Current decimation factor, or 0 if not initialized + */ +uint8_t decimation_filter_get_factor(void); + +/** + * @brief Check if decimation filter is initialized + * + * @return true if initialized, false otherwise + */ +bool decimation_filter_is_initialized(void); + +#ifdef __cplusplus +} +#endif + +#endif /* _DECIMATION_FILTER_H_ */ From 41423d0d82c7b88f613dd325013dd05961c180fa Mon Sep 17 00:00:00 2001 From: mkuettner97 Date: Wed, 10 Dec 2025 08:30:22 +0100 Subject: [PATCH 02/16] draft: decimation filter q15 --- src/audio/CMakeLists.txt | 1 + src/audio/audio_datapath.c | 53 +++-- src/audio/decimation_filter.c | 379 +++++----------------------------- src/audio/decimation_filter.h | 100 ++------- 4 files changed, 93 insertions(+), 440 deletions(-) diff --git a/src/audio/CMakeLists.txt b/src/audio/CMakeLists.txt index ab99ae2b..08070d02 100644 --- a/src/audio/CMakeLists.txt +++ b/src/audio/CMakeLists.txt @@ -10,6 +10,7 @@ target_sources(app PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/audio_datapath.c ${CMAKE_CURRENT_SOURCE_DIR}/sw_codec_select.c ${CMAKE_CURRENT_SOURCE_DIR}/le_audio_rx.c + ${CMAKE_CURRENT_SOURCE_DIR}/decimation_filter.c ${CMAKE_CURRENT_SOURCE_DIR}/sdlogger_wrapper.cpp # ${CMAKE_CURRENT_SOURCE_DIR}/rx_publish.c # ${CMAKE_CURRENT_SOURCE_DIR}/pdm_mic.c diff --git a/src/audio/audio_datapath.c b/src/audio/audio_datapath.c index 0b28d728..51e5c27d 100644 --- a/src/audio/audio_datapath.c +++ b/src/audio/audio_datapath.c @@ -28,6 +28,8 @@ #include "Equalizer.h" #include "sdlogger_wrapper.h" +#include "decimation_filter.h" +#include "arm_math.h" #include LOG_MODULE_REGISTER(audio_datapath, CONFIG_AUDIO_DATAPATH_LOG_LEVEL); @@ -193,6 +195,9 @@ int _count = 0; extern struct k_poll_signal encoder_sig; extern struct k_poll_event logger_sig; +/* Decimation buffer for SD card logging */ +static q15_t decimated_audio[BLOCK_SIZE_BYTES / sizeof(q15_t) / 4]; /* /4 for decimation factor 4 */ + // Funktion für den neuen Thread static void data_thread(void *arg1, void *arg2, void *arg3) { @@ -226,29 +231,29 @@ static void data_thread(void *arg1, void *arg2, void *arg3) audio_msg.stream = false; audio_msg.data.id = ID_MICRO; - audio_msg.data.size = BLOCK_SIZE_BYTES; // SENQUEUE_FRAME_SIZE; audio_msg.data.time = time_stamp; - /*k_mutex_lock(&write_mutex, K_FOREVER); - - uint32_t data_size = sizeof(audio_msg.data.id) + sizeof(audio_msg.data.size) + sizeof(audio_msg.data.time); // + audio_msg.data.size; - - uint32_t bytes_written = ring_buf_put(&ring_buffer, (uint8_t *) &audio_msg.data, data_size); - bytes_written += ring_buf_put(&ring_buffer, audio_item.data + (i * BLOCK_SIZE_BYTES), BLOCK_SIZE_BYTES); - - k_mutex_unlock(&write_mutex);*/ - - uint32_t data_size[2] = {sizeof(audio_msg.data.id) + sizeof(audio_msg.data.size) + sizeof(audio_msg.data.time), BLOCK_SIZE_BYTES}; - - const void *data_ptrs[2] = { - &audio_msg.data, - audio_item.data + (i * BLOCK_SIZE_BYTES) - }; - - sdlogger_write_data(&data_ptrs, data_size, 2); - - //sdlogger_write_data(&audio_msg.data, data_size); - //sdlogger_write_data(audio_item.data + (i * BLOCK_SIZE_BYTES), BLOCK_SIZE_BYTES); + /* Decimate audio data from 192kHz to 48kHz (factor 4) */ + q15_t *audio_block = (q15_t *)(audio_item.data + (i * BLOCK_SIZE_BYTES)); + uint32_t num_frames = BLOCK_SIZE_BYTES / sizeof(q15_t) / 2; /* stereo frames */ + int decimated_frames = decimation_filter_process(audio_block, decimated_audio, num_frames); + + if (decimated_frames > 0) { + uint32_t decimated_size = decimated_frames * 2 * sizeof(q15_t); + audio_msg.data.size = decimated_size; + + uint32_t data_size[2] = { + sizeof(audio_msg.data.id) + sizeof(audio_msg.data.size) + sizeof(audio_msg.data.time), + decimated_size + }; + + const void *data_ptrs[2] = { + &audio_msg.data, + decimated_audio + }; + + sdlogger_write_data(&data_ptrs, data_size, 2); + } } k_yield(); @@ -285,6 +290,12 @@ void record_to_sd(bool active) { void start_data_thread(void) { if (data_thread_id == NULL) { + /* Initialize decimation filter for 192kHz -> 48kHz (factor 4) */ + int ret = decimation_filter_init(4); + if (ret) { + LOG_ERR("Failed to initialize decimation filter: %d", ret); + } + data_thread_id = k_thread_create(&data_thread_data, data_thread_stack, CONFIG_ENCODER_STACK_SIZE, data_thread, NULL, NULL, NULL, K_PRIO_PREEMPT(5), 0, K_NO_WAIT); //CONFIG_DATA_THREAD_PRIO diff --git a/src/audio/decimation_filter.c b/src/audio/decimation_filter.c index ff7a95fe..4c919652 100644 --- a/src/audio/decimation_filter.c +++ b/src/audio/decimation_filter.c @@ -5,368 +5,83 @@ */ #include "decimation_filter.h" -#include "arm_math.h" - #include + LOG_MODULE_REGISTER(decimation_filter, CONFIG_AUDIO_DATAPATH_LOG_LEVEL); -/* Maximum supported samples per block */ -#define MAX_SAMPLES_PER_BLOCK 2048 +#define MAX_FRAMES 512 +#define NUM_STAGES 2 -/* Butterworth lowpass filter coefficients for anti-aliasing - * Cutoff at Fs/2/decimation_factor with some margin - * Default: 5-stage Butterworth at 0.4 * (Fs_in / decimation_factor) - */ -#define FILTER_NUM_STAGES 2 +/* Anti-aliasing filter coefficients (Q1.15 format, 5 per stage: b0,0,b1,b2,-a1,-a2) */ +static const q15_t coeff_dec4[NUM_STAGES * 5] = { + /* Stage 1: Butterworth LP, Fc=19.2kHz @ 192kHz */ + 16, 0, 32, 16, 32016, -31357, + /* Stage 2 */ + 32768, 0, 16384, 32768, 32015, -31311 +}; -/* Filter state and coefficients */ -static struct { - bool initialized; - uint32_t input_sample_rate; - uint8_t decimation_factor; - - /* CMSIS-DSP biquad filter instances for stereo */ - arm_biquad_cascade_stereo_df2T_instance_f32 biquad_stereo; - float stereo_state[4 * FILTER_NUM_STAGES]; /* 4 states per stage for stereo */ - float stereo_coeffs[5 * FILTER_NUM_STAGES]; /* 5 coeffs per stage: b0, b1, b2, -a1, -a2 */ - - /* CMSIS-DSP biquad filter instance for mono */ - arm_biquad_casd_df1_inst_f32 biquad_mono; - float mono_state[4 * FILTER_NUM_STAGES]; /* 4 states per stage for mono */ - float mono_coeffs[5 * FILTER_NUM_STAGES]; /* 5 coeffs per stage */ - - /* Working buffers for format conversion */ - float input_buf_f32[MAX_SAMPLES_PER_BLOCK * 2]; /* *2 for stereo */ - float filtered_buf_f32[MAX_SAMPLES_PER_BLOCK * 2]; -} decimation_ctx; +static const q15_t coeff_dec2[NUM_STAGES * 5] = { + /* Stage 1: Butterworth LP, Fc=0.4*Fs @ 2x */ + 256, 0, 512, 256, 29133, -26513, + /* Stage 2 */ + 32768, 0, 16384, 32768, 29129, -26469 +}; -/** - * @brief Design anti-aliasing filter coefficients - * - * Generates a 2-stage Butterworth lowpass filter - * Cutoff frequency: 0.4 * (input_sample_rate / decimation_factor) - * This provides adequate anti-aliasing margin - */ -static void design_antialiasing_filter(void) -{ - /* Calculate normalized cutoff frequency */ - float cutoff_hz = 0.4f * ((float)decimation_ctx.input_sample_rate / - (float)decimation_ctx.decimation_factor); - float normalized_cutoff = cutoff_hz / ((float)decimation_ctx.input_sample_rate / 2.0f); - - /* Pre-computed Butterworth coefficients for common decimation scenarios - * These are normalized for Fc = 0.4 * Fs_out at various input rates - * For 192kHz -> 48kHz (decimation = 4), Fc ≈ 19.2kHz - */ - - if (decimation_ctx.decimation_factor == 4) { - /* Butterworth 2-stage lowpass, Fc = 0.4 * 48kHz = 19.2kHz at 192kHz - * Designed with scipy.signal.butter(4, 0.4/4, 'low', analog=False) - */ - - /* Stage 1 coefficients */ - decimation_ctx.stereo_coeffs[0] = 0.00048853f; /* b0 */ - decimation_ctx.stereo_coeffs[1] = 0.00097706f; /* b1 */ - decimation_ctx.stereo_coeffs[2] = 0.00048853f; /* b2 */ - decimation_ctx.stereo_coeffs[3] = 1.95558033f; /* -a1 */ - decimation_ctx.stereo_coeffs[4] = -0.95753446f; /* -a2 */ - - /* Stage 2 coefficients */ - decimation_ctx.stereo_coeffs[5] = 1.0f; /* b0 */ - decimation_ctx.stereo_coeffs[6] = 2.0f; /* b1 */ - decimation_ctx.stereo_coeffs[7] = 1.0f; /* b2 */ - decimation_ctx.stereo_coeffs[8] = 1.95557364f; /* -a1 */ - decimation_ctx.stereo_coeffs[9] = -0.95654896f; /* -a2 */ - } else if (decimation_ctx.decimation_factor == 2) { - /* Butterworth 2-stage lowpass, Fc = 0.4 * Fs_out at 2x input rate */ - - /* Stage 1 */ - decimation_ctx.stereo_coeffs[0] = 0.00780863f; - decimation_ctx.stereo_coeffs[1] = 0.01561726f; - decimation_ctx.stereo_coeffs[2] = 0.00780863f; - decimation_ctx.stereo_coeffs[3] = 1.77863177f; - decimation_ctx.stereo_coeffs[4] = -0.80986630f; - - /* Stage 2 */ - decimation_ctx.stereo_coeffs[5] = 1.0f; - decimation_ctx.stereo_coeffs[6] = 2.0f; - decimation_ctx.stereo_coeffs[7] = 1.0f; - decimation_ctx.stereo_coeffs[8] = 1.77859908f; - decimation_ctx.stereo_coeffs[9] = -0.80872483f; - } else { - /* Generic case - simple 2-stage Butterworth approximation */ - float w0 = 3.14159265f * normalized_cutoff; - float alpha = sinf(w0) / (2.0f * 0.707f); /* Q = 0.707 for Butterworth */ - - float b0 = (1.0f - cosf(w0)) / 2.0f; - float b1 = 1.0f - cosf(w0); - float b2 = (1.0f - cosf(w0)) / 2.0f; - float a0 = 1.0f + alpha; - float a1 = -2.0f * cosf(w0); - float a2 = 1.0f - alpha; - - /* Normalize and convert to CMSIS format */ - decimation_ctx.stereo_coeffs[0] = b0 / a0; - decimation_ctx.stereo_coeffs[1] = b1 / a0; - decimation_ctx.stereo_coeffs[2] = b2 / a0; - decimation_ctx.stereo_coeffs[3] = -a1 / a0; - decimation_ctx.stereo_coeffs[4] = -a2 / a0; - - /* Duplicate for stage 2 (same coefficients) */ - for (int i = 0; i < 5; i++) { - decimation_ctx.stereo_coeffs[5 + i] = decimation_ctx.stereo_coeffs[i]; - } - } - - /* Copy coefficients for mono filter */ - memcpy(decimation_ctx.mono_coeffs, decimation_ctx.stereo_coeffs, - sizeof(decimation_ctx.stereo_coeffs)); - - LOG_DBG("Anti-aliasing filter designed: Fc=%.1f Hz (normalized=%.3f)", - cutoff_hz, normalized_cutoff); -} +static struct { + bool init; + uint8_t factor; + arm_biquad_casd_df1_inst_q15 biquad; + q15_t state[4 * NUM_STAGES]; + q15_t postshift; +} ctx; -int decimation_filter_init(uint32_t input_sample_rate, uint8_t decimation_factor) +int decimation_filter_init(uint8_t decimation_factor) { - if (decimation_factor == 0 || decimation_factor > 16) { - LOG_ERR("Invalid decimation factor: %d (must be 1-16)", decimation_factor); + if (decimation_factor != 2 && decimation_factor != 4 && decimation_factor != 8) { return -EINVAL; } - if (input_sample_rate < 8000 || input_sample_rate > 384000) { - LOG_ERR("Invalid input sample rate: %d Hz", input_sample_rate); - return -EINVAL; - } - - decimation_ctx.input_sample_rate = input_sample_rate; - decimation_ctx.decimation_factor = decimation_factor; - - /* Design anti-aliasing filter coefficients */ - design_antialiasing_filter(); - - /* Initialize CMSIS-DSP stereo biquad cascade filter */ - arm_biquad_cascade_stereo_df2T_init_f32(&decimation_ctx.biquad_stereo, - FILTER_NUM_STAGES, - decimation_ctx.stereo_coeffs, - decimation_ctx.stereo_state); - - /* Initialize CMSIS-DSP mono biquad cascade filter */ - arm_biquad_cascade_df1_init_f32(&decimation_ctx.biquad_mono, - FILTER_NUM_STAGES, - decimation_ctx.mono_coeffs, - decimation_ctx.mono_state); + ctx.factor = decimation_factor; - /* Clear filter states */ - memset(decimation_ctx.stereo_state, 0, sizeof(decimation_ctx.stereo_state)); - memset(decimation_ctx.mono_state, 0, sizeof(decimation_ctx.mono_state)); + const q15_t *coeffs = (decimation_factor == 4) ? coeff_dec4 : coeff_dec2; + ctx.postshift = 2; /* Shift for Q1.15 */ - decimation_ctx.initialized = true; + arm_biquad_cascade_df1_init_q15(&ctx.biquad, NUM_STAGES, coeffs, ctx.state, ctx.postshift); + memset(ctx.state, 0, sizeof(ctx.state)); - LOG_INF("Decimation filter initialized: %d Hz -> %d Hz (factor %d)", - input_sample_rate, - input_sample_rate / decimation_factor, - decimation_factor); + ctx.init = true; + LOG_INF("Decimation filter init: factor=%d", decimation_factor); return 0; } -int decimation_filter_process_stereo(const int16_t *input, int16_t *output, uint32_t num_frames) -{ - if (!decimation_ctx.initialized) { - LOG_ERR("Decimation filter not initialized"); - return -EINVAL; - } - - if (input == NULL || output == NULL || num_frames == 0) { - LOG_ERR("Invalid parameters"); - return -EINVAL; - } - - if (num_frames > (MAX_SAMPLES_PER_BLOCK / 2)) { - LOG_ERR("Input buffer too large: %d frames (max %d)", - num_frames, MAX_SAMPLES_PER_BLOCK / 2); - return -EINVAL; - } - - uint32_t num_samples = num_frames * 2; /* Stereo: 2 samples per frame */ - - /* Convert int16 to float32 (scale by 1/32768.0) */ - const float scale_to_float = 1.0f / 32768.0f; - for (uint32_t i = 0; i < num_samples; i++) { - decimation_ctx.input_buf_f32[i] = (float)input[i] * scale_to_float; - } - - /* Apply anti-aliasing filter using CMSIS-DSP stereo biquad */ - arm_biquad_cascade_stereo_df2T_f32(&decimation_ctx.biquad_stereo, - decimation_ctx.input_buf_f32, - decimation_ctx.filtered_buf_f32, - num_frames); - - /* Decimate: keep every Nth sample */ - uint32_t output_frames = num_frames / decimation_ctx.decimation_factor; - const float scale_to_int16 = 32767.0f; - - for (uint32_t i = 0; i < output_frames; i++) { - uint32_t in_idx = i * decimation_ctx.decimation_factor * 2; - uint32_t out_idx = i * 2; - - /* Convert back to int16 with clamping */ - float val_l = decimation_ctx.filtered_buf_f32[in_idx] * scale_to_int16; - float val_r = decimation_ctx.filtered_buf_f32[in_idx + 1] * scale_to_int16; - - /* Clamp to int16 range */ - if (val_l > 32767.0f) val_l = 32767.0f; - if (val_l < -32768.0f) val_l = -32768.0f; - if (val_r > 32767.0f) val_r = 32767.0f; - if (val_r < -32768.0f) val_r = -32768.0f; - - output[out_idx] = (int16_t)val_l; - output[out_idx + 1] = (int16_t)val_r; - } - - return output_frames; -} - -int decimation_filter_process_stereo_f32(const float *input, float *output, uint32_t num_frames) -{ - if (!decimation_ctx.initialized) { - LOG_ERR("Decimation filter not initialized"); - return -EINVAL; - } - - if (input == NULL || output == NULL || num_frames == 0) { - LOG_ERR("Invalid parameters"); - return -EINVAL; - } - - if (num_frames > (MAX_SAMPLES_PER_BLOCK / 2)) { - LOG_ERR("Input buffer too large: %d frames (max %d)", - num_frames, MAX_SAMPLES_PER_BLOCK / 2); - return -EINVAL; - } - - /* Apply anti-aliasing filter using CMSIS-DSP stereo biquad */ - arm_biquad_cascade_stereo_df2T_f32(&decimation_ctx.biquad_stereo, - input, - decimation_ctx.filtered_buf_f32, - num_frames); - - /* Decimate: keep every Nth sample */ - uint32_t output_frames = num_frames / decimation_ctx.decimation_factor; - - for (uint32_t i = 0; i < output_frames; i++) { - uint32_t in_idx = i * decimation_ctx.decimation_factor * 2; - uint32_t out_idx = i * 2; - - output[out_idx] = decimation_ctx.filtered_buf_f32[in_idx]; - output[out_idx + 1] = decimation_ctx.filtered_buf_f32[in_idx + 1]; - } - - return output_frames; -} - -int decimation_filter_process_mono(const int16_t *input, int16_t *output, uint32_t num_samples) -{ - if (!decimation_ctx.initialized) { - LOG_ERR("Decimation filter not initialized"); - return -EINVAL; - } - - if (input == NULL || output == NULL || num_samples == 0) { - LOG_ERR("Invalid parameters"); - return -EINVAL; - } - - if (num_samples > MAX_SAMPLES_PER_BLOCK) { - LOG_ERR("Input buffer too large: %d samples (max %d)", - num_samples, MAX_SAMPLES_PER_BLOCK); - return -EINVAL; - } - - /* Convert int16 to float32 */ - const float scale_to_float = 1.0f / 32768.0f; - for (uint32_t i = 0; i < num_samples; i++) { - decimation_ctx.input_buf_f32[i] = (float)input[i] * scale_to_float; - } - - /* Apply anti-aliasing filter using CMSIS-DSP mono biquad */ - arm_biquad_cascade_df1_f32(&decimation_ctx.biquad_mono, - decimation_ctx.input_buf_f32, - decimation_ctx.filtered_buf_f32, - num_samples); - - /* Decimate: keep every Nth sample */ - uint32_t output_samples = num_samples / decimation_ctx.decimation_factor; - const float scale_to_int16 = 32767.0f; - - for (uint32_t i = 0; i < output_samples; i++) { - uint32_t in_idx = i * decimation_ctx.decimation_factor; - - /* Convert back to int16 with clamping */ - float val = decimation_ctx.filtered_buf_f32[in_idx] * scale_to_int16; - - if (val > 32767.0f) val = 32767.0f; - if (val < -32768.0f) val = -32768.0f; - - output[i] = (int16_t)val; - } - - return output_samples; -} - -int decimation_filter_process_mono_f32(const float *input, float *output, uint32_t num_samples) +int decimation_filter_process(const q15_t *input, q15_t *output, uint32_t num_frames) { - if (!decimation_ctx.initialized) { - LOG_ERR("Decimation filter not initialized"); - return -EINVAL; - } - - if (input == NULL || output == NULL || num_samples == 0) { - LOG_ERR("Invalid parameters"); + if (!ctx.init || !input || !output || num_frames == 0 || num_frames > MAX_FRAMES) { return -EINVAL; } - if (num_samples > MAX_SAMPLES_PER_BLOCK) { - LOG_ERR("Input buffer too large: %d samples (max %d)", - num_samples, MAX_SAMPLES_PER_BLOCK); - return -EINVAL; - } + static q15_t temp[MAX_FRAMES * 2]; + uint32_t num_samples = num_frames * 2; - /* Apply anti-aliasing filter using CMSIS-DSP mono biquad */ - arm_biquad_cascade_df1_f32(&decimation_ctx.biquad_mono, - input, - decimation_ctx.filtered_buf_f32, - num_samples); + /* Apply anti-aliasing filter */ + arm_biquad_cascade_df1_q15(&ctx.biquad, (q15_t*)input, temp, num_samples); - /* Decimate: keep every Nth sample */ - uint32_t output_samples = num_samples / decimation_ctx.decimation_factor; + /* Decimate */ + uint32_t out_frames = num_frames / ctx.factor; + uint32_t step = ctx.factor * 2; - for (uint32_t i = 0; i < output_samples; i++) { - output[i] = decimation_ctx.filtered_buf_f32[i * decimation_ctx.decimation_factor]; + for (uint32_t i = 0; i < out_frames; i++) { + output[i * 2] = temp[i * step]; + output[i * 2 + 1] = temp[i * step + 1]; } - return output_samples; + return out_frames; } void decimation_filter_reset(void) { - if (!decimation_ctx.initialized) { - return; + if (ctx.init) { + memset(ctx.state, 0, sizeof(ctx.state)); } - - /* Clear filter states */ - memset(decimation_ctx.stereo_state, 0, sizeof(decimation_ctx.stereo_state)); - memset(decimation_ctx.mono_state, 0, sizeof(decimation_ctx.mono_state)); - - LOG_DBG("Decimation filter state reset"); -} - -uint8_t decimation_filter_get_factor(void) -{ - return decimation_ctx.initialized ? decimation_ctx.decimation_factor : 0; -} - -bool decimation_filter_is_initialized(void) -{ - return decimation_ctx.initialized; } diff --git a/src/audio/decimation_filter.h b/src/audio/decimation_filter.h index 87c24612..da5d8093 100644 --- a/src/audio/decimation_filter.h +++ b/src/audio/decimation_filter.h @@ -9,108 +9,34 @@ #include #include -#include + +#include "arm_math.h" #ifdef __cplusplus extern "C" { #endif /** - * @brief Initialize the decimation filter with CMSIS-DSP - * - * This function sets up the anti-aliasing lowpass filter and decimation - * parameters. Must be called before processing any audio data. - * - * @param input_sample_rate Input sampling rate in Hz (e.g., 192000) - * @param decimation_factor Decimation factor (e.g., 4 for 192kHz -> 48kHz) - * - * @retval 0 if successful - * @retval -EINVAL if parameters are invalid + * @brief Initialize decimation filter + * @param decimation_factor Decimation factor (2, 4, or 8) + * @return 0 on success, -EINVAL on error */ -int decimation_filter_init(uint32_t input_sample_rate, uint8_t decimation_factor); +int decimation_filter_init(uint8_t decimation_factor); /** - * @brief Process stereo audio data with decimation filter - * - * Applies anti-aliasing lowpass filter and decimates the signal. - * Input and output buffers can be the same for in-place processing. - * - * @param input Pointer to input buffer (interleaved stereo samples) - * @param output Pointer to output buffer (interleaved stereo samples) - * @param num_frames Number of stereo frames in input buffer - * - * @retval Number of output frames produced (num_frames / decimation_factor) - * @retval -EINVAL if filter not initialized or invalid parameters + * @brief Process stereo Q15 audio with decimation + * @param input Input buffer (interleaved stereo Q15) + * @param output Output buffer (interleaved stereo Q15) + * @param num_frames Number of input stereo frames + * @return Number of output frames, or negative on error */ -int decimation_filter_process_stereo(const int16_t *input, int16_t *output, uint32_t num_frames); +int decimation_filter_process(const q15_t *input, q15_t *output, uint32_t num_frames); /** - * @brief Process stereo audio data with decimation filter (float version) - * - * Applies anti-aliasing lowpass filter and decimates the signal. - * Uses floating point internally for better precision. - * - * @param input Pointer to input buffer (interleaved stereo float samples) - * @param output Pointer to output buffer (interleaved stereo float samples) - * @param num_frames Number of stereo frames in input buffer - * - * @retval Number of output frames produced (num_frames / decimation_factor) - * @retval -EINVAL if filter not initialized or invalid parameters - */ -int decimation_filter_process_stereo_f32(const float *input, float *output, uint32_t num_frames); - -/** - * @brief Process mono audio data with decimation filter - * - * Applies anti-aliasing lowpass filter and decimates the signal. - * Input and output buffers can be the same for in-place processing. - * - * @param input Pointer to input buffer (mono samples) - * @param output Pointer to output buffer (mono samples) - * @param num_samples Number of samples in input buffer - * - * @retval Number of output samples produced (num_samples / decimation_factor) - * @retval -EINVAL if filter not initialized or invalid parameters - */ -int decimation_filter_process_mono(const int16_t *input, int16_t *output, uint32_t num_samples); - -/** - * @brief Process mono audio data with decimation filter (float version) - * - * Applies anti-aliasing lowpass filter and decimates the signal. - * Uses floating point internally for better precision. - * - * @param input Pointer to input buffer (mono float samples) - * @param output Pointer to output buffer (mono float samples) - * @param num_samples Number of samples in input buffer - * - * @retval Number of output samples produced (num_samples / decimation_factor) - * @retval -EINVAL if filter not initialized or invalid parameters - */ -int decimation_filter_process_mono_f32(const float *input, float *output, uint32_t num_samples); - -/** - * @brief Reset the decimation filter state - * - * Clears the filter state variables. Useful when starting a new stream - * or recovering from an error condition. + * @brief Reset filter state */ void decimation_filter_reset(void); -/** - * @brief Get current decimation factor - * - * @return Current decimation factor, or 0 if not initialized - */ -uint8_t decimation_filter_get_factor(void); - -/** - * @brief Check if decimation filter is initialized - * - * @return true if initialized, false otherwise - */ -bool decimation_filter_is_initialized(void); - #ifdef __cplusplus } #endif From 715b7d6e8b894bbbee1bfbfffa850a3d1fcf16a2 Mon Sep 17 00:00:00 2001 From: mkuettner97 Date: Wed, 10 Dec 2025 10:31:10 +0100 Subject: [PATCH 03/16] draft: fix coefficients --- src/audio/decimation_filter.c | 26 +++++++++++++++++--------- 1 file changed, 17 insertions(+), 9 deletions(-) diff --git a/src/audio/decimation_filter.c b/src/audio/decimation_filter.c index 4c919652..2e4eadbc 100644 --- a/src/audio/decimation_filter.c +++ b/src/audio/decimation_filter.c @@ -12,21 +12,29 @@ LOG_MODULE_REGISTER(decimation_filter, CONFIG_AUDIO_DATAPATH_LOG_LEVEL); #define MAX_FRAMES 512 #define NUM_STAGES 2 -/* Anti-aliasing filter coefficients (Q1.15 format, 5 per stage: b0,0,b1,b2,-a1,-a2) */ -static const q15_t coeff_dec4[NUM_STAGES * 5] = { +static const q15_t coeff_dec4[NUM_STAGES * 6] = { /* Stage 1: Butterworth LP, Fc=19.2kHz @ 192kHz */ - 16, 0, 32, 16, 32016, -31357, + 167, 0, 335, 167, 14015, -3436, /* Stage 2 */ - 32768, 0, 16384, 32768, 32015, -31311 + 16384, 0, 32767, 16384, 18236, -9405 }; -static const q15_t coeff_dec2[NUM_STAGES * 5] = { - /* Stage 1: Butterworth LP, Fc=0.4*Fs @ 2x */ - 256, 0, 512, 256, 29133, -26513, +static const q15_t coeff_dec3[NUM_STAGES * 6] = { + /* Stage 1: Butterworth LP, Fc=19.2kHz @ 192kHz */ + 427, 0, 855, 427, 9102, -1819, + /* Stage 2 */ + 16384, 0, 32767, 16384, 12306, -8227 +}; + +static const q15_t coeff_dec2[NUM_STAGES * 6] = { + /* Stage 1: Butterworth LP, Fc=19.2kHz @ 192kHz */ + 1540, 0, 3080, 1540, 0, -648, /* Stage 2 */ - 32768, 0, 16384, 32768, 29129, -26469 + 16384, 0, 32767, 16384, 0, -7315 }; +const int post_shift = 1; + static struct { bool init; uint8_t factor; @@ -44,7 +52,7 @@ int decimation_filter_init(uint8_t decimation_factor) ctx.factor = decimation_factor; const q15_t *coeffs = (decimation_factor == 4) ? coeff_dec4 : coeff_dec2; - ctx.postshift = 2; /* Shift for Q1.15 */ + ctx.postshift = post_shift; /* Shift for Q1.15 */ arm_biquad_cascade_df1_init_q15(&ctx.biquad, NUM_STAGES, coeffs, ctx.state, ctx.postshift); memset(ctx.state, 0, sizeof(ctx.state)); From 7f8eb0247ce8bf88de03a829259ef1b3a656e3ad Mon Sep 17 00:00:00 2001 From: mkuettner97 Date: Wed, 10 Dec 2025 10:59:46 +0100 Subject: [PATCH 04/16] draft: f32 for stereo --- src/audio/audio_datapath.c | 8 ++-- src/audio/decimation_filter.c | 73 +++++++++++++++++++++++------------ src/audio/decimation_filter.h | 8 ++-- 3 files changed, 57 insertions(+), 32 deletions(-) diff --git a/src/audio/audio_datapath.c b/src/audio/audio_datapath.c index 51e5c27d..3b1daf9a 100644 --- a/src/audio/audio_datapath.c +++ b/src/audio/audio_datapath.c @@ -196,7 +196,7 @@ extern struct k_poll_signal encoder_sig; extern struct k_poll_event logger_sig; /* Decimation buffer for SD card logging */ -static q15_t decimated_audio[BLOCK_SIZE_BYTES / sizeof(q15_t) / 4]; /* /4 for decimation factor 4 */ +static int16_t decimated_audio[BLOCK_SIZE_BYTES / sizeof(int16_t) / 4]; /* /4 for decimation factor 4 */ // Funktion für den neuen Thread static void data_thread(void *arg1, void *arg2, void *arg3) @@ -234,12 +234,12 @@ static void data_thread(void *arg1, void *arg2, void *arg3) audio_msg.data.time = time_stamp; /* Decimate audio data from 192kHz to 48kHz (factor 4) */ - q15_t *audio_block = (q15_t *)(audio_item.data + (i * BLOCK_SIZE_BYTES)); - uint32_t num_frames = BLOCK_SIZE_BYTES / sizeof(q15_t) / 2; /* stereo frames */ + int16_t *audio_block = (int16_t *)(audio_item.data + (i * BLOCK_SIZE_BYTES)); + uint32_t num_frames = BLOCK_SIZE_BYTES / sizeof(int16_t) / 2; /* stereo frames */ int decimated_frames = decimation_filter_process(audio_block, decimated_audio, num_frames); if (decimated_frames > 0) { - uint32_t decimated_size = decimated_frames * 2 * sizeof(q15_t); + uint32_t decimated_size = decimated_frames * 2 * sizeof(int16_t); audio_msg.data.size = decimated_size; uint32_t data_size[2] = { diff --git a/src/audio/decimation_filter.c b/src/audio/decimation_filter.c index 2e4eadbc..bef52bd2 100644 --- a/src/audio/decimation_filter.c +++ b/src/audio/decimation_filter.c @@ -5,6 +5,7 @@ */ #include "decimation_filter.h" +#include "arm_math.h" #include LOG_MODULE_REGISTER(decimation_filter, CONFIG_AUDIO_DATAPATH_LOG_LEVEL); @@ -12,35 +13,48 @@ LOG_MODULE_REGISTER(decimation_filter, CONFIG_AUDIO_DATAPATH_LOG_LEVEL); #define MAX_FRAMES 512 #define NUM_STAGES 2 -static const q15_t coeff_dec4[NUM_STAGES * 6] = { +/* Float32 coefficients for stereo DF2T biquad (b0, b1, b2, -a1, -a2 per stage) */ +// static const float32_t coeff_dec4[NUM_STAGES * 5] = { +// /* Stage 1: Butterworth LP, Fc=19.2kHz @ 192kHz */ +// 0.00512f, 0.01024f, 0.00512f, 1.14441f, -0.20489f, +// /* Stage 2 */ +// 1.0f, 2.0f, 1.0f, 1.11169f, -0.28652f +// }; + +// static const float32_t coeff_dec2[NUM_STAGES * 5] = { +// /* Stage 1: Butterworth LP, Fc=~24kHz @ 96kHz */ +// 0.04702f, 0.09404f, 0.04702f, 0.00000f, -0.01977f, +// /* Stage 2 */ +// 1.0f, 2.0f, 1.0f, 0.00000f, -0.22314f +// }; + +static const float32_t coeff_dec4[NUM_STAGES * 5] = { /* Stage 1: Butterworth LP, Fc=19.2kHz @ 192kHz */ - 167, 0, 335, 167, 14015, -3436, + 0.01020948, 0.02041896, 0.01020948, 0.85539793, -0.20971536, /* Stage 2 */ - 16384, 0, 32767, 16384, 18236, -9405 + 1.0f, 2.0f, 1.0f, 1.11302985, -0.57406192 }; -static const q15_t coeff_dec3[NUM_STAGES * 6] = { +static const float32_t coeff_dec2[NUM_STAGES * 5] = { /* Stage 1: Butterworth LP, Fc=19.2kHz @ 192kHz */ - 427, 0, 855, 427, 9102, -1819, + 9.39808514e-02, 1.87961703e-01, 9.39808514e-02, 1.38777878e-16, -3.95661299e-02, /* Stage 2 */ - 16384, 0, 32767, 16384, 12306, -8227 + 1.0f, 2.0f, 1.0f, 1.11022302e-16, -4.46462692e-01 }; -static const q15_t coeff_dec2[NUM_STAGES * 6] = { +static const float32_t coeff_dec3[NUM_STAGES * 5] = { /* Stage 1: Butterworth LP, Fc=19.2kHz @ 192kHz */ - 1540, 0, 3080, 1540, 0, -648, + 0.01020948, 0.02041896, 0.01020948, 0.85539793, -0.20971536, /* Stage 2 */ - 16384, 0, 32767, 16384, 0, -7315 + 1.0f, 2.0f, 1.0f, 0.75108142, -0.50216284 }; -const int post_shift = 1; - static struct { bool init; uint8_t factor; - arm_biquad_casd_df1_inst_q15 biquad; - q15_t state[4 * NUM_STAGES]; - q15_t postshift; + arm_biquad_cascade_stereo_df2T_instance_f32 biquad; + float32_t state[4 * NUM_STAGES]; /* 4 states per stage for stereo DF2T */ + float32_t temp_f32[MAX_FRAMES * 2]; /* temp buffer for float conversion */ } ctx; int decimation_filter_init(uint8_t decimation_factor) @@ -51,10 +65,9 @@ int decimation_filter_init(uint8_t decimation_factor) ctx.factor = decimation_factor; - const q15_t *coeffs = (decimation_factor == 4) ? coeff_dec4 : coeff_dec2; - ctx.postshift = post_shift; /* Shift for Q1.15 */ + const float32_t *coeffs = (decimation_factor == 4) ? coeff_dec4 : coeff_dec2; - arm_biquad_cascade_df1_init_q15(&ctx.biquad, NUM_STAGES, coeffs, ctx.state, ctx.postshift); + arm_biquad_cascade_stereo_df2T_init_f32(&ctx.biquad, NUM_STAGES, coeffs, ctx.state); memset(ctx.state, 0, sizeof(ctx.state)); ctx.init = true; @@ -63,25 +76,37 @@ int decimation_filter_init(uint8_t decimation_factor) return 0; } -int decimation_filter_process(const q15_t *input, q15_t *output, uint32_t num_frames) +int decimation_filter_process(const int16_t *input, int16_t *output, uint32_t num_frames) { if (!ctx.init || !input || !output || num_frames == 0 || num_frames > MAX_FRAMES) { return -EINVAL; } - static q15_t temp[MAX_FRAMES * 2]; uint32_t num_samples = num_frames * 2; + const float32_t scale_to_f32 = 1.0f / 32768.0f; + const float32_t scale_to_i16 = 32767.0f; - /* Apply anti-aliasing filter */ - arm_biquad_cascade_df1_q15(&ctx.biquad, (q15_t*)input, temp, num_samples); + /* Convert int16 to float32 */ + for (uint32_t i = 0; i < num_samples; i++) { + ctx.temp_f32[i] = (float32_t)input[i]; // * scale_to_f32; + } + + /* Apply anti-aliasing filter using stereo DF2T */ + static float32_t filtered[MAX_FRAMES * 2]; + arm_biquad_cascade_stereo_df2T_f32(&ctx.biquad, ctx.temp_f32, filtered, num_frames); + + arm_clip_f32(filtered, filtered, -32768.0f, 32767.0f, num_samples); - /* Decimate */ + /* Decimate and convert back to int16 */ uint32_t out_frames = num_frames / ctx.factor; uint32_t step = ctx.factor * 2; for (uint32_t i = 0; i < out_frames; i++) { - output[i * 2] = temp[i * step]; - output[i * 2 + 1] = temp[i * step + 1]; + float32_t val_l = filtered[i * step]; // * scale_to_i16; + float32_t val_r = filtered[i * step + 1]; // * scale_to_i16; + + output[i * 2] = (int16_t)val_l; + output[i * 2 + 1] = (int16_t)val_r; } return out_frames; diff --git a/src/audio/decimation_filter.h b/src/audio/decimation_filter.h index da5d8093..db80ff49 100644 --- a/src/audio/decimation_filter.h +++ b/src/audio/decimation_filter.h @@ -24,13 +24,13 @@ extern "C" { int decimation_filter_init(uint8_t decimation_factor); /** - * @brief Process stereo Q15 audio with decimation - * @param input Input buffer (interleaved stereo Q15) - * @param output Output buffer (interleaved stereo Q15) + * @brief Process stereo int16 audio with decimation + * @param input Input buffer (interleaved stereo int16) + * @param output Output buffer (interleaved stereo int16) * @param num_frames Number of input stereo frames * @return Number of output frames, or negative on error */ -int decimation_filter_process(const q15_t *input, q15_t *output, uint32_t num_frames); +int decimation_filter_process(const int16_t *input, int16_t *output, uint32_t num_frames); /** * @brief Reset filter state From 04b4e31386436a9a87da1d558025454813999107 Mon Sep 17 00:00:00 2001 From: mkuettner97 Date: Wed, 10 Dec 2025 15:51:07 +0100 Subject: [PATCH 05/16] draft: cascaded decimator --- src/audio/CMakeLists.txt | 4 +- src/audio/audio_datapath.c | 24 ++- src/audio/audio_datapath.h | 40 ++++ src/audio/audio_datapath_decimator.cpp | 160 ++++++++++++++++ src/audio/decimation_filter.c | 101 +++++----- src/audio/decimation_filter.cpp | 246 +++++++++++++++++++++++++ src/audio/decimation_filter.h | 120 +++++++++++- src/audio/decimator_example.cpp | 93 ++++++++++ 8 files changed, 732 insertions(+), 56 deletions(-) create mode 100644 src/audio/audio_datapath_decimator.cpp create mode 100644 src/audio/decimation_filter.cpp create mode 100644 src/audio/decimator_example.cpp diff --git a/src/audio/CMakeLists.txt b/src/audio/CMakeLists.txt index 08070d02..c795b02b 100644 --- a/src/audio/CMakeLists.txt +++ b/src/audio/CMakeLists.txt @@ -10,7 +10,9 @@ target_sources(app PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/audio_datapath.c ${CMAKE_CURRENT_SOURCE_DIR}/sw_codec_select.c ${CMAKE_CURRENT_SOURCE_DIR}/le_audio_rx.c - ${CMAKE_CURRENT_SOURCE_DIR}/decimation_filter.c +# ${CMAKE_CURRENT_SOURCE_DIR}/decimation_filter.c + ${CMAKE_CURRENT_SOURCE_DIR}/decimation_filter.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/audio_datapath_decimator.cpp ${CMAKE_CURRENT_SOURCE_DIR}/sdlogger_wrapper.cpp # ${CMAKE_CURRENT_SOURCE_DIR}/rx_publish.c # ${CMAKE_CURRENT_SOURCE_DIR}/pdm_mic.c diff --git a/src/audio/audio_datapath.c b/src/audio/audio_datapath.c index 3b1daf9a..b8df5a5f 100644 --- a/src/audio/audio_datapath.c +++ b/src/audio/audio_datapath.c @@ -233,10 +233,16 @@ static void data_thread(void *arg1, void *arg2, void *arg3) audio_msg.data.id = ID_MICRO; audio_msg.data.time = time_stamp; - /* Decimate audio data from 192kHz to 48kHz (factor 4) */ + /* Decimate audio data from 192kHz to 48kHz (factor 4) using CascadedDecimator */ int16_t *audio_block = (int16_t *)(audio_item.data + (i * BLOCK_SIZE_BYTES)); uint32_t num_frames = BLOCK_SIZE_BYTES / sizeof(int16_t) / 2; /* stereo frames */ - int decimated_frames = decimation_filter_process(audio_block, decimated_audio, num_frames); + + int decimated_frames = audio_decimator_process_wrapper(audio_block, decimated_audio, num_frames); + /*if (decimated_frames < 0) { + /* Fallback to C implementation if wrapper fails */ + //decimated_frames = decimation_filter_process(audio_block, decimated_audio, num_frames); + //}*/ + if (decimated_frames > 0) { uint32_t decimated_size = decimated_frames * 2 * sizeof(int16_t); @@ -291,9 +297,14 @@ void start_data_thread(void) { if (data_thread_id == NULL) { /* Initialize decimation filter for 192kHz -> 48kHz (factor 4) */ - int ret = decimation_filter_init(4); - if (ret) { - LOG_ERR("Failed to initialize decimation filter: %d", ret); + int ret = audio_decimator_init_wrapper(4); + if (ret != 0) { + LOG_ERR("Failed to initialize CascadedDecimator via wrapper: %d", ret); + /* Fallback to C implementation */ + /*ret = decimation_filter_init(4); + if (ret != 0) { + LOG_ERR("Fallback decimation filter init also failed: %d", ret); + }*/ } data_thread_id = k_thread_create(&data_thread_data, data_thread_stack, CONFIG_ENCODER_STACK_SIZE, @@ -1213,6 +1224,9 @@ int audio_datapath_stop(void) data_fifo_empty(ctrl_blk.in.fifo); + /* Cleanup CascadedDecimator on stop */ + audio_decimator_cleanup_wrapper(); + return 0; } else { return -EALREADY; diff --git a/src/audio/audio_datapath.h b/src/audio/audio_datapath.h index 5e463367..31d5d240 100644 --- a/src/audio/audio_datapath.h +++ b/src/audio/audio_datapath.h @@ -103,6 +103,46 @@ int audio_datapath_release(); //void set_ring_buffer(struct ring_buf *ring_buf); +/** + * @brief C wrapper for decimator initialization + */ +int audio_decimator_init_wrapper(uint8_t factor); + +/** + * @brief C wrapper for decimator processing + */ +int audio_decimator_process_wrapper(const int16_t* input, int16_t* output, uint32_t num_frames); + +/** + * @brief C wrapper for decimator cleanup + */ +void audio_decimator_cleanup_wrapper(void); + +/** + * @brief C wrapper for decimator reset + */ +void audio_decimator_reset_wrapper(void); + +#ifdef __cplusplus +/** + * @brief Reset the audio decimator filter state + */ +void audio_datapath_decimator_reset(void); + +/** + * @brief Change decimation factor dynamically + * @param new_factor New total decimation factor (4, 6, 8, or 12) + * @return 0 on success, negative on error + */ +int audio_datapath_decimator_set_factor(uint8_t new_factor); + +/** + * @brief Get current decimation factor + * @return Current total decimation factor, or 0 if not initialized + */ +uint8_t audio_datapath_decimator_get_factor(void); +#endif + #ifdef __cplusplus } #endif diff --git a/src/audio/audio_datapath_decimator.cpp b/src/audio/audio_datapath_decimator.cpp new file mode 100644 index 00000000..a62d8f39 --- /dev/null +++ b/src/audio/audio_datapath_decimator.cpp @@ -0,0 +1,160 @@ +/* + * Audio datapath utility functions for CascadedDecimator + */ + +#include "decimation_filter.h" + +#include +#include +LOG_MODULE_DECLARE(audio_datapath); + +#ifdef __cplusplus + +/* CascadedDecimator instance for direct C++ usage */ +static CascadedDecimator* g_audio_decimator = nullptr; + +/** + * @brief Reset the audio decimator filter state + */ +void audio_datapath_decimator_reset(void) { + if (g_audio_decimator) { + g_audio_decimator->reset(); + LOG_DBG("CascadedDecimator state reset"); + } +} + +/** + * @brief Change decimation factor dynamically + * @param new_factor New total decimation factor (4, 6, 8, or 12) + * @return 0 on success, negative on error + */ +int audio_datapath_decimator_set_factor(uint8_t new_factor) { + if (g_audio_decimator) { + delete g_audio_decimator; + } + + g_audio_decimator = new CascadedDecimator(new_factor); + if (!g_audio_decimator) { + LOG_ERR("Failed to create new CascadedDecimator with factor %d", new_factor); + return -ENOMEM; + } + + int ret = g_audio_decimator->init(); + if (ret != 0) { + LOG_ERR("Failed to initialize new CascadedDecimator: %d", ret); + delete g_audio_decimator; + g_audio_decimator = nullptr; + return ret; + } + + LOG_INF("CascadedDecimator changed to factor %d", new_factor); + return 0; +} + +/** + * @brief Get current decimation factor + * @return Current total decimation factor, or 0 if not initialized + */ +uint8_t audio_datapath_decimator_get_factor(void) { + return g_audio_decimator ? g_audio_decimator->getTotalFactor() : 0; +} + +/** + * @brief Initialize the audio decimator with specified factor + * @param factor Decimation factor (4, 6, 8, or 12) + * @return 0 on success, negative on error + */ +int audio_datapath_decimator_init(uint8_t factor) { + if (g_audio_decimator) { + delete g_audio_decimator; + g_audio_decimator = nullptr; + } + + g_audio_decimator = new CascadedDecimator(factor); + if (!g_audio_decimator) { + LOG_ERR("Failed to create CascadedDecimator with factor %d", factor); + return -ENOMEM; + } + + int ret = g_audio_decimator->init(); + if (ret != 0) { + LOG_ERR("Failed to initialize CascadedDecimator: %d", ret); + delete g_audio_decimator; + g_audio_decimator = nullptr; + return ret; + } + + LOG_INF("CascadedDecimator (%dx) initialized successfully", factor); + return 0; +} + +/** + * @brief Process audio data through the decimator + * @param input Input buffer (interleaved stereo int16) + * @param output Output buffer (interleaved stereo int16) + * @param num_frames Number of input stereo frames + * @return Number of output frames, or negative on error + */ +int audio_datapath_decimator_process(const int16_t* input, int16_t* output, uint32_t num_frames) { + if (!g_audio_decimator) { + LOG_ERR("CascadedDecimator not initialized"); + return -EINVAL; + } + + return g_audio_decimator->process(input, output, num_frames); +} + +/** + * @brief Cleanup the audio decimator + */ +void audio_datapath_decimator_cleanup(void) { + if (g_audio_decimator) { + LOG_DBG("Cleaning up CascadedDecimator"); + delete g_audio_decimator; + g_audio_decimator = nullptr; + } +} + +#endif + +extern "C" { + /** + * @brief C wrapper for decimator initialization + */ + int audio_decimator_init_wrapper(uint8_t factor) { +#ifdef __cplusplus + return audio_datapath_decimator_init(factor); +#else + return -ENOTSUP; +#endif + } + + /** + * @brief C wrapper for decimator processing + */ + int audio_decimator_process_wrapper(const int16_t* input, int16_t* output, uint32_t num_frames) { +#ifdef __cplusplus + return audio_datapath_decimator_process(input, output, num_frames); +#else + return -ENOTSUP; +#endif + } + + /** + * @brief C wrapper for decimator cleanup + */ + void audio_decimator_cleanup_wrapper(void) { +#ifdef __cplusplus + audio_datapath_decimator_cleanup(); +#endif + } + + /** + * @brief C wrapper for decimator reset + */ + void audio_decimator_reset_wrapper(void) { +#ifdef __cplusplus + audio_datapath_decimator_reset(); +#endif + } +} \ No newline at end of file diff --git a/src/audio/decimation_filter.c b/src/audio/decimation_filter.c index bef52bd2..9179ed3a 100644 --- a/src/audio/decimation_filter.c +++ b/src/audio/decimation_filter.c @@ -10,85 +10,92 @@ LOG_MODULE_REGISTER(decimation_filter, CONFIG_AUDIO_DATAPATH_LOG_LEVEL); +// C interface using CascadedDecimator internally +#ifdef __cplusplus +static CascadedDecimator* g_decimator = nullptr; +#else +// Fallback for pure C compilation #define MAX_FRAMES 512 #define NUM_STAGES 2 -/* Float32 coefficients for stereo DF2T biquad (b0, b1, b2, -a1, -a2 per stage) */ -// static const float32_t coeff_dec4[NUM_STAGES * 5] = { -// /* Stage 1: Butterworth LP, Fc=19.2kHz @ 192kHz */ -// 0.00512f, 0.01024f, 0.00512f, 1.14441f, -0.20489f, -// /* Stage 2 */ -// 1.0f, 2.0f, 1.0f, 1.11169f, -0.28652f -// }; - -// static const float32_t coeff_dec2[NUM_STAGES * 5] = { -// /* Stage 1: Butterworth LP, Fc=~24kHz @ 96kHz */ -// 0.04702f, 0.09404f, 0.04702f, 0.00000f, -0.01977f, -// /* Stage 2 */ -// 1.0f, 2.0f, 1.0f, 0.00000f, -0.22314f -// }; - static const float32_t coeff_dec4[NUM_STAGES * 5] = { /* Stage 1: Butterworth LP, Fc=19.2kHz @ 192kHz */ - 0.01020948, 0.02041896, 0.01020948, 0.85539793, -0.20971536, - /* Stage 2 */ - 1.0f, 2.0f, 1.0f, 1.11302985, -0.57406192 -}; - -static const float32_t coeff_dec2[NUM_STAGES * 5] = { - /* Stage 1: Butterworth LP, Fc=19.2kHz @ 192kHz */ - 9.39808514e-02, 1.87961703e-01, 9.39808514e-02, 1.38777878e-16, -3.95661299e-02, + 0.01020948f, 0.02041896f, 0.01020948f, 0.85539793f, -0.20971536f, /* Stage 2 */ - 1.0f, 2.0f, 1.0f, 1.11022302e-16, -4.46462692e-01 -}; - -static const float32_t coeff_dec3[NUM_STAGES * 5] = { - /* Stage 1: Butterworth LP, Fc=19.2kHz @ 192kHz */ - 0.01020948, 0.02041896, 0.01020948, 0.85539793, -0.20971536, - /* Stage 2 */ - 1.0f, 2.0f, 1.0f, 0.75108142, -0.50216284 + 1.0f, 2.0f, 1.0f, 1.11302985f, -0.57406192f }; static struct { bool init; uint8_t factor; arm_biquad_cascade_stereo_df2T_instance_f32 biquad; - float32_t state[4 * NUM_STAGES]; /* 4 states per stage for stereo DF2T */ - float32_t temp_f32[MAX_FRAMES * 2]; /* temp buffer for float conversion */ + float32_t state[4 * NUM_STAGES]; + float32_t temp_f32[MAX_FRAMES * 2]; } ctx; +#endif int decimation_filter_init(uint8_t decimation_factor) { - if (decimation_factor != 2 && decimation_factor != 4 && decimation_factor != 8) { +#ifdef __cplusplus + if (g_decimator) { + delete g_decimator; + } + + g_decimator = new CascadedDecimator(decimation_factor); + if (!g_decimator) { + LOG_ERR("Failed to create CascadedDecimator"); + return -ENOMEM; + } + + int ret = g_decimator->init(); + if (ret != 0) { + delete g_decimator; + g_decimator = nullptr; + LOG_ERR("Failed to initialize CascadedDecimator: %d", ret); + return ret; + } + + LOG_INF("Decimation filter initialized with factor %d", decimation_factor); + return 0; +#else + // Fallback C implementation + if (decimation_factor != 4) { + LOG_ERR("C fallback only supports factor 4"); return -EINVAL; } ctx.factor = decimation_factor; - const float32_t *coeffs = (decimation_factor == 4) ? coeff_dec4 : coeff_dec2; - - arm_biquad_cascade_stereo_df2T_init_f32(&ctx.biquad, NUM_STAGES, coeffs, ctx.state); + arm_biquad_cascade_stereo_df2T_init_f32(&ctx.biquad, NUM_STAGES, coeff_dec4, ctx.state); memset(ctx.state, 0, sizeof(ctx.state)); ctx.init = true; LOG_INF("Decimation filter init: factor=%d", decimation_factor); return 0; +#endif } int decimation_filter_process(const int16_t *input, int16_t *output, uint32_t num_frames) { +#ifdef __cplusplus + if (!g_decimator) { + LOG_ERR("Decimator not initialized"); + return -EINVAL; + } + + return g_decimator->process(input, output, num_frames); +#else + // Fallback C implementation if (!ctx.init || !input || !output || num_frames == 0 || num_frames > MAX_FRAMES) { return -EINVAL; } uint32_t num_samples = num_frames * 2; - const float32_t scale_to_f32 = 1.0f / 32768.0f; - const float32_t scale_to_i16 = 32767.0f; /* Convert int16 to float32 */ for (uint32_t i = 0; i < num_samples; i++) { - ctx.temp_f32[i] = (float32_t)input[i]; // * scale_to_f32; + ctx.temp_f32[i] = (float32_t)input[i]; } /* Apply anti-aliasing filter using stereo DF2T */ @@ -102,19 +109,23 @@ int decimation_filter_process(const int16_t *input, int16_t *output, uint32_t nu uint32_t step = ctx.factor * 2; for (uint32_t i = 0; i < out_frames; i++) { - float32_t val_l = filtered[i * step]; // * scale_to_i16; - float32_t val_r = filtered[i * step + 1]; // * scale_to_i16; - - output[i * 2] = (int16_t)val_l; - output[i * 2 + 1] = (int16_t)val_r; + output[i * 2] = (int16_t)filtered[i * step]; + output[i * 2 + 1] = (int16_t)filtered[i * step + 1]; } return out_frames; +#endif } void decimation_filter_reset(void) { +#ifdef __cplusplus + if (g_decimator) { + g_decimator->reset(); + } +#else if (ctx.init) { memset(ctx.state, 0, sizeof(ctx.state)); } +#endif } diff --git a/src/audio/decimation_filter.cpp b/src/audio/decimation_filter.cpp new file mode 100644 index 00000000..68622e68 --- /dev/null +++ b/src/audio/decimation_filter.cpp @@ -0,0 +1,246 @@ +/* + * Copyright (c) 2025 + * + * SPDX-License-Identifier: LicenseRef-PCFT + */ + +#include "decimation_filter.h" +#include +#include +#include + +LOG_MODULE_REGISTER(decimator_cpp, CONFIG_AUDIO_DATAPATH_LOG_LEVEL); + +// Filter coefficients for different decimation factors +static const float32_t coeff_dec2[2 * 5] = { + /* Stage 1: Butterworth LP, Fc=0.4*Fs_out */ + 9.39808514e-02f, 1.87961703e-01f, 9.39808514e-02f, 1.38777878e-16f, -3.95661299e-02f, + /* Stage 2 */ + 1.0f, 2.0f, 1.0f, 1.11022302e-16f, -4.46462692e-01f +}; + +static const float32_t coeff_dec3[2 * 5] = { + /* Stage 1: Butterworth LP, Fc=0.33*Fs_out */ + 0.01020948f, 0.02041896f, 0.01020948f, 0.85539793f, -0.20971536f, + /* Stage 2 */ + 1.0f, 2.0f, 1.0f, 0.75108142f, -0.50216284f +}; + +// Decimator class implementation +Decimator::Decimator(uint8_t factor) + : factor_(factor), initialized_(false) { + memset(state_, 0, sizeof(state_)); + memset(temp_f32_, 0, sizeof(temp_f32_)); +} + +int Decimator::init() { + if (factor_ != 2 && factor_ != 3) { + LOG_ERR("Invalid decimation factor: %d (only 2 or 3 supported)", factor_); + return -EINVAL; + } + + const float32_t* coeffs = getCoefficients(); + if (!coeffs) { + LOG_ERR("Failed to get coefficients for factor %d", factor_); + return -EINVAL; + } + + arm_biquad_cascade_stereo_df2T_init_f32(&biquad_, NUM_STAGES, coeffs, state_); + memset(state_, 0, sizeof(state_)); + + initialized_ = true; + LOG_DBG("Decimator initialized with factor %d", factor_); + + return 0; +} + +int Decimator::process(const int16_t* input, int16_t* output, uint32_t num_frames) { + if (!initialized_ || !input || !output || num_frames == 0 || num_frames > MAX_FRAMES) { + return -EINVAL; + } + + uint32_t num_samples = num_frames * 2; + + // Convert int16 to float32 + for (uint32_t i = 0; i < num_samples; i++) { + temp_f32_[i] = static_cast(input[i]); + } + + // Apply anti-aliasing filter using stereo DF2T + static float32_t filtered[MAX_FRAMES * 2]; + arm_biquad_cascade_stereo_df2T_f32(&biquad_, temp_f32_, filtered, num_frames); + + // Clip to prevent overflow + arm_clip_f32(filtered, filtered, -32768.0f, 32767.0f, num_samples); + + // Decimate and convert back to int16 + uint32_t out_frames = num_frames / factor_; + uint32_t step = factor_ * 2; + + for (uint32_t i = 0; i < out_frames; i++) { + output[i * 2] = static_cast(filtered[i * step]); + output[i * 2 + 1] = static_cast(filtered[i * step + 1]); + } + + return out_frames; +} + +void Decimator::reset() { + if (initialized_) { + memset(state_, 0, sizeof(state_)); + } +} + +const float32_t* Decimator::getCoefficients() const { + switch (factor_) { + case 2: return coeff_dec2; + case 3: return coeff_dec3; + default: return nullptr; + } +} + +// CascadedDecimator class implementation +CascadedDecimator::CascadedDecimator(uint8_t total_factor) + : total_factor_(total_factor), num_stages_(0) { + memset(stages_, 0, sizeof(stages_)); + memset(temp_buffers_, 0, sizeof(temp_buffers_)); + setupStages(); +} + +CascadedDecimator::~CascadedDecimator() { + cleanupStages(); +} + +void CascadedDecimator::setupStages() { + switch (total_factor_) { + case 4: // 2x -> 2x + num_stages_ = 2; + stages_[0] = new Decimator(2); + stages_[1] = new Decimator(2); + break; + + case 6: // 3x -> 2x + num_stages_ = 2; + stages_[0] = new Decimator(3); + stages_[1] = new Decimator(2); + break; + + case 8: // 2x -> 2x -> 2x + num_stages_ = 3; + stages_[0] = new Decimator(2); + stages_[1] = new Decimator(2); + stages_[2] = new Decimator(2); + break; + + case 12: // 3x -> 2x -> 2x + num_stages_ = 3; + stages_[0] = new Decimator(3); + stages_[1] = new Decimator(2); + stages_[2] = new Decimator(2); + break; + + case 16: // 2x -> 2x -> 2x -> 2x + num_stages_ = 4; + stages_[0] = new Decimator(2); + stages_[1] = new Decimator(2); + stages_[2] = new Decimator(2); + stages_[3] = new Decimator(2); + break; + + case 24: // 3x -> 2x -> 2x -> 2x + num_stages_ = 4; + stages_[0] = new Decimator(3); + stages_[1] = new Decimator(2); + stages_[2] = new Decimator(2); + stages_[3] = new Decimator(2); + break; + + default: + LOG_ERR("Unsupported total decimation factor: %d", total_factor_); + num_stages_ = 0; + return; + } + + // Allocate temporary buffers between stages + for (uint8_t i = 0; i < num_stages_ - 1; i++) { + temp_buffers_[i] = new int16_t[MAX_FRAMES * 2]; + } + + LOG_DBG("CascadedDecimator setup for factor %d with %d stages", total_factor_, num_stages_); +} + +void CascadedDecimator::cleanupStages() { + for (uint8_t i = 0; i < num_stages_; i++) { + delete stages_[i]; + stages_[i] = nullptr; + } + + for (uint8_t i = 0; i < MAX_STAGES - 1; i++) { + delete[] temp_buffers_[i]; + temp_buffers_[i] = nullptr; + } +} + +int CascadedDecimator::init() { + if (num_stages_ == 0) { + LOG_ERR("No stages configured for total factor %d", total_factor_); + return -EINVAL; + } + + for (uint8_t i = 0; i < num_stages_; i++) { + if (!stages_[i]) { + LOG_ERR("Stage %d is null", i); + return -EINVAL; + } + + int ret = stages_[i]->init(); + if (ret != 0) { + LOG_ERR("Failed to initialize stage %d: %d", i, ret); + return ret; + } + } + + LOG_INF("CascadedDecimator initialized: total factor %d", total_factor_); + return 0; +} + +int CascadedDecimator::process(const int16_t* input, int16_t* output, uint32_t num_frames) { + if (num_stages_ == 0 || !input || !output || num_frames == 0) { + return -EINVAL; + } + + const int16_t* stage_input = input; + int16_t* stage_output = nullptr; + int frames = num_frames; + + for (uint8_t i = 0; i < num_stages_; i++) { + // Determine output buffer for this stage + if (i == num_stages_ - 1) { + // Last stage outputs to final output buffer + stage_output = output; + } else { + // Intermediate stage outputs to temp buffer + stage_output = temp_buffers_[i]; + } + + // Process this stage + frames = stages_[i]->process(stage_input, stage_output, frames); + if (frames < 0) { + LOG_ERR("Stage %d processing failed: %d", i, frames); + return frames; + } + + // Next stage input is this stage's output + stage_input = stage_output; + } + + return frames; +} + +void CascadedDecimator::reset() { + for (uint8_t i = 0; i < num_stages_; i++) { + if (stages_[i]) { + stages_[i]->reset(); + } + } +} \ No newline at end of file diff --git a/src/audio/decimation_filter.h b/src/audio/decimation_filter.h index db80ff49..c8292e22 100644 --- a/src/audio/decimation_filter.h +++ b/src/audio/decimation_filter.h @@ -9,22 +9,132 @@ #include #include - #include "arm_math.h" #ifdef __cplusplus + +/** + * @brief Single stage decimation filter class + */ +class Decimator { +public: + /** + * @brief Constructor + * @param factor Decimation factor (2 or 3) + */ + Decimator(uint8_t factor); + + /** + * @brief Destructor + */ + ~Decimator() = default; + + /** + * @brief Initialize the decimator + * @return 0 on success, negative on error + */ + int init(); + + /** + * @brief Process stereo int16 audio with decimation + * @param input Input buffer (interleaved stereo int16) + * @param output Output buffer (interleaved stereo int16) + * @param num_frames Number of input stereo frames + * @return Number of output frames, or negative on error + */ + int process(const int16_t* input, int16_t* output, uint32_t num_frames); + + /** + * @brief Reset filter state + */ + void reset(); + + /** + * @brief Get decimation factor + * @return Decimation factor + */ + uint8_t getFactor() const { return factor_; } + +private: + static constexpr uint32_t MAX_FRAMES = 512; + static constexpr uint32_t NUM_STAGES = 2; + + uint8_t factor_; + bool initialized_; + arm_biquad_cascade_stereo_df2T_instance_f32 biquad_; + float32_t state_[4 * NUM_STAGES]; + float32_t temp_f32_[MAX_FRAMES * 2]; + + const float32_t* getCoefficients() const; +}; + +/** + * @brief Cascaded decimation filter class + */ +class CascadedDecimator { +public: + /** + * @brief Constructor for predefined cascaded decimation factors + * @param total_factor Total decimation factor (4, 6, 8, or 12) + */ + CascadedDecimator(uint8_t total_factor); + + /** + * @brief Destructor + */ + ~CascadedDecimator(); + + /** + * @brief Initialize all cascaded decimators + * @return 0 on success, negative on error + */ + int init(); + + /** + * @brief Process stereo int16 audio with cascaded decimation + * @param input Input buffer (interleaved stereo int16) + * @param output Output buffer (interleaved stereo int16) + * @param num_frames Number of input stereo frames + * @return Number of output frames, or negative on error + */ + int process(const int16_t* input, int16_t* output, uint32_t num_frames); + + /** + * @brief Reset all filter states + */ + void reset(); + + /** + * @brief Get total decimation factor + * @return Total decimation factor + */ + uint8_t getTotalFactor() const { return total_factor_; } + +private: + static constexpr uint32_t MAX_STAGES = 4; + static constexpr uint32_t MAX_FRAMES = 512; + + uint8_t total_factor_; + uint8_t num_stages_; + Decimator* stages_[MAX_STAGES]; + int16_t* temp_buffers_[MAX_STAGES - 1]; + + void setupStages(); + void cleanupStages(); +}; + extern "C" { #endif /** - * @brief Initialize decimation filter - * @param decimation_factor Decimation factor (2, 4, or 8) + * @brief Initialize decimation filter (C interface) + * @param decimation_factor Decimation factor * @return 0 on success, -EINVAL on error */ int decimation_filter_init(uint8_t decimation_factor); /** - * @brief Process stereo int16 audio with decimation + * @brief Process stereo int16 audio with decimation (C interface) * @param input Input buffer (interleaved stereo int16) * @param output Output buffer (interleaved stereo int16) * @param num_frames Number of input stereo frames @@ -33,7 +143,7 @@ int decimation_filter_init(uint8_t decimation_factor); int decimation_filter_process(const int16_t *input, int16_t *output, uint32_t num_frames); /** - * @brief Reset filter state + * @brief Reset filter state (C interface) */ void decimation_filter_reset(void); diff --git a/src/audio/decimator_example.cpp b/src/audio/decimator_example.cpp new file mode 100644 index 00000000..a59ebfb7 --- /dev/null +++ b/src/audio/decimator_example.cpp @@ -0,0 +1,93 @@ +/* + * Copyright (c) 2025 + * + * SPDX-License-Identifier: LicenseRef-PCFT + */ + +#include "decimation_filter.h" +#include + +LOG_MODULE_REGISTER(decimator_example, CONFIG_AUDIO_DATAPATH_LOG_LEVEL); + +/** + * @brief Example showing how to use the new Decimator and CascadedDecimator classes + */ + +void decimator_usage_example() { + // Example 1: Single stage decimation by factor 2 + Decimator dec2(2); + if (dec2.init() == 0) { + LOG_INF("2x Decimator initialized successfully"); + + int16_t input_2x[96]; // 48 stereo frames at high rate + int16_t output_2x[48]; // 24 stereo frames at output rate + + // Fill input with test data + for (int i = 0; i < 96; i++) { + input_2x[i] = i * 100; + } + + int result = dec2.process(input_2x, output_2x, 48); + LOG_INF("2x Decimator processed %d frames -> %d frames", 48, result); + } + + // Example 2: Single stage decimation by factor 3 + Decimator dec3(3); + if (dec3.init() == 0) { + LOG_INF("3x Decimator initialized successfully"); + + int16_t input_3x[144]; // 72 stereo frames at high rate + int16_t output_3x[48]; // 24 stereo frames at output rate + + int result = dec3.process(input_3x, output_3x, 72); + LOG_INF("3x Decimator processed %d frames -> %d frames", 72, result); + } + + // Example 3: Cascaded decimation by factor 4 (2x -> 2x) + CascadedDecimator dec4(4); + if (dec4.init() == 0) { + LOG_INF("4x CascadedDecimator (2x->2x) initialized successfully"); + + int16_t input_4x[192]; // 96 stereo frames at input rate (e.g., 192kHz) + int16_t output_4x[48]; // 24 stereo frames at output rate (e.g., 48kHz) + + int result = dec4.process(input_4x, output_4x, 96); + LOG_INF("4x CascadedDecimator processed %d frames -> %d frames", 96, result); + } + + // Example 4: Cascaded decimation by factor 6 (3x -> 2x) + CascadedDecimator dec6(6); + if (dec6.init() == 0) { + LOG_INF("6x CascadedDecimator (3x->2x) initialized successfully"); + + int16_t input_6x[288]; // 144 stereo frames at input rate + int16_t output_6x[48]; // 24 stereo frames at output rate + + int result = dec6.process(input_6x, output_6x, 144); + LOG_INF("6x CascadedDecimator processed %d frames -> %d frames", 144, result); + } + + // Example 5: Cascaded decimation by factor 8 (2x -> 2x -> 2x) + CascadedDecimator dec8(8); + if (dec8.init() == 0) { + LOG_INF("8x CascadedDecimator (2x->2x->2x) initialized successfully"); + + int16_t input_8x[384]; // 192 stereo frames at input rate + int16_t output_8x[48]; // 24 stereo frames at output rate + + int result = dec8.process(input_8x, output_8x, 192); + LOG_INF("8x CascadedDecimator processed %d frames -> %d frames", 192, result); + } + + // Example 6: Cascaded decimation by factor 12 (3x -> 2x -> 2x) + CascadedDecimator dec12(12); + if (dec12.init() == 0) { + LOG_INF("12x CascadedDecimator (3x->2x->2x) initialized successfully"); + + int16_t input_12x[576]; // 288 stereo frames at input rate + int16_t output_12x[48]; // 24 stereo frames at output rate + + int result = dec12.process(input_12x, output_12x, 288); + LOG_INF("12x CascadedDecimator processed %d frames -> %d frames", 288, result); + } +} \ No newline at end of file From 8b508eb253f00ca628e71641c65148c5f6472eb3 Mon Sep 17 00:00:00 2001 From: mkuettner97 Date: Wed, 10 Dec 2025 16:45:43 +0100 Subject: [PATCH 06/16] draft: enable downsampling make configurable via App --- src/ParseInfo/DefaultSensors.h | 6 +- src/SensorManager/Microphone.cpp | 15 +-- src/SensorManager/Microphone.h | 2 +- src/audio/CMakeLists.txt | 1 - src/audio/audio_datapath.c | 22 +---- src/audio/audio_datapath.h | 21 +--- src/audio/audio_datapath_decimator.cpp | 76 +------------- src/audio/decimation_filter.c | 131 ------------------------- src/audio/decimator_example.cpp | 93 ------------------ 9 files changed, 24 insertions(+), 343 deletions(-) delete mode 100644 src/audio/decimation_filter.c delete mode 100644 src/audio/decimator_example.cpp diff --git a/src/ParseInfo/DefaultSensors.h b/src/ParseInfo/DefaultSensors.h index e6e3ef21..8ed9255e 100644 --- a/src/ParseInfo/DefaultSensors.h +++ b/src/ParseInfo/DefaultSensors.h @@ -144,8 +144,8 @@ SensorScheme defaultSensors[SENSOR_COUNT] = { .availableOptions = DATA_STORAGE | FREQUENCIES_DEFINED, // no streaming .frequencyOptions = { .frequencyCount = sizeof(Microphone::sample_rates.reg_vals), - .defaultFrequencyIndex = 1, - .maxBleFrequencyIndex = 1, + .defaultFrequencyIndex = 0, + .maxBleFrequencyIndex = 0, .frequencies = Microphone::sample_rates.sample_rates, }, }, @@ -159,7 +159,7 @@ SensorScheme defaultSensors[SENSOR_COUNT] = { .availableOptions = DATA_STREAMING | DATA_STORAGE | FREQUENCIES_DEFINED, .frequencyOptions = { .frequencyCount = sizeof(PPG::sample_rates.reg_vals), - .defaultFrequencyIndex = 2, + .defaultFrequencyIndex = 1, .maxBleFrequencyIndex = 12, .frequencies = PPG::sample_rates.sample_rates, }, diff --git a/src/SensorManager/Microphone.cpp b/src/SensorManager/Microphone.cpp index fe907e2d..91e53c96 100644 --- a/src/SensorManager/Microphone.cpp +++ b/src/SensorManager/Microphone.cpp @@ -26,19 +26,18 @@ extern void empty_fifo(); #endif #include -//LOG_MODULE_DECLARE(BMX160); LOG_MODULE_REGISTER(microphone, CONFIG_LOG_DEFAULT_LEVEL); extern struct data_fifo fifo_rx; Microphone Microphone::sensor; -const SampleRateSetting<1> Microphone::sample_rates = { - { 0 }, +const SampleRateSetting<9> Microphone::sample_rates = { + { 1, 2, 3, 4, 6, 8, 12, 16, 24 }, - { 48000 }, + { 48000, 24000, 16000, 12000, 8000, 6000, 4000, 3000, 2000 }, - { 48000.0 } + { 48000.0, 24000.0, 16000.0, 12000.0, 8000.0, 6000.0, 4000.0, 3000.0, 2000.0 } }; bool Microphone::init(struct k_msgq * queue) { @@ -55,7 +54,7 @@ bool Microphone::init(struct k_msgq * queue) { } void Microphone::start(int sample_rate_idx) { - ARG_UNUSED(sample_rate_idx); + //ARG_UNUSED(sample_rate_idx); int ret; @@ -63,6 +62,8 @@ void Microphone::start(int sample_rate_idx) { record_to_sd(true); + if (sample_rate_idx > 0) audio_datapath_decimator_init(sample_rates.reg_vals[sample_rate_idx]); + audio_datapath_aquire(&fifo_rx); _running = true; @@ -78,5 +79,7 @@ void Microphone::stop() { audio_datapath_release(); + audio_datapath_decimator_cleanup(); + _running = false; } \ No newline at end of file diff --git a/src/SensorManager/Microphone.h b/src/SensorManager/Microphone.h index d5c63c6d..5eb6d213 100644 --- a/src/SensorManager/Microphone.h +++ b/src/SensorManager/Microphone.h @@ -13,7 +13,7 @@ class Microphone : public EdgeMlSensor { void start(int sample_rate_idx) override; void stop() override; - const static SampleRateSetting<1> sample_rates; + const static SampleRateSetting<9> sample_rates; private: bool _active = false; }; diff --git a/src/audio/CMakeLists.txt b/src/audio/CMakeLists.txt index c795b02b..a58215fc 100644 --- a/src/audio/CMakeLists.txt +++ b/src/audio/CMakeLists.txt @@ -10,7 +10,6 @@ target_sources(app PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/audio_datapath.c ${CMAKE_CURRENT_SOURCE_DIR}/sw_codec_select.c ${CMAKE_CURRENT_SOURCE_DIR}/le_audio_rx.c -# ${CMAKE_CURRENT_SOURCE_DIR}/decimation_filter.c ${CMAKE_CURRENT_SOURCE_DIR}/decimation_filter.cpp ${CMAKE_CURRENT_SOURCE_DIR}/audio_datapath_decimator.cpp ${CMAKE_CURRENT_SOURCE_DIR}/sdlogger_wrapper.cpp diff --git a/src/audio/audio_datapath.c b/src/audio/audio_datapath.c index b8df5a5f..21f38331 100644 --- a/src/audio/audio_datapath.c +++ b/src/audio/audio_datapath.c @@ -237,13 +237,8 @@ static void data_thread(void *arg1, void *arg2, void *arg3) int16_t *audio_block = (int16_t *)(audio_item.data + (i * BLOCK_SIZE_BYTES)); uint32_t num_frames = BLOCK_SIZE_BYTES / sizeof(int16_t) / 2; /* stereo frames */ - int decimated_frames = audio_decimator_process_wrapper(audio_block, decimated_audio, num_frames); - /*if (decimated_frames < 0) { - /* Fallback to C implementation if wrapper fails */ - //decimated_frames = decimation_filter_process(audio_block, decimated_audio, num_frames); - //}*/ - - + int decimated_frames = audio_datapath_decimator_process(audio_block, decimated_audio, num_frames); + if (decimated_frames > 0) { uint32_t decimated_size = decimated_frames * 2 * sizeof(int16_t); audio_msg.data.size = decimated_size; @@ -296,17 +291,6 @@ void record_to_sd(bool active) { void start_data_thread(void) { if (data_thread_id == NULL) { - /* Initialize decimation filter for 192kHz -> 48kHz (factor 4) */ - int ret = audio_decimator_init_wrapper(4); - if (ret != 0) { - LOG_ERR("Failed to initialize CascadedDecimator via wrapper: %d", ret); - /* Fallback to C implementation */ - /*ret = decimation_filter_init(4); - if (ret != 0) { - LOG_ERR("Fallback decimation filter init also failed: %d", ret); - }*/ - } - data_thread_id = k_thread_create(&data_thread_data, data_thread_stack, CONFIG_ENCODER_STACK_SIZE, data_thread, NULL, NULL, NULL, K_PRIO_PREEMPT(5), 0, K_NO_WAIT); //CONFIG_DATA_THREAD_PRIO @@ -1225,7 +1209,7 @@ int audio_datapath_stop(void) data_fifo_empty(ctrl_blk.in.fifo); /* Cleanup CascadedDecimator on stop */ - audio_decimator_cleanup_wrapper(); + audio_datapath_decimator_cleanup(); return 0; } else { diff --git a/src/audio/audio_datapath.h b/src/audio/audio_datapath.h index 31d5d240..85459457 100644 --- a/src/audio/audio_datapath.h +++ b/src/audio/audio_datapath.h @@ -106,42 +106,29 @@ int audio_datapath_release(); /** * @brief C wrapper for decimator initialization */ -int audio_decimator_init_wrapper(uint8_t factor); +int audio_datapath_decimator_init(uint8_t factor); /** * @brief C wrapper for decimator processing */ -int audio_decimator_process_wrapper(const int16_t* input, int16_t* output, uint32_t num_frames); +int audio_datapath_decimator_process(const int16_t* input, int16_t* output, uint32_t num_frames); /** * @brief C wrapper for decimator cleanup */ -void audio_decimator_cleanup_wrapper(void); +void audio_datapath_decimator_cleanup(void); /** * @brief C wrapper for decimator reset */ -void audio_decimator_reset_wrapper(void); - -#ifdef __cplusplus -/** - * @brief Reset the audio decimator filter state - */ void audio_datapath_decimator_reset(void); -/** - * @brief Change decimation factor dynamically - * @param new_factor New total decimation factor (4, 6, 8, or 12) - * @return 0 on success, negative on error - */ -int audio_datapath_decimator_set_factor(uint8_t new_factor); - /** * @brief Get current decimation factor * @return Current total decimation factor, or 0 if not initialized */ uint8_t audio_datapath_decimator_get_factor(void); -#endif + #ifdef __cplusplus } diff --git a/src/audio/audio_datapath_decimator.cpp b/src/audio/audio_datapath_decimator.cpp index a62d8f39..727dcdd3 100644 --- a/src/audio/audio_datapath_decimator.cpp +++ b/src/audio/audio_datapath_decimator.cpp @@ -13,6 +13,7 @@ LOG_MODULE_DECLARE(audio_datapath); /* CascadedDecimator instance for direct C++ usage */ static CascadedDecimator* g_audio_decimator = nullptr; +extern "C" { /** * @brief Reset the audio decimator filter state */ @@ -23,34 +24,6 @@ void audio_datapath_decimator_reset(void) { } } -/** - * @brief Change decimation factor dynamically - * @param new_factor New total decimation factor (4, 6, 8, or 12) - * @return 0 on success, negative on error - */ -int audio_datapath_decimator_set_factor(uint8_t new_factor) { - if (g_audio_decimator) { - delete g_audio_decimator; - } - - g_audio_decimator = new CascadedDecimator(new_factor); - if (!g_audio_decimator) { - LOG_ERR("Failed to create new CascadedDecimator with factor %d", new_factor); - return -ENOMEM; - } - - int ret = g_audio_decimator->init(); - if (ret != 0) { - LOG_ERR("Failed to initialize new CascadedDecimator: %d", ret); - delete g_audio_decimator; - g_audio_decimator = nullptr; - return ret; - } - - LOG_INF("CascadedDecimator changed to factor %d", new_factor); - return 0; -} - /** * @brief Get current decimation factor * @return Current total decimation factor, or 0 if not initialized @@ -84,7 +57,7 @@ int audio_datapath_decimator_init(uint8_t factor) { return ret; } - LOG_INF("CascadedDecimator (%dx) initialized successfully", factor); + LOG_DBG("CascadedDecimator (%dx) initialized successfully", factor); return 0; } @@ -115,46 +88,5 @@ void audio_datapath_decimator_cleanup(void) { } } -#endif - -extern "C" { - /** - * @brief C wrapper for decimator initialization - */ - int audio_decimator_init_wrapper(uint8_t factor) { -#ifdef __cplusplus - return audio_datapath_decimator_init(factor); -#else - return -ENOTSUP; -#endif - } - - /** - * @brief C wrapper for decimator processing - */ - int audio_decimator_process_wrapper(const int16_t* input, int16_t* output, uint32_t num_frames) { -#ifdef __cplusplus - return audio_datapath_decimator_process(input, output, num_frames); -#else - return -ENOTSUP; -#endif - } - - /** - * @brief C wrapper for decimator cleanup - */ - void audio_decimator_cleanup_wrapper(void) { -#ifdef __cplusplus - audio_datapath_decimator_cleanup(); -#endif - } - - /** - * @brief C wrapper for decimator reset - */ - void audio_decimator_reset_wrapper(void) { -#ifdef __cplusplus - audio_datapath_decimator_reset(); -#endif - } -} \ No newline at end of file +}; +#endif \ No newline at end of file diff --git a/src/audio/decimation_filter.c b/src/audio/decimation_filter.c deleted file mode 100644 index 9179ed3a..00000000 --- a/src/audio/decimation_filter.c +++ /dev/null @@ -1,131 +0,0 @@ -/* - * Copyright (c) 2025 - * - * SPDX-License-Identifier: LicenseRef-PCFT - */ - -#include "decimation_filter.h" -#include "arm_math.h" -#include - -LOG_MODULE_REGISTER(decimation_filter, CONFIG_AUDIO_DATAPATH_LOG_LEVEL); - -// C interface using CascadedDecimator internally -#ifdef __cplusplus -static CascadedDecimator* g_decimator = nullptr; -#else -// Fallback for pure C compilation -#define MAX_FRAMES 512 -#define NUM_STAGES 2 - -static const float32_t coeff_dec4[NUM_STAGES * 5] = { - /* Stage 1: Butterworth LP, Fc=19.2kHz @ 192kHz */ - 0.01020948f, 0.02041896f, 0.01020948f, 0.85539793f, -0.20971536f, - /* Stage 2 */ - 1.0f, 2.0f, 1.0f, 1.11302985f, -0.57406192f -}; - -static struct { - bool init; - uint8_t factor; - arm_biquad_cascade_stereo_df2T_instance_f32 biquad; - float32_t state[4 * NUM_STAGES]; - float32_t temp_f32[MAX_FRAMES * 2]; -} ctx; -#endif - -int decimation_filter_init(uint8_t decimation_factor) -{ -#ifdef __cplusplus - if (g_decimator) { - delete g_decimator; - } - - g_decimator = new CascadedDecimator(decimation_factor); - if (!g_decimator) { - LOG_ERR("Failed to create CascadedDecimator"); - return -ENOMEM; - } - - int ret = g_decimator->init(); - if (ret != 0) { - delete g_decimator; - g_decimator = nullptr; - LOG_ERR("Failed to initialize CascadedDecimator: %d", ret); - return ret; - } - - LOG_INF("Decimation filter initialized with factor %d", decimation_factor); - return 0; -#else - // Fallback C implementation - if (decimation_factor != 4) { - LOG_ERR("C fallback only supports factor 4"); - return -EINVAL; - } - - ctx.factor = decimation_factor; - - arm_biquad_cascade_stereo_df2T_init_f32(&ctx.biquad, NUM_STAGES, coeff_dec4, ctx.state); - memset(ctx.state, 0, sizeof(ctx.state)); - - ctx.init = true; - LOG_INF("Decimation filter init: factor=%d", decimation_factor); - - return 0; -#endif -} - -int decimation_filter_process(const int16_t *input, int16_t *output, uint32_t num_frames) -{ -#ifdef __cplusplus - if (!g_decimator) { - LOG_ERR("Decimator not initialized"); - return -EINVAL; - } - - return g_decimator->process(input, output, num_frames); -#else - // Fallback C implementation - if (!ctx.init || !input || !output || num_frames == 0 || num_frames > MAX_FRAMES) { - return -EINVAL; - } - - uint32_t num_samples = num_frames * 2; - - /* Convert int16 to float32 */ - for (uint32_t i = 0; i < num_samples; i++) { - ctx.temp_f32[i] = (float32_t)input[i]; - } - - /* Apply anti-aliasing filter using stereo DF2T */ - static float32_t filtered[MAX_FRAMES * 2]; - arm_biquad_cascade_stereo_df2T_f32(&ctx.biquad, ctx.temp_f32, filtered, num_frames); - - arm_clip_f32(filtered, filtered, -32768.0f, 32767.0f, num_samples); - - /* Decimate and convert back to int16 */ - uint32_t out_frames = num_frames / ctx.factor; - uint32_t step = ctx.factor * 2; - - for (uint32_t i = 0; i < out_frames; i++) { - output[i * 2] = (int16_t)filtered[i * step]; - output[i * 2 + 1] = (int16_t)filtered[i * step + 1]; - } - - return out_frames; -#endif -} - -void decimation_filter_reset(void) -{ -#ifdef __cplusplus - if (g_decimator) { - g_decimator->reset(); - } -#else - if (ctx.init) { - memset(ctx.state, 0, sizeof(ctx.state)); - } -#endif -} diff --git a/src/audio/decimator_example.cpp b/src/audio/decimator_example.cpp deleted file mode 100644 index a59ebfb7..00000000 --- a/src/audio/decimator_example.cpp +++ /dev/null @@ -1,93 +0,0 @@ -/* - * Copyright (c) 2025 - * - * SPDX-License-Identifier: LicenseRef-PCFT - */ - -#include "decimation_filter.h" -#include - -LOG_MODULE_REGISTER(decimator_example, CONFIG_AUDIO_DATAPATH_LOG_LEVEL); - -/** - * @brief Example showing how to use the new Decimator and CascadedDecimator classes - */ - -void decimator_usage_example() { - // Example 1: Single stage decimation by factor 2 - Decimator dec2(2); - if (dec2.init() == 0) { - LOG_INF("2x Decimator initialized successfully"); - - int16_t input_2x[96]; // 48 stereo frames at high rate - int16_t output_2x[48]; // 24 stereo frames at output rate - - // Fill input with test data - for (int i = 0; i < 96; i++) { - input_2x[i] = i * 100; - } - - int result = dec2.process(input_2x, output_2x, 48); - LOG_INF("2x Decimator processed %d frames -> %d frames", 48, result); - } - - // Example 2: Single stage decimation by factor 3 - Decimator dec3(3); - if (dec3.init() == 0) { - LOG_INF("3x Decimator initialized successfully"); - - int16_t input_3x[144]; // 72 stereo frames at high rate - int16_t output_3x[48]; // 24 stereo frames at output rate - - int result = dec3.process(input_3x, output_3x, 72); - LOG_INF("3x Decimator processed %d frames -> %d frames", 72, result); - } - - // Example 3: Cascaded decimation by factor 4 (2x -> 2x) - CascadedDecimator dec4(4); - if (dec4.init() == 0) { - LOG_INF("4x CascadedDecimator (2x->2x) initialized successfully"); - - int16_t input_4x[192]; // 96 stereo frames at input rate (e.g., 192kHz) - int16_t output_4x[48]; // 24 stereo frames at output rate (e.g., 48kHz) - - int result = dec4.process(input_4x, output_4x, 96); - LOG_INF("4x CascadedDecimator processed %d frames -> %d frames", 96, result); - } - - // Example 4: Cascaded decimation by factor 6 (3x -> 2x) - CascadedDecimator dec6(6); - if (dec6.init() == 0) { - LOG_INF("6x CascadedDecimator (3x->2x) initialized successfully"); - - int16_t input_6x[288]; // 144 stereo frames at input rate - int16_t output_6x[48]; // 24 stereo frames at output rate - - int result = dec6.process(input_6x, output_6x, 144); - LOG_INF("6x CascadedDecimator processed %d frames -> %d frames", 144, result); - } - - // Example 5: Cascaded decimation by factor 8 (2x -> 2x -> 2x) - CascadedDecimator dec8(8); - if (dec8.init() == 0) { - LOG_INF("8x CascadedDecimator (2x->2x->2x) initialized successfully"); - - int16_t input_8x[384]; // 192 stereo frames at input rate - int16_t output_8x[48]; // 24 stereo frames at output rate - - int result = dec8.process(input_8x, output_8x, 192); - LOG_INF("8x CascadedDecimator processed %d frames -> %d frames", 192, result); - } - - // Example 6: Cascaded decimation by factor 12 (3x -> 2x -> 2x) - CascadedDecimator dec12(12); - if (dec12.init() == 0) { - LOG_INF("12x CascadedDecimator (3x->2x->2x) initialized successfully"); - - int16_t input_12x[576]; // 288 stereo frames at input rate - int16_t output_12x[48]; // 24 stereo frames at output rate - - int result = dec12.process(input_12x, output_12x, 288); - LOG_INF("12x CascadedDecimator processed %d frames -> %d frames", 288, result); - } -} \ No newline at end of file From 9ea62e6ff11ef2bd06179bf6817dfdd621ef36fa Mon Sep 17 00:00:00 2001 From: mkuettner97 Date: Wed, 10 Dec 2025 22:27:15 +0100 Subject: [PATCH 07/16] draft: no decimator option --- src/SensorManager/Microphone.cpp | 2 +- src/audio/audio_datapath.c | 28 ++++++++++++++++------------ src/audio/decimation_filter.cpp | 17 ++++++++++++++++- 3 files changed, 33 insertions(+), 14 deletions(-) diff --git a/src/SensorManager/Microphone.cpp b/src/SensorManager/Microphone.cpp index 91e53c96..d95fbb97 100644 --- a/src/SensorManager/Microphone.cpp +++ b/src/SensorManager/Microphone.cpp @@ -62,7 +62,7 @@ void Microphone::start(int sample_rate_idx) { record_to_sd(true); - if (sample_rate_idx > 0) audio_datapath_decimator_init(sample_rates.reg_vals[sample_rate_idx]); + audio_datapath_decimator_init(sample_rates.reg_vals[sample_rate_idx]); audio_datapath_aquire(&fifo_rx); diff --git a/src/audio/audio_datapath.c b/src/audio/audio_datapath.c index 21f38331..97a83e0c 100644 --- a/src/audio/audio_datapath.c +++ b/src/audio/audio_datapath.c @@ -238,21 +238,25 @@ static void data_thread(void *arg1, void *arg2, void *arg3) uint32_t num_frames = BLOCK_SIZE_BYTES / sizeof(int16_t) / 2; /* stereo frames */ int decimated_frames = audio_datapath_decimator_process(audio_block, decimated_audio, num_frames); - - if (decimated_frames > 0) { - uint32_t decimated_size = decimated_frames * 2 * sizeof(int16_t); - audio_msg.data.size = decimated_size; - uint32_t data_size[2] = { - sizeof(audio_msg.data.id) + sizeof(audio_msg.data.size) + sizeof(audio_msg.data.time), - decimated_size - }; + uint32_t decimated_size = decimated_frames * 2 * sizeof(int16_t); + audio_msg.data.size = decimated_size; + + uint32_t data_size[2] = { + sizeof(audio_msg.data.id) + sizeof(audio_msg.data.size) + sizeof(audio_msg.data.time), + decimated_size + }; - const void *data_ptrs[2] = { - &audio_msg.data, - decimated_audio - }; + void *data_ptrs[2] = { + &audio_msg.data, + decimated_audio + }; + if (decimated_frames == num_frames) { + data_ptrs[1] = audio_block; + } + + if (decimated_frames > 0) { sdlogger_write_data(&data_ptrs, data_size, 2); } } diff --git a/src/audio/decimation_filter.cpp b/src/audio/decimation_filter.cpp index 68622e68..e807ac37 100644 --- a/src/audio/decimation_filter.cpp +++ b/src/audio/decimation_filter.cpp @@ -113,6 +113,17 @@ CascadedDecimator::~CascadedDecimator() { void CascadedDecimator::setupStages() { switch (total_factor_) { + case 1: // No decimation + num_stages_ = 0; + break; + case 2: // 2x + num_stages_ = 1; + stages_[0] = new Decimator(2); + break; + case 3: // 3x + num_stages_ = 1; + stages_[0] = new Decimator(3); + break; case 4: // 2x -> 2x num_stages_ = 2; stages_[0] = new Decimator(2); @@ -205,7 +216,11 @@ int CascadedDecimator::init() { } int CascadedDecimator::process(const int16_t* input, int16_t* output, uint32_t num_frames) { - if (num_stages_ == 0 || !input || !output || num_frames == 0) { + if (num_stages_ == 0) { + return num_frames; + } + + if (!input || !output || num_frames == 0) { return -EINVAL; } From f01f1cc2cd2b6cc129e072b11c61a11749c14a4b Mon Sep 17 00:00:00 2001 From: mkuettner97 Date: Thu, 11 Dec 2025 15:59:31 +0100 Subject: [PATCH 08/16] fix decimation factor 1 --- src/SensorManager/Microphone.cpp | 2 ++ src/audio/decimation_filter.cpp | 5 ----- unicast_server/main.cpp | 10 ---------- 3 files changed, 2 insertions(+), 15 deletions(-) diff --git a/src/SensorManager/Microphone.cpp b/src/SensorManager/Microphone.cpp index d95fbb97..9df3f71d 100644 --- a/src/SensorManager/Microphone.cpp +++ b/src/SensorManager/Microphone.cpp @@ -62,6 +62,8 @@ void Microphone::start(int sample_rate_idx) { record_to_sd(true); + LOG_INF("Starting Microphone at %d Hz", sample_rates.sample_rates[sample_rate_idx]); + audio_datapath_decimator_init(sample_rates.reg_vals[sample_rate_idx]); audio_datapath_aquire(&fifo_rx); diff --git a/src/audio/decimation_filter.cpp b/src/audio/decimation_filter.cpp index e807ac37..7d49f1a0 100644 --- a/src/audio/decimation_filter.cpp +++ b/src/audio/decimation_filter.cpp @@ -193,11 +193,6 @@ void CascadedDecimator::cleanupStages() { } int CascadedDecimator::init() { - if (num_stages_ == 0) { - LOG_ERR("No stages configured for total factor %d", total_factor_); - return -EINVAL; - } - for (uint8_t i = 0; i < num_stages_; i++) { if (!stages_[i]) { LOG_ERR("Stage %d is null", i); diff --git a/unicast_server/main.cpp b/unicast_server/main.cpp index ed11507c..706cab4e 100644 --- a/unicast_server/main.cpp +++ b/unicast_server/main.cpp @@ -95,16 +95,6 @@ int main(void) { init_sensor_manager(); - //sensor_config imu = {ID_IMU, 80, 0}; - //sensor_config imu = {ID_PPG, 400, 0}; - //sensor_config temp = {ID_OPTTEMP, 10, 0}; - // sensor_config temp = {ID_BONE_CONDUCTION, 100, 0}; - - //config_sensor(&temp); - - //sensor_config ppg = {ID_PPG, 400, 0}; - //config_sensor(&ppg); - ret = init_led_service(); ERR_CHK(ret); From 511ded42da96752f0f0ff70070e0fc6dd1be00e6 Mon Sep 17 00:00:00 2001 From: mkuettner97 Date: Thu, 11 Dec 2025 18:54:11 +0100 Subject: [PATCH 09/16] src/audio/audio_datapath.c: stop micro correctly --- src/audio/audio_datapath.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/audio/audio_datapath.c b/src/audio/audio_datapath.c index 97a83e0c..fdcab035 100644 --- a/src/audio/audio_datapath.c +++ b/src/audio/audio_datapath.c @@ -644,6 +644,9 @@ static void tone_stop_worker(struct k_work *work) tone_active = false; memset(test_tone_buf, 0, sizeof(test_tone_buf)); LOG_DBG("Tone stopped"); + + struct sensor_config mic = {ID_MICRO, 0, 0}; + config_sensor(&mic); } K_WORK_DEFINE(tone_stop_work, tone_stop_worker); From 119a4a8abd0f9497466f05405a5468271a477f91 Mon Sep 17 00:00:00 2001 From: mkuettner97 Date: Tue, 16 Dec 2025 13:19:48 +0100 Subject: [PATCH 10/16] fix: sd card crash at the end of the recording --- src/SensorManager/Microphone.cpp | 8 +-- src/SensorManager/SensorManager.cpp | 6 +++ src/audio/audio_datapath.c | 68 ++++++++++++++++---------- src/audio/audio_datapath.h | 10 ++++ src/audio/audio_datapath_decimator.cpp | 10 ++-- src/audio/decimation_filter.h | 28 ----------- 6 files changed, 68 insertions(+), 62 deletions(-) diff --git a/src/SensorManager/Microphone.cpp b/src/SensorManager/Microphone.cpp index 9df3f71d..09afd394 100644 --- a/src/SensorManager/Microphone.cpp +++ b/src/SensorManager/Microphone.cpp @@ -60,13 +60,13 @@ void Microphone::start(int sample_rate_idx) { if (!_active) return; - record_to_sd(true); - LOG_INF("Starting Microphone at %d Hz", sample_rates.sample_rates[sample_rate_idx]); + audio_datapath_aquire(&fifo_rx); + audio_datapath_decimator_init(sample_rates.reg_vals[sample_rate_idx]); - audio_datapath_aquire(&fifo_rx); + record_to_sd(true); _running = true; } @@ -81,7 +81,7 @@ void Microphone::stop() { audio_datapath_release(); - audio_datapath_decimator_cleanup(); + //audio_datapath_decimator_cleanup(); _running = false; } \ No newline at end of file diff --git a/src/SensorManager/SensorManager.cpp b/src/SensorManager/SensorManager.cpp index 1c8a994b..ae04829b 100644 --- a/src/SensorManager/SensorManager.cpp +++ b/src/SensorManager/SensorManager.cpp @@ -22,6 +22,8 @@ #include #include +#include "audio_datapath.h" + #include #include @@ -128,6 +130,10 @@ void stop_sensor_manager() { LOG_DBG("Stopping sensor manager"); + // Stop audio recording/processing first to prevent race condition + //extern "C" void audio_datapath_stop_recording(void); + audio_datapath_stop_recording(); + Baro::sensor.stop(); IMU::sensor.stop(); PPG::sensor.stop(); diff --git a/src/audio/audio_datapath.c b/src/audio/audio_datapath.c index fdcab035..3f5b6ae7 100644 --- a/src/audio/audio_datapath.c +++ b/src/audio/audio_datapath.c @@ -224,40 +224,49 @@ static void data_thread(void *arg1, void *arg2, void *arg3) data_fifo_block_free(ctrl_blk.in.fifo, tmp_pcm_raw_data[i]); - if (_record_to_sd) { - struct sensor_msg audio_msg; - - audio_msg.sd = true; - audio_msg.stream = false; - - audio_msg.data.id = ID_MICRO; - audio_msg.data.time = time_stamp; + unsigned int logger_signaled; + k_poll_signal_check(&logger_sig, &logger_signaled, &ret); - /* Decimate audio data from 192kHz to 48kHz (factor 4) using CascadedDecimator */ + if (ret == 0 && _record_to_sd) { + /* Decimate audio data from 48kHz to the desired sampling rate */ int16_t *audio_block = (int16_t *)(audio_item.data + (i * BLOCK_SIZE_BYTES)); uint32_t num_frames = BLOCK_SIZE_BYTES / sizeof(int16_t) / 2; /* stereo frames */ - + int decimated_frames = audio_datapath_decimator_process(audio_block, decimated_audio, num_frames); + + // If decimator returns 0 frames (e.g. during cleanup), skip processing + if (decimated_frames <= 0) { + continue; + } - uint32_t decimated_size = decimated_frames * 2 * sizeof(int16_t); - audio_msg.data.size = decimated_size; + if (logger_signaled != 0 && _record_to_sd) { + struct sensor_msg audio_msg; + + audio_msg.sd = true; + audio_msg.stream = false; + + audio_msg.data.id = ID_MICRO; + audio_msg.data.time = time_stamp; - uint32_t data_size[2] = { - sizeof(audio_msg.data.id) + sizeof(audio_msg.data.size) + sizeof(audio_msg.data.time), - decimated_size - }; + audio_msg.data.size = decimated_frames * 2 * sizeof(int16_t); - void *data_ptrs[2] = { - &audio_msg.data, - decimated_audio - }; + uint32_t data_size[2] = { + sizeof(audio_msg.data.id) + sizeof(audio_msg.data.size) + sizeof(audio_msg.data.time), + audio_msg.data.size + }; - if (decimated_frames == num_frames) { - data_ptrs[1] = audio_block; - } - - if (decimated_frames > 0) { - sdlogger_write_data(&data_ptrs, data_size, 2); + void *data_ptrs[2] = { + &audio_msg.data, + decimated_audio + }; + + if (decimated_frames == num_frames) { + data_ptrs[1] = audio_block; + } + + if (decimated_frames > 0) { + sdlogger_write_data(&data_ptrs, data_size, 2); + } } } @@ -291,6 +300,13 @@ void record_to_sd(bool active) { _record_to_sd = active; } +void audio_datapath_stop_recording(void) { + // Stop SD recording + _record_to_sd = false; + + LOG_DBG("Audio recording stopped safely"); +} + // Funktion, um den neuen Thread zu starten void start_data_thread(void) { diff --git a/src/audio/audio_datapath.h b/src/audio/audio_datapath.h index 85459457..6f758fad 100644 --- a/src/audio/audio_datapath.h +++ b/src/audio/audio_datapath.h @@ -35,6 +35,16 @@ int audio_datapath_tone_play(uint16_t freq, uint16_t dur_ms, float amplitude); */ void audio_datapath_tone_stop(void); +/** + * @brief Stops buffer recording safely + */ +void record_to_buffer_stop(void); + +/** + * @brief Stops all audio recording safely (buffer and SD) + */ +void audio_datapath_stop_recording(void); + /** * @brief Set the presentation delay * diff --git a/src/audio/audio_datapath_decimator.cpp b/src/audio/audio_datapath_decimator.cpp index 727dcdd3..04190e61 100644 --- a/src/audio/audio_datapath_decimator.cpp +++ b/src/audio/audio_datapath_decimator.cpp @@ -69,12 +69,14 @@ int audio_datapath_decimator_init(uint8_t factor) { * @return Number of output frames, or negative on error */ int audio_datapath_decimator_process(const int16_t* input, int16_t* output, uint32_t num_frames) { - if (!g_audio_decimator) { - LOG_ERR("CascadedDecimator not initialized"); - return -EINVAL; + // Thread-safe check for decimator availability + CascadedDecimator* decimator = g_audio_decimator; + if (!decimator) { + LOG_WRN("CascadedDecimator not available, returning 0 frames"); + return 0; // Return 0 frames instead of error to prevent crash } - return g_audio_decimator->process(input, output, num_frames); + return decimator->process(input, output, num_frames); } /** diff --git a/src/audio/decimation_filter.h b/src/audio/decimation_filter.h index c8292e22..2afd709f 100644 --- a/src/audio/decimation_filter.h +++ b/src/audio/decimation_filter.h @@ -122,33 +122,5 @@ class CascadedDecimator { void setupStages(); void cleanupStages(); }; - -extern "C" { -#endif - -/** - * @brief Initialize decimation filter (C interface) - * @param decimation_factor Decimation factor - * @return 0 on success, -EINVAL on error - */ -int decimation_filter_init(uint8_t decimation_factor); - -/** - * @brief Process stereo int16 audio with decimation (C interface) - * @param input Input buffer (interleaved stereo int16) - * @param output Output buffer (interleaved stereo int16) - * @param num_frames Number of input stereo frames - * @return Number of output frames, or negative on error - */ -int decimation_filter_process(const int16_t *input, int16_t *output, uint32_t num_frames); - -/** - * @brief Reset filter state (C interface) - */ -void decimation_filter_reset(void); - -#ifdef __cplusplus -} #endif - #endif /* _DECIMATION_FILTER_H_ */ From 951d1becd748a474f8245f7414810bc9b87c2f64 Mon Sep 17 00:00:00 2001 From: mkuettner97 Date: Mon, 22 Dec 2025 09:48:07 +0100 Subject: [PATCH 11/16] fix: add mutex to decimator to prevent crash at end of recording --- src/audio/audio_datapath_decimator.cpp | 44 +++++++++++++++++++++++--- 1 file changed, 39 insertions(+), 5 deletions(-) diff --git a/src/audio/audio_datapath_decimator.cpp b/src/audio/audio_datapath_decimator.cpp index 04190e61..73c9e43d 100644 --- a/src/audio/audio_datapath_decimator.cpp +++ b/src/audio/audio_datapath_decimator.cpp @@ -13,6 +13,12 @@ LOG_MODULE_DECLARE(audio_datapath); /* CascadedDecimator instance for direct C++ usage */ static CascadedDecimator* g_audio_decimator = nullptr; +/* Mutex to protect access to the decimator during cleanup */ +K_MUTEX_DEFINE(decimator_mutex); + +/* Flag to indicate decimator is being used - provides fast-path check */ +static volatile bool g_decimator_in_use = false; + extern "C" { /** * @brief Reset the audio decimator filter state @@ -38,6 +44,9 @@ uint8_t audio_datapath_decimator_get_factor(void) { * @return 0 on success, negative on error */ int audio_datapath_decimator_init(uint8_t factor) { + /* Lock mutex to ensure no concurrent access during init */ + k_mutex_lock(&decimator_mutex, K_FOREVER); + if (g_audio_decimator) { delete g_audio_decimator; g_audio_decimator = nullptr; @@ -46,6 +55,7 @@ int audio_datapath_decimator_init(uint8_t factor) { g_audio_decimator = new CascadedDecimator(factor); if (!g_audio_decimator) { LOG_ERR("Failed to create CascadedDecimator with factor %d", factor); + k_mutex_unlock(&decimator_mutex); return -ENOMEM; } @@ -54,10 +64,12 @@ int audio_datapath_decimator_init(uint8_t factor) { LOG_ERR("Failed to initialize CascadedDecimator: %d", ret); delete g_audio_decimator; g_audio_decimator = nullptr; + k_mutex_unlock(&decimator_mutex); return ret; } LOG_DBG("CascadedDecimator (%dx) initialized successfully", factor); + k_mutex_unlock(&decimator_mutex); return 0; } @@ -69,25 +81,47 @@ int audio_datapath_decimator_init(uint8_t factor) { * @return Number of output frames, or negative on error */ int audio_datapath_decimator_process(const int16_t* input, int16_t* output, uint32_t num_frames) { - // Thread-safe check for decimator availability - CascadedDecimator* decimator = g_audio_decimator; - if (!decimator) { + /* Lock mutex to prevent cleanup during processing */ + if (k_mutex_lock(&decimator_mutex, K_NO_WAIT) != 0) { + /* Mutex is held by cleanup - decimator is being deleted */ + LOG_DBG("Decimator locked for cleanup, skipping process"); + return 0; + } + + if (!g_audio_decimator) { + k_mutex_unlock(&decimator_mutex); LOG_WRN("CascadedDecimator not available, returning 0 frames"); - return 0; // Return 0 frames instead of error to prevent crash + return 0; } - return decimator->process(input, output, num_frames); + g_decimator_in_use = true; + int result = g_audio_decimator->process(input, output, num_frames); + g_decimator_in_use = false; + + k_mutex_unlock(&decimator_mutex); + return result; } /** * @brief Cleanup the audio decimator */ void audio_datapath_decimator_cleanup(void) { + /* Acquire mutex to ensure no processing is happening */ + k_mutex_lock(&decimator_mutex, K_FOREVER); + if (g_audio_decimator) { + /* Wait briefly if decimator was recently in use (safety margin) */ + if (g_decimator_in_use) { + LOG_WRN("Decimator still in use, waiting..."); + k_sleep(K_MSEC(5)); + } + LOG_DBG("Cleaning up CascadedDecimator"); delete g_audio_decimator; g_audio_decimator = nullptr; } + + k_mutex_unlock(&decimator_mutex); } }; From 36113b4fa737ac73e7fa51eeaa3c185d6358cd8a Mon Sep 17 00:00:00 2001 From: Dennis Moschina <45356478+DennisMoschina@users.noreply.github.com> Date: Sat, 31 Jan 2026 00:52:41 +0100 Subject: [PATCH 12/16] src/SD_Card/SDLogger/SDLogger.cpp: reset signal state properly --- src/SD_Card/SDLogger/SDLogger.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/src/SD_Card/SDLogger/SDLogger.cpp b/src/SD_Card/SDLogger/SDLogger.cpp index fe89aaba..8ee93b36 100644 --- a/src/SD_Card/SDLogger/SDLogger.cpp +++ b/src/SD_Card/SDLogger/SDLogger.cpp @@ -89,6 +89,10 @@ void sd_listener_callback(const struct zbus_channel *chan) } } +inline void reset_logger_signal() { + k_poll_signal_reset(&logger_sig); + logger_evt.state = K_POLL_STATE_NOT_READY; +} void SDLogger::sensor_sd_task() { int ret; @@ -112,7 +116,7 @@ void SDLogger::sensor_sd_task() { // If a close/flush is in progress, do not write concurrently. if (atomic_get(&g_stop_writing)) { - k_poll_signal_reset(&logger_sig); + reset_logger_signal(); continue; } @@ -122,7 +126,7 @@ void SDLogger::sensor_sd_task() { ring_buf_reset(&ring_buffer); k_mutex_unlock(&ring_mutex); sdlogger.is_open = false; - k_poll_signal_reset(&logger_sig); + reset_logger_signal(); continue; } @@ -148,7 +152,7 @@ void SDLogger::sensor_sd_task() { if (claimed == 0 || data == nullptr) { // Nothing to write right now. - k_poll_signal_reset(&logger_sig); + reset_logger_signal(); continue; } @@ -165,7 +169,7 @@ void SDLogger::sensor_sd_task() { // Do not advance the ring buffer on error. // Wakeups will continue; user can call end(). - k_poll_signal_reset(&logger_sig); + reset_logger_signal(); continue; } @@ -177,7 +181,7 @@ void SDLogger::sensor_sd_task() { k_yield(); } - k_poll_signal_reset(&logger_sig); + reset_logger_signal(); STACK_USAGE_PRINT("sensor_msg_thread", &sdlogger.thread_data); } From 2a9c80c9886ce5f14e42b36f63d11c270486305c Mon Sep 17 00:00:00 2001 From: Dennis Moschina <45356478+DennisMoschina@users.noreply.github.com> Date: Sat, 31 Jan 2026 00:54:24 +0100 Subject: [PATCH 13/16] src/audio/audio_datapath.c: removed k_poll_signal_check to avoid deadlocks --- src/audio/audio_datapath.c | 54 ++++++++++++++++---------------------- 1 file changed, 23 insertions(+), 31 deletions(-) diff --git a/src/audio/audio_datapath.c b/src/audio/audio_datapath.c index 3f5b6ae7..e144939f 100644 --- a/src/audio/audio_datapath.c +++ b/src/audio/audio_datapath.c @@ -225,9 +225,8 @@ static void data_thread(void *arg1, void *arg2, void *arg3) data_fifo_block_free(ctrl_blk.in.fifo, tmp_pcm_raw_data[i]); unsigned int logger_signaled; - k_poll_signal_check(&logger_sig, &logger_signaled, &ret); - if (ret == 0 && _record_to_sd) { + if (_record_to_sd) { /* Decimate audio data from 48kHz to the desired sampling rate */ int16_t *audio_block = (int16_t *)(audio_item.data + (i * BLOCK_SIZE_BYTES)); uint32_t num_frames = BLOCK_SIZE_BYTES / sizeof(int16_t) / 2; /* stereo frames */ @@ -239,34 +238,32 @@ static void data_thread(void *arg1, void *arg2, void *arg3) continue; } - if (logger_signaled != 0 && _record_to_sd) { - struct sensor_msg audio_msg; - - audio_msg.sd = true; - audio_msg.stream = false; - - audio_msg.data.id = ID_MICRO; - audio_msg.data.time = time_stamp; + struct sensor_msg audio_msg; + + audio_msg.sd = true; + audio_msg.stream = false; + + audio_msg.data.id = ID_MICRO; + audio_msg.data.time = time_stamp; - audio_msg.data.size = decimated_frames * 2 * sizeof(int16_t); + audio_msg.data.size = decimated_frames * 2 * sizeof(int16_t); - uint32_t data_size[2] = { - sizeof(audio_msg.data.id) + sizeof(audio_msg.data.size) + sizeof(audio_msg.data.time), - audio_msg.data.size - }; + uint32_t data_size[2] = { + sizeof(audio_msg.data.id) + sizeof(audio_msg.data.size) + sizeof(audio_msg.data.time), + audio_msg.data.size + }; - void *data_ptrs[2] = { - &audio_msg.data, - decimated_audio - }; + void *data_ptrs[2] = { + &audio_msg.data, + decimated_audio + }; - if (decimated_frames == num_frames) { - data_ptrs[1] = audio_block; - } - - if (decimated_frames > 0) { - sdlogger_write_data(&data_ptrs, data_size, 2); - } + if (decimated_frames == num_frames) { + data_ptrs[1] = audio_block; + } + + if (decimated_frames > 0) { + sdlogger_write_data(&data_ptrs, data_size, 2); } } @@ -285,11 +282,6 @@ static void data_thread(void *arg1, void *arg2, void *arg3) } } -/*void set_ring_buffer(struct ring_buf *buf) -{ - ring_buffer = buf; -}*/ - void set_sensor_queue(struct k_msgq *queue) { sensor_queue = queue; From 585a809fcdeaaf951c08ad58c63b403781ec75fc Mon Sep 17 00:00:00 2001 From: Dennis Moschina <45356478+DennisMoschina@users.noreply.github.com> Date: Sat, 31 Jan 2026 00:55:22 +0100 Subject: [PATCH 14/16] src/SensorManager/Microphone.cpp: fixed printing mic sampling rate wrongly --- src/SensorManager/Microphone.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/SensorManager/Microphone.cpp b/src/SensorManager/Microphone.cpp index 09afd394..bf6a48c6 100644 --- a/src/SensorManager/Microphone.cpp +++ b/src/SensorManager/Microphone.cpp @@ -60,7 +60,7 @@ void Microphone::start(int sample_rate_idx) { if (!_active) return; - LOG_INF("Starting Microphone at %d Hz", sample_rates.sample_rates[sample_rate_idx]); + LOG_INF("Starting Microphone at %f Hz", sample_rates.sample_rates[sample_rate_idx]); audio_datapath_aquire(&fifo_rx); From acfd5da7d53a5e66120924bb8bf3830c0ca368ae Mon Sep 17 00:00:00 2001 From: Dennis Moschina <45356478+DennisMoschina@users.noreply.github.com> Date: Wed, 4 Feb 2026 12:34:28 +0100 Subject: [PATCH 15/16] src/SD_Card/SDLogger.cpp: make sure the logger event is reset correctly --- src/SD_Card/SDLogger/SDLogger.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/SD_Card/SDLogger/SDLogger.cpp b/src/SD_Card/SDLogger/SDLogger.cpp index 8ee93b36..8ec00f27 100644 --- a/src/SD_Card/SDLogger/SDLogger.cpp +++ b/src/SD_Card/SDLogger/SDLogger.cpp @@ -426,7 +426,6 @@ int SDLogger::end() { } if (!sd_card->is_mounted()) { - //k_poll_signal_reset(&logger_sig); is_open = false; return -ENODEV; } @@ -449,13 +448,13 @@ int SDLogger::end() { ret = sd_card->close_file(); k_mutex_unlock(&file_mutex); if (ret < 0) { - k_poll_signal_reset(&logger_sig); + reset_logger_signal(); return ret; } is_open = false; - k_poll_signal_reset(&logger_sig); + reset_logger_signal(); atomic_clear(&g_stop_writing); atomic_clear(&g_sd_removed); From f93a529b2a625c23209d110ae3a850cd47465011 Mon Sep 17 00:00:00 2001 From: Dennis Moschina <45356478+DennisMoschina@users.noreply.github.com> Date: Wed, 4 Feb 2026 13:22:57 +0100 Subject: [PATCH 16/16] src/SD_Card/SDLogger.cpp: allow writing multiple blocks at once, only wake up sd thread if enough data is available --- src/SD_Card/SDLogger/SDLogger.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/SD_Card/SDLogger/SDLogger.cpp b/src/SD_Card/SDLogger/SDLogger.cpp index 8ec00f27..d3a7ee2d 100644 --- a/src/SD_Card/SDLogger/SDLogger.cpp +++ b/src/SD_Card/SDLogger/SDLogger.cpp @@ -147,7 +147,7 @@ void SDLogger::sensor_sd_task() { // Claim up to one SD block from the ring buffer under lock. k_mutex_lock(&ring_mutex, K_FOREVER); - uint32_t claimed = ring_buf_get_claim(&ring_buffer, &data, SD_BLOCK_SIZE); + uint32_t claimed = ring_buf_get_claim(&ring_buffer, &data, fill - (fill % SD_BLOCK_SIZE)); k_mutex_unlock(&ring_mutex); if (claimed == 0 || data == nullptr) { @@ -358,7 +358,9 @@ int SDLogger::write_sensor_data(const void* const* data_blocks, const size_t* le k_mutex_unlock(&ring_mutex); - k_poll_signal_raise(&logger_sig, 0); + if (ring_buf_size_get(&ring_buffer) >= SD_BLOCK_SIZE) { + k_poll_signal_raise(&logger_sig, 0); + } return 0; }