The ESP-IDF project, docs, tools, tests and web UI move from v2/ to the root, and the Makefile, README, build scripts and test runner drop the v2/ prefix. Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
242 lines
7.0 KiB
C
242 lines
7.0 KiB
C
#include "bench.h"
|
|
|
|
#include <math.h>
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
#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 <idx> <esp_timer us>" 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;
|
|
}
|