From 08b081874a70b95f815ecf29de525ab3325d268a Mon Sep 17 00:00:00 2001 From: Dennis Moschina <45356478+DennisMoschina@users.noreply.github.com> Date: Mon, 11 May 2026 11:25:57 +0200 Subject: [PATCH] feat(storage): improve SD logging and USB mass storage --- include/openearable_common.h | 10 +- src/SD_Card/SDLogger/CMakeLists.txt | 1 + src/SD_Card/SDLogger/SDLogger.cpp | 120 +++---- src/SD_Card/SDLogger/SDLogger.h | 19 +- src/SD_Card/SDLogger/sd_bench.c | 205 ++++++++++++ .../SD_Card_Manager/SD_Card_Manager.cpp | 113 +++++-- src/SD_Card/SD_Card_Manager/SD_Card_Manager.h | 8 +- src/audio/CMakeLists.txt | 3 - src/audio/audio_datapath.c | 31 +- src/audio/audio_datapath.h | 6 +- src/audio/audio_system.c | 2 +- src/audio/sdlogger_wrapper.cpp | 12 - src/audio/sdlogger_wrapper.h | 23 -- src/audio/streamctrl.c | 25 +- src/bluetooth/bt_management/bt_mgmt.c | 18 +- .../bt_stream/unicast/unicast_server.c | 10 +- src/openearable_common.c | 12 - src/time_sync/time_sync.c | 77 +++++ tools/analyze_packet_loss.py | 308 ++++++++++++++++++ 19 files changed, 775 insertions(+), 228 deletions(-) create mode 100644 src/SD_Card/SDLogger/sd_bench.c delete mode 100644 src/audio/sdlogger_wrapper.cpp delete mode 100644 src/audio/sdlogger_wrapper.h delete mode 100644 src/openearable_common.c create mode 100755 tools/analyze_packet_loss.py diff --git a/include/openearable_common.h b/include/openearable_common.h index 12c728a5..133dced2 100644 --- a/include/openearable_common.h +++ b/include/openearable_common.h @@ -28,7 +28,7 @@ uint64_t oe_micros(); #define load_switch_sd_id DT_NODELABEL(load_switch_sd) #define load_switch_1_8_id DT_NODELABEL(load_switch) -#define load_switch_3_3_id DT_NODELABEL(bq25120a) +#define load_switch_3_3_id DT_CHILD(DT_NODELABEL(bq25120a), load_switch) extern const struct device *const cons; extern const struct device *const ls_1_8; @@ -85,7 +85,6 @@ enum sensor_id { ID_TEMP_BARO=1, ID_MICRO=2, ID_PPG=4, - ID_PULSOX=5, ID_OPTTEMP=6, ID_BONE_CONDUCTION=7, }; @@ -138,11 +137,4 @@ struct sd_msg { bool removed; }; -#include "audio_i2s.h" - -struct audio_rx_data { - char data[FRAME_SIZE_BYTES]; - size_t size; -}; - #endif \ No newline at end of file diff --git a/src/SD_Card/SDLogger/CMakeLists.txt b/src/SD_Card/SDLogger/CMakeLists.txt index 93fcf222..d6253b25 100644 --- a/src/SD_Card/SDLogger/CMakeLists.txt +++ b/src/SD_Card/SDLogger/CMakeLists.txt @@ -1,3 +1,4 @@ target_sources(app PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/SDLogger.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/sd_bench.c ) \ No newline at end of file diff --git a/src/SD_Card/SDLogger/SDLogger.cpp b/src/SD_Card/SDLogger/SDLogger.cpp index fe89aaba..5c69fbde 100644 --- a/src/SD_Card/SDLogger/SDLogger.cpp +++ b/src/SD_Card/SDLogger/SDLogger.cpp @@ -8,6 +8,7 @@ #include "PowerManager.h" #include #include "audio_datapath.h" +#include "channel_assignment.h" #include "StateIndicator.h" @@ -16,16 +17,8 @@ LOG_MODULE_REGISTER(sd_logger, CONFIG_LOG_DEFAULT_LEVEL); ZBUS_CHAN_DECLARE(sd_card_chan); -void sensor_listener_cb(const struct zbus_channel *chan); - -K_MSGQ_DEFINE(sd_sensor_queue, sizeof(sensor_data), CONFIG_SENSOR_SD_SUB_QUEUE_SIZE, 4); -ZBUS_LISTENER_DEFINE(sensor_data_listener, sensor_listener_cb); - -// Define thread stack K_THREAD_STACK_DEFINE(thread_stack, CONFIG_SENSOR_SD_STACK_SIZE); -ZBUS_CHAN_DECLARE(sensor_chan); - void sd_listener_callback(const struct zbus_channel *chan); ZBUS_LISTENER_DEFINE(sd_card_event_listener, sd_listener_callback); @@ -36,7 +29,7 @@ static k_tid_t thread_id; struct ring_buf ring_buffer; struct k_mutex ring_mutex; // Protects ring_buffer operations struct k_mutex file_mutex; // Protects sd_card open/write/close -uint8_t buffer[BUFFER_SIZE]; // Ring Buffer Speicher +uint8_t buffer[BUFFER_SIZE]; // Coordination flags (atomic because they are accessed from multiple threads) static atomic_t g_stop_writing; // 1 while end()/flush/close is in progress @@ -44,6 +37,16 @@ static atomic_t g_sd_removed; // 1 if SD was removed while recording uint32_t count_max_buffer_fill = 0; +static struct { + uint32_t ring_full; + uint32_t ring_mutex_fail; + uint32_t sd_writes; + uint32_t sd_write_max_us; + uint64_t sd_write_total_us; + uint32_t bytes_written; + uint32_t bytes_dropped; +} sd_stats; + 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); @@ -60,20 +63,6 @@ SDLogger::~SDLogger() { } -//static bool _prio_boost = false; - -void sensor_listener_cb(const struct zbus_channel *chan) { - const sensor_msg* msg = (sensor_msg*)zbus_chan_const_msg(chan); - - if (msg->sd) { - int ret = sdlogger.write_sensor_data(msg->data); - if (ret < 0) { - LOG_WRN("Failed to enqueue sensor data for SD: %d", ret); - } - } -} - - void sd_listener_callback(const struct zbus_channel *chan) { const struct sd_msg * sd_msg_event = (sd_msg *)zbus_chan_const_msg(&sd_card_chan); @@ -110,9 +99,13 @@ void SDLogger::sensor_sd_task() { continue; } + // Reset signal early so signals raised during the drain loop + // are preserved for the next k_poll wakeup. + k_poll_signal_reset(&logger_sig); + logger_evt.state = K_POLL_STATE_NOT_READY; + // If a close/flush is in progress, do not write concurrently. if (atomic_get(&g_stop_writing)) { - k_poll_signal_reset(&logger_sig); continue; } @@ -122,7 +115,6 @@ 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); continue; } @@ -132,53 +124,60 @@ void SDLogger::sensor_sd_task() { return; } - uint32_t fill = ring_buf_size_get(&ring_buffer); + // Drain all available full blocks from the ring buffer. + while (ring_buf_size_get(&ring_buffer) >= SD_BLOCK_SIZE) { + if (atomic_get(&g_stop_writing) || atomic_get(&g_sd_removed)) { + break; + } - if (fill >= SD_BLOCK_SIZE) { + uint32_t fill = ring_buf_size_get(&ring_buffer); if (fill > count_max_buffer_fill) { count_max_buffer_fill = fill; } uint8_t *data = nullptr; - // Claim up to one SD block from the ring buffer under lock. + // Claim up to 4 SD blocks 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, 4 * SD_BLOCK_SIZE); k_mutex_unlock(&ring_mutex); if (claimed == 0 || data == nullptr) { - // Nothing to write right now. - k_poll_signal_reset(&logger_sig); - continue; + break; } // Write the claimed bytes under file lock. size_t write_size = claimed; int written; + uint64_t t0 = micros(); k_mutex_lock(&file_mutex, K_FOREVER); written = sdlogger.sd_card->write((char*)data, &write_size, false); k_mutex_unlock(&file_mutex); + uint32_t elapsed = (uint32_t)(micros() - t0); + + sd_stats.sd_writes++; + sd_stats.sd_write_total_us += elapsed; + if (elapsed > sd_stats.sd_write_max_us) { + sd_stats.sd_write_max_us = elapsed; + } if (written < 0) { state_indicator.set_sd_state(SD_FAULT); LOG_ERR("SD write failed: %d", written); - - // Do not advance the ring buffer on error. - // Wakeups will continue; user can call end(). - k_poll_signal_reset(&logger_sig); - continue; + break; } + sd_stats.bytes_written += (uint32_t)written; + // Advance ring buffer by the number of bytes actually written. k_mutex_lock(&ring_mutex, K_FOREVER); ring_buf_get_finish(&ring_buffer, (uint32_t)written); k_mutex_unlock(&ring_mutex); - } else { + + // Let lower-priority threads (sensor work queue) run between writes k_yield(); } - k_poll_signal_reset(&logger_sig); - STACK_USAGE_PRINT("sensor_msg_thread", &sdlogger.thread_data); } } @@ -186,15 +185,11 @@ void SDLogger::sensor_sd_task() { int SDLogger::init() { int ret; - sd_card->init(); - ring_buf_init(&ring_buffer, BUFFER_SIZE, buffer); atomic_clear(&g_stop_writing); atomic_clear(&g_sd_removed); - //set_ring_buffer(&ring_buffer); - k_poll_signal_init(&logger_sig); thread_id = k_thread_create( @@ -208,12 +203,6 @@ int SDLogger::init() { return ret; } - ret = zbus_chan_add_obs(&sensor_chan, &sensor_data_listener, ZBUS_ADD_OBS_TIMEOUT_MS); - if (ret) { - LOG_ERR("Failed to add sensor sub"); - return ret; - } - ret = zbus_chan_add_obs(&sd_card_chan, &sd_card_event_listener, ZBUS_ADD_OBS_TIMEOUT_MS); if (ret) { LOG_ERR("Failed to add sd sub"); @@ -271,6 +260,9 @@ int SDLogger::begin(const std::string& filename) { ring_buf_reset(&ring_buffer); k_mutex_unlock(&ring_mutex); + memset(&sd_stats, 0, sizeof(sd_stats)); + count_max_buffer_fill = 0; + ret = write_header(); if (ret < 0) { state_indicator.set_sd_state(SD_FAULT); @@ -290,6 +282,11 @@ int SDLogger::write_header() { header->version = SENSOR_LOG_VERSION; header->timestamp = micros(); + header->device_id = oe_boot_state.device_id; + + enum audio_channel ch; + channel_assignment_get(&ch); + header->channel = (uint8_t)ch; int ret; k_mutex_lock(&file_mutex, K_FOREVER); @@ -320,8 +317,11 @@ int SDLogger::write_sensor_data(const void* const* data_blocks, const size_t* le return -ENODEV; } - // Do not block producers; if mutex is contended, drop quickly - if (k_mutex_lock(&ring_mutex, K_NO_WAIT) != 0) { + // Allow a short wait for mutex contention to clear. + // The writer holds ring_mutex only during fast ring_buf operations. + if (k_mutex_lock(&ring_mutex, K_USEC(200)) != 0) { + sd_stats.ring_mutex_fail++; + sd_stats.bytes_dropped += total_length; return -EAGAIN; } @@ -329,8 +329,9 @@ int SDLogger::write_sensor_data(const void* const* data_blocks, const size_t* le // in SD_BLOCK_SIZE chunks to keep SD writer alignment and minimize partial writes. uint32_t space = ring_buf_space_get(&ring_buffer); if (space < total_length) { - LOG_ERR("Ring buffer low on space: have %u, need %zu. Skipping data", - space, total_length); + sd_stats.ring_full++; + sd_stats.bytes_dropped += total_length; + LOG_DBG("Ring buffer full: have %u, need %zu", space, total_length); k_mutex_unlock(&ring_mutex); return -ENOSPC; } @@ -439,7 +440,13 @@ int SDLogger::end() { LOG_INF("Close File ...."); - LOG_DBG("Max buffer fill: %d bytes", count_max_buffer_fill); + uint32_t avg_write_us = sd_stats.sd_writes ? (uint32_t)(sd_stats.sd_write_total_us / sd_stats.sd_writes) : 0; + LOG_INF("SD stats: ring_full=%u ring_mutex_fail=%u bytes_dropped=%u", + sd_stats.ring_full, sd_stats.ring_mutex_fail, sd_stats.bytes_dropped); + LOG_INF("SD stats: writes=%u bytes=%u max_fill=%u/%u", + sd_stats.sd_writes, sd_stats.bytes_written, count_max_buffer_fill, (unsigned)BUFFER_SIZE); + LOG_INF("SD stats: write_avg=%u us write_max=%u us", + avg_write_us, sd_stats.sd_write_max_us); k_mutex_lock(&file_mutex, K_FOREVER); ret = sd_card->close_file(); @@ -455,6 +462,9 @@ int SDLogger::end() { atomic_clear(&g_stop_writing); atomic_clear(&g_sd_removed); + /* Unmount the SD card so USB MSC can re-enable for host access. */ + sd_card->unmount(); + return 0; } diff --git a/src/SD_Card/SDLogger/SDLogger.h b/src/SD_Card/SDLogger/SDLogger.h index 1b52e524..3747c27e 100644 --- a/src/SD_Card/SDLogger/SDLogger.h +++ b/src/SD_Card/SDLogger/SDLogger.h @@ -10,7 +10,7 @@ constexpr size_t SD_BLOCK_SIZE = 4096; -constexpr size_t BUFFER_BLOCK_COUNT = 8; // Number of blocks in the buffer +constexpr size_t BUFFER_BLOCK_COUNT = 16; // Number of blocks in the buffer constexpr size_t BUFFER_SIZE = SD_BLOCK_SIZE * BUFFER_BLOCK_COUNT; // BUFFER_SIZE must always be a multiple of SD_BLOCK_SIZE to ensure proper block alignment @@ -18,38 +18,27 @@ constexpr size_t BUFFER_SIZE = SD_BLOCK_SIZE * BUFFER_BLOCK_COUNT; // and will not work correctly otherwise. static_assert(BUFFER_SIZE % SD_BLOCK_SIZE == 0, "BUFFER_SIZE must be a multiple of SD_BLOCK_SIZE"); -// Forward declare the work handler -//static void sd_work_handler(struct k_work* work); - -// Singleton pattern class SDLogger { -protected: - // Add static instance pointer for work handler - //static SDLogger* instance_ptr; - //friend void sd_work_handler(struct k_work* work); - private: - SDCardManager* sd_card = nullptr; bool is_open = false; - //uint8_t buffer[BUFFER_SIZE]; // Ring Buffer Speicher - //size_t buffer_pos = 0; std::string current_file; int write_header(); //Write file header with version and timestamp 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; + uint64_t device_id; + uint8_t channel; // AUDIO_CH_L (0) = left, AUDIO_CH_R (1) = right }; struct sensor_data msg; - //struct sensor_data* const data_buf = &(msg.data); static void sensor_sd_task(); friend void sd_listener_callback(const struct zbus_channel *chan); diff --git a/src/SD_Card/SDLogger/sd_bench.c b/src/SD_Card/SDLogger/sd_bench.c new file mode 100644 index 00000000..93a7486b --- /dev/null +++ b/src/SD_Card/SDLogger/sd_bench.c @@ -0,0 +1,205 @@ +/** + * Standalone SD card write benchmark. + * + * Writes blocks directly to a file on the SD card (bypassing the ring buffer + * and zbus pipeline) and reports per-write latency statistics. This lets us + * measure raw SD write performance independently of the sensor stack. + * + * Usage: sd_bench [block_size] [interval_us] + * + * duration_s – how long to run (seconds) + * block_size – bytes per write (default 512) + * interval_us – delay between writes in µs (default 1000 = 1 ms) + * + * Example: sd_bench 10 4096 2000 + * → 10 s of 4 KB writes every 2 ms ≈ 2 MB/s + */ + +#include +#include +#include +#include +#include +#include + +LOG_MODULE_REGISTER(sd_bench, LOG_LEVEL_INF); + +/* Histogram buckets (µs): <500, <1000, <2000, <5000, <10000, <50000, >=50000 */ +#define HIST_BUCKETS 7 +static const uint32_t hist_limits[HIST_BUCKETS] = { + 500, 1000, 2000, 5000, 10000, 50000, UINT32_MAX +}; +static const char *hist_labels[HIST_BUCKETS] = { + "<0.5ms", "<1ms", "<2ms", "<5ms", "<10ms", "<50ms", ">=50ms" +}; + +static int cmd_sd_bench(const struct shell *sh, size_t argc, char **argv) +{ + if (argc < 2) { + shell_error(sh, "Usage: sd_bench [block_size] [interval_us]"); + return -EINVAL; + } + + int duration_s = atoi(argv[1]); + int block_size = (argc > 2) ? atoi(argv[2]) : 512; + int interval_us = (argc > 3) ? atoi(argv[3]) : 1000; + + if (duration_s <= 0 || block_size <= 0 || block_size > 16384) { + shell_error(sh, "Bad params (block_size max 16384)"); + return -EINVAL; + } + + /* Fill a write buffer with a repeating pattern */ + static uint8_t wbuf[16384]; + memset(wbuf, 0xAA, sizeof(wbuf)); + + /* Mount SD card only if nothing else has already mounted "/SD:". + * Zephyr's fs_mount logs "file system already mounted!!" at + * whenever a mount struct is re-registered, so we query the live + * mount table rather than tracking our own flag. */ + static FATFS fat_fs; + static struct fs_mount_t mnt = { + .type = FS_FATFS, + .fs_data = &fat_fs, + .mnt_point = "/SD:", + }; + bool already_mounted = false; + int idx = 0; + const char *mname; + while (fs_readmount(&idx, &mname) == 0) { + if (strcmp(mname, mnt.mnt_point) == 0) { + already_mounted = true; + break; + } + } + bool we_mounted = false; + if (!already_mounted) { + int mret = fs_mount(&mnt); + if (mret < 0) { + shell_error(sh, "SD mount failed: %d", mret); + return mret; + } + we_mounted = true; + } + + /* Open a test file */ + struct fs_file_t fp; + fs_file_t_init(&fp); + + const char *path = "/SD:/sd_bench.bin"; + int ret = fs_open(&fp, path, FS_O_CREATE | FS_O_WRITE | FS_O_APPEND); + if (ret < 0) { + shell_error(sh, "fs_open failed: %d", ret); + return ret; + } + + shell_print(sh, "SD bench: %d s, %d B/write, %d us interval", + duration_s, block_size, interval_us); + + /* Stats */ + uint32_t hist[HIST_BUCKETS] = {0}; + uint32_t count = 0; + uint64_t total_us = 0; + uint32_t min_us = UINT32_MAX; + uint32_t max_us = 0; + uint64_t total_bytes = 0; + + /* Collect the 16 largest write times for reporting */ + uint32_t top[16] = {0}; + + uint64_t end_time = k_uptime_get() + (uint64_t)duration_s * 1000; + + while (k_uptime_get() < end_time) { + uint64_t t0 = k_cycle_get_32(); + + ret = fs_write(&fp, wbuf, block_size); + + uint64_t t1 = k_cycle_get_32(); + + if (ret < 0) { + shell_error(sh, "fs_write failed: %d", ret); + break; + } + + uint32_t elapsed_us = k_cyc_to_us_floor32(t1 - t0); + + count++; + total_us += elapsed_us; + total_bytes += ret; + if (elapsed_us < min_us) min_us = elapsed_us; + if (elapsed_us > max_us) max_us = elapsed_us; + + /* Histogram */ + for (int i = 0; i < HIST_BUCKETS; i++) { + if (elapsed_us < hist_limits[i]) { + hist[i]++; + break; + } + } + + /* Track top-N worst writes */ + for (int i = 0; i < 16; i++) { + if (elapsed_us > top[i]) { + /* Shift down */ + for (int j = 15; j > i; j--) top[j] = top[j-1]; + top[i] = elapsed_us; + break; + } + } + + if (interval_us > 0) { + k_usleep(interval_us); + } + } + + fs_sync(&fp); + fs_close(&fp); + /* Clean up test file */ + fs_unlink(path); + + /* Only unmount if we were the ones who mounted — leave any pre-existing + * owner (e.g. sd_card module) undisturbed. */ + if (we_mounted) { + int uret = fs_unmount(&mnt); + if (uret < 0) { + shell_warn(sh, "SD unmount returned %d", uret); + } + } + + if (count == 0) { + shell_print(sh, "No writes completed"); + return 0; + } + + uint32_t mean_us = (uint32_t)(total_us / count); + + shell_print(sh, "\n=== SD Bench Results ==="); + shell_print(sh, " Writes: %u (%llu bytes, %.1f KB/s)", + count, total_bytes, + (double)total_bytes / duration_s / 1024.0); + shell_print(sh, " Latency (us): min=%u mean=%u max=%u", + min_us, mean_us, max_us); + + shell_print(sh, "\n Histogram:"); + for (int i = 0; i < HIST_BUCKETS; i++) { + if (hist[i] > 0) { + shell_print(sh, " %7s: %u (%.1f%%)", + hist_labels[i], hist[i], + (double)hist[i] / count * 100.0); + } + } + + shell_print(sh, "\n Top-16 worst writes (us):"); + char line[128]; + int pos = 0; + for (int i = 0; i < 16 && top[i] > 0; i++) { + pos += snprintf(line + pos, sizeof(line) - pos, "%u ", top[i] / 1000); + } + shell_print(sh, " %s (ms)", line); + + return 0; +} + +SHELL_CMD_ARG_REGISTER(sd_bench, NULL, + "SD write benchmark: sd_bench [block_size] [interval_us]", + cmd_sd_bench, 2, 2); diff --git a/src/SD_Card/SD_Card_Manager/SD_Card_Manager.cpp b/src/SD_Card/SD_Card_Manager/SD_Card_Manager.cpp index 9620a2eb..bf027504 100644 --- a/src/SD_Card/SD_Card_Manager/SD_Card_Manager.cpp +++ b/src/SD_Card/SD_Card_Manager/SD_Card_Manager.cpp @@ -10,6 +10,11 @@ #include #include +#if defined(CONFIG_USB_DEVICE_STACK_NEXT) && defined(CONFIG_USBD_MSC_CLASS) +#include +extern struct usbd_context *g_usbd; +#endif + #include "openearable_common.h" #include "SDLogger.h" @@ -39,12 +44,26 @@ void SDCardManager::unmount_work_handler(struct k_work *work) { bool _inserted = sdcard_manager.sd_inserted(); - sd_msg msg = { .removed = true }; + if (_inserted && !sdcard_manager.ls_acquired) { + ret = sdcard_manager.acquire_ls(); + if (ret && ret != -EALREADY) { + LOG_ERR("Failed to acquire rails on SD insertion: %d", ret); + return; + } + LOG_INF("SD card inserted; rails up for USB MSC."); - if (!_inserted) { + sd_msg msg = { .removed = false }; + ret = zbus_chan_pub(&sd_card_chan, &msg, K_FOREVER); + if (ret != 0) { + LOG_ERR("Failed to publish sd_card_chan: %d", ret); + } + } else if (!_inserted && sdcard_manager.ls_acquired) { ret = sdcard_manager.unmount(); LOG_INF("SD card unmounted due to card removal."); - + + sdcard_manager.release_ls(); + + sd_msg msg = { .removed = true }; ret = zbus_chan_pub(&sd_card_chan, &msg, K_FOREVER); if (ret != 0) { LOG_ERR("Failed to publish sd_card_chan: %d", ret); @@ -76,57 +95,47 @@ SDCardManager::~SDCardManager() { } -int SDCardManager::aquire_ls() { +int SDCardManager::acquire_ls() { int ret; - if (ls_aquired) return -EALREADY; + if (ls_acquired) return -EALREADY; + /* SD path: ls_1_8 (level-shifter low side) + ls_sd (card VDD). + * ls_3_3 is not required by the SD card per schematic. */ ret = pm_device_runtime_get(ls_1_8); if (ret) { LOG_ERR("Failed to get ls_1_8"); return ret; } - ret = pm_device_runtime_get(ls_3_3); - if (ret) { - pm_device_runtime_put(ls_1_8); - LOG_ERR("Failed to get ls_3_3"); - return ret; - } - ret = pm_device_runtime_get(ls_sd); if (ret) { pm_device_runtime_put(ls_1_8); - pm_device_runtime_put(ls_3_3); LOG_ERR("Failed to get ls_sd"); return ret; } - ls_aquired = true; + ls_acquired = true; return 0; } int SDCardManager::release_ls() { - int ret; - - if (!ls_aquired) return -EALREADY; + if (!ls_acquired) return -EALREADY; - ret = pm_device_runtime_put(ls_1_8); - ret = pm_device_runtime_put(ls_3_3); - ret = pm_device_runtime_put(ls_sd); + pm_device_runtime_put(ls_1_8); + pm_device_runtime_put(ls_sd); - ls_aquired = false; + ls_acquired = false; return 0; } void SDCardManager::init() { - int ret; + int ret; if (!device_is_ready(sd_state_pin.port)) { - ret = aquire_ls(); - LOG_ERR("SD state GPIO device not ready\n"); + LOG_ERR("SD state GPIO device not ready"); return; } @@ -136,7 +145,25 @@ void SDCardManager::init() { gpio_init_callback(&sd_state_cb, sd_card_state_change_isr, sd_state_cb.pin_mask | BIT(sd_state_pin.pin)); ret = gpio_add_callback(sd_state_pin.port, &sd_state_cb); - if (ret) LOG_ERR("Failed to add callback"); + if (ret) LOG_ERR("Failed to add callback"); + + /* The card-detect GPIO on this board only reports presence once the SD + * rails are up (the detect switch references card VDD). Probe by briefly + * bringing rails up; keep them if a card is there, release otherwise. */ + ret = acquire_ls(); + if (ret && ret != -EALREADY) { + LOG_ERR("Failed to acquire SD rails for presence probe: %d", ret); + return; + } + + k_usleep(1000); + + if (sd_inserted()) { + LOG_INF("SD card present at boot; rails up."); + } else { + release_ls(); + LOG_INF("No SD card at boot; rails down."); + } } int SDCardManager::unmount() { @@ -161,7 +188,18 @@ int SDCardManager::unmount() { this->mounted = false; - release_ls(); +#if defined(CONFIG_USB_DEVICE_STACK_NEXT) && defined(CONFIG_USBD_MSC_CLASS) + /* Re-enable USB so the host can access the SD card via MSC. + * Rails are held by card-presence, independent of mount state. */ + if (g_usbd) { + ret = usbd_enable(g_usbd); + if (ret) { + LOG_ERR("Failed to re-enable USB: %d", ret); + } else { + LOG_INF("USB MSC re-enabled"); + } + } +#endif } return 0; @@ -175,26 +213,35 @@ int SDCardManager::mount() { uint32_t sector_count; size_t sector_size; - ret = aquire_ls(); - bool _sd_inserted = sd_inserted(); if (!_sd_inserted) { - release_ls(); LOG_ERR("No SD card inserted."); return -ENODEV; } +#if defined(CONFIG_USB_DEVICE_STACK_NEXT) && defined(CONFIG_USBD_MSC_CLASS) + /* Disable USB so the host releases the SD card, giving firmware + * exclusive access for filesystem operations. Rails are already held + * by card-presence. */ + if (g_usbd) { + ret = usbd_disable(g_usbd); + if (ret) { + LOG_WRN("Failed to disable USB: %d", ret); + } else { + LOG_INF("USB MSC disabled for SD card access"); + } + } +#endif + ret = disk_access_init(sd_dev); if (ret) { - release_ls(); LOG_DBG("SD card init failed, please check if SD card inserted"); return -ENODEV; } ret = disk_access_ioctl(sd_dev, DISK_IOCTL_GET_SECTOR_COUNT, §or_count); if (ret) { - release_ls(); LOG_ERR("Unable to get sector count"); return ret; } @@ -203,7 +250,6 @@ int SDCardManager::mount() { ret = disk_access_ioctl(sd_dev, DISK_IOCTL_GET_SECTOR_SIZE, §or_size); if (ret) { - release_ls(); LOG_ERR("Unable to get sector size"); return ret; } @@ -225,7 +271,6 @@ int SDCardManager::mount() { LOG_ERR("Mnt. disk failed, could be format issue. should be FAT/exFAT. Error: %d", ret); if (ret != -EBUSY) { - release_ls(); return ret; } } @@ -234,7 +279,6 @@ int SDCardManager::mount() { ret = k_mutex_lock(&m_sem_sd_mngr_oper_ongoing, K_FOREVER); if (ret) { k_mutex_unlock(&m_sem_sd_mngr_oper_ongoing); - release_ls(); LOG_ERR("Sem take failed. Ret: %d", ret); return ret; } @@ -243,7 +287,6 @@ int SDCardManager::mount() { ret = fs_opendir(&this->dirp, this->path.c_str()); k_mutex_unlock(&m_sem_sd_mngr_oper_ongoing); if (ret) { - release_ls(); LOG_ERR("Open root dir failed. Error: %d", ret); return ret; } diff --git a/src/SD_Card/SD_Card_Manager/SD_Card_Manager.h b/src/SD_Card/SD_Card_Manager/SD_Card_Manager.h index a83a8534..22d1fb6a 100644 --- a/src/SD_Card/SD_Card_Manager/SD_Card_Manager.h +++ b/src/SD_Card/SD_Card_Manager/SD_Card_Manager.h @@ -22,6 +22,9 @@ class SDCardManager { SDCardManager(); ~SDCardManager(); + int acquire_ls(); + int release_ls(); + void init(); /** @@ -188,10 +191,7 @@ class SDCardManager { static void unmount_work_handler(struct k_work *work); - int aquire_ls(); - int release_ls(); - - bool ls_aquired = false; + bool ls_acquired = false; bool sd_inserted(); diff --git a/src/audio/CMakeLists.txt b/src/audio/CMakeLists.txt index ab99ae2b..3c834b1a 100644 --- a/src/audio/CMakeLists.txt +++ b/src/audio/CMakeLists.txt @@ -10,9 +10,6 @@ target_sources(app PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/audio_datapath.c ${CMAKE_CURRENT_SOURCE_DIR}/sw_codec_select.c ${CMAKE_CURRENT_SOURCE_DIR}/le_audio_rx.c - ${CMAKE_CURRENT_SOURCE_DIR}/sdlogger_wrapper.cpp - # ${CMAKE_CURRENT_SOURCE_DIR}/rx_publish.c - # ${CMAKE_CURRENT_SOURCE_DIR}/pdm_mic.c # ${CMAKE_CURRENT_SOURCE_DIR}/Equalizer.cpp ) diff --git a/src/audio/audio_datapath.c b/src/audio/audio_datapath.c index 0b28d728..fcfbd179 100644 --- a/src/audio/audio_datapath.c +++ b/src/audio/audio_datapath.c @@ -27,7 +27,7 @@ #include "sd_card_playback.h" #include "Equalizer.h" -#include "sdlogger_wrapper.h" +#include "sensor_sink.h" #include LOG_MODULE_REGISTER(audio_datapath, CONFIG_AUDIO_DATAPATH_LOG_LEVEL); @@ -171,18 +171,12 @@ static struct { #define SENQUEUE_FRAME_SIZE 32 -static struct k_msgq * sensor_queue; - -//extern struct audio_data fifo_rx; - -//K_MSGQ_DEFINE(rx_queue, sizeof(struct audio_data), 16, 4); -extern struct k_msgq_t encoder_queue; +extern struct k_msgq encoder_queue; // Definition eines zbus-Kanals ZBUS_CHAN_DEFINE(audio_channel, struct audio_data, NULL, NULL, ZBUS_OBSERVERS_EMPTY, ZBUS_MSG_INIT(0)); -// Thread-Stack und Daten -K_THREAD_STACK_DEFINE(data_thread_stack, CONFIG_ENCODER_STACK_SIZE); //CONFIG_DATA_THREAD_STACK_SIZE +K_THREAD_STACK_DEFINE(data_thread_stack, CONFIG_ENCODER_STACK_SIZE); static struct k_thread data_thread_data; static k_tid_t data_thread_id; @@ -193,21 +187,16 @@ int _count = 0; extern struct k_poll_signal encoder_sig; extern struct k_poll_event logger_sig; -// Funktion für den neuen Thread static void data_thread(void *arg1, void *arg2, void *arg3) { - //struct audio_data audio_item; void *tmp_pcm_raw_data[CONFIG_FIFO_FRAME_SPLIT_NUM]; - //char pcm_raw_data[FRAME_SIZE_BYTES]; size_t pcm_block_size; int ret; struct audio_rx_data audio_item; - //memcpy(audio_item.data, pcm_raw_data, FRAME_SIZE_BYTES); audio_item.size = FRAME_SIZE_BYTES; while (1) { - // Daten aus der data_queue lesen for (int i = 0; i < CONFIG_FIFO_FRAME_SPLIT_NUM; i++) { // wait for next sample block to be available ret = data_fifo_pointer_last_filled_get(ctrl_blk.in.fifo, &tmp_pcm_raw_data[i], &pcm_block_size, K_FOREVER); @@ -245,10 +234,7 @@ static void data_thread(void *arg1, void *arg2, void *arg3) audio_item.data + (i * BLOCK_SIZE_BYTES) }; - sdlogger_write_data(&data_ptrs, data_size, 2); - - //sdlogger_write_data(&audio_msg.data, data_size); - //sdlogger_write_data(audio_item.data + (i * BLOCK_SIZE_BYTES), BLOCK_SIZE_BYTES); + sensor_sink_write_sd(data_ptrs, data_size, 2); } k_yield(); @@ -271,17 +257,10 @@ static void data_thread(void *arg1, void *arg2, void *arg3) ring_buffer = buf; }*/ -void set_sensor_queue(struct k_msgq *queue) -{ - sensor_queue = queue; -} - - void record_to_sd(bool active) { _record_to_sd = active; } -// Funktion, um den neuen Thread zu starten void start_data_thread(void) { if (data_thread_id == NULL) { @@ -1209,7 +1188,7 @@ int audio_datapath_stop(void) } // TODO: not clean with the argument --> move to init? -int audio_datapath_aquire(struct data_fifo *fifo_rx) { +int audio_datapath_acquire(struct data_fifo *fifo_rx) { int ret = 0; if (_count == 0) { uint32_t alloced_cnt; diff --git a/src/audio/audio_datapath.h b/src/audio/audio_datapath.h index 5e463367..76a73e89 100644 --- a/src/audio/audio_datapath.h +++ b/src/audio/audio_datapath.h @@ -96,13 +96,9 @@ void start_data_thread(void); void record_to_sd(bool active); -void set_sensor_queue(struct k_msgq *queue); - -int audio_datapath_aquire(struct data_fifo *fifo_rx); +int audio_datapath_acquire(struct data_fifo *fifo_rx); int audio_datapath_release(); -//void set_ring_buffer(struct ring_buf *ring_buf); - #ifdef __cplusplus } #endif diff --git a/src/audio/audio_system.c b/src/audio/audio_system.c index e71b7a75..7bf61c01 100644 --- a/src/audio/audio_system.c +++ b/src/audio/audio_system.c @@ -428,7 +428,7 @@ void audio_system_start(void) ERR_CHK(ret); #else - ret = audio_datapath_aquire(&fifo_rx); + ret = audio_datapath_acquire(&fifo_rx); ERR_CHK(ret); ret = hw_codec_default_conf_enable(); diff --git a/src/audio/sdlogger_wrapper.cpp b/src/audio/sdlogger_wrapper.cpp deleted file mode 100644 index c9d19238..00000000 --- a/src/audio/sdlogger_wrapper.cpp +++ /dev/null @@ -1,12 +0,0 @@ -#include "sdlogger_wrapper.h" -#include "openearable_common.h" -#include "SDLogger.h" - -// Assuming sdlogger is an accessible global object -extern SDLogger sdlogger; - -extern "C" { - int sdlogger_write_data(const void* const* data_blocks, const size_t* lengths, size_t block_count) { - return sdlogger.write_sensor_data(data_blocks, lengths, block_count); - } -} diff --git a/src/audio/sdlogger_wrapper.h b/src/audio/sdlogger_wrapper.h deleted file mode 100644 index 06485b62..00000000 --- a/src/audio/sdlogger_wrapper.h +++ /dev/null @@ -1,23 +0,0 @@ -#ifndef SDLOGGER_WRAPPER_H -#define SDLOGGER_WRAPPER_H - -#include "openearable_common.h" - -#ifdef __cplusplus -extern "C" { -#endif - -/** - * @brief Wrapper function to call sdlogger.write_data from C code - * - * @param data Pointer to sensor data header - * @param size Size of the audio data - * @return int Return code (0 for success) - */ -int sdlogger_write_data(const void* const* data_blocks, const size_t* lengths, size_t block_count); - -#ifdef __cplusplus -} -#endif - -#endif /* SDLOGGER_WRAPPER_H */ diff --git a/src/audio/streamctrl.c b/src/audio/streamctrl.c index 5a1e7edb..b416445a 100644 --- a/src/audio/streamctrl.c +++ b/src/audio/streamctrl.c @@ -27,6 +27,10 @@ #include "fw_info_app.h" #include "BootState.h" +#include "uicr.h" +#include "channel_assignment.h" + +#include #include LOG_MODULE_REGISTER(streamctrl, CONFIG_MAIN_LOG_LEVEL); @@ -539,21 +543,16 @@ static void write_sirk(uint32_t sirk) { else LOG_ERR("UICR writing error: %i", ret); } -// Callback-Funktion für gefundene Geräte static void device_found(const bt_addr_le_t *addr, int8_t rssi, uint8_t type, struct net_buf_simple *ad) { - char addr_str[BT_ADDR_LE_STR_LEN]; - bt_addr_le_to_str(addr, addr_str, sizeof(addr_str)); - - int ret; - bool is_le_audio_device = false; + bool is_le_audio_device = false; uint8_t csis_rsi[6]; uint8_t chip_id[8]; while (ad->len > 0) { uint8_t len = net_buf_simple_pull_u8(ad); if (len == 0 || len > ad->len) { - break; // Ungültige Länge + break; } uint8_t type = net_buf_simple_pull_u8(ad); @@ -600,9 +599,9 @@ static void device_found(const bt_addr_le_t *addr, int8_t rssi, uint8_t type, st channel_assignment_get(&channel); if (channel == AUDIO_CH_L) { - LOG_INF("Device ID 1: %016X", oe_boot_state.device_id); - LOG_INF("Device ID 2: %016X", *((uint32_t *) chip_id)); - LOG_INF("New Sirk: %016X", new_sirk); + LOG_INF("Device ID 1: %08X", (uint32_t)oe_boot_state.device_id); + LOG_INF("Device ID 2: %08X", *((uint32_t *) chip_id)); + LOG_INF("New Sirk: %08X", new_sirk); //TODO: check if the device wants to pair (sirk == device_id) //TODO: check channel @@ -617,7 +616,7 @@ static void device_found(const bt_addr_le_t *addr, int8_t rssi, uint8_t type, st if ((r[BT_CSIP_CRYPTO_PRAND_SIZE - 1] & BIT(7)) || ((r[BT_CSIP_CRYPTO_PRAND_SIZE - 1] & BIT(6)) == 0)) { - LOG_WRN("Invalid r %s", bt_hex(r, BT_CSIP_CRYPTO_PRAND_SIZE)); + LOG_WRN("Invalid r"); } // r' = padding || r @@ -625,9 +624,9 @@ static void device_found(const bt_addr_le_t *addr, int8_t rssi, uint8_t type, st memcpy(res, r, BT_CSIP_CRYPTO_PRAND_SIZE); memset(sirk, 0, BT_CSIP_CRYPTO_KEY_SIZE + 1); - snprintf(sirk, BT_CSIP_SIRK_SIZE, "%08X", new_sirk); + snprintf((char *)sirk, BT_CSIP_SIRK_SIZE, "%08X", new_sirk); - int err = bt_encrypt_le(sirk, res, res); + (void)bt_encrypt_le(sirk, res, res); memcpy(out, res, BT_CSIP_CRYPTO_HASH_SIZE); diff --git a/src/bluetooth/bt_management/bt_mgmt.c b/src/bluetooth/bt_management/bt_mgmt.c index da38a9cb..9775e7f5 100644 --- a/src/bluetooth/bt_management/bt_mgmt.c +++ b/src/bluetooth/bt_management/bt_mgmt.c @@ -18,7 +18,7 @@ #include "macros_common.h" #include "zbus_common.h" -#include "button_assignments.h" +#include "button_manager.h" #include "bt_mgmt_ctlr_cfg_internal.h" #include "bt_mgmt_adv_internal.h" @@ -26,6 +26,9 @@ #include "bt_mgmt_conn_interval.h" #include "BootState.h" +#include "uicr.h" + +#include #include LOG_MODULE_REGISTER(bt_mgmt, CONFIG_BT_MGMT_LOG_LEVEL); @@ -83,18 +86,9 @@ static struct bt_le_conn_param *conn_param = BT_LE_CONN_PARAM(CONFIG_BLE_ACL_CON //callback static void conn_params_updated(struct bt_conn *conn, uint16_t interval, uint16_t latency, uint16_t timeout) { - struct bt_mgmt_msg msg; - int ret; - LOG_INF("Conn params updated: interval %d unit, latency %d, timeout: %d0 ms",interval, latency, timeout); bt_mgmt_ci_on_conn_param_updated(conn, interval, latency, timeout); - - /*msg.event = BT_MGMT_CONNECTED; - msg.conn = conn; - - ret = zbus_chan_pub(&bt_mgmt_chan, &msg, K_NO_WAIT); - ERR_CHK(ret);*/ } static void connected_cb(struct bt_conn *conn, uint8_t err) @@ -163,7 +157,7 @@ static void connected_cb(struct bt_conn *conn, uint8_t err) err = bt_conn_le_param_update(conn, conn_param); if (err) { LOG_ERR("Cannot update conneciton parameter (err: %d)", err); - return err; + return; } LOG_INF("Connection parameters update requested: interval_min %d interval_max %d latency %d timeout %d", conn_param->interval_min, conn_param->interval_max, @@ -408,7 +402,7 @@ int bt_mgmt_init(void) bt_gatt_cb_register(&gatt_callbacks); uint32_t sirk = uicr_sirk_get(); - snprintf(name, CONFIG_BT_DEVICE_NAME_MAX, "%s-%04X", CONFIG_BT_DEVICE_NAME, (sirk != 0xFFFFFFFFU ? sirk : oe_boot_state.device_id) & 0xFFFF); + snprintf(name, CONFIG_BT_DEVICE_NAME_MAX, "%s-%04X", CONFIG_BT_DEVICE_NAME, (unsigned int)((sirk != 0xFFFFFFFFU ? sirk : (uint32_t)oe_boot_state.device_id) & 0xFFFF)); ret = bt_set_name(name); if (ret) { diff --git a/src/bluetooth/bt_stream/unicast/unicast_server.c b/src/bluetooth/bt_stream/unicast/unicast_server.c index c1aa6384..e1cde28c 100644 --- a/src/bluetooth/bt_stream/unicast/unicast_server.c +++ b/src/bluetooth/bt_stream/unicast/unicast_server.c @@ -23,7 +23,11 @@ #include "le_audio.h" #include "BootState.h" - +#include "uicr.h" +#include "channel_assignment.h" + +#include + #include LOG_MODULE_REGISTER(unicast_server, CONFIG_UNICAST_SERVER_LOG_LEVEL); @@ -706,9 +710,9 @@ static uint8_t device_identifier[] = { memset(sirk_string, 0, sizeof(sirk_string)); if (sirk != 0xFFFFFFFF) { - snprintf(sirk_string, 16, "%08X", sirk); //"%016llX" + snprintf((char *)sirk_string, 16, "%08X", sirk); } else { - snprintf(sirk_string, 16, "%08X", oe_boot_state.device_id); //"%016llX" + snprintf((char *)sirk_string, 16, "%08X", (uint32_t)oe_boot_state.device_id); } // LOG_INF("SIRK as String: %s", sirk_string); diff --git a/src/openearable_common.c b/src/openearable_common.c deleted file mode 100644 index 2327c376..00000000 --- a/src/openearable_common.c +++ /dev/null @@ -1,12 +0,0 @@ -/* - * Copyright (c) 2023 Nordic Semiconductor ASA - * - * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause - */ - -#include "openearable_common.h" - -const struct device *const cons = DEVICE_DT_GET(DT_CHOSEN(zephyr_console)); -const struct device *const ls_1_8 = DEVICE_DT_GET(load_switch_1_8_id); -const struct device *const ls_3_3 = DEVICE_DT_GET(load_switch_3_3_id); -const struct device *const ls_sd = DEVICE_DT_GET(load_switch_sd_id); diff --git a/src/time_sync/time_sync.c b/src/time_sync/time_sync.c index f1841e4e..26c7cb03 100644 --- a/src/time_sync/time_sync.c +++ b/src/time_sync/time_sync.c @@ -8,9 +8,14 @@ #include #include #include +#include +#include #include "openearable_common.h" +#include +#include + #include LOG_MODULE_REGISTER(time_sync, LOG_LEVEL_DBG); @@ -178,6 +183,78 @@ void rtt_cfg_changed(const struct bt_gatt_attr *attr, } +DWORD get_fattime(void) +{ + uint64_t now_us = get_current_time_us(); + time_t secs = (time_t)(now_us / 1000000ULL); + struct tm tm; + gmtime_r(&secs, &tm); + + return ((DWORD)(tm.tm_year - 80) << 25) | + ((DWORD)(tm.tm_mon + 1) << 21) | + ((DWORD)tm.tm_mday << 16) | + ((DWORD)tm.tm_hour << 11) | + ((DWORD)tm.tm_min << 5) | + ((DWORD)(tm.tm_sec / 2)); +} + +/* --- Shell commands ---------------------------------------------------- */ + +/* time set – set wall-clock from host */ +static int cmd_time_set(const struct shell *sh, size_t argc, char **argv) +{ + if (argc < 2) { + shell_error(sh, "Usage: time set "); + return -EINVAL; + } + + uint64_t epoch_us = strtoull(argv[1], NULL, 10); + if (epoch_us == 0) { + shell_error(sh, "Invalid timestamp"); + return -EINVAL; + } + + uint64_t uptime = get_time_since_boot_us(); + time_offset_us = (int64_t)(epoch_us - uptime); + + time_t secs = (time_t)(epoch_us / 1000000ULL); + struct tm tm; + gmtime_r(&secs, &tm); + shell_print(sh, "Time set: %04d-%02d-%02d %02d:%02d:%02d UTC (offset=%lld us)", + tm.tm_year + 1900, tm.tm_mon + 1, tm.tm_mday, + tm.tm_hour, tm.tm_min, tm.tm_sec, + (long long)time_offset_us); + return 0; +} + +/* time get – show current wall-clock */ +static int cmd_time_get(const struct shell *sh, size_t argc, char **argv) +{ + ARG_UNUSED(argc); + ARG_UNUSED(argv); + + uint64_t now_us = get_current_time_us(); + time_t secs = (time_t)(now_us / 1000000ULL); + struct tm tm; + gmtime_r(&secs, &tm); + shell_print(sh, "%04d-%02d-%02d %02d:%02d:%02d.%06llu UTC (%llu us)", + tm.tm_year + 1900, tm.tm_mon + 1, tm.tm_mday, + tm.tm_hour, tm.tm_min, tm.tm_sec, + (unsigned long long)(now_us % 1000000ULL), + (unsigned long long)now_us); + return 0; +} + +SHELL_STATIC_SUBCMD_SET_CREATE(time_cmds, + SHELL_CMD_ARG(set, NULL, + "Set wall-clock: time set ", + cmd_time_set, 2, 0), + SHELL_CMD(get, NULL, "Show current wall-clock time", cmd_time_get), + SHELL_SUBCMD_SET_END +); + +SHELL_CMD_REGISTER(time, &time_cmds, "Time sync commands", NULL); + BT_GATT_SERVICE_DEFINE(time_sync_service, BT_GATT_PRIMARY_SERVICE(BT_UUID_TIME_SYNC_SERVICE), BT_GATT_CHARACTERISTIC(BT_UUID_TIME_SYNC_OFFSET_CHARAC, diff --git a/tools/analyze_packet_loss.py b/tools/analyze_packet_loss.py new file mode 100755 index 00000000..1a15463b --- /dev/null +++ b/tools/analyze_packet_loss.py @@ -0,0 +1,308 @@ +#!/usr/bin/env python3 +"""Analyze packet loss in OpenEarable .oe sensor log files. + +Supports file format versions 1-3. Reports per-sensor packet counts, +timing statistics, gap analysis, and comparison with known baselines. + +Usage: + python tools/analyze_packet_loss.py data/sensor_log_*.oe +""" + +import argparse +import struct +import sys +from collections import defaultdict +from pathlib import Path + +import numpy as np + + +SENSOR_SID = { + "imu": 0, + "barometer": 1, + "microphone": 2, + "ppg": 4, + "optical_temp": 6, + "bone_acc": 7, +} +SID_NAME = {v: k for k, v in SENSOR_SID.items()} + +# Per-sample sizes for multi-sample sensors (used to compute samples per packet) +SAMPLE_SIZES = { + "bone_acc": 6, # 3 × int16 + "ppg": 16, # 4 × uint32 +} + +# Microphone-specific config +MIC_CONFIG = { + "frames_per_packet": 48, + "sample_rate": 48000, + "nominal_interval_ms": 1.0, +} + + +def parse_file_header(f): + """Parse file header, returns (version, timestamp_us, device_id, channel). + device_id and channel are None for versions < 3.""" + data = f.read(10) + version, timestamp = struct.unpack("= 3: + extra = f.read(9) # uint64 device_id + uint8 channel + device_id, channel = struct.unpack(" list of (timestamp_s, n_samples).""" + packets = defaultdict(list) + + while True: + header = f.read(10) + if len(header) < 10: + break + sid, size, time = struct.unpack(" 192 or sid > 7: + break + + data = f.read(size) + if len(data) < size: + break + + ts = time / 1e6 + name = SID_NAME.get(sid, f"sid{sid}") + packets[name].append((ts, count_samples(name, size))) + + parsed_to = f.tell() + return packets, parsed_to + + +def analyze_sensor(name, entries): + """Analyze packet timing for a single sensor. Returns a stats dict.""" + timestamps = np.array([e[0] for e in entries]) + sample_counts = np.array([e[1] for e in entries]) + total_samples = int(sample_counts.sum()) + + stats = { + "packets": len(timestamps), + "total_samples": total_samples, + "time_span_s": 0.0, + } + + if len(timestamps) < 2: + return stats + + diffs = np.diff(timestamps) + span = timestamps[-1] - timestamps[0] + stats["time_span_s"] = span + + stats["interval_mean_ms"] = diffs.mean() * 1000 + stats["interval_median_ms"] = float(np.median(diffs) * 1000) + stats["interval_std_ms"] = diffs.std() * 1000 + stats["interval_min_ms"] = diffs.min() * 1000 + stats["interval_max_ms"] = diffs.max() * 1000 + + # Gap counts at various thresholds + for thresh_ms in [5, 10, 50, 100, 500]: + stats[f"gaps_gt_{thresh_ms}ms"] = int((diffs > thresh_ms / 1000).sum()) + + # Largest gaps + big_mask = diffs > 0.005 + if big_mask.any(): + big_gaps = sorted(diffs[big_mask] * 1000, reverse=True) + stats["largest_gaps_ms"] = big_gaps[:15] + + # Microphone normal-interval check + if name == "microphone": + normal = (diffs >= 0.0008) & (diffs <= 0.0012) + stats["normal_interval_pct"] = normal.sum() / len(diffs) * 100 + + # Multi-sample packet distribution + if name in SAMPLE_SIZES: + unique, counts = np.unique(sample_counts, return_counts=True) + stats["samples_per_packet_dist"] = dict(zip(unique.astype(int).tolist(), counts.tolist())) + + # Estimate sample rate and compute dropout for all sensors. + if name == "microphone": + # Known fixed rate; frame counts don't reflect the true sample rate + est_rate = MIC_CONFIG["sample_rate"] + else: + # Use the median of per-interval sample rates from "normal" intervals + # (below 2× the median interval) to reject gaps. + median_diff = float(np.median(diffs)) + normal_mask = diffs < (2 * median_diff) + if normal_mask.any(): + normal_rates = sample_counts[:-1][normal_mask] / diffs[normal_mask] + est_rate = float(np.median(normal_rates)) + else: + est_rate = total_samples / span if span > 0 else 0 + + if est_rate > 0: + stats["estimated_rate_hz"] = est_rate + expected_samples = span * est_rate + stats["expected_samples"] = int(expected_samples) + stats["sample_loss_pct"] = (1 - total_samples / expected_samples) * 100 + stats["dropped_time_s"] = (expected_samples - total_samples) / est_rate + + return stats + + +def print_report(filename, version, timestamp_us, device_id, channel, + packets, parsed_to, file_size): + """Print a formatted analysis report.""" + print(f"\n{'='*60}") + print(f"File: {filename}") + print(f" Version: {version}, Size: {file_size:,} bytes, Parsed: {parsed_to/file_size*100:.1f}%") + print(f" Start timestamp: {timestamp_us} us ({timestamp_us/1e6:.2f}s)") + if device_id is not None: + print(f" Device ID: {device_id:#x}, Channel: {channel}") + + # List all sensors present and missing + present = sorted(packets.keys()) + all_sensors = ["imu", "barometer", "microphone", "ppg", "optical_temp", "bone_acc"] + missing = [s for s in all_sensors if s not in present] + print(f"\n Sensors present: {', '.join(present) if present else 'none'}") + if missing: + print(f" Sensors MISSING: {', '.join(missing)}") + + for name in present: + entries = packets[name] + stats = analyze_sensor(name, entries) + print(f"\n--- {name.upper()} ---") + print(f" Packets: {stats['packets']:,}") + print(f" Time span: {stats['time_span_s']:.3f}s") + + if stats["time_span_s"] == 0: + continue + + if name in SAMPLE_SIZES: + print(f" Total samples: {stats['total_samples']:,}") + + print(f" Interval (ms): median={stats['interval_median_ms']:.3f}, " + f"mean={stats['interval_mean_ms']:.3f}, " + f"std={stats['interval_std_ms']:.3f}") + print(f" Interval range (ms): [{stats['interval_min_ms']:.3f}, {stats['interval_max_ms']:.1f}]") + + if "normal_interval_pct" in stats: + print(f" Normal interval (0.8-1.2ms): {stats['normal_interval_pct']:.1f}%") + + # Gap summary + gap_lines = [] + for thresh_ms in [5, 10, 50, 100, 500]: + count = stats.get(f"gaps_gt_{thresh_ms}ms", 0) + if count > 0: + gap_lines.append(f">{thresh_ms}ms: {count}") + if gap_lines: + print(f" Gaps: {', '.join(gap_lines)}") + + if "largest_gaps_ms" in stats: + gaps_str = ", ".join(f"{g:.1f}" for g in stats["largest_gaps_ms"][:10]) + print(f" Largest gaps (ms): [{gaps_str}]") + + # Loss estimates (for all sensors) + if "estimated_rate_hz" in stats: + print(f" Estimated rate: {stats['estimated_rate_hz']:.1f} Hz") + print(f" Expected samples: {stats.get('expected_samples', '?'):,}") + loss = stats.get('sample_loss_pct', 0) + print(f" Sample loss: {loss:.2f}%") + print(f" Dropped time: {stats.get('dropped_time_s', 0):.2f}s") + if "samples_per_packet_dist" in stats: + print(f" Samples/packet: {stats['samples_per_packet_dist']}") + + # Cross-sensor time alignment (microphone as reference, or first sensor) + if len(present) >= 2: + print(f"\n--- SENSOR TIME ALIGNMENT ---") + + # Gather start/end/span for each sensor + sensor_times = {} + for name in present: + ts = np.array([e[0] for e in packets[name]]) + sensor_times[name] = (ts[0], ts[-1], ts[-1] - ts[0]) + + # Use microphone as reference if present, otherwise first sensor + ref = "microphone" if "microphone" in sensor_times else present[0] + ref_start, ref_end, ref_span = sensor_times[ref] + others = [n for n in present if n != ref] + + print(f" Reference: {ref} ({ref_span:.3f}s)") + print(f" {'Sensor':<15} {'Start vs ref':>14} {'End vs ref':>14} {'Span diff':>12}") + for name in others: + start, end, span = sensor_times[name] + start_off = (start - ref_start) * 1000 + end_off = (end - ref_end) * 1000 + span_diff = (span - ref_span) * 1000 + print(f" {name:<15} {start_off:+13.1f}ms {end_off:+13.1f}ms {span_diff:+11.1f}ms") + + # Overlap analysis + overlap_start = max(t[0] for t in sensor_times.values()) + overlap_end = min(t[1] for t in sensor_times.values()) + global_span = max(t[1] for t in sensor_times.values()) - min(t[0] for t in sensor_times.values()) + if overlap_end > overlap_start: + overlap = overlap_end - overlap_start + print(f"\n Common overlap: {overlap:.3f}s " + f"({overlap/global_span*100:.1f}% of {global_span:.3f}s total span)") + else: + print(f"\n WARNING: No time overlap between all sensors!") + + # Dropout summary across all sensors + print(f"\n--- DROPOUT SUMMARY ---") + for name in present: + entries = packets[name] + s = analyze_sensor(name, entries) + loss = s.get("sample_loss_pct") + if loss is not None: + rate = s.get("estimated_rate_hz", 0) + total = s.get("total_samples", len(entries)) + expected = s.get("expected_samples", total) + print(f" {name:<15} {loss:6.2f}% ({total:,} / {expected:,} samples, {rate:.1f} Hz)") + + print() + + +def analyze_file(filepath): + """Analyze a single .oe file.""" + filepath = Path(filepath) + file_size = filepath.stat().st_size + + with open(filepath, "rb") as f: + version, timestamp_us, device_id, channel = parse_file_header(f) + packets, parsed_to = parse_packets(f, version) + + print_report( + filepath.name, version, timestamp_us, device_id, channel, + packets, parsed_to, file_size, + ) + + +def main(): + parser = argparse.ArgumentParser(description="Analyze packet loss in .oe sensor log files") + parser.add_argument("files", nargs="+", help="One or more .oe files to analyze") + args = parser.parse_args() + + for filepath in args.files: + try: + analyze_file(filepath) + except Exception as e: + print(f"\nERROR processing {filepath}: {e}", file=sys.stderr) + + +if __name__ == "__main__": + main()