#include "bench.h" #include #include #include #include "driver/gpio.h" #include "esp_timer.h" #include "espnow_tx.h" #include "freertos/FreeRTOS.h" #include "freertos/portmacro.h" #include "freertos/queue.h" #include "freertos/task.h" #define RING 64 #define MAX_DRIVERS 8 typedef struct { uint16_t idx; int64_t tx_us; int64_t first_us, last_us; uint8_t echoes; int fire_in_ms; } beat_t; typedef struct { uint8_t mac[6]; uint32_t echoes; } drv_t; static portMUX_TYPE s_mux = portMUX_INITIALIZER_UNLOCKED; static beat_t s_ring[RING]; static drv_t s_drv[MAX_DRIVERS]; static uint32_t s_sent, s_echoes, s_late_echoes; static int64_t s_min_us, s_max_us, s_sum_us; static int64_t s_spread_sum, s_spread_max; static uint32_t s_spread_n; static int s_remaining; static bool s_running; static float s_bpm; #if CONFIG_BEAT_TEST_GPIO >= 0 static bool s_gpio_ready; static int s_level; #endif static esp_timer_handle_t s_timer; static QueueHandle_t s_logq; // USB latency test: one "BT " line per synthetic beat, printed from a low-priority // task so the beat path itself never waits on the console. tools/usb_latency.py reads these. typedef struct { uint16_t idx; int64_t tx_us; } beat_log_t; static void log_task(void *arg) { beat_log_t l; for (;;) { if (xQueueReceive(s_logq, &l, portMAX_DELAY) == pdTRUE) { printf("BT %u %lld\n", l.idx, (long long)l.tx_us); } } } static uint32_t s_gen_count; static void gpio_mark(void) { #if CONFIG_BEAT_TEST_GPIO >= 0 if (!s_gpio_ready) { gpio_set_direction(CONFIG_BEAT_TEST_GPIO, GPIO_MODE_OUTPUT); s_gpio_ready = true; } s_level = !s_level; gpio_set_level(CONFIG_BEAT_TEST_GPIO, s_level); #endif } void bench_on_beat_tx(uint16_t idx, int64_t tx_us, int fire_in_ms) { gpio_mark(); // edge as the first copy goes out if (s_running && s_logq) { beat_log_t l = { .idx = idx, .tx_us = tx_us }; xQueueSend(s_logq, &l, 0); } portENTER_CRITICAL(&s_mux); beat_t *b = &s_ring[idx % RING]; memset(b, 0, sizeof(*b)); b->idx = idx; b->tx_us = tx_us; b->fire_in_ms = fire_in_ms; s_sent++; portEXIT_CRITICAL(&s_mux); } void bench_on_echo(const uint8_t mac[6], uint16_t idx, int64_t rx_us) { portENTER_CRITICAL(&s_mux); beat_t *b = &s_ring[idx % RING]; if (b->idx != idx || b->tx_us == 0) { s_late_echoes++; // beat already overwritten in the ring portEXIT_CRITICAL(&s_mux); return; } // Round trip minus the deliberate fire_in wait: airtime out + driver work + airtime back int64_t dt = rx_us - b->tx_us - (int64_t)b->fire_in_ms * 1000; if (s_echoes == 0 || dt < s_min_us) { s_min_us = dt; } if (s_echoes == 0 || dt > s_max_us) { s_max_us = dt; } s_sum_us += dt; s_echoes++; if (b->echoes++ == 0) { b->first_us = rx_us; } else { int64_t spread = rx_us - b->first_us; if (spread > s_spread_max) { s_spread_max = spread; } s_spread_n++; // one sample per extra driver that heard this beat s_spread_sum += spread; } b->last_us = rx_us; drv_t *d = NULL; for (int i = 0; i < MAX_DRIVERS && !d; i++) { if (memcmp(s_drv[i].mac, mac, 6) == 0 && s_drv[i].echoes) { d = &s_drv[i]; } } for (int i = 0; i < MAX_DRIVERS && !d; i++) { if (!s_drv[i].echoes) { d = &s_drv[i]; memcpy(d->mac, mac, 6); } } if (d) { d->echoes++; } portEXIT_CRITICAL(&s_mux); } static void gen_cb(void *arg) { if (!s_running) { return; } s_gen_count++; // Downbeat on every fourth beat, like a 4/4 bar espnow_tx_beat(s_bpm, (s_gen_count % 4) == 1, ((s_gen_count - 1) % 4) + 1); if (s_remaining > 0 && --s_remaining == 0) { bench_stop(); } } esp_err_t bench_start(float bpm, int count) { if (bpm < 30 || bpm > 300) { return ESP_ERR_INVALID_ARG; } bench_stop(); if (!s_logq) { s_logq = xQueueCreate(32, sizeof(beat_log_t)); if (s_logq) { xTaskCreate(log_task, "bench_log", 3072, NULL, 2, NULL); } } portENTER_CRITICAL(&s_mux); memset(s_ring, 0, sizeof(s_ring)); memset(s_drv, 0, sizeof(s_drv)); s_sent = s_echoes = s_late_echoes = 0; s_min_us = s_max_us = s_sum_us = 0; s_spread_sum = s_spread_max = 0; s_spread_n = 0; portEXIT_CRITICAL(&s_mux); if (!s_timer) { const esp_timer_create_args_t ta = { .callback = gen_cb, .name = "bench_beat", .dispatch_method = ESP_TIMER_TASK }; esp_err_t err = esp_timer_create(&ta, &s_timer); if (err != ESP_OK) { return err; } } s_bpm = bpm; s_remaining = count; s_gen_count = 0; s_running = true; return esp_timer_start_periodic(s_timer, (uint64_t)(60e6 / bpm)); } bool bench_running(void) { return s_running; } void bench_stop(void) { s_running = false; if (s_timer) { esp_timer_stop(s_timer); } } cJSON *bench_results(void) { cJSON *o = cJSON_CreateObject(); portENTER_CRITICAL(&s_mux); uint32_t sent = s_sent, echoes = s_echoes, late = s_late_echoes, spread_n = s_spread_n; int64_t mn = s_min_us, mx = s_max_us, sum = s_sum_us, ssum = s_spread_sum, smax = s_spread_max; drv_t drv[MAX_DRIVERS]; memcpy(drv, s_drv, sizeof(drv)); portEXIT_CRITICAL(&s_mux); int ndrv = 0; cJSON *drivers = cJSON_AddObjectToObject(o, "drivers"); for (int i = 0; i < MAX_DRIVERS; i++) { if (drv[i].echoes) { char key[13]; snprintf(key, sizeof(key), "%02x%02x%02x%02x%02x%02x", drv[i].mac[0], drv[i].mac[1], drv[i].mac[2], drv[i].mac[3], drv[i].mac[4], drv[i].mac[5]); cJSON *d = cJSON_AddObjectToObject(drivers, key); cJSON_AddNumberToObject(d, "echoes", drv[i].echoes); cJSON_AddNumberToObject(d, "hit_pct", sent ? 100.0 * drv[i].echoes / sent : 0); ndrv++; } } cJSON_AddBoolToObject(o, "running", s_running); cJSON_AddNumberToObject(o, "beats_sent", sent); cJSON_AddNumberToObject(o, "echoes", echoes); cJSON_AddNumberToObject(o, "late_echoes", late); // Fraction of beats each driver acted on; ~100 with the double send even at moderate loss cJSON_AddNumberToObject(o, "hit_pct_total", sent && ndrv ? 100.0 * echoes / ((double)sent * ndrv) : 0); cJSON_AddNumberToObject(o, "rtt_minus_lead_min_us", mn); cJSON_AddNumberToObject(o, "rtt_minus_lead_avg_us", echoes ? (double)sum / echoes : 0); cJSON_AddNumberToObject(o, "rtt_minus_lead_max_us", mx); // Arrival spread between drivers for the same beat (needs two or more drivers echoing) cJSON_AddNumberToObject(o, "spread_avg_us", spread_n ? (double)ssum / spread_n : 0); cJSON_AddNumberToObject(o, "spread_max_us", smax); bool beats_lossy; cJSON_AddNumberToObject(o, "loss_pct", espnow_tx_loss(&beats_lossy)); cJSON_AddBoolToObject(o, "loss_includes_beats", beats_lossy); return o; }