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/SD_Card/SDLogger/SDLogger.cpp b/src/SD_Card/SDLogger/SDLogger.cpp index fe89aaba..d3a7ee2d 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; } @@ -143,12 +147,12 @@ 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) { // 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); } @@ -354,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; } @@ -422,7 +428,6 @@ int SDLogger::end() { } if (!sd_card->is_mounted()) { - //k_poll_signal_reset(&logger_sig); is_open = false; return -ENODEV; } @@ -445,13 +450,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); diff --git a/src/SensorManager/Microphone.cpp b/src/SensorManager/Microphone.cpp index fe907e2d..bf6a48c6 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,16 +54,20 @@ 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; if (!_active) return; - record_to_sd(true); + LOG_INF("Starting Microphone at %f Hz", sample_rates.sample_rates[sample_rate_idx]); audio_datapath_aquire(&fifo_rx); + audio_datapath_decimator_init(sample_rates.reg_vals[sample_rate_idx]); + + record_to_sd(true); + _running = true; } @@ -78,5 +81,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/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/CMakeLists.txt b/src/audio/CMakeLists.txt index ab99ae2b..a58215fc 100644 --- a/src/audio/CMakeLists.txt +++ b/src/audio/CMakeLists.txt @@ -10,6 +10,8 @@ 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.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 0b28d728..e144939f 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 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) { @@ -219,36 +224,47 @@ 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; + 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 */ + + 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; + } + struct sensor_msg audio_msg; audio_msg.sd = true; 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; + audio_msg.data.size = decimated_frames * 2 * sizeof(int16_t); - 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}; + uint32_t data_size[2] = { + sizeof(audio_msg.data.id) + sizeof(audio_msg.data.size) + sizeof(audio_msg.data.time), + audio_msg.data.size + }; - const void *data_ptrs[2] = { + void *data_ptrs[2] = { &audio_msg.data, - audio_item.data + (i * BLOCK_SIZE_BYTES) + decimated_audio }; - 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); + if (decimated_frames == num_frames) { + data_ptrs[1] = audio_block; + } + + if (decimated_frames > 0) { + sdlogger_write_data(&data_ptrs, data_size, 2); + } } k_yield(); @@ -266,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; @@ -281,6 +292,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) { @@ -634,6 +652,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); @@ -1202,6 +1223,9 @@ int audio_datapath_stop(void) data_fifo_empty(ctrl_blk.in.fifo); + /* Cleanup CascadedDecimator on stop */ + audio_datapath_decimator_cleanup(); + return 0; } else { return -EALREADY; diff --git a/src/audio/audio_datapath.h b/src/audio/audio_datapath.h index 5e463367..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 * @@ -103,6 +113,33 @@ int audio_datapath_release(); //void set_ring_buffer(struct ring_buf *ring_buf); +/** + * @brief C wrapper for decimator initialization + */ +int audio_datapath_decimator_init(uint8_t factor); + +/** + * @brief C wrapper for decimator processing + */ +int audio_datapath_decimator_process(const int16_t* input, int16_t* output, uint32_t num_frames); + +/** + * @brief C wrapper for decimator cleanup + */ +void audio_datapath_decimator_cleanup(void); + +/** + * @brief C wrapper for decimator reset + */ +void audio_datapath_decimator_reset(void); + +/** + * @brief Get current decimation factor + * @return Current total decimation factor, or 0 if not initialized + */ +uint8_t audio_datapath_decimator_get_factor(void); + + #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..73c9e43d --- /dev/null +++ b/src/audio/audio_datapath_decimator.cpp @@ -0,0 +1,128 @@ +/* + * 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; + +/* 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 + */ +void audio_datapath_decimator_reset(void) { + if (g_audio_decimator) { + g_audio_decimator->reset(); + LOG_DBG("CascadedDecimator state reset"); + } +} + +/** + * @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) { + /* 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; + } + + 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; + } + + 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; + k_mutex_unlock(&decimator_mutex); + return ret; + } + + LOG_DBG("CascadedDecimator (%dx) initialized successfully", factor); + k_mutex_unlock(&decimator_mutex); + 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) { + /* 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; + } + + 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); +} + +}; +#endif \ No newline at end of file diff --git a/src/audio/decimation_filter.cpp b/src/audio/decimation_filter.cpp new file mode 100644 index 00000000..7d49f1a0 --- /dev/null +++ b/src/audio/decimation_filter.cpp @@ -0,0 +1,256 @@ +/* + * 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 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); + 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() { + 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) { + return num_frames; + } + + if (!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 new file mode 100644 index 00000000..2afd709f --- /dev/null +++ b/src/audio/decimation_filter.h @@ -0,0 +1,126 @@ +/* + * Copyright (c) 2025 + * + * SPDX-License-Identifier: LicenseRef-PCFT + */ + +#ifndef _DECIMATION_FILTER_H_ +#define _DECIMATION_FILTER_H_ + +#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(); +}; +#endif +#endif /* _DECIMATION_FILTER_H_ */ 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);