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 = diff --git a/src/ParseInfo/DefaultSensors.h b/src/ParseInfo/DefaultSensors.h index e6e3ef21..97ce4166 100644 --- a/src/ParseInfo/DefaultSensors.h +++ b/src/ParseInfo/DefaultSensors.h @@ -141,11 +141,11 @@ SensorScheme defaultSensors[SENSOR_COUNT] = { .groupCount = MICRO_GROUP_COUNT, .groups = microGroups, .configOptions = { - .availableOptions = DATA_STORAGE | FREQUENCIES_DEFINED, // no streaming + .availableOptions = DATA_STORAGE | FREQUENCIES_DEFINED, .frequencyOptions = { .frequencyCount = sizeof(Microphone::sample_rates.reg_vals), - .defaultFrequencyIndex = 1, - .maxBleFrequencyIndex = 1, + .defaultFrequencyIndex = 8, + .maxBleFrequencyIndex = 8, .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/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..6aad24a2 100644 --- a/src/ParseInfo/SensorScheme.h +++ b/src/ParseInfo/SensorScheme.h @@ -65,6 +65,24 @@ 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); float getSampleRateForSensor(struct SensorScheme* sensorScheme, uint8_t frequencyIndex); @@ -72,4 +90,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 fe89aaba..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); @@ -89,6 +115,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 +142,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 +152,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 +173,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 +195,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 +207,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); } @@ -275,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; } @@ -284,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; } @@ -354,7 +432,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 +502,6 @@ int SDLogger::end() { } if (!sd_card->is_mounted()) { - //k_poll_signal_reset(&logger_sig); is_open = false; return -ENODEV; } @@ -445,13 +524,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/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 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; diff --git a/src/SensorManager/Microphone.cpp b/src/SensorManager/Microphone.cpp index fe907e2d..aaddcdb9 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 = { + { 24, 16, 12, 8, 6, 4, 3, 2, 1 }, - { 48000 }, + { 2000, 3000, 4000, 6000, 8000, 12000, 16000, 24000, 48000 }, - { 48000.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) { @@ -55,16 +54,21 @@ 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]); + record_to_sd(_sd_logging); audio_datapath_aquire(&fifo_rx); + audio_datapath_decimator_init(sample_rates.reg_vals[sample_rate_idx]); + + record_to_sd(true); + _running = true; } @@ -78,5 +82,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..49d9997a 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(); @@ -257,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"); @@ -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/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/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/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); diff --git a/src/bluetooth/gatt_services/sensor_service.c b/src/bluetooth/gatt_services/sensor_service.c index 17e9cd25..3d821d14 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,18 +25,19 @@ 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; 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 @@ -45,9 +47,110 @@ 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. + */ +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; + 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 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; + k_spin_unlock(¬ify_state_lock, key); + + k_msgq_purge(&gatt_queue); +} + +/** + * @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(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) +{ + k_spinlock_key_t key; + + if (context == NULL) { + return; + } + + 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 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, uint32_t generation) +{ + k_spinlock_key_t key; + + if (context == NULL) { + return; + } + + 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; -int MAX_NOTIFIES_IN_FLIGHT = 4; + 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) { @@ -57,13 +160,15 @@ 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: - connection_complete = false; - notify_enabled = false; - k_msgq_purge(&gatt_queue); + reset_sensor_notification_state(); break; } } @@ -71,7 +176,10 @@ 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"); @@ -81,7 +189,9 @@ static void sensor_ccc_cfg_changed(const struct bt_gatt_attr *attr, 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, @@ -180,7 +290,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 +305,57 @@ BT_GATT_CHARACTERISTIC(BT_UUID_SENSOR_RECORDING_NAME, read_sensor_rec_name, write_sensor_rec_name, NULL), ); -static void notify_complete() { - notify_count--; +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 (notify_count < 0) { - notify_count = 0; - LOG_WRN("Notify count went below zero!"); + 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); + + if (context == NULL) { + return; + } + + generation = context->generation; + release_notify_context(context, generation); } static void notification_task(void) { int ret; + struct sensor_data sensor_data; while (1) { ret = k_msgq_get(&gatt_queue, &sensor_data, K_FOREVER); @@ -215,26 +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; - - 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; - - while(notify_count >= MAX_NOTIFIES_IN_FLIGHT) { - k_yield(); // maybe replace with k_sleep? + 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; + + 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"); + break; } - notify_count++; - - ret = bt_gatt_notify_cb(NULL, ¶ms); + context->params.len = size; + ret = bt_gatt_notify_cb(NULL, &context->params); if (ret != 0) { LOG_WRN("Failed to send data: %d.\n", ret); + release_notify_context(context, generation); + } else { + mark_notify_context_in_flight(context, generation); } + break; } } } 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 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); 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 */