328 lines
10 KiB
Python
328 lines
10 KiB
Python
#!/usr/bin/env python3
|
|
import argparse
|
|
import re
|
|
import sys
|
|
import threading
|
|
import time
|
|
from collections import deque
|
|
from queue import Empty, Queue
|
|
|
|
try:
|
|
import pygame
|
|
except ImportError as exc:
|
|
raise SystemExit(
|
|
"Missing dependency: pygame. Install with `pip install pygame pyserial`."
|
|
) from exc
|
|
|
|
try:
|
|
import serial
|
|
except ImportError as exc:
|
|
raise SystemExit(
|
|
"Missing dependency: pyserial. Install with `pip install pygame pyserial`."
|
|
) from exc
|
|
|
|
|
|
IMU_RE = re.compile(
|
|
r"\[<IMU>\]:\s*(-?\d+)\s+(-?\d+)\s+(-?\d+)\s+(-?\d+)\s+(-?\d+)\s+(-?\d+)"
|
|
)
|
|
|
|
BACKGROUND = (18, 20, 24)
|
|
GRID = (48, 54, 64)
|
|
AXIS = (140, 148, 160)
|
|
ACC_X_TRACE = (90, 170, 255)
|
|
ACC_Y_TRACE = (80, 220, 140)
|
|
ACC_Z_TRACE = (255, 200, 60)
|
|
GYR_X_TRACE = (240, 80, 80)
|
|
GYR_Y_TRACE = (220, 80, 220)
|
|
GYR_Z_TRACE = (240, 240, 240)
|
|
TEXT = (230, 235, 240)
|
|
ERROR = (255, 110, 110)
|
|
|
|
|
|
def parse_args() -> argparse.Namespace:
|
|
parser = argparse.ArgumentParser(
|
|
description="Read BMI160 IMU values from a serial port and draw them live."
|
|
)
|
|
parser.add_argument("port", help="Serial port, for example /dev/ttyUSB0 or COM3")
|
|
parser.add_argument(
|
|
"-b",
|
|
"--baudrate",
|
|
type=int,
|
|
default=115200,
|
|
help="Serial baudrate (default: 115200)",
|
|
)
|
|
parser.add_argument(
|
|
"--history-seconds",
|
|
type=float,
|
|
default=10.0,
|
|
help="Initial visible history window in seconds (default: 10)",
|
|
)
|
|
parser.add_argument(
|
|
"--max-samples",
|
|
type=int,
|
|
default=30000,
|
|
help="Maximum number of samples to keep in memory (default: 30000)",
|
|
)
|
|
parser.add_argument(
|
|
"--width",
|
|
type=int,
|
|
default=1200,
|
|
help="Window width in pixels (default: 1200)",
|
|
)
|
|
parser.add_argument(
|
|
"--height",
|
|
type=int,
|
|
default=700,
|
|
help="Window height in pixels (default: 700)",
|
|
)
|
|
return parser.parse_args()
|
|
|
|
|
|
def serial_reader(port: str, baudrate: int, output: Queue) -> None:
|
|
try:
|
|
with serial.Serial(port, baudrate=baudrate, timeout=1) as ser:
|
|
while True:
|
|
raw_line = ser.readline()
|
|
if not raw_line:
|
|
continue
|
|
line = raw_line.decode("utf-8", errors="replace").strip()
|
|
timestamp = time.monotonic()
|
|
match = IMU_RE.search(line)
|
|
if not match:
|
|
continue
|
|
|
|
values = [int(group) for group in match.groups()]
|
|
for stream_name, value in zip(
|
|
("acc_x", "acc_y", "acc_z", "gyr_x", "gyr_y", "gyr_z"),
|
|
values,
|
|
):
|
|
output.put((stream_name, timestamp, value))
|
|
except serial.SerialException as exc:
|
|
output.put(("error", f"Serial error: {exc}"))
|
|
|
|
|
|
def drain_queue(
|
|
queue: Queue,
|
|
sample_sets: dict[str, deque[tuple[float, int]]],
|
|
) -> str | None:
|
|
error_message = None
|
|
while True:
|
|
try:
|
|
item = queue.get_nowait()
|
|
except Empty:
|
|
return error_message
|
|
|
|
if item[0] == "error":
|
|
error_message = item[1]
|
|
continue
|
|
|
|
stream_name, timestamp, value = item
|
|
sample_sets[stream_name].append((timestamp, value))
|
|
|
|
|
|
def draw_grid(
|
|
surface: pygame.Surface,
|
|
rect: pygame.Rect,
|
|
font: pygame.font.Font,
|
|
view_span: float,
|
|
y_min: float,
|
|
y_max: float,
|
|
title: str,
|
|
) -> None:
|
|
pygame.draw.rect(surface, GRID, rect, width=1)
|
|
|
|
for fraction in (0.0, 0.25, 0.5, 0.75, 1.0):
|
|
y = rect.top + round(fraction * rect.height)
|
|
pygame.draw.line(surface, GRID, (rect.left, y), (rect.right, y), 1)
|
|
value = y_max - fraction * (y_max - y_min)
|
|
label = f"{value:.0f}"
|
|
text = font.render(label, True, AXIS)
|
|
surface.blit(text, (10, y - text.get_height() // 2))
|
|
|
|
for fraction in (0.0, 0.25, 0.5, 0.75, 1.0):
|
|
x = rect.left + round(fraction * rect.width)
|
|
pygame.draw.line(surface, GRID, (x, rect.top), (x, rect.bottom), 1)
|
|
seconds_ago = view_span * (1.0 - fraction)
|
|
label = f"-{seconds_ago:.1f}s" if seconds_ago > 0.05 else "now"
|
|
text = font.render(label, True, AXIS)
|
|
surface.blit(text, (x - text.get_width() // 2, rect.bottom + 8))
|
|
|
|
surface.blit(font.render(title, True, TEXT), (rect.left + 8, rect.top + 8))
|
|
|
|
|
|
def draw_trace(
|
|
surface: pygame.Surface,
|
|
rect: pygame.Rect,
|
|
samples: deque[tuple[float, int]],
|
|
view_span: float,
|
|
now: float,
|
|
y_min: float,
|
|
y_max: float,
|
|
color: tuple[int, int, int],
|
|
) -> int | None:
|
|
visible_points = []
|
|
latest_value = None
|
|
y_span = max(y_max - y_min, 1e-6)
|
|
|
|
for timestamp, value in samples:
|
|
age = now - timestamp
|
|
if age < 0 or age > view_span:
|
|
continue
|
|
x = rect.right - (age / view_span) * rect.width
|
|
y = rect.bottom - ((value - y_min) / y_span) * rect.height
|
|
visible_points.append((round(x), round(y)))
|
|
latest_value = value
|
|
|
|
if len(visible_points) >= 2:
|
|
pygame.draw.lines(surface, color, False, visible_points, 2)
|
|
elif len(visible_points) == 1:
|
|
pygame.draw.circle(surface, color, visible_points[0], 2)
|
|
|
|
return latest_value
|
|
|
|
|
|
def visible_range(
|
|
sample_sets: dict[str, deque[tuple[float, int]]], stream_names: tuple[str, ...], now: float, view_span: float
|
|
) -> tuple[float, float]:
|
|
max_abs = 1.0
|
|
|
|
for stream_name in stream_names:
|
|
for timestamp, value in sample_sets[stream_name]:
|
|
age = now - timestamp
|
|
if 0 <= age <= view_span:
|
|
max_abs = max(max_abs, abs(value))
|
|
|
|
return -max_abs, max_abs
|
|
|
|
|
|
def zoom(view_span: float, direction: int, initial_span: float) -> float:
|
|
if direction > 0:
|
|
return max(1.0, view_span / 1.2)
|
|
return min(max(initial_span * 20.0, 60.0), view_span * 1.2)
|
|
|
|
|
|
def main() -> int:
|
|
args = parse_args()
|
|
sample_sets = {
|
|
"acc_x": deque(maxlen=args.max_samples),
|
|
"acc_y": deque(maxlen=args.max_samples),
|
|
"acc_z": deque(maxlen=args.max_samples),
|
|
"gyr_x": deque(maxlen=args.max_samples),
|
|
"gyr_y": deque(maxlen=args.max_samples),
|
|
"gyr_z": deque(maxlen=args.max_samples),
|
|
}
|
|
queue: Queue = Queue()
|
|
|
|
thread = threading.Thread(
|
|
target=serial_reader, args=(args.port, args.baudrate, queue), daemon=True
|
|
)
|
|
thread.start()
|
|
|
|
pygame.init()
|
|
pygame.display.set_caption(f"IMU stream: {args.port} @ {args.baudrate}")
|
|
screen = pygame.display.set_mode((args.width, args.height), pygame.RESIZABLE)
|
|
font = pygame.font.SysFont("monospace", 18)
|
|
small_font = pygame.font.SysFont("monospace", 14)
|
|
clock = pygame.time.Clock()
|
|
|
|
view_span = max(args.history_seconds, 1.0)
|
|
error_message = None
|
|
running = True
|
|
|
|
while running:
|
|
for event in pygame.event.get():
|
|
if event.type == pygame.QUIT:
|
|
running = False
|
|
elif event.type == pygame.MOUSEWHEEL:
|
|
view_span = zoom(view_span, event.y, args.history_seconds)
|
|
elif event.type == pygame.MOUSEBUTTONDOWN:
|
|
if event.button == 4:
|
|
view_span = zoom(view_span, 1, args.history_seconds)
|
|
elif event.button == 5:
|
|
view_span = zoom(view_span, -1, args.history_seconds)
|
|
|
|
queued_error = drain_queue(queue, sample_sets)
|
|
if queued_error is not None:
|
|
error_message = queued_error
|
|
|
|
width, height = screen.get_size()
|
|
now = time.monotonic()
|
|
plot_width = max(100, width - 100)
|
|
panel_height = max(100, (height - 140) // 2)
|
|
acc_rect = pygame.Rect(70, 40, plot_width, panel_height)
|
|
gyr_rect = pygame.Rect(70, acc_rect.bottom + 40, plot_width, panel_height)
|
|
acc_y_min, acc_y_max = visible_range(
|
|
sample_sets, ("acc_x", "acc_y", "acc_z"), now, view_span
|
|
)
|
|
gyr_y_min, gyr_y_max = visible_range(
|
|
sample_sets, ("gyr_x", "gyr_y", "gyr_z"), now, view_span
|
|
)
|
|
|
|
screen.fill(BACKGROUND)
|
|
draw_grid(screen, acc_rect, small_font, view_span, acc_y_min, acc_y_max, "Accelerometer")
|
|
draw_grid(screen, gyr_rect, small_font, view_span, gyr_y_min, gyr_y_max, "Gyroscope")
|
|
|
|
latest_acc_x = draw_trace(
|
|
screen, acc_rect, sample_sets["acc_x"], view_span, now, acc_y_min, acc_y_max, ACC_X_TRACE
|
|
)
|
|
latest_acc_y = draw_trace(
|
|
screen, acc_rect, sample_sets["acc_y"], view_span, now, acc_y_min, acc_y_max, ACC_Y_TRACE
|
|
)
|
|
latest_acc_z = draw_trace(
|
|
screen, acc_rect, sample_sets["acc_z"], view_span, now, acc_y_min, acc_y_max, ACC_Z_TRACE
|
|
)
|
|
latest_gyr_x = draw_trace(
|
|
screen, gyr_rect, sample_sets["gyr_x"], view_span, now, gyr_y_min, gyr_y_max, GYR_X_TRACE
|
|
)
|
|
latest_gyr_y = draw_trace(
|
|
screen, gyr_rect, sample_sets["gyr_y"], view_span, now, gyr_y_min, gyr_y_max, GYR_Y_TRACE
|
|
)
|
|
latest_gyr_z = draw_trace(
|
|
screen, gyr_rect, sample_sets["gyr_z"], view_span, now, gyr_y_min, gyr_y_max, GYR_Z_TRACE
|
|
)
|
|
|
|
header = f"port={args.port} baud={args.baudrate} zoom={view_span:.1f}s"
|
|
if latest_acc_x is not None:
|
|
header += f" acc=({latest_acc_x}, {latest_acc_y}, {latest_acc_z})"
|
|
if latest_gyr_x is not None:
|
|
header += f" gyr=({latest_gyr_x}, {latest_gyr_y}, {latest_gyr_z})"
|
|
screen.blit(font.render(header, True, TEXT), (10, 8))
|
|
screen.blit(
|
|
small_font.render("mouse wheel: zoom horizontal axis", True, AXIS),
|
|
(10, height - 24),
|
|
)
|
|
legend_items = (
|
|
("ACC X", ACC_X_TRACE),
|
|
("ACC Y", ACC_Y_TRACE),
|
|
("ACC Z", ACC_Z_TRACE),
|
|
("GYR X", GYR_X_TRACE),
|
|
("GYR Y", GYR_Y_TRACE),
|
|
("GYR Z", GYR_Z_TRACE),
|
|
)
|
|
legend_x = 10
|
|
legend_y = 34
|
|
for label, color in legend_items:
|
|
pygame.draw.line(
|
|
screen,
|
|
color,
|
|
(legend_x, legend_y + 9),
|
|
(legend_x + 24, legend_y + 9),
|
|
3,
|
|
)
|
|
screen.blit(small_font.render(label, True, TEXT), (legend_x + 30, legend_y))
|
|
legend_x += 105
|
|
|
|
if error_message:
|
|
error_surface = font.render(error_message, True, ERROR)
|
|
screen.blit(error_surface, (10, 56))
|
|
|
|
pygame.display.flip()
|
|
clock.tick(60)
|
|
|
|
pygame.quit()
|
|
return 0
|
|
|
|
|
|
if __name__ == "__main__":
|
|
raise SystemExit(main())
|