From ccfc3e5d56d0d6634066c67680efe3b386fdab35 Mon Sep 17 00:00:00 2001 From: mkuettner97 Date: Tue, 9 Dec 2025 11:14:46 +0100 Subject: [PATCH 01/36] 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 7ba6169190d0789697fdb91296425228b6ab37ce Mon Sep 17 00:00:00 2001 From: mkuettner97 Date: Wed, 10 Dec 2025 08:30:22 +0100 Subject: [PATCH 02/36] 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 fc44bde344944d983a3c1bd28e5649f7b9e598fa Mon Sep 17 00:00:00 2001 From: mkuettner97 Date: Wed, 10 Dec 2025 10:31:10 +0100 Subject: [PATCH 03/36] 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 4d1884b1f4cb7844857e521349f086e6ec40cf6e Mon Sep 17 00:00:00 2001 From: mkuettner97 Date: Wed, 10 Dec 2025 10:59:46 +0100 Subject: [PATCH 04/36] 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 76818061fe2510a0054983c2c27c326551ce28e7 Mon Sep 17 00:00:00 2001 From: mkuettner97 Date: Wed, 10 Dec 2025 15:51:07 +0100 Subject: [PATCH 05/36] 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 ab87053a47b9d10377d086d4c3aa8089a09dc53e Mon Sep 17 00:00:00 2001 From: mkuettner97 Date: Wed, 10 Dec 2025 16:45:43 +0100 Subject: [PATCH 06/36] 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 a63d37dcbade6ff41f5a623347755d8e008b92d9 Mon Sep 17 00:00:00 2001 From: mkuettner97 Date: Wed, 10 Dec 2025 22:27:15 +0100 Subject: [PATCH 07/36] 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 089ea928323593b6e57e0db3660017fd5ea0e032 Mon Sep 17 00:00:00 2001 From: mkuettner97 Date: Thu, 11 Dec 2025 15:59:31 +0100 Subject: [PATCH 08/36] 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 a6da0bdf7abc9dcc0e5b869a06fadd4cbbbd20d4 Mon Sep 17 00:00:00 2001 From: mkuettner97 Date: Thu, 11 Dec 2025 18:54:11 +0100 Subject: [PATCH 09/36] 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 c8701625db50842fcd727b3a24c504a9209749db Mon Sep 17 00:00:00 2001 From: mkuettner97 Date: Tue, 16 Dec 2025 13:19:48 +0100 Subject: [PATCH 10/36] 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 7644454c9a1a30e5ae320b165a70865e5e8a659b Mon Sep 17 00:00:00 2001 From: mkuettner97 Date: Mon, 22 Dec 2025 09:48:07 +0100 Subject: [PATCH 11/36] 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 405a46c3562baa7e60bec609498c0bbf20a33fae 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/36] 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 a9a35d548551dbec1ac9f17d93b7825d3252805f 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/36] 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 f4259d8c420a83e5fa053c579df0a471e9dab201 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/36] 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 f642a9477b90bd6f8898a72eaf904a6239250781 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/36] 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 e2872415e2a41757d77f79b749cb23ef1a5f7dd2 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/36] 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; } From 8e4cefffd00ffe4b5882826c9ed25cac334d259d Mon Sep 17 00:00:00 2001 From: Dennis Moschina <45356478+DennisMoschina@users.noreply.github.com> Date: Wed, 4 Mar 2026 12:16:12 +0100 Subject: [PATCH 17/36] feat(version): introduce automatic semantic versioning --- version.cmake | 149 ++++++++++++++++++++++++++++++++++++++++++++++++-- version.h.in | 2 +- 2 files changed, 144 insertions(+), 7 deletions(-) diff --git a/version.cmake b/version.cmake index e1796f39..50ec31a5 100644 --- a/version.cmake +++ b/version.cmake @@ -1,16 +1,153 @@ -file(STRINGS "${CMAKE_CURRENT_SOURCE_DIR}/VERSION" version_lines) +get_filename_component(VERSION_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" DIRECTORY) + +set(VERSION_FILE "${VERSION_CMAKE_DIR}/VERSION") + +file(STRINGS "${VERSION_FILE}" version_lines) foreach(line ${version_lines}) if(line MATCHES "^VERSION_MAJOR = (.*)$") - set(VERSION_MAJOR ${CMAKE_MATCH_1}) + set(VERSION_MAJOR "${CMAKE_MATCH_1}") elseif(line MATCHES "^VERSION_MINOR = (.*)$") - set(VERSION_MINOR ${CMAKE_MATCH_1}) + set(VERSION_MINOR "${CMAKE_MATCH_1}") elseif(line MATCHES "^PATCHLEVEL = (.*)$") - set(VERSION_PATCH ${CMAKE_MATCH_1}) + set(VERSION_PATCH "${CMAKE_MATCH_1}") + elseif(line MATCHES "^VERSION_TWEAK = (.*)$") + set(VERSION_TWEAK "${CMAKE_MATCH_1}") + elseif(line MATCHES "^EXTRAVERSION = (.*)$") + set(VERSION_EXTRA "${CMAKE_MATCH_1}") + endif() +endforeach() + +foreach(required_var VERSION_MAJOR VERSION_MINOR VERSION_PATCH) + if(NOT DEFINED ${required_var}) + message(FATAL_ERROR "Missing ${required_var} in ${VERSION_FILE}") endif() endforeach() +set(FIRMWARE_BASE_VERSION "${VERSION_MAJOR}.${VERSION_MINOR}.${VERSION_PATCH}") +if(DEFINED VERSION_TWEAK AND NOT VERSION_TWEAK STREQUAL "" AND NOT VERSION_TWEAK STREQUAL "0") + string(APPEND FIRMWARE_BASE_VERSION ".${VERSION_TWEAK}") +endif() +if(DEFINED VERSION_EXTRA AND NOT VERSION_EXTRA STREQUAL "") + string(APPEND FIRMWARE_BASE_VERSION "${VERSION_EXTRA}") +endif() + +set(FIRMWARE_VERSION "${FIRMWARE_BASE_VERSION}") + +function(openearable_resolve_git_dir out_var) + set(git_path "${VERSION_CMAKE_DIR}/.git") + + if(IS_DIRECTORY "${git_path}") + set(${out_var} "${git_path}" PARENT_SCOPE) + return() + endif() + + if(EXISTS "${git_path}") + file(READ "${git_path}" git_dir_contents) + string(REGEX MATCH "gitdir: (.+)" _ "${git_dir_contents}") + if(CMAKE_MATCH_1) + set(parsed_git_dir "${CMAKE_MATCH_1}") + if(IS_ABSOLUTE "${parsed_git_dir}") + set(resolved_git_dir "${parsed_git_dir}") + else() + set(resolved_git_dir "${VERSION_CMAKE_DIR}/${parsed_git_dir}") + endif() + file(TO_CMAKE_PATH "${resolved_git_dir}" resolved_git_dir) + set(${out_var} "${resolved_git_dir}" PARENT_SCOPE) + endif() + endif() +endfunction() + +function(openearable_track_git_state git_dir) + if(NOT git_dir OR NOT EXISTS "${git_dir}/HEAD") + return() + endif() + + set_property( + DIRECTORY APPEND PROPERTY CMAKE_CONFIGURE_DEPENDS + "${git_dir}/HEAD" + "${git_dir}/index" + "${git_dir}/packed-refs" + ) + + file(READ "${git_dir}/HEAD" git_head) + string(STRIP "${git_head}" git_head) + if(git_head MATCHES "^ref: (.+)$") + set_property( + DIRECTORY APPEND PROPERTY CMAKE_CONFIGURE_DEPENDS + "${git_dir}/${CMAKE_MATCH_1}" + ) + endif() +endfunction() + +function(openearable_resolve_pr_suffix out_var) + set(pr_number "") + + if(DEFINED ENV{GITHUB_EVENT_NAME} AND "$ENV{GITHUB_EVENT_NAME}" STREQUAL "pull_request") + if(DEFINED ENV{GITHUB_REF} AND "$ENV{GITHUB_REF}" MATCHES "^refs/pull/([0-9]+)/") + set(pr_number "${CMAKE_MATCH_1}") + elseif(DEFINED ENV{GITHUB_HEAD_REF} AND DEFINED ENV{GITHUB_REF_NAME} AND + "$ENV{GITHUB_REF_NAME}" MATCHES "^([0-9]+)/") + set(pr_number "${CMAKE_MATCH_1}") + endif() + endif() + + if(pr_number STREQUAL "" AND DEFINED ENV{SYSTEM_PULLREQUEST_PULLREQUESTNUMBER}) + set(pr_number "$ENV{SYSTEM_PULLREQUEST_PULLREQUESTNUMBER}") + endif() + + if(pr_number STREQUAL "" AND DEFINED ENV{CHANGE_ID}) + set(pr_number "$ENV{CHANGE_ID}") + endif() + + if(pr_number STREQUAL "") + set(${out_var} "" PARENT_SCOPE) + else() + set(${out_var} "-pr${pr_number}" PARENT_SCOPE) + endif() +endfunction() + +find_package(Git QUIET) +openearable_resolve_pr_suffix(PR_SUFFIX) +if(GIT_FOUND) + openearable_resolve_git_dir(PROJECT_GIT_DIR) + if(PROJECT_GIT_DIR) + openearable_track_git_state("${PROJECT_GIT_DIR}") + + execute_process( + COMMAND "${GIT_EXECUTABLE}" -C "${VERSION_CMAKE_DIR}" describe --tags --long --dirty --match "v[0-9]*" + OUTPUT_VARIABLE git_describe + OUTPUT_STRIP_TRAILING_WHITESPACE + RESULT_VARIABLE git_describe_result + ERROR_QUIET + ) + + if(git_describe_result EQUAL 0 AND + git_describe MATCHES "^v([0-9]+\\.[0-9]+\\.[0-9]+(\\.[0-9]+)?)(-[0-9A-Za-z.-]+)?-([0-9]+)-(g[0-9a-f]+)(-dirty)?$") + set(GIT_TAG_VERSION "${CMAKE_MATCH_1}${CMAKE_MATCH_3}") + set(GIT_COMMITS_SINCE_TAG "${CMAKE_MATCH_4}") + set(GIT_SHORT_SHA "${CMAKE_MATCH_5}") + set(GIT_DIRTY_SUFFIX "${CMAKE_MATCH_6}") + + if(GIT_COMMITS_SINCE_TAG EQUAL 0 AND GIT_DIRTY_SUFFIX STREQUAL "") + set(FIRMWARE_VERSION "${GIT_TAG_VERSION}${PR_SUFFIX}") + else() + set(FIRMWARE_VERSION "${GIT_TAG_VERSION}-dev.${GIT_COMMITS_SINCE_TAG}${PR_SUFFIX}+${GIT_SHORT_SHA}") + if(NOT GIT_DIRTY_SUFFIX STREQUAL "") + string(APPEND FIRMWARE_VERSION ".dirty") + endif() + endif() + endif() + endif() +endif() + +if(PR_SUFFIX AND FIRMWARE_VERSION STREQUAL FIRMWARE_BASE_VERSION) + set(FIRMWARE_VERSION "${FIRMWARE_VERSION}${PR_SUFFIX}") +endif() + +message(STATUS "Firmware version: ${FIRMWARE_VERSION}") + configure_file( - ${CMAKE_CURRENT_SOURCE_DIR}/version.h.in - ${CMAKE_CURRENT_BINARY_DIR}/include/generated/version.h + "${VERSION_CMAKE_DIR}/version.h.in" + "${CMAKE_CURRENT_BINARY_DIR}/include/generated/version.h" @ONLY ) diff --git a/version.h.in b/version.h.in index f88aec38..a3cae258 100644 --- a/version.h.in +++ b/version.h.in @@ -1,6 +1,6 @@ #ifndef VERSION_H #define VERSION_H -#define FIRMWARE_VERSION "@VERSION_MAJOR@.@VERSION_MINOR@.@VERSION_PATCH@" +#define FIRMWARE_VERSION "@FIRMWARE_VERSION@" #endif /* VERSION_H */ From f4d892a1701608309305b59a36b1c4d2dbd1b0d9 Mon Sep 17 00:00:00 2001 From: Oliver Bagge Date: Fri, 30 Jan 2026 15:42:03 +0100 Subject: [PATCH 18/36] enable audio streaming --- src/ParseInfo/DefaultSensors.h | 2 +- src/SensorManager/Microphone.cpp | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/src/ParseInfo/DefaultSensors.h b/src/ParseInfo/DefaultSensors.h index 8ed9255e..128730f2 100644 --- a/src/ParseInfo/DefaultSensors.h +++ b/src/ParseInfo/DefaultSensors.h @@ -141,7 +141,7 @@ SensorScheme defaultSensors[SENSOR_COUNT] = { .groupCount = MICRO_GROUP_COUNT, .groups = microGroups, .configOptions = { - .availableOptions = DATA_STORAGE | FREQUENCIES_DEFINED, // no streaming + .availableOptions = DATA_STREAMING | DATA_STORAGE | FREQUENCIES_DEFINED, // no streaming .frequencyOptions = { .frequencyCount = sizeof(Microphone::sample_rates.reg_vals), .defaultFrequencyIndex = 0, diff --git a/src/SensorManager/Microphone.cpp b/src/SensorManager/Microphone.cpp index bf6a48c6..cd8afc37 100644 --- a/src/SensorManager/Microphone.cpp +++ b/src/SensorManager/Microphone.cpp @@ -61,6 +61,7 @@ void Microphone::start(int sample_rate_idx) { if (!_active) return; LOG_INF("Starting Microphone at %f Hz", sample_rates.sample_rates[sample_rate_idx]); + record_to_sd(_sd_logging); audio_datapath_aquire(&fifo_rx); From 8fdc81ecc09b87d9d231e001f6d7eb5c164ce426 Mon Sep 17 00:00:00 2001 From: Oliver Bagge Date: Mon, 2 Feb 2026 14:10:22 +0100 Subject: [PATCH 19/36] remove no streaming comment --- src/ParseInfo/DefaultSensors.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ParseInfo/DefaultSensors.h b/src/ParseInfo/DefaultSensors.h index 128730f2..cf794995 100644 --- a/src/ParseInfo/DefaultSensors.h +++ b/src/ParseInfo/DefaultSensors.h @@ -141,7 +141,7 @@ SensorScheme defaultSensors[SENSOR_COUNT] = { .groupCount = MICRO_GROUP_COUNT, .groups = microGroups, .configOptions = { - .availableOptions = DATA_STREAMING | DATA_STORAGE | FREQUENCIES_DEFINED, // no streaming + .availableOptions = DATA_STREAMING | DATA_STORAGE | FREQUENCIES_DEFINED, .frequencyOptions = { .frequencyCount = sizeof(Microphone::sample_rates.reg_vals), .defaultFrequencyIndex = 0, From 7e0331c6be19523608733bdddd1928faa9a5b633 Mon Sep 17 00:00:00 2001 From: Dennis Moschina <45356478+DennisMoschina@users.noreply.github.com> Date: Tue, 21 Apr 2026 14:45:49 +0200 Subject: [PATCH 20/36] fix(sensor): enhance GATT notification handling with context management --- src/bluetooth/gatt_services/sensor_service.c | 129 +++++++++++++++---- 1 file changed, 106 insertions(+), 23 deletions(-) diff --git a/src/bluetooth/gatt_services/sensor_service.c b/src/bluetooth/gatt_services/sensor_service.c index 17e9cd25..254cd2fc 100644 --- a/src/bluetooth/gatt_services/sensor_service.c +++ b/src/bluetooth/gatt_services/sensor_service.c @@ -10,6 +10,7 @@ LOG_MODULE_REGISTER(sensor_manager, CONFIG_MODULE_BUTTON_HANDLER_LOG_LEVEL); #define MAX_SENSOR_REC_NAME_LENGTH 64 +#define MAX_NOTIFIES_IN_FLIGHT 4 static struct k_thread thread_data_notify; @@ -24,8 +25,7 @@ static K_THREAD_STACK_DEFINE(thread_stack_notify, CONFIG_SENSOR_GATT_NOTIFY_STAC K_MSGQ_DEFINE(gatt_queue, sizeof(struct sensor_data), CONFIG_SENSOR_GATT_SUB_QUEUE_SIZE, 4); -//static struct sensor_msg msg; -static struct sensor_data sensor_data; +static struct sensor_data sensor_data_value; static struct sensor_config config; static bool notify_enabled = false; @@ -45,9 +45,78 @@ ZBUS_LISTENER_DEFINE(sensor_queue_listener, sensor_queue_listener_cb); static bool connection_complete = false; -int notify_count = 0; +/** + * @brief Tracks one in-flight GATT notification and its dedicated payload copy. + * + * @details The Bluetooth stack completes notifications asynchronously, so each + * pending notification needs stable storage until the completion callback runs. + */ +struct sensor_notify_context { + struct bt_gatt_notify_params params; + struct sensor_data payload; + bool in_use; +}; + +static int notify_count = 0; +static struct sensor_notify_context notify_contexts[MAX_NOTIFIES_IN_FLIGHT]; + +/** + * @brief Reset volatile notification state after a disconnect or local teardown. + */ +static void reset_sensor_notification_state(void) +{ + notify_enabled = false; + sensor_config_status_ntfy_enabled = false; + connection_complete = false; + notify_count = 0; + + for (size_t i = 0; i < ARRAY_SIZE(notify_contexts); i++) { + notify_contexts[i].in_use = false; + } -int MAX_NOTIFIES_IN_FLIGHT = 4; + k_msgq_purge(&gatt_queue); +} + +/** + * @brief Reserve a context for the next asynchronous notification. + * + * @retval Pointer to a free notification context. + * @retval NULL No context is currently available. + */ +static struct sensor_notify_context *acquire_notify_context(void) +{ + for (size_t i = 0; i < ARRAY_SIZE(notify_contexts); i++) { + if (!notify_contexts[i].in_use) { + notify_contexts[i].in_use = true; + return ¬ify_contexts[i]; + } + } + + return NULL; +} + +/** + * @brief Release an in-flight notification context and update backpressure state. + * + * @param[in] context Context to release. May be NULL. + */ +static void release_notify_context(struct sensor_notify_context *context) +{ + if (context != NULL) { + if (!context->in_use) { + return; + } + + context->in_use = false; + } + + if (notify_count > 0) { + notify_count--; + } else { + LOG_WRN("Notify count went below zero!"); + notify_count = 0; + } +} static void connect_evt_handler(const struct zbus_channel *chan) { @@ -61,9 +130,7 @@ static void connect_evt_handler(const struct zbus_channel *chan) break; case BT_MGMT_DISCONNECTED: - connection_complete = false; - notify_enabled = false; - k_msgq_purge(&gatt_queue); + reset_sensor_notification_state(); break; } } @@ -76,6 +143,12 @@ static void sensor_ccc_cfg_changed(const struct bt_gatt_attr *attr, LOG_INF("Sensor data notifications %s", notify_enabled ? "enabled" : "disabled"); k_msgq_purge(&gatt_queue); + if (!notify_enabled) { + notify_count = 0; + for (size_t i = 0; i < ARRAY_SIZE(notify_contexts); i++) { + notify_contexts[i].in_use = false; + } + } } static void sensor_config_status_ccc_cfg_changed(const struct bt_gatt_attr *attr, @@ -180,7 +253,7 @@ BT_GATT_CHARACTERISTIC(BT_UUID_SENSOR_CONFIG, BT_GATT_CHARACTERISTIC(BT_UUID_SENSOR_DATA, BT_GATT_CHRC_NOTIFY, BT_GATT_PERM_NONE, - NULL, NULL, &sensor_data), + NULL, NULL, &sensor_data_value), BT_GATT_CCC(sensor_ccc_cfg_changed, BT_GATT_PERM_READ | BT_GATT_PERM_WRITE), BT_GATT_CHARACTERISTIC(BT_UUID_SENSOR_CONFIG_STATUS, @@ -195,17 +268,19 @@ BT_GATT_CHARACTERISTIC(BT_UUID_SENSOR_RECORDING_NAME, read_sensor_rec_name, write_sensor_rec_name, NULL), ); -static void notify_complete() { - notify_count--; +/** + * @brief Release notification resources after the Bluetooth stack completes a send. + */ +static void notify_complete(struct bt_conn *conn, void *user_data) +{ + ARG_UNUSED(conn); - if (notify_count < 0) { - notify_count = 0; - LOG_WRN("Notify count went below zero!"); - } + release_notify_context((struct sensor_notify_context *)user_data); } static void notification_task(void) { int ret; + struct sensor_data sensor_data; while (1) { ret = k_msgq_get(&gatt_queue, &sensor_data, K_FOREVER); @@ -217,23 +292,31 @@ static void notification_task(void) { if (connection_complete && notify_enabled) { const uint16_t size = sizeof(sensor_data.id) + sizeof(sensor_data.size) + sizeof(sensor_data.time) + sensor_data.size; - - static struct bt_gatt_notify_params params; - params.attr = &sensor_service.attrs[4]; - params.data = &sensor_data; - params.len = size; - params.func = notify_complete; - params.user_data = NULL; + struct sensor_notify_context *context; while(notify_count >= MAX_NOTIFIES_IN_FLIGHT) { - k_yield(); // maybe replace with k_sleep? + k_sleep(K_MSEC(1)); } + context = acquire_notify_context(); + if (context == NULL) { + LOG_WRN("No free GATT notify context available"); + continue; + } + + context->payload = sensor_data; + context->params.attr = &sensor_service.attrs[4]; + context->params.data = &context->payload; + context->params.len = size; + context->params.func = notify_complete; + context->params.user_data = context; + notify_count++; - ret = bt_gatt_notify_cb(NULL, ¶ms); + ret = bt_gatt_notify_cb(NULL, &context->params); if (ret != 0) { LOG_WRN("Failed to send data: %d.\n", ret); + release_notify_context(context); } } } From 6f021cfe79c10d00046b70d1cdfb5ba348dc5e4b Mon Sep 17 00:00:00 2001 From: Dennis Moschina <45356478+DennisMoschina@users.noreply.github.com> Date: Tue, 21 Apr 2026 15:15:21 +0200 Subject: [PATCH 21/36] fix(sensor): improve notification context management with spinlock protection --- src/bluetooth/gatt_services/sensor_service.c | 172 ++++++++++++++----- 1 file changed, 127 insertions(+), 45 deletions(-) diff --git a/src/bluetooth/gatt_services/sensor_service.c b/src/bluetooth/gatt_services/sensor_service.c index 254cd2fc..3d821d14 100644 --- a/src/bluetooth/gatt_services/sensor_service.c +++ b/src/bluetooth/gatt_services/sensor_service.c @@ -30,12 +30,14 @@ static struct sensor_config config; static bool notify_enabled = false; static bool sensor_config_status_ntfy_enabled = false; +static struct k_spinlock notify_state_lock; void set_sensor_recording_name(const char *name); static char sensor_recording_name[MAX_SENSOR_REC_NAME_LENGTH] = "sensor_log_"; static struct sensor_config *active_sensor_configs; static size_t active_sensor_configs_size = 0; +static void notify_complete(struct bt_conn *conn, void *user_data); static void connect_evt_handler(const struct zbus_channel *chan); ZBUS_LISTENER_DEFINE(bt_mgmt_evt_listen2, connect_evt_handler); //static @@ -51,28 +53,37 @@ static bool connection_complete = false; * @details The Bluetooth stack completes notifications asynchronously, so each * pending notification needs stable storage until the completion callback runs. */ +enum sensor_notify_context_state { + SENSOR_NOTIFY_CONTEXT_FREE = 0, + SENSOR_NOTIFY_CONTEXT_RESERVED, + SENSOR_NOTIFY_CONTEXT_IN_FLIGHT, +}; + struct sensor_notify_context { struct bt_gatt_notify_params params; struct sensor_data payload; - bool in_use; + enum sensor_notify_context_state state; + uint32_t generation; }; static int notify_count = 0; static struct sensor_notify_context notify_contexts[MAX_NOTIFIES_IN_FLIGHT]; /** - * @brief Reset volatile notification state after a disconnect or local teardown. + * @brief Disable new sensor notifications and drop queued payloads. + * + * @details Reserved or already queued notifications are left to retire through + * the normal send-failure or completion paths so that slot ownership remains + * well defined across disconnect races. */ static void reset_sensor_notification_state(void) { + k_spinlock_key_t key = k_spin_lock(¬ify_state_lock); + notify_enabled = false; sensor_config_status_ntfy_enabled = false; connection_complete = false; - notify_count = 0; - - for (size_t i = 0; i < ARRAY_SIZE(notify_contexts); i++) { - notify_contexts[i].in_use = false; - } + k_spin_unlock(¬ify_state_lock, key); k_msgq_purge(&gatt_queue); } @@ -80,42 +91,65 @@ static void reset_sensor_notification_state(void) /** * @brief Reserve a context for the next asynchronous notification. * + * @param[in] data Payload to copy into the reserved notification context. * @retval Pointer to a free notification context. * @retval NULL No context is currently available. */ -static struct sensor_notify_context *acquire_notify_context(void) +static struct sensor_notify_context *acquire_notify_context(const struct sensor_data *data, + uint32_t *generation); + +/** + * @brief Mark a reserved context as queued in the Bluetooth stack. + * + * @param[in] context Context to update. + * @param[in] generation Reservation generation owned by the caller. + */ +static void mark_notify_context_in_flight(struct sensor_notify_context *context, uint32_t generation) { - for (size_t i = 0; i < ARRAY_SIZE(notify_contexts); i++) { - if (!notify_contexts[i].in_use) { - notify_contexts[i].in_use = true; - return ¬ify_contexts[i]; - } + k_spinlock_key_t key; + + if (context == NULL) { + return; } - return NULL; + key = k_spin_lock(¬ify_state_lock); + if (context->generation == generation && + context->state == SENSOR_NOTIFY_CONTEXT_RESERVED) { + context->state = SENSOR_NOTIFY_CONTEXT_IN_FLIGHT; + } + k_spin_unlock(¬ify_state_lock, key); } /** - * @brief Release an in-flight notification context and update backpressure state. + * @brief Release a reserved or completed notification context. * * @param[in] context Context to release. May be NULL. + * @param[in] generation Reservation generation owned by the caller. */ -static void release_notify_context(struct sensor_notify_context *context) +static void release_notify_context(struct sensor_notify_context *context, uint32_t generation) { - if (context != NULL) { - if (!context->in_use) { - return; - } + k_spinlock_key_t key; + + if (context == NULL) { + return; + } - context->in_use = false; + key = k_spin_lock(¬ify_state_lock); + if (context->generation != generation || + context->state == SENSOR_NOTIFY_CONTEXT_FREE) { + k_spin_unlock(¬ify_state_lock, key); + return; } + context->state = SENSOR_NOTIFY_CONTEXT_FREE; + if (notify_count > 0) { notify_count--; } else { LOG_WRN("Notify count went below zero!"); notify_count = 0; } + k_spin_unlock(¬ify_state_lock, key); } static void connect_evt_handler(const struct zbus_channel *chan) @@ -126,8 +160,12 @@ static void connect_evt_handler(const struct zbus_channel *chan) switch (msg->event) { case BT_MGMT_CONNECTED: + { + k_spinlock_key_t key = k_spin_lock(¬ify_state_lock); connection_complete = true; + k_spin_unlock(¬ify_state_lock, key); break; + } case BT_MGMT_DISCONNECTED: reset_sensor_notification_state(); @@ -138,23 +176,22 @@ static void connect_evt_handler(const struct zbus_channel *chan) static void sensor_ccc_cfg_changed(const struct bt_gatt_attr *attr, uint16_t value) { + k_spinlock_key_t key = k_spin_lock(¬ify_state_lock); + notify_enabled = (value == BT_GATT_CCC_NOTIFY); + k_spin_unlock(¬ify_state_lock, key); LOG_INF("Sensor data notifications %s", notify_enabled ? "enabled" : "disabled"); k_msgq_purge(&gatt_queue); - if (!notify_enabled) { - notify_count = 0; - for (size_t i = 0; i < ARRAY_SIZE(notify_contexts); i++) { - notify_contexts[i].in_use = false; - } - } } static void sensor_config_status_ccc_cfg_changed(const struct bt_gatt_attr *attr, uint16_t value) { + k_spinlock_key_t key = k_spin_lock(¬ify_state_lock); sensor_config_status_ntfy_enabled = (value == BT_GATT_CCC_NOTIFY); + k_spin_unlock(¬ify_state_lock, key); } static ssize_t write_config(struct bt_conn *conn, @@ -268,14 +305,52 @@ BT_GATT_CHARACTERISTIC(BT_UUID_SENSOR_RECORDING_NAME, read_sensor_rec_name, write_sensor_rec_name, NULL), ); +static struct sensor_notify_context *acquire_notify_context(const struct sensor_data *data, + uint32_t *generation) +{ + k_spinlock_key_t key = k_spin_lock(¬ify_state_lock); + + if (!connection_complete || !notify_enabled || notify_count >= MAX_NOTIFIES_IN_FLIGHT) { + k_spin_unlock(¬ify_state_lock, key); + return NULL; + } + + for (size_t i = 0; i < ARRAY_SIZE(notify_contexts); i++) { + if (notify_contexts[i].state == SENSOR_NOTIFY_CONTEXT_FREE) { + notify_contexts[i].state = SENSOR_NOTIFY_CONTEXT_RESERVED; + notify_contexts[i].generation++; + notify_contexts[i].payload = *data; + notify_contexts[i].params.attr = &sensor_service.attrs[4]; + notify_contexts[i].params.data = ¬ify_contexts[i].payload; + notify_contexts[i].params.func = notify_complete; + notify_contexts[i].params.user_data = ¬ify_contexts[i]; + notify_count++; + *generation = notify_contexts[i].generation; + k_spin_unlock(¬ify_state_lock, key); + return ¬ify_contexts[i]; + } + } + + k_spin_unlock(¬ify_state_lock, key); + return NULL; +} + /** * @brief Release notification resources after the Bluetooth stack completes a send. */ static void notify_complete(struct bt_conn *conn, void *user_data) { + struct sensor_notify_context *context = (struct sensor_notify_context *)user_data; + uint32_t generation; + ARG_UNUSED(conn); - release_notify_context((struct sensor_notify_context *)user_data); + if (context == NULL) { + return; + } + + generation = context->generation; + release_notify_context(context, generation); } static void notification_task(void) { @@ -290,34 +365,41 @@ static void notification_task(void) { continue; } - if (connection_complete && notify_enabled) { - const uint16_t size = sizeof(sensor_data.id) + sizeof(sensor_data.size) + sizeof(sensor_data.time) + sensor_data.size; + while (1) { + const uint16_t size = sizeof(sensor_data.id) + sizeof(sensor_data.size) + + sizeof(sensor_data.time) + sensor_data.size; struct sensor_notify_context *context; + uint32_t generation; - while(notify_count >= MAX_NOTIFIES_IN_FLIGHT) { - k_sleep(K_MSEC(1)); - } - - context = acquire_notify_context(); + context = acquire_notify_context(&sensor_data, &generation); if (context == NULL) { + k_spinlock_key_t key = k_spin_lock(¬ify_state_lock); + bool can_retry = connection_complete && notify_enabled; + bool saturated = notify_count >= MAX_NOTIFIES_IN_FLIGHT; + k_spin_unlock(¬ify_state_lock, key); + + if (!can_retry) { + break; + } + + if (saturated) { + k_sleep(K_MSEC(1)); + continue; + } + LOG_WRN("No free GATT notify context available"); - continue; + break; } - context->payload = sensor_data; - context->params.attr = &sensor_service.attrs[4]; - context->params.data = &context->payload; context->params.len = size; - context->params.func = notify_complete; - context->params.user_data = context; - - notify_count++; - ret = bt_gatt_notify_cb(NULL, &context->params); if (ret != 0) { LOG_WRN("Failed to send data: %d.\n", ret); - release_notify_context(context); + release_notify_context(context, generation); + } else { + mark_notify_context_in_flight(context, generation); } + break; } } } From 2c2b0dbc94ea844cbc30c17ed5381f75bb7fbce3 Mon Sep 17 00:00:00 2001 From: Dennis Moschina <45356478+DennisMoschina@users.noreply.github.com> Date: Tue, 21 Apr 2026 16:01:53 +0200 Subject: [PATCH 22/36] chore(VERSION): updated patch level --- VERSION | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/VERSION b/VERSION index 0b3afa17..ae3bfdfb 100644 --- a/VERSION +++ b/VERSION @@ -1,5 +1,5 @@ VERSION_MAJOR = 2 VERSION_MINOR = 2 -PATCHLEVEL = 2 +PATCHLEVEL = 3 VERSION_TWEAK = 0 EXTRAVERSION = From 56f9961b32475a9e57918d545fe480ddd16deef2 Mon Sep 17 00:00:00 2001 From: Dennis Moschina <45356478+DennisMoschina@users.noreply.github.com> Date: Mon, 27 Apr 2026 13:26:27 +0200 Subject: [PATCH 23/36] chore: bump version to 2.3.0 --- VERSION | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/VERSION b/VERSION index ae3bfdfb..d3e37008 100644 --- a/VERSION +++ b/VERSION @@ -1,5 +1,5 @@ VERSION_MAJOR = 2 -VERSION_MINOR = 2 -PATCHLEVEL = 3 +VERSION_MINOR = 3 +PATCHLEVEL = 0 VERSION_TWEAK = 0 EXTRAVERSION = From 46450e894212ba67311573539e6dc9b14b1c4cbb Mon Sep 17 00:00:00 2001 From: Dennis Moschina <45356478+DennisMoschina@users.noreply.github.com> Date: Mon, 4 May 2026 14:27:36 +0200 Subject: [PATCH 24/36] fix(microphone): reorder sample rates for correct initialization Co-authored-by: Copilot --- src/ParseInfo/DefaultSensors.h | 4 ++-- src/SensorManager/Microphone.cpp | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/ParseInfo/DefaultSensors.h b/src/ParseInfo/DefaultSensors.h index cf794995..28016c26 100644 --- a/src/ParseInfo/DefaultSensors.h +++ b/src/ParseInfo/DefaultSensors.h @@ -144,8 +144,8 @@ SensorScheme defaultSensors[SENSOR_COUNT] = { .availableOptions = DATA_STREAMING | DATA_STORAGE | FREQUENCIES_DEFINED, .frequencyOptions = { .frequencyCount = sizeof(Microphone::sample_rates.reg_vals), - .defaultFrequencyIndex = 0, - .maxBleFrequencyIndex = 0, + .defaultFrequencyIndex = 8, + .maxBleFrequencyIndex = 8, .frequencies = Microphone::sample_rates.sample_rates, }, }, diff --git a/src/SensorManager/Microphone.cpp b/src/SensorManager/Microphone.cpp index cd8afc37..aaddcdb9 100644 --- a/src/SensorManager/Microphone.cpp +++ b/src/SensorManager/Microphone.cpp @@ -33,11 +33,11 @@ extern struct data_fifo fifo_rx; Microphone Microphone::sensor; const SampleRateSetting<9> Microphone::sample_rates = { - { 1, 2, 3, 4, 6, 8, 12, 16, 24 }, + { 24, 16, 12, 8, 6, 4, 3, 2, 1 }, - { 48000, 24000, 16000, 12000, 8000, 6000, 4000, 3000, 2000 }, + { 2000, 3000, 4000, 6000, 8000, 12000, 16000, 24000, 48000 }, - { 48000.0, 24000.0, 16000.0, 12000.0, 8000.0, 6000.0, 4000.0, 3000.0, 2000.0 } + { 2000.0, 3000.0, 4000.0, 6000.0, 8000.0, 12000.0, 16000.0, 24000.0, 48000.0 } }; bool Microphone::init(struct k_msgq * queue) { From 7d5eacafcc22d916197704bb75746ae33c97cbed Mon Sep 17 00:00:00 2001 From: habibialireza Date: Wed, 29 Apr 2026 11:52:04 +0200 Subject: [PATCH 25/36] parsing scheme is being stored in .oe files. README in ParseInfo has more information --- src/ParseInfo/README | 50 ++++++++++++++++++- src/ParseInfo/SensorScheme.cpp | 74 +++++++++++++++++++++++++++- src/ParseInfo/SensorScheme.h | 4 +- src/SD_Card/SDLogger/SDLogger.cpp | 80 +++++++++++++++++++++++++++++-- src/SD_Card/SDLogger/SDLogger.h | 10 ++-- 5 files changed, 208 insertions(+), 10 deletions(-) diff --git a/src/ParseInfo/README b/src/ParseInfo/README index cfffef73..d1b70507 100644 --- a/src/ParseInfo/README +++ b/src/ParseInfo/README @@ -170,4 +170,52 @@ enum SensorConfigOptions { - `Length`: Length of the frequency array - `Default Frequency Index`: Index of the default frequency - `Max BLE Index`: Index of the maximum frequency for BLE streaming -- `Frequency Array`: Array of available frequencies \ No newline at end of file +- `Frequency Array`: Array of available frequencies + +## OE File Header + +`.oe` files written by `SDLogger` now embed the active parse scheme in the file header so the file stays self-describing. +All numeric fields are written in the device's little-endian binary representation. + +### Header version `0x0002` + +Files only contain the fixed header below and rely on a hard-coded parser outside the file: + +| Bytes | Field | Type | +|-------|-----------|--------| +| 0-1 | Version | uint16 | +| 2-9 | Timestamp | uint64 | + +### Header version `0x0003` + +Files with version `0x0003` extend the fixed header and add earable identity metadata ahead of it and append a serialized ParseInfo block. +`header_size` points to the first sensor data record in the file. +`Parse Info Size` is the size of the `Parse Info Blob`. +`Device ID` is the immutable Nordic chip identifier captured at boot. +`Side` is encoded as `0` for left, `1` for right, and `255` for unknown. +The ParseInfo blob starts immediately after `Side`. + +| Bytes | Field | Type | +|-------|-----------------|--------| +| 0-1 | Version | uint16 | +| 2-9 | Timestamp | uint64 | +| 10-13 | Header Size | uint32 | +| 14-17 | Parse Info Size | uint32 | +| 18-25 | Device ID | uint64 | +| 26 | Side | uint8 | +| 27-X | Parse Info Blob | byte[] | + +### Parse Info Blob + +The blob starts with the existing `Sensor List` serialization and then stores each referenced sensor scheme in the same order as the sensor IDs: + +| Bytes | Field | Type | +|-------|--------------------|--------| +| 0 | Sensor Count | uint8 | +| 1-N | Sensor IDs | uint8[] | +| ... | Sensor Scheme Size | uint16 | +| ... | Sensor Scheme | byte[] | +| ... | Sensor Scheme Size | uint16 | +| ... | Sensor Scheme | byte[] | + +Each `Sensor Scheme` payload uses the existing `Single Sensor Scheme` serialization already documented above. diff --git a/src/ParseInfo/SensorScheme.cpp b/src/ParseInfo/SensorScheme.cpp index 5b5de02e..292dc403 100644 --- a/src/ParseInfo/SensorScheme.cpp +++ b/src/ParseInfo/SensorScheme.cpp @@ -3,6 +3,8 @@ #include #include +#include +#include #include #include @@ -348,9 +350,77 @@ int initSensorSchemeForId(uint8_t id) { } struct SensorScheme* getSensorSchemeForId(uint8_t id) { - return sensorSchemesMap[id]; + auto sensorSchemeIt = sensorSchemesMap.find(id); + if (sensorSchemeIt == sensorSchemesMap.end()) { + return NULL; + } + + return sensorSchemeIt->second; } struct ParseInfoScheme* getParseInfoScheme() { return parseInfoSchemeStruct; -} \ No newline at end of file +} + +size_t getParseInfoStorageSize() { + if ((parseInfoSchemeStruct == NULL) || (parseInfoScheme == NULL)) { + return 0; + } + + size_t size = parseInfoSchemeSize; + + for (size_t i = 0; i < parseInfoSchemeStruct->sensorCount; i++) { + SensorScheme* scheme = getSensorSchemeForId(parseInfoSchemeStruct->sensorIds[i]); + if (scheme == NULL) { + return 0; + } + + size += sizeof(uint16_t); + size += getSensorSchemeSize(scheme); + } + + return size; +} + +ssize_t serializeParseInfoStorage(char* buffer, size_t bufferSize) { + if ((parseInfoSchemeStruct == NULL) || (parseInfoScheme == NULL) || (buffer == NULL)) { + return -ENODATA; + } + + size_t size = getParseInfoStorageSize(); + if ((size == 0) || (size > bufferSize)) { + return -ENOMEM; + } + + char* bufferStart = buffer; + memcpy(buffer, parseInfoScheme, parseInfoSchemeSize); + buffer += parseInfoSchemeSize; + + for (size_t i = 0; i < parseInfoSchemeStruct->sensorCount; i++) { + SensorScheme* scheme = getSensorSchemeForId(parseInfoSchemeStruct->sensorIds[i]); + if (scheme == NULL) { + return -ENOENT; + } + + size_t sensorSchemeSize = getSensorSchemeSize(scheme); + if (sensorSchemeSize > UINT16_MAX) { + return -EOVERFLOW; + } + + uint16_t encodedSchemeSize = (uint16_t)sensorSchemeSize; + memcpy(buffer, &encodedSchemeSize, sizeof(encodedSchemeSize)); + buffer += sizeof(encodedSchemeSize); + + ssize_t writtenSize = serializeSensorScheme(scheme, buffer, bufferSize - (buffer - bufferStart)); + if (writtenSize < 0) { + return writtenSize; + } + if ((size_t)writtenSize != sensorSchemeSize) { + return -EIO; + } + + buffer += writtenSize; + } + + return buffer - bufferStart; +} diff --git a/src/ParseInfo/SensorScheme.h b/src/ParseInfo/SensorScheme.h index 6c3a8d48..29c3ccf7 100644 --- a/src/ParseInfo/SensorScheme.h +++ b/src/ParseInfo/SensorScheme.h @@ -64,6 +64,8 @@ int initParseInfoService(struct ParseInfoScheme* scheme, struct SensorScheme* se struct SensorScheme* getSensorSchemeForId(uint8_t id); struct ParseInfoScheme* getParseInfoScheme(); +size_t getParseInfoStorageSize(); +ssize_t serializeParseInfoStorage(char* buffer, size_t bufferSize); float getSampleRateForSensorId(uint8_t id, uint8_t frequencyIndex); float getSampleRateForSensor(struct SensorScheme* sensorScheme, uint8_t frequencyIndex); @@ -72,4 +74,4 @@ float getSampleRateForSensor(struct SensorScheme* sensorScheme, uint8_t frequenc } #endif -#endif // _SENSOR_SCHEME_H \ No newline at end of file +#endif // _SENSOR_SCHEME_H diff --git a/src/SD_Card/SDLogger/SDLogger.cpp b/src/SD_Card/SDLogger/SDLogger.cpp index d3a7ee2d..335d4b1b 100644 --- a/src/SD_Card/SDLogger/SDLogger.cpp +++ b/src/SD_Card/SDLogger/SDLogger.cpp @@ -5,8 +5,12 @@ #include "../SensorManager/SensorManager.h" #include "macros_common.h" #include "SDLogger.h" +#include "BootState.h" #include "PowerManager.h" +#include "SensorScheme.h" +#include "channel_assignment.h" #include +#include #include "audio_datapath.h" #include "StateIndicator.h" @@ -48,6 +52,28 @@ struct k_poll_signal logger_sig; static struct k_poll_event logger_evt = K_POLL_EVENT_INITIALIZER(K_POLL_TYPE_SIGNAL, K_POLL_MODE_NOTIFY_ONLY, &logger_sig); +namespace { + +constexpr uint8_t OE_HEADER_SIDE_LEFT = 0x00; +constexpr uint8_t OE_HEADER_SIDE_RIGHT = 0x01; +constexpr uint8_t OE_HEADER_SIDE_UNKNOWN = 0xFF; + +uint8_t get_header_side() { + enum audio_channel channel; + channel_assignment_get(&channel); + + if (channel == AUDIO_CH_L) { + return OE_HEADER_SIDE_LEFT; + } + if (channel == AUDIO_CH_R) { + return OE_HEADER_SIDE_RIGHT; + } + + return OE_HEADER_SIDE_UNKNOWN; +} + +} // namespace + SDLogger::SDLogger() { sd_card = &sdcard_manager; k_mutex_init(&ring_mutex); @@ -279,6 +305,11 @@ int SDLogger::begin(const std::string& filename) { if (ret < 0) { state_indicator.set_sd_state(SD_FAULT); LOG_ERR("Failed to write header: %d", ret); + k_mutex_lock(&file_mutex, K_FOREVER); + sd_card->close_file(); + k_mutex_unlock(&file_mutex); + current_file.clear(); + is_open = false; return ret; } @@ -288,17 +319,60 @@ int SDLogger::begin(const std::string& filename) { } int SDLogger::write_header() { - size_t header_size = sizeof(FileHeader); - uint8_t header_buffer[header_size]; + const size_t parse_info_size = getParseInfoStorageSize(); + if (parse_info_size == 0) { + LOG_ERR("Parse info scheme is unavailable"); + return -ENODATA; + } + + const size_t header_size = sizeof(FileHeader) + parse_info_size; + if ((header_size > UINT32_MAX) || (parse_info_size > UINT32_MAX)) { + LOG_ERR("Parse info header too large: header=%zu parse_info=%zu", header_size, parse_info_size); + return -EOVERFLOW; + } + + uint8_t* header_buffer = static_cast(k_malloc(header_size)); + if (header_buffer == nullptr) { + LOG_ERR("Failed to allocate %zu bytes for OE header", header_size); + return -ENOMEM; + } + FileHeader* header = reinterpret_cast(header_buffer); header->version = SENSOR_LOG_VERSION; header->timestamp = micros(); + header->header_size = header_size; + header->parse_info_size = parse_info_size; + header->device_id = oe_boot_state.device_id; + header->side = get_header_side(); + + ssize_t serialized_size = serializeParseInfoStorage( + reinterpret_cast(header_buffer + sizeof(FileHeader)), + parse_info_size + ); + if (serialized_size < 0) { + k_free(header_buffer); + LOG_ERR("Failed to serialize parse info storage: %d", (int)serialized_size); + return (int)serialized_size; + } + if ((size_t)serialized_size != parse_info_size) { + k_free(header_buffer); + LOG_ERR("Parse info size mismatch: %d != %zu", (int)serialized_size, parse_info_size); + return -EIO; + } int ret; + size_t bytes_to_write = header_size; k_mutex_lock(&file_mutex, K_FOREVER); - ret = sd_card->write((char *)header_buffer, &header_size, false); + ret = sd_card->write(reinterpret_cast(header_buffer), &bytes_to_write, false); k_mutex_unlock(&file_mutex); + k_free(header_buffer); + + if ((ret >= 0) && ((size_t)ret != header_size)) { + LOG_ERR("Incomplete header write: %d != %zu", ret, header_size); + return -EIO; + } + return ret; } diff --git a/src/SD_Card/SDLogger/SDLogger.h b/src/SD_Card/SDLogger/SDLogger.h index 1b52e524..279fdae0 100644 --- a/src/SD_Card/SDLogger/SDLogger.h +++ b/src/SD_Card/SDLogger/SDLogger.h @@ -37,14 +37,18 @@ class SDLogger { //size_t buffer_pos = 0; std::string current_file; - int write_header(); //Write file header with version and timestamp + int write_header(); // Write the file header, device metadata, and embedded parse metadata. int flush(); // Flush any buffered data to the SD card - static constexpr uint16_t SENSOR_LOG_VERSION = 0x0002; + static constexpr uint16_t SENSOR_LOG_VERSION = 0x0003; struct __attribute__((packed)) FileHeader { uint16_t version; uint64_t timestamp; + uint32_t header_size; + uint32_t parse_info_size; + uint64_t device_id; + uint8_t side; }; struct sensor_data msg; @@ -96,4 +100,4 @@ class SDLogger { extern SDLogger sdlogger; -#endif \ No newline at end of file +#endif From b5c51edd5f79c9945687b2129fea6f47032c0a84 Mon Sep 17 00:00:00 2001 From: Dennis Moschina <45356478+DennisMoschina@users.noreply.github.com> Date: Thu, 30 Apr 2026 15:09:22 +0200 Subject: [PATCH 26/36] doc(SensorScheme): added documentation to public functions --- src/ParseInfo/SensorScheme.h | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/src/ParseInfo/SensorScheme.h b/src/ParseInfo/SensorScheme.h index 29c3ccf7..6aad24a2 100644 --- a/src/ParseInfo/SensorScheme.h +++ b/src/ParseInfo/SensorScheme.h @@ -64,7 +64,23 @@ int initParseInfoService(struct ParseInfoScheme* scheme, struct SensorScheme* se struct SensorScheme* getSensorSchemeForId(uint8_t id); struct ParseInfoScheme* getParseInfoScheme(); + +/** + * @brief Calculate the byte length of the serialized parse-info storage blob. + * + * The storage blob contains the serialized sensor list followed by each sensor + * scheme prefixed with a uint16 length. Returns 0 if parse-info state is not + * initialized or any scheme cannot be represented in the storage format. + */ size_t getParseInfoStorageSize(); + +/** + * @brief Serialize parse-info state into an SD-log file header payload. + * + * @param buffer Destination buffer for the serialized parse-info storage blob. + * @param bufferSize Available bytes in @p buffer. + * @return Number of bytes written on success, or a negative errno value. + */ ssize_t serializeParseInfoStorage(char* buffer, size_t bufferSize); float getSampleRateForSensorId(uint8_t id, uint8_t frequencyIndex); From 1e0d6b7ce9f1aa17297573d4387e58a2eba8f51a Mon Sep 17 00:00:00 2001 From: Dennis Moschina <45356478+DennisMoschina@users.noreply.github.com> Date: Thu, 30 Apr 2026 15:10:06 +0200 Subject: [PATCH 27/36] feat(sensor_service): added function to get sensor config status --- src/bluetooth/gatt_services/sensor_service.c | 17 +++++++++++++++++ src/bluetooth/gatt_services/sensor_service.h | 11 ++++++++++- 2 files changed, 27 insertions(+), 1 deletion(-) diff --git a/src/bluetooth/gatt_services/sensor_service.c b/src/bluetooth/gatt_services/sensor_service.c index 3d821d14..b97b8b0b 100644 --- a/src/bluetooth/gatt_services/sensor_service.c +++ b/src/bluetooth/gatt_services/sensor_service.c @@ -6,6 +6,8 @@ #include "macros_common.h" +#include + #include LOG_MODULE_REGISTER(sensor_manager, CONFIG_MODULE_BUTTON_HANDLER_LOG_LEVEL); @@ -447,6 +449,21 @@ int init_sensor_config_status() { return 0; } +int get_sensor_config_status(uint8_t sensor_id, struct sensor_config *config) { + if (config == NULL) { + return -EINVAL; + } + + for (size_t i = 0; i < active_sensor_configs_size; i++) { + if (active_sensor_configs[i].sensorId == sensor_id) { + *config = active_sensor_configs[i]; + return 0; + } + } + + return -ENOENT; +} + int set_sensor_config_status(struct sensor_config config) { LOG_DBG("Setting sensor config status for sensorId: %i", config.sensorId); diff --git a/src/bluetooth/gatt_services/sensor_service.h b/src/bluetooth/gatt_services/sensor_service.h index 239062fe..152aaf11 100644 --- a/src/bluetooth/gatt_services/sensor_service.h +++ b/src/bluetooth/gatt_services/sensor_service.h @@ -35,6 +35,15 @@ int init_sensor_service(); const char *get_sensor_recording_name(); //int send_sensor_data(); //struct sensor_data * data); +/** + * @brief Copy the active runtime configuration for a sensor. + * + * @param sensor_id Sensor identifier to query. + * @param config Destination for the active configuration. + * @return 0 on success, or a negative errno value if no active config exists. + */ +int get_sensor_config_status(uint8_t sensor_id, struct sensor_config *config); + int set_sensor_config_status(struct sensor_config config); void temp_disable_notifies(bool disable); @@ -43,4 +52,4 @@ void temp_disable_notifies(bool disable); } #endif -#endif \ No newline at end of file +#endif From aa9577218d414c94624c51187f7793393722ccec Mon Sep 17 00:00:00 2001 From: Dennis Moschina <45356478+DennisMoschina@users.noreply.github.com> Date: Thu, 30 Apr 2026 15:10:39 +0200 Subject: [PATCH 28/36] feat(sensor_scheme): add function to retrieve sensor scheme with active frequency --- src/ParseInfo/SensorScheme.cpp | 40 ++++++++++++++++++++++++++++++++-- 1 file changed, 38 insertions(+), 2 deletions(-) diff --git a/src/ParseInfo/SensorScheme.cpp b/src/ParseInfo/SensorScheme.cpp index 292dc403..47a526a2 100644 --- a/src/ParseInfo/SensorScheme.cpp +++ b/src/ParseInfo/SensorScheme.cpp @@ -235,6 +235,36 @@ ssize_t serializeSensorScheme(SensorScheme* scheme, char* buffer, size_t bufferS return buffer - bufferStart; } +/** + * @brief Return a sensor scheme copy whose default frequency index matches runtime state. + */ +static SensorScheme getSensorSchemeWithActiveFrequency(SensorScheme* scheme) { + SensorScheme activeScheme = *scheme; + if (!(scheme->configOptions.availableOptions & FREQUENCIES_DEFINED)) { + return activeScheme; + } + + struct sensor_config activeConfig; + int ret = get_sensor_config_status(scheme->id, &activeConfig); + if (ret < 0) { + return activeScheme; + } + + if (activeConfig.sampleRateIndex >= scheme->configOptions.frequencyOptions.frequencyCount) { + LOG_WRN( + "Ignoring invalid active sample rate index %u for sensor %u", + activeConfig.sampleRateIndex, + scheme->id + ); + return activeScheme; + } + + activeScheme.configOptions = scheme->configOptions; + activeScheme.configOptions.frequencyOptions = scheme->configOptions.frequencyOptions; + activeScheme.configOptions.frequencyOptions.defaultFrequencyIndex = activeConfig.sampleRateIndex; + return activeScheme; +} + size_t getSchemeSize(ParseInfoScheme* scheme) { size_t size = 0; size += 1; // sensorCount @@ -375,8 +405,13 @@ size_t getParseInfoStorageSize() { return 0; } + size_t sensorSchemeSize = getSensorSchemeSize(scheme); + if (sensorSchemeSize > UINT16_MAX) { + return 0; + } + size += sizeof(uint16_t); - size += getSensorSchemeSize(scheme); + size += sensorSchemeSize; } return size; @@ -411,7 +446,8 @@ ssize_t serializeParseInfoStorage(char* buffer, size_t bufferSize) { memcpy(buffer, &encodedSchemeSize, sizeof(encodedSchemeSize)); buffer += sizeof(encodedSchemeSize); - ssize_t writtenSize = serializeSensorScheme(scheme, buffer, bufferSize - (buffer - bufferStart)); + SensorScheme activeScheme = getSensorSchemeWithActiveFrequency(scheme); + ssize_t writtenSize = serializeSensorScheme(&activeScheme, buffer, bufferSize - (buffer - bufferStart)); if (writtenSize < 0) { return writtenSize; } From 6c520a6378e7a124188c7e3bcf0e754a6dcf765f Mon Sep 17 00:00:00 2001 From: Dennis Moschina <45356478+DennisMoschina@users.noreply.github.com> Date: Thu, 30 Apr 2026 15:11:05 +0200 Subject: [PATCH 29/36] fix(sensor_manager): update config_work_handler to set sensor config status after everything else is set up --- src/SensorManager/SensorManager.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/SensorManager/SensorManager.cpp b/src/SensorManager/SensorManager.cpp index ae04829b..66142439 100644 --- a/src/SensorManager/SensorManager.cpp +++ b/src/SensorManager/SensorManager.cpp @@ -221,6 +221,8 @@ static void config_work_handler(struct k_work *work) { } } + set_sensor_config_status(config); + if (config.storageOptions & DATA_STORAGE) { sd_sensors.insert(config.sensorId); @@ -248,8 +250,6 @@ static void config_work_handler(struct k_work *work) { // TODO: if (ble_sensors.empty()) ... } - set_sensor_config_status(config); - if (active_sensors == 0) stop_sensor_manager(); } @@ -263,4 +263,4 @@ void config_sensor(struct sensor_config * config) { //k_work_queue_drain(&sensor_work_q, true); k_work_submit(&config_work); //k_work_queue_unplug(&sensor_work_q); -} \ No newline at end of file +} From eda4dc75d9f1cebd872a4185e652d04a008df0d2 Mon Sep 17 00:00:00 2001 From: Dennis Moschina <45356478+DennisMoschina@users.noreply.github.com> Date: Thu, 30 Apr 2026 15:11:25 +0200 Subject: [PATCH 30/36] doc(ParseInfo): document Default Frequency Index for schemes in .oe headers --- src/ParseInfo/README | 1 + 1 file changed, 1 insertion(+) diff --git a/src/ParseInfo/README b/src/ParseInfo/README index d1b70507..eba4681b 100644 --- a/src/ParseInfo/README +++ b/src/ParseInfo/README @@ -219,3 +219,4 @@ The blob starts with the existing `Sensor List` serialization and then stores ea | ... | Sensor Scheme | byte[] | Each `Sensor Scheme` payload uses the existing `Single Sensor Scheme` serialization already documented above. +For schemes embedded in an `.oe` header, `Default Frequency Index` is set to the active runtime sampling-rate index at the time the header is written. From eff6b195248baaeb2dcde57320237b3f7d89b321 Mon Sep 17 00:00:00 2001 From: Dennis Moschina <45356478+DennisMoschina@users.noreply.github.com> Date: Tue, 5 May 2026 11:23:33 +0200 Subject: [PATCH 31/36] revert(parse-info): remove active sample rate header changes --- src/ParseInfo/README | 1 - src/ParseInfo/SensorScheme.cpp | 40 +------------------- src/SensorManager/SensorManager.cpp | 6 +-- src/bluetooth/gatt_services/sensor_service.c | 17 --------- src/bluetooth/gatt_services/sensor_service.h | 11 +----- 5 files changed, 6 insertions(+), 69 deletions(-) diff --git a/src/ParseInfo/README b/src/ParseInfo/README index eba4681b..d1b70507 100644 --- a/src/ParseInfo/README +++ b/src/ParseInfo/README @@ -219,4 +219,3 @@ The blob starts with the existing `Sensor List` serialization and then stores ea | ... | Sensor Scheme | byte[] | Each `Sensor Scheme` payload uses the existing `Single Sensor Scheme` serialization already documented above. -For schemes embedded in an `.oe` header, `Default Frequency Index` is set to the active runtime sampling-rate index at the time the header is written. diff --git a/src/ParseInfo/SensorScheme.cpp b/src/ParseInfo/SensorScheme.cpp index 47a526a2..292dc403 100644 --- a/src/ParseInfo/SensorScheme.cpp +++ b/src/ParseInfo/SensorScheme.cpp @@ -235,36 +235,6 @@ ssize_t serializeSensorScheme(SensorScheme* scheme, char* buffer, size_t bufferS return buffer - bufferStart; } -/** - * @brief Return a sensor scheme copy whose default frequency index matches runtime state. - */ -static SensorScheme getSensorSchemeWithActiveFrequency(SensorScheme* scheme) { - SensorScheme activeScheme = *scheme; - if (!(scheme->configOptions.availableOptions & FREQUENCIES_DEFINED)) { - return activeScheme; - } - - struct sensor_config activeConfig; - int ret = get_sensor_config_status(scheme->id, &activeConfig); - if (ret < 0) { - return activeScheme; - } - - if (activeConfig.sampleRateIndex >= scheme->configOptions.frequencyOptions.frequencyCount) { - LOG_WRN( - "Ignoring invalid active sample rate index %u for sensor %u", - activeConfig.sampleRateIndex, - scheme->id - ); - return activeScheme; - } - - activeScheme.configOptions = scheme->configOptions; - activeScheme.configOptions.frequencyOptions = scheme->configOptions.frequencyOptions; - activeScheme.configOptions.frequencyOptions.defaultFrequencyIndex = activeConfig.sampleRateIndex; - return activeScheme; -} - size_t getSchemeSize(ParseInfoScheme* scheme) { size_t size = 0; size += 1; // sensorCount @@ -405,13 +375,8 @@ size_t getParseInfoStorageSize() { return 0; } - size_t sensorSchemeSize = getSensorSchemeSize(scheme); - if (sensorSchemeSize > UINT16_MAX) { - return 0; - } - size += sizeof(uint16_t); - size += sensorSchemeSize; + size += getSensorSchemeSize(scheme); } return size; @@ -446,8 +411,7 @@ ssize_t serializeParseInfoStorage(char* buffer, size_t bufferSize) { memcpy(buffer, &encodedSchemeSize, sizeof(encodedSchemeSize)); buffer += sizeof(encodedSchemeSize); - SensorScheme activeScheme = getSensorSchemeWithActiveFrequency(scheme); - ssize_t writtenSize = serializeSensorScheme(&activeScheme, buffer, bufferSize - (buffer - bufferStart)); + ssize_t writtenSize = serializeSensorScheme(scheme, buffer, bufferSize - (buffer - bufferStart)); if (writtenSize < 0) { return writtenSize; } diff --git a/src/SensorManager/SensorManager.cpp b/src/SensorManager/SensorManager.cpp index 66142439..ae04829b 100644 --- a/src/SensorManager/SensorManager.cpp +++ b/src/SensorManager/SensorManager.cpp @@ -221,8 +221,6 @@ static void config_work_handler(struct k_work *work) { } } - set_sensor_config_status(config); - if (config.storageOptions & DATA_STORAGE) { sd_sensors.insert(config.sensorId); @@ -250,6 +248,8 @@ static void config_work_handler(struct k_work *work) { // TODO: if (ble_sensors.empty()) ... } + set_sensor_config_status(config); + if (active_sensors == 0) stop_sensor_manager(); } @@ -263,4 +263,4 @@ void config_sensor(struct sensor_config * config) { //k_work_queue_drain(&sensor_work_q, true); k_work_submit(&config_work); //k_work_queue_unplug(&sensor_work_q); -} +} \ No newline at end of file diff --git a/src/bluetooth/gatt_services/sensor_service.c b/src/bluetooth/gatt_services/sensor_service.c index b97b8b0b..3d821d14 100644 --- a/src/bluetooth/gatt_services/sensor_service.c +++ b/src/bluetooth/gatt_services/sensor_service.c @@ -6,8 +6,6 @@ #include "macros_common.h" -#include - #include LOG_MODULE_REGISTER(sensor_manager, CONFIG_MODULE_BUTTON_HANDLER_LOG_LEVEL); @@ -449,21 +447,6 @@ int init_sensor_config_status() { return 0; } -int get_sensor_config_status(uint8_t sensor_id, struct sensor_config *config) { - if (config == NULL) { - return -EINVAL; - } - - for (size_t i = 0; i < active_sensor_configs_size; i++) { - if (active_sensor_configs[i].sensorId == sensor_id) { - *config = active_sensor_configs[i]; - return 0; - } - } - - return -ENOENT; -} - int set_sensor_config_status(struct sensor_config config) { LOG_DBG("Setting sensor config status for sensorId: %i", config.sensorId); diff --git a/src/bluetooth/gatt_services/sensor_service.h b/src/bluetooth/gatt_services/sensor_service.h index 152aaf11..239062fe 100644 --- a/src/bluetooth/gatt_services/sensor_service.h +++ b/src/bluetooth/gatt_services/sensor_service.h @@ -35,15 +35,6 @@ int init_sensor_service(); const char *get_sensor_recording_name(); //int send_sensor_data(); //struct sensor_data * data); -/** - * @brief Copy the active runtime configuration for a sensor. - * - * @param sensor_id Sensor identifier to query. - * @param config Destination for the active configuration. - * @return 0 on success, or a negative errno value if no active config exists. - */ -int get_sensor_config_status(uint8_t sensor_id, struct sensor_config *config); - int set_sensor_config_status(struct sensor_config config); void temp_disable_notifies(bool disable); @@ -52,4 +43,4 @@ void temp_disable_notifies(bool disable); } #endif -#endif +#endif \ No newline at end of file From 3a70ef0ef4492159e9b3244e79632792898ce600 Mon Sep 17 00:00:00 2001 From: mkuettner97 Date: Thu, 7 May 2026 15:40:39 +0200 Subject: [PATCH 32/36] fix gyro --- src/SensorManager/BMX160/DFRobot_BMX160.cpp | 3 ++- src/SensorManager/BMX160/DFRobot_BMX160.h | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/SensorManager/BMX160/DFRobot_BMX160.cpp b/src/SensorManager/BMX160/DFRobot_BMX160.cpp index 22234e83..73f46b42 100644 --- a/src/SensorManager/BMX160/DFRobot_BMX160.cpp +++ b/src/SensorManager/BMX160/DFRobot_BMX160.cpp @@ -182,9 +182,10 @@ void DFRobot_BMX160::setGyroRange(eGyroRange_t bits){ gyroRange = BMX160_GYRO_SENSITIVITY_2000DPS; break; default: - gyroRange = BMX160_GYRO_SENSITIVITY_250DPS; + gyroRange = BMX160_GYRO_SENSITIVITY_2000DPS; break; } + writeBmxReg(BMX160_GYRO_RANGE_ADDR, bits); } void DFRobot_BMX160::setAccelRange(eAccelRange_t bits){ diff --git a/src/SensorManager/BMX160/DFRobot_BMX160.h b/src/SensorManager/BMX160/DFRobot_BMX160.h index 3e08f7ef..b6585c90 100644 --- a/src/SensorManager/BMX160/DFRobot_BMX160.h +++ b/src/SensorManager/BMX160/DFRobot_BMX160.h @@ -1109,7 +1109,7 @@ class DFRobot_BMX160{ void setMagnConf(); float accelRange = BMX160_ACCEL_MG_LSB_2G * EARTH_ACC; - float gyroRange = BMX160_GYRO_SENSITIVITY_250DPS; + float gyroRange = BMX160_GYRO_SENSITIVITY_2000DPS; uint8_t _addr = DT_REG_ADDR(DT_NODELABEL(bmx160)); sBmx160Dev_t* Obmx160; From f9a5a9ebbcb2748437a7f026e100b08a91f45241 Mon Sep 17 00:00:00 2001 From: mkuettner97 Date: Fri, 8 May 2026 14:09:35 +0200 Subject: [PATCH 33/36] reduce PDM clock speed to 3.072 MHz --- src/drivers/ADAU1860.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/ADAU1860.cpp b/src/drivers/ADAU1860.cpp index 3d3e2d88..b75fa521 100644 --- a/src/drivers/ADAU1860.cpp +++ b/src/drivers/ADAU1860.cpp @@ -213,7 +213,7 @@ int ADAU1860::begin() { writeReg(registers::DMIC_VOL0, &dmic_vol, sizeof(dmic_vol)); writeReg(registers::DMIC_VOL1, &dmic_vol, sizeof(dmic_vol)); - uint8_t dmic_ctrl1 = 0x34; // ... | 6.144 MHz + uint8_t dmic_ctrl1 = 0x33; // ... | 3.072 MHz writeReg(registers::DMIC_CTRL1, &dmic_ctrl1, sizeof(dmic_ctrl1)); uint8_t dmic_ctrl2 = 0x04; // 192kHz From 8505ce4ded26848261a388689f076f7a6bbc5a2c Mon Sep 17 00:00:00 2001 From: Dennis Moschina <45356478+DennisMoschina@users.noreply.github.com> Date: Mon, 18 May 2026 09:44:21 +0200 Subject: [PATCH 34/36] Revert "chore: bump version to 2.3.0" This reverts commit 56f9961b32475a9e57918d545fe480ddd16deef2. --- VERSION | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/VERSION b/VERSION index d3e37008..ae3bfdfb 100644 --- a/VERSION +++ b/VERSION @@ -1,5 +1,5 @@ VERSION_MAJOR = 2 -VERSION_MINOR = 3 -PATCHLEVEL = 0 +VERSION_MINOR = 2 +PATCHLEVEL = 3 VERSION_TWEAK = 0 EXTRAVERSION = From a5fe3e777cfe1c11f506fae256cf8994087a4121 Mon Sep 17 00:00:00 2001 From: Dennis Moschina <45356478+DennisMoschina@users.noreply.github.com> Date: Tue, 12 May 2026 13:49:43 +0200 Subject: [PATCH 35/36] fix(audio): guard encoder lifecycle during mic capture --- src/audio/audio_datapath.c | 2 +- src/audio/audio_system.c | 25 +++++++++++++++++++++++++ src/audio/audio_system.h | 8 ++++++++ src/audio/streamctrl.c | 10 ++++++---- 4 files changed, 40 insertions(+), 5 deletions(-) diff --git a/src/audio/audio_datapath.c b/src/audio/audio_datapath.c index e144939f..49d9997a 100644 --- a/src/audio/audio_datapath.c +++ b/src/audio/audio_datapath.c @@ -273,7 +273,7 @@ static void data_thread(void *arg1, void *arg2, void *arg3) unsigned int signaled; k_poll_signal_check(&encoder_sig, &signaled, &ret); - if (ret == 0 && signaled != 0) { + if (ret == 0 && signaled != 0 && audio_system_encoder_is_started()) { ret = k_msgq_put(&encoder_queue, &audio_item, K_NO_WAIT); if (ret) { LOG_WRN("encoder queue full"); diff --git a/src/audio/audio_system.c b/src/audio/audio_system.c index e71b7a75..c27d4cfd 100644 --- a/src/audio/audio_system.c +++ b/src/audio/audio_system.c @@ -8,6 +8,7 @@ #include #include +#include #include #include #include @@ -45,6 +46,7 @@ static K_SEM_DEFINE(sem_encoder_start, 0, 1); static struct k_thread encoder_thread_data; static k_tid_t encoder_thread_id; +static atomic_t encoder_started; struct k_poll_signal encoder_sig; @@ -157,6 +159,11 @@ static void encoder_thread(void *arg1, void *arg2, void *arg3) continue; } + if (!audio_system_encoder_is_started()) { + LOG_DBG("Dropping stale encoder frame while encoder is stopped"); + continue; + } + if (sw_codec_cfg.encoder.enabled) { if (test_tone_size) { /* Test tone takes over audio stream */ @@ -201,8 +208,17 @@ static void encoder_thread(void *arg1, void *arg2, void *arg3) void audio_system_encoder_start(void) { + if (!sw_codec_cfg.initialized || !sw_codec_cfg.encoder.enabled || !sw_codec_is_initialized()) { + LOG_WRN("Encoder start ignored because codec is not initialized"); + atomic_clear(&encoder_started); + k_poll_signal_reset(&encoder_sig); + k_msgq_purge(&encoder_queue); + return; + } + LOG_DBG("Encoder started"); k_msgq_purge(&encoder_queue); + atomic_set(&encoder_started, true); k_poll_signal_raise(&encoder_sig, 0); /*if (IS_ENABLED(CONFIG_AUDIO_MIC_PDM)) { pdm_mic_start(); @@ -211,7 +227,15 @@ void audio_system_encoder_start(void) void audio_system_encoder_stop(void) { + atomic_clear(&encoder_started); k_poll_signal_reset(&encoder_sig); + k_msgq_purge(&encoder_queue); +} + +bool audio_system_encoder_is_started(void) +{ + return atomic_get(&encoder_started) && sw_codec_cfg.initialized && + sw_codec_cfg.encoder.enabled && sw_codec_is_initialized(); } int audio_system_encode_test_tone_set(uint32_t freq) @@ -452,6 +476,7 @@ void audio_system_stop(void) } LOG_DBG("Stopping codec"); + audio_system_encoder_stop(); #if ((CONFIG_AUDIO_DEV == GATEWAY) && CONFIG_AUDIO_SOURCE_USB) audio_usb_stop(); diff --git a/src/audio/audio_system.h b/src/audio/audio_system.h index 55264185..0c5829e9 100644 --- a/src/audio/audio_system.h +++ b/src/audio/audio_system.h @@ -30,6 +30,14 @@ void audio_system_encoder_start(void); */ void audio_system_encoder_stop(void); +/** + * @brief Check if the encoder is ready to accept PCM frames. + * + * @retval true The software codec encoder is initialized and streaming is enabled. + * @retval false The encoder is stopped or the codec has not been initialized. + */ +bool audio_system_encoder_is_started(void); + /** * @brief Toggle a test tone on and off. * diff --git a/src/audio/streamctrl.c b/src/audio/streamctrl.c index 5a1e7edb..1fd37f58 100644 --- a/src/audio/streamctrl.c +++ b/src/audio/streamctrl.c @@ -190,17 +190,19 @@ static void le_audio_msg_sub_thread(void) case LE_AUDIO_EVT_STREAMING: LOG_DBG("LE audio evt streaming"); - if (msg.dir == BT_AUDIO_DIR_SOURCE) { - audio_system_encoder_start(); - } - if (strm_state == STATE_STREAMING) { LOG_DBG("Got streaming event in streaming state"); + if (msg.dir == BT_AUDIO_DIR_SOURCE) { + audio_system_encoder_start(); + } break; } audio_system_start(); stream_state_set(STATE_STREAMING); + if (msg.dir == BT_AUDIO_DIR_SOURCE) { + audio_system_encoder_start(); + } #if CONFIG_BOARD_NRF5340_AUDIO_DK_NRF5340_CPUAPP ret = led_blink(LED_APP_1_BLUE); ERR_CHK(ret); From 7bcbac76f2b679e91b61a7fe7f0eda2f6d4f5d87 Mon Sep 17 00:00:00 2001 From: Dennis Moschina <45356478+DennisMoschina@users.noreply.github.com> Date: Thu, 21 May 2026 13:07:04 +0200 Subject: [PATCH 36/36] fix(DefaultSensors): remove DATA_STREAMING option from availableOptions --- src/ParseInfo/DefaultSensors.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ParseInfo/DefaultSensors.h b/src/ParseInfo/DefaultSensors.h index 28016c26..97ce4166 100644 --- a/src/ParseInfo/DefaultSensors.h +++ b/src/ParseInfo/DefaultSensors.h @@ -141,7 +141,7 @@ SensorScheme defaultSensors[SENSOR_COUNT] = { .groupCount = MICRO_GROUP_COUNT, .groups = microGroups, .configOptions = { - .availableOptions = DATA_STREAMING | DATA_STORAGE | FREQUENCIES_DEFINED, + .availableOptions = DATA_STORAGE | FREQUENCIES_DEFINED, .frequencyOptions = { .frequencyCount = sizeof(Microphone::sample_rates.reg_vals), .defaultFrequencyIndex = 8,