florian.berthold
64f9e34fc3
Complete working firmware including: - CMakeLists.txt for Pico SDK build - SSD1306 OLED driver (128x32, I2C) - High-resolution latency measurement using hardware timer - Debounced button with short/long press detection - Three modes: Single, Continuous, Stats - USB serial debugging output Includes 8x8 font with numbers and letters for display.
119 lines
2.8 KiB
C
119 lines
2.8 KiB
C
/**
|
|
* SN-L00 Latency Measurement
|
|
*
|
|
* High-resolution latency measurement using RP2040 hardware timer
|
|
*/
|
|
|
|
#include "pico/stdlib.h"
|
|
#include "hardware/gpio.h"
|
|
#include "hardware/timer.h"
|
|
|
|
#include "config.h"
|
|
#include "latency.h"
|
|
|
|
// Statistics storage
|
|
static float samples[STATS_SAMPLES];
|
|
static uint32_t sample_index = 0;
|
|
static uint32_t sample_count = 0;
|
|
static float min_latency = 0;
|
|
static float max_latency = 0;
|
|
|
|
void latency_init(void) {
|
|
// Configure trigger output pin
|
|
gpio_init(PIN_TRIG_OUT);
|
|
gpio_set_dir(PIN_TRIG_OUT, GPIO_OUT);
|
|
gpio_put(PIN_TRIG_OUT, 0);
|
|
|
|
// Configure return input pin
|
|
gpio_init(PIN_RETURN_IN);
|
|
gpio_set_dir(PIN_RETURN_IN, GPIO_IN);
|
|
gpio_pull_down(PIN_RETURN_IN); // Default low
|
|
|
|
latency_reset_stats();
|
|
}
|
|
|
|
measurement_t latency_measure(void) {
|
|
measurement_t result = {
|
|
.valid = false,
|
|
.latency_ms = 0,
|
|
.latency_us = 0
|
|
};
|
|
|
|
// Ensure output is low before starting
|
|
gpio_put(PIN_TRIG_OUT, 0);
|
|
sleep_us(100);
|
|
|
|
// Record start time and send trigger
|
|
uint64_t start_us = time_us_64();
|
|
gpio_put(PIN_TRIG_OUT, 1);
|
|
|
|
// Wait for trigger pulse duration
|
|
sleep_ms(TRIGGER_PULSE_MS);
|
|
gpio_put(PIN_TRIG_OUT, 0);
|
|
|
|
// Wait for return signal with timeout
|
|
uint64_t timeout_us = start_us + (MEASURE_TIMEOUT_MS * 1000);
|
|
|
|
while (time_us_64() < timeout_us) {
|
|
if (gpio_get(PIN_RETURN_IN)) {
|
|
// Got return signal!
|
|
uint64_t end_us = time_us_64();
|
|
result.latency_us = end_us - start_us;
|
|
result.latency_ms = (float)result.latency_us / 1000.0f;
|
|
result.valid = true;
|
|
break;
|
|
}
|
|
// Tight polling loop - don't sleep to maintain resolution
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
void latency_reset_stats(void) {
|
|
for (int i = 0; i < STATS_SAMPLES; i++) {
|
|
samples[i] = 0;
|
|
}
|
|
sample_index = 0;
|
|
sample_count = 0;
|
|
min_latency = 0;
|
|
max_latency = 0;
|
|
}
|
|
|
|
void latency_add_to_stats(float latency_ms) {
|
|
// Add to circular buffer
|
|
samples[sample_index] = latency_ms;
|
|
sample_index = (sample_index + 1) % STATS_SAMPLES;
|
|
|
|
if (sample_count < STATS_SAMPLES) {
|
|
sample_count++;
|
|
}
|
|
|
|
// Update min/max
|
|
if (sample_count == 1) {
|
|
min_latency = latency_ms;
|
|
max_latency = latency_ms;
|
|
} else {
|
|
if (latency_ms < min_latency) min_latency = latency_ms;
|
|
if (latency_ms > max_latency) max_latency = latency_ms;
|
|
}
|
|
}
|
|
|
|
stats_t latency_get_stats(void) {
|
|
stats_t s = {
|
|
.min_ms = min_latency,
|
|
.max_ms = max_latency,
|
|
.avg_ms = 0,
|
|
.count = sample_count
|
|
};
|
|
|
|
if (sample_count > 0) {
|
|
float sum = 0;
|
|
for (uint32_t i = 0; i < sample_count; i++) {
|
|
sum += samples[i];
|
|
}
|
|
s.avg_ms = sum / sample_count;
|
|
}
|
|
|
|
return s;
|
|
}
|