diff --git a/include/openearable_common.h b/include/openearable_common.h index 12c728a5..dbbfb5d8 100644 --- a/include/openearable_common.h +++ b/include/openearable_common.h @@ -88,6 +88,8 @@ enum sensor_id { ID_PULSOX=5, ID_OPTTEMP=6, ID_BONE_CONDUCTION=7, + ID_MICRO_INNER=8, + ID_MICRO_OUTER=9, }; struct battery_data { @@ -145,4 +147,4 @@ struct audio_rx_data { size_t size; }; -#endif \ No newline at end of file +#endif diff --git a/prj.conf b/prj.conf index 6e2a1100..f80687e5 100644 --- a/prj.conf +++ b/prj.conf @@ -154,7 +154,10 @@ CONFIG_SFLOAT=y #CONFIG_RESET_ON_FATAL_ERROR=y -CONFIG_STREAM_BIDIRECTIONAL=y +CONFIG_STREAM_BIDIRECTIONAL=n +CONFIG_MIC_BLE_STREAM=y +CONFIG_BT_ASCS_MAX_ASE_SRC_COUNT=0 +CONFIG_BT_PAC_SRC_NOTIFIABLE=n # CONFIG_NRFX_PDM=y diff --git a/prj_fota.conf b/prj_fota.conf index 6b7263a0..72405bbe 100644 --- a/prj_fota.conf +++ b/prj_fota.conf @@ -170,7 +170,10 @@ CONFIG_SFLOAT=y #CONFIG_RESET_ON_FATAL_ERROR=y -CONFIG_STREAM_BIDIRECTIONAL=y +CONFIG_STREAM_BIDIRECTIONAL=n +CONFIG_MIC_BLE_STREAM=y +CONFIG_BT_ASCS_MAX_ASE_SRC_COUNT=0 +CONFIG_BT_PAC_SRC_NOTIFIABLE=n # CONFIG_NRFX_PDM=y diff --git a/src/ParseInfo/DefaultSensors.h b/src/ParseInfo/DefaultSensors.h index e6e3ef21..cce21215 100644 --- a/src/ParseInfo/DefaultSensors.h +++ b/src/ParseInfo/DefaultSensors.h @@ -12,6 +12,7 @@ #include "../SensorManager/Temp.h" #include "../SensorManager/BoneConduction.h" #include "../SensorManager/Microphone.h" +#include "../SensorManager/MicrophoneBle.h" // ============= Microphones ============= @@ -27,6 +28,24 @@ SensorComponentGroup microGroups[MICRO_GROUP_COUNT] = { { .name = "MICROPHONE", .componentCount = MICRO_CHANNEL_COUNT, .components = microComponenents }, }; +#define MICRO_BLE_CHANNEL_COUNT 1 +SensorComponent microBleInnerComponents[MICRO_BLE_CHANNEL_COUNT] = { + { .name = "INNER", .unit = "ADC", .parseType = PARSE_TYPE_INT16 }, +}; + +SensorComponent microBleOuterComponents[MICRO_BLE_CHANNEL_COUNT] = { + { .name = "OUTER", .unit = "ADC", .parseType = PARSE_TYPE_INT16 }, +}; + +#define MICRO_BLE_GROUP_COUNT 1 +SensorComponentGroup microBleInnerGroups[MICRO_BLE_GROUP_COUNT] = { + { .name = "INNER_MICROPHONE", .componentCount = MICRO_BLE_CHANNEL_COUNT, .components = microBleInnerComponents }, +}; + +SensorComponentGroup microBleOuterGroups[MICRO_BLE_GROUP_COUNT] = { + { .name = "OUTER_MICROPHONE", .componentCount = MICRO_BLE_CHANNEL_COUNT, .components = microBleOuterComponents }, +}; + // ============= IMU ============= #define IMU_ACC_COUNT 3 @@ -118,7 +137,7 @@ SensorComponentGroup baroGroups[BARO_GROUP_COUNT] = { // ============= Sensors ============= -#define SENSOR_COUNT 6 +#define SENSOR_COUNT 8 SensorScheme defaultSensors[SENSOR_COUNT] = { { .name = "9-Axis IMU", @@ -150,6 +169,36 @@ SensorScheme defaultSensors[SENSOR_COUNT] = { }, }, }, + { + .name = "Inner Microphone", + .id = ID_MICRO_INNER, + .groupCount = MICRO_BLE_GROUP_COUNT, + .groups = microBleInnerGroups, + .configOptions = { + .availableOptions = DATA_STREAMING | FREQUENCIES_DEFINED, + .frequencyOptions = { + .frequencyCount = sizeof(MicrophoneBle::sample_rates.reg_vals), + .defaultFrequencyIndex = 0, + .maxBleFrequencyIndex = 0, + .frequencies = MicrophoneBle::sample_rates.sample_rates, + }, + }, + }, + { + .name = "Outer Microphone", + .id = ID_MICRO_OUTER, + .groupCount = MICRO_BLE_GROUP_COUNT, + .groups = microBleOuterGroups, + .configOptions = { + .availableOptions = DATA_STREAMING | FREQUENCIES_DEFINED, + .frequencyOptions = { + .frequencyCount = sizeof(MicrophoneBle::sample_rates.reg_vals), + .defaultFrequencyIndex = 0, + .maxBleFrequencyIndex = 0, + .frequencies = MicrophoneBle::sample_rates.sample_rates, + }, + }, + }, { .name = "Pulse Oximeter", .id = ID_PPG, @@ -214,7 +263,7 @@ SensorScheme defaultSensors[SENSOR_COUNT] = { ParseInfoScheme defaultSensorIds = { .sensorCount = SENSOR_COUNT, - .sensorIds = (uint8_t[]){ ID_IMU, ID_PPG, ID_OPTTEMP, ID_TEMP_BARO, ID_BONE_CONDUCTION, ID_MICRO }, + .sensorIds = (uint8_t[]){ ID_IMU, ID_PPG, ID_OPTTEMP, ID_TEMP_BARO, ID_BONE_CONDUCTION, ID_MICRO, ID_MICRO_INNER, ID_MICRO_OUTER }, }; #endif // _DEFAULT_SENSORS_H diff --git a/src/SensorManager/CMakeLists.txt b/src/SensorManager/CMakeLists.txt index 36f09f91..0587d14a 100644 --- a/src/SensorManager/CMakeLists.txt +++ b/src/SensorManager/CMakeLists.txt @@ -15,4 +15,6 @@ target_sources(app PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/BoneConduction.cpp ${CMAKE_CURRENT_SOURCE_DIR}/Microphone.cpp ${CMAKE_CURRENT_SOURCE_DIR}/Microphone.c + ${CMAKE_CURRENT_SOURCE_DIR}/MicrophoneBle.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/MicBleStreamer.cpp ) diff --git a/src/SensorManager/MicBleStreamer.cpp b/src/SensorManager/MicBleStreamer.cpp new file mode 100644 index 00000000..86742265 --- /dev/null +++ b/src/SensorManager/MicBleStreamer.cpp @@ -0,0 +1,294 @@ +#include "MicBleStreamer.h" + +#include "audio_i2s.h" +#include "openearable_common.h" + +#include +#include +#include +#include +#include + +LOG_MODULE_REGISTER(mic_ble_streamer, CONFIG_LOG_DEFAULT_LEVEL); + +BUILD_ASSERT(CONFIG_AUDIO_SAMPLE_RATE_HZ == 48000, + "BLE microphone streamer expects 48 kHz I2S input"); +BUILD_ASSERT(CONFIG_AUDIO_BIT_DEPTH_BITS == 16, + "BLE microphone streamer expects 16-bit PCM input"); +BUILD_ASSERT(CONFIG_I2S_CH_NUM == 2, + "BLE microphone streamer expects stereo I2S input"); + +static constexpr uint32_t MIC_BLE_SAMPLE_RATE_HZ = 1000; +static constexpr uint32_t MIC_BLE_DECIMATION = CONFIG_AUDIO_SAMPLE_RATE_HZ / MIC_BLE_SAMPLE_RATE_HZ; +static constexpr uint32_t MIC_BLE_SAMPLE_PERIOD_US = 1000000 / MIC_BLE_SAMPLE_RATE_HZ; +static constexpr uint32_t MIC_BLE_LOW_PASS_HZ = 460; +static constexpr uint32_t MIC_BLE_FILTER_TAPS = 240; +static constexpr uint8_t MIC_BLE_SAMPLES_PER_PACKET = + (SENSOR_DATA_FIXED_LENGTH - sizeof(uint16_t)) / sizeof(int16_t); +static constexpr uint64_t MIC_BLE_FILTER_DELAY_US = + (((uint64_t)MIC_BLE_FILTER_TAPS - 1) * 1000000ULL) / (2ULL * CONFIG_AUDIO_SAMPLE_RATE_HZ); + +BUILD_ASSERT(CONFIG_AUDIO_SAMPLE_RATE_HZ % MIC_BLE_SAMPLE_RATE_HZ == 0, + "BLE microphone sample rate must divide the audio sample rate"); +BUILD_ASSERT(MIC_BLE_FILTER_TAPS % MIC_BLE_DECIMATION == 0, + "CMSIS FIR decimator requires taps to be a multiple of the decimation factor"); +BUILD_ASSERT(BLOCK_SIZE_BYTES == + (MIC_BLE_DECIMATION * CONFIG_I2S_CH_NUM * CONFIG_AUDIO_BIT_DEPTH_OCTETS), + "BLE microphone streamer expects one 1 ms I2S block per decimated sample"); + +enum mic_ble_channel { + MIC_BLE_CHANNEL_INNER = 0, + MIC_BLE_CHANNEL_OUTER = 1, + MIC_BLE_CHANNEL_COUNT, +}; + +struct mic_ble_channel_state { + bool enabled; + bool decimator_ready; + uint8_t sensor_id; + uint8_t i2s_channel; + arm_fir_decimate_instance_q15 decimator; + q15_t filter_state[MIC_BLE_FILTER_TAPS + MIC_BLE_DECIMATION - 1]; + q15_t input[MIC_BLE_DECIMATION]; + int16_t packet[MIC_BLE_SAMPLES_PER_PACKET]; + uint8_t packet_count; + uint64_t packet_time_us; +}; + +static K_MUTEX_DEFINE(mic_ble_lock); + +static struct k_msgq *sensor_queue; +static bool filter_coeffs_ready; +static q15_t filter_coeffs[MIC_BLE_FILTER_TAPS]; +static double filter_coeffs_float[MIC_BLE_FILTER_TAPS]; + +static struct mic_ble_channel_state channels[MIC_BLE_CHANNEL_COUNT] = { + { + .sensor_id = ID_MICRO_INNER, + .i2s_channel = 1, + }, + { + .sensor_id = ID_MICRO_OUTER, + .i2s_channel = 0, + }, +}; + +static void filter_coeffs_init(void) +{ + if (filter_coeffs_ready) { + return; + } + + const double pi = 3.14159265358979323846; + const double normalized_cutoff = (double)MIC_BLE_LOW_PASS_HZ / (double)CONFIG_AUDIO_SAMPLE_RATE_HZ; + const double center = ((double)MIC_BLE_FILTER_TAPS - 1.0) * 0.5; + double sum = 0.0; + + for (uint32_t i = 0; i < MIC_BLE_FILTER_TAPS; i++) { + const double x = (double)i - center; + double sinc; + + if (fabs(x) < 1e-12) { + sinc = 2.0 * normalized_cutoff; + } else { + sinc = sin(2.0 * pi * normalized_cutoff * x) / (pi * x); + } + + const double window = 0.54 - (0.46 * cos((2.0 * pi * (double)i) / + ((double)MIC_BLE_FILTER_TAPS - 1.0))); + filter_coeffs_float[i] = sinc * window; + sum += filter_coeffs_float[i]; + } + + for (uint32_t i = 0; i < MIC_BLE_FILTER_TAPS; i++) { + int32_t q15 = (int32_t)lrint((filter_coeffs_float[i] / sum) * 32767.0); + + if (q15 > INT16_MAX) { + q15 = INT16_MAX; + } else if (q15 < INT16_MIN) { + q15 = INT16_MIN; + } + + filter_coeffs[MIC_BLE_FILTER_TAPS - 1U - i] = (q15_t)q15; + } + + filter_coeffs_ready = true; +} + +static struct mic_ble_channel_state *channel_for_sensor(uint8_t sensor_id) +{ + for (uint8_t i = 0; i < MIC_BLE_CHANNEL_COUNT; i++) { + if (channels[i].sensor_id == sensor_id) { + return &channels[i]; + } + } + + return nullptr; +} + +static int channel_decimator_reset(struct mic_ble_channel_state *channel) +{ + arm_status status; + + filter_coeffs_init(); + memset(channel->filter_state, 0, sizeof(channel->filter_state)); + + status = arm_fir_decimate_init_q15(&channel->decimator, MIC_BLE_FILTER_TAPS, + MIC_BLE_DECIMATION, filter_coeffs, + channel->filter_state, MIC_BLE_DECIMATION); + if (status != ARM_MATH_SUCCESS) { + LOG_ERR("Failed to initialize mic decimator: %d", status); + channel->decimator_ready = false; + return -EINVAL; + } + + channel->packet_count = 0; + channel->packet_time_us = 0; + channel->decimator_ready = true; + + return 0; +} + +static bool any_channel_active_locked(void) +{ + for (uint8_t i = 0; i < MIC_BLE_CHANNEL_COUNT; i++) { + if (channels[i].enabled) { + return true; + } + } + + return false; +} + +static uint64_t timestamp_with_filter_delay(uint64_t sample_time_us) +{ + if (sample_time_us > MIC_BLE_FILTER_DELAY_US) { + return sample_time_us - MIC_BLE_FILTER_DELAY_US; + } + + return 0; +} + +static void packet_emit(struct mic_ble_channel_state *channel) +{ + struct sensor_msg msg = {0}; + uint16_t dt_us = MIC_BLE_SAMPLE_PERIOD_US; + + msg.sd = false; + msg.stream = true; + msg.data.id = channel->sensor_id; + msg.data.size = (channel->packet_count * sizeof(int16_t)) + sizeof(dt_us); + msg.data.time = channel->packet_time_us; + + memcpy(msg.data.data, channel->packet, channel->packet_count * sizeof(int16_t)); + memcpy(&msg.data.data[msg.data.size - sizeof(dt_us)], &dt_us, sizeof(dt_us)); + + if (sensor_queue != nullptr) { + int ret = k_msgq_put(sensor_queue, &msg, K_NO_WAIT); + if (ret != 0) { + LOG_WRN("BLE mic sensor queue full"); + } + } + + channel->packet_count = 0; + channel->packet_time_us = 0; +} + +static void packet_add_sample(struct mic_ble_channel_state *channel, int16_t sample, + uint64_t sample_time_us) +{ + if (channel->packet_count == 0) { + channel->packet_time_us = sample_time_us; + } + + channel->packet[channel->packet_count] = sample; + channel->packet_count++; + + if (channel->packet_count >= MIC_BLE_SAMPLES_PER_PACKET) { + packet_emit(channel); + } +} + +void mic_ble_streamer_set_sensor_queue(struct k_msgq *queue) +{ + k_mutex_lock(&mic_ble_lock, K_FOREVER); + sensor_queue = queue; + k_mutex_unlock(&mic_ble_lock); +} + +int mic_ble_streamer_enable(uint8_t sensor_id, bool enable) +{ + int ret = 0; + + k_mutex_lock(&mic_ble_lock, K_FOREVER); + + struct mic_ble_channel_state *channel = channel_for_sensor(sensor_id); + if (channel == nullptr) { + ret = -EINVAL; + goto out; + } + + if (enable) { + ret = channel_decimator_reset(channel); + if (ret == 0) { + channel->enabled = true; + } + } else { + channel->enabled = false; + channel->packet_count = 0; + channel->packet_time_us = 0; + } + +out: + k_mutex_unlock(&mic_ble_lock); + return ret; +} + +bool mic_ble_streamer_is_active(void) +{ + bool active; + + k_mutex_lock(&mic_ble_lock, K_FOREVER); + active = any_channel_active_locked(); + k_mutex_unlock(&mic_ble_lock); + + return active; +} + +void mic_ble_streamer_process_i2s_block(const void *pcm_block, size_t size, + uint64_t first_sample_time_us) +{ + if (pcm_block == nullptr || size != BLOCK_SIZE_BYTES) { + return; + } + + k_mutex_lock(&mic_ble_lock, K_FOREVER); + + if (!any_channel_active_locked()) { + k_mutex_unlock(&mic_ble_lock); + return; + } + + const int16_t *pcm = static_cast(pcm_block); + const uint64_t sample_time_us = timestamp_with_filter_delay(first_sample_time_us); + + for (uint8_t channel_idx = 0; channel_idx < MIC_BLE_CHANNEL_COUNT; channel_idx++) { + struct mic_ble_channel_state *channel = &channels[channel_idx]; + + if (!channel->enabled || !channel->decimator_ready) { + continue; + } + + for (uint32_t i = 0; i < MIC_BLE_DECIMATION; i++) { + channel->input[i] = pcm[(i * CONFIG_I2S_CH_NUM) + channel->i2s_channel]; + } + + q15_t output; + arm_fir_decimate_q15(&channel->decimator, channel->input, &output, + MIC_BLE_DECIMATION); + + packet_add_sample(channel, output, sample_time_us); + } + + k_mutex_unlock(&mic_ble_lock); +} diff --git a/src/SensorManager/MicBleStreamer.h b/src/SensorManager/MicBleStreamer.h new file mode 100644 index 00000000..102e60d0 --- /dev/null +++ b/src/SensorManager/MicBleStreamer.h @@ -0,0 +1,23 @@ +#ifndef MIC_BLE_STREAMER_H +#define MIC_BLE_STREAMER_H + +#include +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +void mic_ble_streamer_set_sensor_queue(struct k_msgq *queue); +int mic_ble_streamer_enable(uint8_t sensor_id, bool enable); +bool mic_ble_streamer_is_active(void); +void mic_ble_streamer_process_i2s_block(const void *pcm_block, size_t size, + uint64_t first_sample_time_us); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/src/SensorManager/MicrophoneBle.cpp b/src/SensorManager/MicrophoneBle.cpp new file mode 100644 index 00000000..9537d15d --- /dev/null +++ b/src/SensorManager/MicrophoneBle.cpp @@ -0,0 +1,88 @@ +#include "MicrophoneBle.h" + +#include "MicBleStreamer.h" +#include "audio_datapath.h" +#include "hw_codec.h" + +#include + +extern "C" { +#include +extern void init_fifo(void); +} + +extern struct data_fifo fifo_rx; + +MicrophoneBle MicrophoneBle::inner(ID_MICRO_INNER); +MicrophoneBle MicrophoneBle::outer(ID_MICRO_OUTER); + +const SampleRateSetting<1> MicrophoneBle::sample_rates = { + { 0 }, + + { 1000 }, + + { 1000.0 } +}; + +MicrophoneBle::MicrophoneBle(uint8_t sensor_id) : _sensor_id(sensor_id) +{ +} + +bool MicrophoneBle::init(struct k_msgq *queue) +{ + _active = true; + + sensor_queue = queue; + + set_sensor_queue(queue); + mic_ble_streamer_set_sensor_queue(queue); + init_fifo(); + + return true; +} + +void MicrophoneBle::start(int sample_rate_idx) +{ + ARG_UNUSED(sample_rate_idx); + + if (!_active || _running) { + return; + } + + int ret = mic_ble_streamer_enable(_sensor_id, true); + if (ret != 0) { + return; + } + + ret = audio_datapath_aquire(&fifo_rx); + if (ret != 0) { + mic_ble_streamer_enable(_sensor_id, false); + return; + } + + ret = hw_codec_default_conf_enable(); + if (ret != 0) { + audio_datapath_release(); + mic_ble_streamer_enable(_sensor_id, false); + return; + } + + _running = true; +} + +void MicrophoneBle::stop() +{ + if (!_active) { + return; + } + _active = false; + + if (!_running) { + return; + } + + mic_ble_streamer_enable(_sensor_id, false); + audio_datapath_release(); + + _running = false; +} diff --git a/src/SensorManager/MicrophoneBle.h b/src/SensorManager/MicrophoneBle.h new file mode 100644 index 00000000..b2071418 --- /dev/null +++ b/src/SensorManager/MicrophoneBle.h @@ -0,0 +1,25 @@ +#ifndef MICROPHONE_BLE_H +#define MICROPHONE_BLE_H + +#include "EdgeMLSensor.h" +#include "openearable_common.h" + +class MicrophoneBle : public EdgeMlSensor { +public: + static MicrophoneBle inner; + static MicrophoneBle outer; + + bool init(struct k_msgq *queue) override; + void start(int sample_rate_idx) override; + void stop() override; + + const static SampleRateSetting<1> sample_rates; + +private: + explicit MicrophoneBle(uint8_t sensor_id); + + uint8_t _sensor_id; + bool _active = false; +}; + +#endif diff --git a/src/SensorManager/SensorManager.cpp b/src/SensorManager/SensorManager.cpp index 1c8a994b..2266048d 100644 --- a/src/SensorManager/SensorManager.cpp +++ b/src/SensorManager/SensorManager.cpp @@ -13,6 +13,7 @@ #include "Temp.h" #include "BoneConduction.h" #include "Microphone.h" +#include "MicrophoneBle.h" #include "openearable_common.h" #include "StateIndicator.h" @@ -134,6 +135,8 @@ void stop_sensor_manager() { Temp::sensor.stop(); BoneConduction::sensor.stop(); Microphone::sensor.stop(); + MicrophoneBle::inner.stop(); + MicrophoneBle::outer.stop(); active_sensors = 0; @@ -164,6 +167,10 @@ EdgeMlSensor * get_sensor(enum sensor_id id) { return &(BoneConduction::sensor); case ID_MICRO: return &(Microphone::sensor); + case ID_MICRO_INNER: + return &(MicrophoneBle::inner); + case ID_MICRO_OUTER: + return &(MicrophoneBle::outer); default: return NULL; } @@ -257,4 +264,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/audio/Kconfig b/src/audio/Kconfig index 41cfac5d..d828b1c8 100644 --- a/src/audio/Kconfig +++ b/src/audio/Kconfig @@ -128,12 +128,19 @@ config AUDIO_SOURCE_I2S bool "Use I2S as audio source" endchoice +config MIC_BLE_STREAM + bool "Use I2S microphone for BLE sensor streaming" + default n + help + Enables microphone capture for the sensor BLE service without exposing + the microphone as a bidirectional LE Audio source. + choice AUDIO_MIC_SOURCE - depends on STREAM_BIDIRECTIONAL + depends on STREAM_BIDIRECTIONAL || MIC_BLE_STREAM prompt "Audio microphone source for bidirectional stream" default AUDIO_MIC_I2S help - Select microphone source for the bidrectional le audio. + Select microphone source for bidirectional LE Audio or BLE sensor streaming. config AUDIO_MIC_PDM bool "Use Mic via PDM" diff --git a/src/audio/audio_datapath.c b/src/audio/audio_datapath.c index 0b28d728..ff1d7db4 100644 --- a/src/audio/audio_datapath.c +++ b/src/audio/audio_datapath.c @@ -25,9 +25,11 @@ #include "audio_system.h" #include "streamctrl.h" #include "sd_card_playback.h" +#include "audio_sync_timer.h" #include "Equalizer.h" #include "sdlogger_wrapper.h" +#include "../SensorManager/MicBleStreamer.h" #include LOG_MODULE_REGISTER(audio_datapath, CONFIG_AUDIO_DATAPATH_LOG_LEVEL); @@ -173,6 +175,69 @@ static struct { static struct k_msgq * sensor_queue; +K_MSGQ_DEFINE(rx_timestamp_queue, sizeof(uint64_t), + CONFIG_FIFO_RX_FRAME_COUNT * CONFIG_FIFO_FRAME_SPLIT_NUM + 2, 4); + +static uint32_t rx_audio_ts_base_us; +static uint64_t rx_time_base_us; +static bool rx_time_base_valid; + +static void rx_timestamp_reset(void) +{ + k_msgq_purge(&rx_timestamp_queue); + rx_audio_ts_base_us = audio_sync_timer_capture(); + rx_time_base_us = micros(); + rx_time_base_valid = true; +} + +static uint64_t rx_audio_ts_to_time_us(uint32_t audio_ts_us) +{ + if (!rx_time_base_valid) { + return micros(); + } + + return rx_time_base_us + (uint32_t)(audio_ts_us - rx_audio_ts_base_us); +} + +static void rx_timestamp_push(uint32_t frame_start_ts_us) +{ + uint64_t block_time_us; + uint32_t block_start_ts_us = frame_start_ts_us - BLK_PERIOD_US; + + block_time_us = rx_audio_ts_to_time_us(block_start_ts_us); + + int ret = k_msgq_put(&rx_timestamp_queue, &block_time_us, K_NO_WAIT); + if (ret != 0) { + uint64_t dropped; + + (void)k_msgq_get(&rx_timestamp_queue, &dropped, K_NO_WAIT); + ret = k_msgq_put(&rx_timestamp_queue, &block_time_us, K_NO_WAIT); + if (ret != 0) { + LOG_WRN("RX timestamp queue full"); + } + } +} + +static uint64_t rx_timestamp_pop(void) +{ + uint64_t block_time_us; + int ret = k_msgq_get(&rx_timestamp_queue, &block_time_us, K_NO_WAIT); + + if (ret != 0) { + LOG_WRN("RX timestamp missing"); + return micros(); + } + + return block_time_us; +} + +static void rx_timestamp_drop_oldest(void) +{ + uint64_t dropped; + + (void)k_msgq_get(&rx_timestamp_queue, &dropped, K_NO_WAIT); +} + //extern struct audio_data fifo_rx; //K_MSGQ_DEFINE(rx_queue, sizeof(struct audio_data), 16, 4); @@ -213,9 +278,12 @@ static void data_thread(void *arg1, void *arg2, void *arg3) ret = data_fifo_pointer_last_filled_get(ctrl_blk.in.fifo, &tmp_pcm_raw_data[i], &pcm_block_size, K_FOREVER); ERR_CHK(ret); - uint64_t time_stamp = micros(); + uint64_t time_stamp = rx_timestamp_pop(); memcpy(audio_item.data + (i * BLOCK_SIZE_BYTES), tmp_pcm_raw_data[i], pcm_block_size); + + mic_ble_streamer_process_i2s_block(tmp_pcm_raw_data[i], pcm_block_size, + time_stamp); data_fifo_block_free(ctrl_blk.in.fifo, tmp_pcm_raw_data[i]); @@ -832,13 +900,15 @@ static void audio_datapath_i2s_blk_complete(uint32_t frame_start_ts_us, uint32_t static uint32_t *rx_buf; static int prev_ret; - if ((IS_ENABLED(CONFIG_STREAM_BIDIRECTIONAL) || (CONFIG_AUDIO_DEV == GATEWAY)) && IS_ENABLED(CONFIG_AUDIO_MIC_I2S)) { + if ((IS_ENABLED(CONFIG_STREAM_BIDIRECTIONAL) || IS_ENABLED(CONFIG_MIC_BLE_STREAM) || + (CONFIG_AUDIO_DEV == GATEWAY)) && IS_ENABLED(CONFIG_AUDIO_MIC_I2S)) { /* Lock last filled buffer into message queue */ if (rx_buf_released != NULL) { ret = data_fifo_block_lock(ctrl_blk.in.fifo, (void **)&rx_buf_released, BLOCK_SIZE_BYTES); ERR_CHK_MSG(ret, "Unable to lock block RX"); + rx_timestamp_push(frame_start_ts_us); } /* Get new empty buffer to send to I2S HW */ @@ -864,6 +934,7 @@ static void audio_datapath_i2s_blk_complete(uint32_t frame_start_ts_us, uint32_t ERR_CHK(ret); data_fifo_block_free(ctrl_blk.in.fifo, data); + rx_timestamp_drop_oldest(); ret = data_fifo_pointer_first_vacant_get(ctrl_blk.in.fifo, (void **)&rx_buf, K_NO_WAIT); @@ -903,7 +974,8 @@ static void audio_datapath_i2s_start(void) } /* RX */ - if ((IS_ENABLED(CONFIG_STREAM_BIDIRECTIONAL) || (CONFIG_AUDIO_DEV == GATEWAY)) && IS_ENABLED(CONFIG_AUDIO_MIC_I2S)) { + if ((IS_ENABLED(CONFIG_STREAM_BIDIRECTIONAL) || IS_ENABLED(CONFIG_MIC_BLE_STREAM) || + (CONFIG_AUDIO_DEV == GATEWAY)) && IS_ENABLED(CONFIG_AUDIO_MIC_I2S)) { uint32_t alloced_cnt; uint32_t locked_cnt; @@ -1179,6 +1251,7 @@ int audio_datapath_start(struct data_fifo *fifo_rx) /* Clear counters and mute initial audio */ memset(&ctrl_blk.out, 0, sizeof(ctrl_blk.out)); + rx_timestamp_reset(); audio_datapath_i2s_start(); ctrl_blk.stream_started = true; @@ -1201,6 +1274,8 @@ int audio_datapath_stop(void) pres_comp_state_set(PRES_STATE_INIT); data_fifo_empty(ctrl_blk.in.fifo); + k_msgq_purge(&rx_timestamp_queue); + rx_time_base_valid = false; return 0; } else { diff --git a/src/bluetooth/bt_stream/unicast/unicast_server.c b/src/bluetooth/bt_stream/unicast/unicast_server.c index c1aa6384..d3ce0048 100644 --- a/src/bluetooth/bt_stream/unicast/unicast_server.c +++ b/src/bluetooth/bt_stream/unicast/unicast_server.c @@ -15,7 +15,7 @@ #include #include #include - + #include "macros_common.h" #include "zbus_common.h" #include "bt_mgmt.h" @@ -767,17 +767,21 @@ static uint8_t device_identifier[] = { return ret; } - ret = bt_pacs_set_supported_contexts(BT_AUDIO_DIR_SOURCE, AVAILABLE_SOURCE_CONTEXT); - - if (ret) { - LOG_ERR("Supported context set failed. Err: %d", ret); - return ret; - } - - ret = bt_pacs_set_available_contexts(BT_AUDIO_DIR_SOURCE, AVAILABLE_SOURCE_CONTEXT); - if (ret) { - LOG_ERR("Available context set failed. Err: %d", ret); - return ret; + if (IS_ENABLED(CONFIG_BT_PAC_SRC)) { + ret = bt_pacs_set_supported_contexts(BT_AUDIO_DIR_SOURCE, + AVAILABLE_SOURCE_CONTEXT); + + if (ret) { + LOG_ERR("Supported context set failed. Err: %d", ret); + return ret; + } + + ret = bt_pacs_set_available_contexts(BT_AUDIO_DIR_SOURCE, + AVAILABLE_SOURCE_CONTEXT); + if (ret) { + LOG_ERR("Available context set failed. Err: %d", ret); + return ret; + } } for (int i = 0; i < ARRAY_SIZE(cap_audio_streams); i++) { @@ -797,9 +801,8 @@ static uint8_t device_identifier[] = { return ret; } } - + initialized = true; - + return 0; - } - \ No newline at end of file +} diff --git a/src/drivers/ADAU1860.cpp b/src/drivers/ADAU1860.cpp index 3d3e2d88..da98d5ee 100644 --- a/src/drivers/ADAU1860.cpp +++ b/src/drivers/ADAU1860.cpp @@ -156,7 +156,8 @@ int ADAU1860::begin() { //uint8_t asrci_route01 = 0x0; // ASRCI0_EN //writeReg(registers::ASRCI_ROUTE01, &asrci_route01, sizeof(asrci_route01)); - if (IS_ENABLED(CONFIG_STREAM_BIDIRECTIONAL) && (CONFIG_AUDIO_DEV == HEADSET)) { + if ((IS_ENABLED(CONFIG_STREAM_BIDIRECTIONAL) || IS_ENABLED(CONFIG_MIC_BLE_STREAM)) && + (CONFIG_AUDIO_DEV == HEADSET)) { // I2S_IN enable | I2S_OUT enable | MIC enable uint8_t sai_clk_pwr = 0x01 | (1 << 1) | (1 << 4); writeReg(registers::SAI_CLK_PWR, &sai_clk_pwr, sizeof(sai_clk_pwr)); @@ -598,4 +599,4 @@ SHELL_STATIC_SUBCMD_SET_CREATE(dsp_cmd, SHELL_SUBCMD_SET_END); SHELL_CMD_REGISTER(dsp, &dsp_cmd, "Set DSP parameters", NULL); -#endif \ No newline at end of file +#endif diff --git a/src/modules/audio_i2s.c b/src/modules/audio_i2s.c index 6cbee8d2..18004407 100644 --- a/src/modules/audio_i2s.c +++ b/src/modules/audio_i2s.c @@ -75,7 +75,8 @@ static void i2s_comp_handler(nrfx_i2s_buffers_t const *released_bufs, uint32_t s void audio_i2s_set_next_buf(const uint8_t *tx_buf, uint32_t *rx_buf) { __ASSERT_NO_MSG(state == AUDIO_I2S_STATE_STARTED); - if ((IS_ENABLED(CONFIG_STREAM_BIDIRECTIONAL) || (CONFIG_AUDIO_DEV == GATEWAY)) && IS_ENABLED(CONFIG_AUDIO_MIC_I2S)) { + if ((IS_ENABLED(CONFIG_STREAM_BIDIRECTIONAL) || IS_ENABLED(CONFIG_MIC_BLE_STREAM) || + (CONFIG_AUDIO_DEV == GATEWAY)) && IS_ENABLED(CONFIG_AUDIO_MIC_I2S)) { __ASSERT_NO_MSG(rx_buf != NULL); } @@ -96,7 +97,8 @@ void audio_i2s_set_next_buf(const uint8_t *tx_buf, uint32_t *rx_buf) void audio_i2s_start(const uint8_t *tx_buf, uint32_t *rx_buf) { __ASSERT_NO_MSG(state == AUDIO_I2S_STATE_IDLE); - if ((IS_ENABLED(CONFIG_STREAM_BIDIRECTIONAL) || (CONFIG_AUDIO_DEV == GATEWAY)) && IS_ENABLED(CONFIG_AUDIO_MIC_I2S)) { + if ((IS_ENABLED(CONFIG_STREAM_BIDIRECTIONAL) || IS_ENABLED(CONFIG_MIC_BLE_STREAM) || + (CONFIG_AUDIO_DEV == GATEWAY)) && IS_ENABLED(CONFIG_AUDIO_MIC_I2S)) { __ASSERT_NO_MSG(rx_buf != NULL); }