Crazyflie motor test stand GUI (cflib + DearPyGui)
uv-managed Python app for ramping Crazyflie 2.1 / Bolt motors on a
thrust test stand over a Crazyradio.
- cf_link.py: thread-safe cflib wrapper — scan/connect, supervisor
arming (send_arming_request) with legacy forceArm fallback, motor
override via motorPowerSet params, battery + estimator-state logging,
firmware-console capture, and a log buffer for the UI console.
- app.py: DearPyGui UI with a per-frame ramp loop.
* Linked / individual motor control, sliders + numeric entry (percent)
* Max-throttle limit, ramp rate, EMERGENCY STOP (button + SPACE)
* ARM button: disabled when disconnected, green when ready, yellow
when armed
* Telemetry: estimator-state panel (pose/yaw/flow), throttle + battery
plots that start at zero and only scroll while armed (freeze on disarm)
* Live console panel (app + cflib + firmware messages)
Benign cflib warnings (stale log blocks, missing optional params) are
filtered; safety teardown disarms on disconnect/quit.
Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
This commit is contained in:
3
test_stand/__init__.py
Normal file
3
test_stand/__init__.py
Normal file
@@ -0,0 +1,3 @@
|
||||
"""test-stand: a UI for ramping Crazyflie motors on a thrust test stand."""
|
||||
|
||||
__version__ = "0.1.0"
|
||||
435
test_stand/app.py
Normal file
435
test_stand/app.py
Normal file
@@ -0,0 +1,435 @@
|
||||
"""DearPyGui front-end for the Crazyflie motor test stand.
|
||||
|
||||
The render loop is driven manually so motor ramps advance smoothly every
|
||||
frame: sliders set per-motor *targets*, and each frame the current output is
|
||||
moved toward its target at the configured ramp rate, then the resulting PWM is
|
||||
pushed to the Crazyflie only when it changes.
|
||||
|
||||
Safety model:
|
||||
* Motors never move until you press ARM (and throttle starts at 0).
|
||||
* A max-throttle limit clamps every target.
|
||||
* EMERGENCY STOP (button or SPACE) cuts all motors instantly.
|
||||
* Disconnecting or losing the link disarms automatically.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import time
|
||||
|
||||
import dearpygui.dearpygui as dpg
|
||||
|
||||
from .cf_link import (CrazyflieLink, LOG_BUFFER, PWM_MAX,
|
||||
ENABLE_INDIVIDUAL, ENABLE_ALL)
|
||||
|
||||
NUM_MOTORS = 4
|
||||
PLOT_WINDOW_S = 30.0 # seconds of history to keep in the plots
|
||||
|
||||
|
||||
class MotorChannel:
|
||||
"""Tracks a motor's target (slider) and ramped current output."""
|
||||
|
||||
def __init__(self) -> None:
|
||||
self.current = 0.0 # % actually being output (after ramp)
|
||||
self.last_pwm = -1 # last value sent to the radio (-1 = never)
|
||||
|
||||
def step(self, target_pct: float, rate_pps: float, dt: float) -> None:
|
||||
delta = target_pct - self.current
|
||||
max_step = rate_pps * dt
|
||||
if abs(delta) <= max_step:
|
||||
self.current = target_pct
|
||||
else:
|
||||
self.current += max_step if delta > 0 else -max_step
|
||||
|
||||
@property
|
||||
def pwm(self) -> int:
|
||||
return int(round(self.current / 100.0 * PWM_MAX))
|
||||
|
||||
|
||||
class TestStandApp:
|
||||
def __init__(self) -> None:
|
||||
self.link = CrazyflieLink()
|
||||
self.motors = [MotorChannel() for _ in range(NUM_MOTORS)]
|
||||
self._last_frame = time.monotonic()
|
||||
self._armed_mode = None # enable mode currently armed with, or None
|
||||
self._plot_t0 = None # monotonic time of arming; None while disarmed
|
||||
self._plot_t: list[float] = []
|
||||
self._plot_throttle: list[float] = []
|
||||
self._plot_vbat: list[float] = []
|
||||
self._console_version = -1
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# UI construction
|
||||
# ------------------------------------------------------------------
|
||||
def build(self) -> None:
|
||||
dpg.create_context()
|
||||
|
||||
with dpg.theme() as estop_theme:
|
||||
with dpg.theme_component(dpg.mvButton):
|
||||
dpg.add_theme_color(dpg.mvThemeCol_Button, (170, 30, 30))
|
||||
dpg.add_theme_color(dpg.mvThemeCol_ButtonHovered, (210, 40, 40))
|
||||
dpg.add_theme_color(dpg.mvThemeCol_ButtonActive, (240, 60, 60))
|
||||
self._estop_theme = estop_theme
|
||||
|
||||
# ARM button: green when ready to arm, yellow once armed.
|
||||
with dpg.theme() as arm_ready_theme:
|
||||
with dpg.theme_component(dpg.mvButton):
|
||||
dpg.add_theme_color(dpg.mvThemeCol_Button, (35, 130, 45))
|
||||
dpg.add_theme_color(dpg.mvThemeCol_ButtonHovered, (45, 160, 55))
|
||||
dpg.add_theme_color(dpg.mvThemeCol_ButtonActive, (55, 185, 65))
|
||||
self._arm_ready_theme = arm_ready_theme
|
||||
|
||||
with dpg.theme() as armed_theme:
|
||||
with dpg.theme_component(dpg.mvButton):
|
||||
dpg.add_theme_color(dpg.mvThemeCol_Button, (200, 170, 30))
|
||||
dpg.add_theme_color(dpg.mvThemeCol_ButtonHovered, (225, 195, 45))
|
||||
dpg.add_theme_color(dpg.mvThemeCol_ButtonActive, (245, 215, 60))
|
||||
dpg.add_theme_color(dpg.mvThemeCol_Text, (20, 20, 20))
|
||||
self._armed_theme = armed_theme
|
||||
|
||||
with dpg.window(tag="primary"):
|
||||
self._build_connection()
|
||||
dpg.add_separator()
|
||||
self._build_safety()
|
||||
dpg.add_separator()
|
||||
self._build_motors()
|
||||
dpg.add_separator()
|
||||
self._build_plots()
|
||||
dpg.add_separator()
|
||||
self._build_console()
|
||||
|
||||
# SPACE = emergency stop, available anywhere.
|
||||
with dpg.handler_registry():
|
||||
dpg.add_key_press_handler(dpg.mvKey_Spacebar, callback=self._on_estop)
|
||||
|
||||
dpg.create_viewport(title="Crazyflie Motor Test Stand", width=900, height=1010)
|
||||
dpg.setup_dearpygui()
|
||||
dpg.show_viewport()
|
||||
dpg.set_primary_window("primary", True)
|
||||
|
||||
def _build_connection(self) -> None:
|
||||
dpg.add_text("Connection")
|
||||
with dpg.group(horizontal=True):
|
||||
dpg.add_combo([], tag="uri_combo", width=320,
|
||||
default_value="(scan for devices)")
|
||||
dpg.add_button(label="Scan", callback=self._on_scan)
|
||||
dpg.add_button(label="Connect", tag="connect_btn",
|
||||
callback=self._on_connect_toggle)
|
||||
with dpg.group(horizontal=True):
|
||||
dpg.add_text("Status:")
|
||||
dpg.add_text("disconnected", tag="status_text")
|
||||
with dpg.group(horizontal=True):
|
||||
dpg.add_text("Battery:")
|
||||
dpg.add_text("-- V", tag="vbat_text")
|
||||
dpg.add_text(" Link:")
|
||||
dpg.add_text("--", tag="link_text")
|
||||
|
||||
def _build_safety(self) -> None:
|
||||
dpg.add_text("Safety")
|
||||
with dpg.group(horizontal=True):
|
||||
dpg.add_button(label="ARM", tag="arm_btn", width=120,
|
||||
callback=self._on_arm_toggle, enabled=False)
|
||||
estop = dpg.add_button(label="EMERGENCY STOP (SPACE)", width=260,
|
||||
callback=self._on_estop)
|
||||
dpg.bind_item_theme(estop, self._estop_theme)
|
||||
dpg.add_slider_float(label="Max throttle limit (%)", tag="max_limit",
|
||||
default_value=30.0, min_value=0.0, max_value=100.0,
|
||||
width=300)
|
||||
dpg.add_slider_float(label="Ramp rate (%/s)", tag="ramp_rate",
|
||||
default_value=20.0, min_value=1.0, max_value=200.0,
|
||||
width=300)
|
||||
dpg.add_text("", tag="warn_text", color=(230, 180, 40))
|
||||
|
||||
def _build_motors(self) -> None:
|
||||
dpg.add_text("Motors")
|
||||
dpg.add_radio_button(("Linked (all together)", "Individual"),
|
||||
tag="mode_radio", default_value="Linked (all together)",
|
||||
horizontal=True, callback=self._on_mode_change)
|
||||
|
||||
with dpg.group(horizontal=True):
|
||||
dpg.add_slider_float(label="", tag="master", default_value=0.0,
|
||||
min_value=0.0, max_value=100.0, width=360,
|
||||
callback=self._sync_from_slider, user_data="master")
|
||||
dpg.add_input_float(tag="master_input", default_value=0.0,
|
||||
min_value=0.0, max_value=100.0, step=1.0,
|
||||
format="%.1f", width=110, min_clamped=True,
|
||||
max_clamped=True, on_enter=True,
|
||||
callback=self._sync_from_input, user_data="master")
|
||||
dpg.add_text("Master throttle (%)")
|
||||
with dpg.group(horizontal=True):
|
||||
dpg.add_button(label="All to 0", callback=lambda: self._set_all_targets(0))
|
||||
dpg.add_text(" (type a value or drag; output ramps at the rate above)")
|
||||
|
||||
for i in range(NUM_MOTORS):
|
||||
with dpg.group(horizontal=True):
|
||||
dpg.add_slider_float(label="", tag=f"motor_{i}", default_value=0.0,
|
||||
min_value=0.0, max_value=100.0, width=280,
|
||||
callback=self._sync_from_slider,
|
||||
user_data=f"motor_{i}")
|
||||
dpg.add_input_float(tag=f"motor_{i}_input", default_value=0.0,
|
||||
min_value=0.0, max_value=100.0, step=1.0,
|
||||
format="%.1f", width=110, min_clamped=True,
|
||||
max_clamped=True, on_enter=True,
|
||||
callback=self._sync_from_input,
|
||||
user_data=f"motor_{i}")
|
||||
dpg.add_text(f"M{i + 1} (%)")
|
||||
dpg.add_text("out: 0.0% (0)", tag=f"motor_out_{i}")
|
||||
|
||||
def _build_plots(self) -> None:
|
||||
dpg.add_text("Telemetry")
|
||||
dpg.add_text("Estimator state")
|
||||
dpg.add_text("Pos (m): --", tag="est_pos")
|
||||
dpg.add_text("Yaw: --", tag="est_yaw")
|
||||
dpg.add_text("Flow counts: --", tag="est_flow")
|
||||
dpg.add_separator()
|
||||
with dpg.plot(label="Output throttle (avg %)", height=180, width=-1):
|
||||
dpg.add_plot_axis(dpg.mvXAxis, label="t (s)", tag="thr_x")
|
||||
dpg.set_axis_limits("thr_x", 0, PLOT_WINDOW_S)
|
||||
with dpg.plot_axis(dpg.mvYAxis, label="%", tag="thr_y"):
|
||||
dpg.set_axis_limits("thr_y", 0, 100)
|
||||
dpg.add_line_series([], [], tag="thr_series")
|
||||
with dpg.plot(label="Battery (V)", height=180, width=-1):
|
||||
dpg.add_plot_axis(dpg.mvXAxis, label="t (s)", tag="vbat_x")
|
||||
dpg.set_axis_limits("vbat_x", 0, PLOT_WINDOW_S)
|
||||
with dpg.plot_axis(dpg.mvYAxis, label="V", tag="vbat_y"):
|
||||
dpg.set_axis_limits("vbat_y", 2.8, 4.3)
|
||||
dpg.add_line_series([], [], tag="vbat_series")
|
||||
|
||||
def _build_console(self) -> None:
|
||||
with dpg.group(horizontal=True):
|
||||
dpg.add_text("Console")
|
||||
dpg.add_button(label="Clear", callback=lambda: LOG_BUFFER.clear())
|
||||
with dpg.child_window(tag="console_child", height=170, width=-1):
|
||||
dpg.add_text("", tag="console_text", wrap=860)
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# Callbacks
|
||||
# ------------------------------------------------------------------
|
||||
def _on_scan(self) -> None:
|
||||
uris = self.link.scan()
|
||||
if uris:
|
||||
dpg.configure_item("uri_combo", items=uris)
|
||||
dpg.set_value("uri_combo", uris[0])
|
||||
else:
|
||||
dpg.configure_item("uri_combo", items=[])
|
||||
dpg.set_value("uri_combo", "(none found)")
|
||||
|
||||
def _on_connect_toggle(self) -> None:
|
||||
state = self.link.snapshot()
|
||||
if state.status in ("connected", "connecting"):
|
||||
self.link.disconnect()
|
||||
return
|
||||
uri = dpg.get_value("uri_combo")
|
||||
if uri and uri.startswith(("radio://", "usb://")):
|
||||
self.link.connect(uri)
|
||||
|
||||
def _on_arm_toggle(self) -> None:
|
||||
state = self.link.snapshot()
|
||||
if state.armed:
|
||||
self._disarm()
|
||||
else:
|
||||
self._arm()
|
||||
|
||||
def _arm(self) -> None:
|
||||
mode = self._enable_mode()
|
||||
missing = self.link.arm(mode)
|
||||
self._armed_mode = mode
|
||||
if missing:
|
||||
dpg.set_value("warn_text",
|
||||
"firmware missing params: " + ", ".join(missing))
|
||||
else:
|
||||
dpg.set_value("warn_text", "")
|
||||
|
||||
def _disarm(self) -> None:
|
||||
self.link.disarm()
|
||||
self._armed_mode = None
|
||||
self._set_all_targets(0)
|
||||
|
||||
def _on_estop(self, *_) -> None:
|
||||
self.link.emergency_stop()
|
||||
self._armed_mode = None
|
||||
self._set_all_targets(0)
|
||||
for m in self.motors:
|
||||
m.current = 0.0
|
||||
|
||||
def _on_mode_change(self, *_) -> None:
|
||||
# Re-arm with the matching enable mode if currently armed.
|
||||
if self._armed_mode is not None:
|
||||
self._arm()
|
||||
|
||||
def _sync_from_slider(self, sender, app_data, user_data) -> None:
|
||||
# Slider moved -> mirror its value into the paired text box.
|
||||
dpg.set_value(f"{user_data}_input", app_data)
|
||||
|
||||
def _sync_from_input(self, sender, app_data, user_data) -> None:
|
||||
# Text box committed -> clamp and mirror into the slider.
|
||||
value = max(0.0, min(100.0, app_data))
|
||||
dpg.set_value(user_data, value)
|
||||
dpg.set_value(f"{user_data}_input", value)
|
||||
|
||||
def _set_target(self, base_tag: str, value: float) -> None:
|
||||
dpg.set_value(base_tag, value)
|
||||
dpg.set_value(f"{base_tag}_input", value)
|
||||
|
||||
def _set_all_targets(self, value: float) -> None:
|
||||
self._set_target("master", value)
|
||||
for i in range(NUM_MOTORS):
|
||||
self._set_target(f"motor_{i}", value)
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# Per-frame update
|
||||
# ------------------------------------------------------------------
|
||||
def _enable_mode(self) -> int:
|
||||
linked = dpg.get_value("mode_radio").startswith("Linked")
|
||||
return ENABLE_ALL if linked else ENABLE_INDIVIDUAL
|
||||
|
||||
def _targets(self) -> list[float]:
|
||||
"""Effective per-motor targets after mode + max-limit are applied."""
|
||||
max_pct = dpg.get_value("max_limit")
|
||||
linked = dpg.get_value("mode_radio").startswith("Linked")
|
||||
if linked:
|
||||
t = dpg.get_value("master")
|
||||
raw = [t] * NUM_MOTORS
|
||||
else:
|
||||
raw = [dpg.get_value(f"motor_{i}") for i in range(NUM_MOTORS)]
|
||||
return [max(0.0, min(max_pct, v)) for v in raw]
|
||||
|
||||
def update(self) -> None:
|
||||
now = time.monotonic()
|
||||
dt = now - self._last_frame
|
||||
self._last_frame = now
|
||||
|
||||
state = self.link.snapshot()
|
||||
rate = dpg.get_value("ramp_rate")
|
||||
targets = self._targets()
|
||||
armed = state.armed and state.status == "connected"
|
||||
linked = dpg.get_value("mode_radio").startswith("Linked")
|
||||
|
||||
for i, m in enumerate(self.motors):
|
||||
# When disarmed, force everything to zero so nothing lingers.
|
||||
m.step(targets[i] if armed else 0.0, rate, dt)
|
||||
|
||||
# Push changed PWM values to the radio.
|
||||
if armed:
|
||||
if linked:
|
||||
pwm = self.motors[0].pwm
|
||||
if pwm != self.motors[0].last_pwm:
|
||||
self.link.set_all(pwm)
|
||||
for m in self.motors:
|
||||
m.last_pwm = pwm
|
||||
else:
|
||||
for i, m in enumerate(self.motors):
|
||||
if m.pwm != m.last_pwm:
|
||||
self.link.set_motor(i + 1, m.pwm)
|
||||
m.last_pwm = m.pwm
|
||||
|
||||
self._refresh_widgets(state, armed)
|
||||
self._refresh_estimator()
|
||||
self._refresh_plots(now, state, armed)
|
||||
self._refresh_console()
|
||||
|
||||
def _refresh_estimator(self) -> None:
|
||||
est = self.link.estimator # atomic read of the published snapshot
|
||||
if not est.valid:
|
||||
dpg.set_value("est_pos", "Pos (m): --")
|
||||
dpg.set_value("est_yaw", "Yaw: --")
|
||||
dpg.set_value("est_flow", "Flow counts: --")
|
||||
return
|
||||
dpg.set_value("est_pos",
|
||||
f"Pos (m): x {est.x:+.2f} y {est.y:+.2f} z {est.z:+.2f}")
|
||||
dpg.set_value("est_yaw", f"Yaw: {est.yaw:+.1f} deg")
|
||||
if est.has_flow:
|
||||
dpg.set_value("est_flow", f"Flow counts: dx {est.dx:+d} dy {est.dy:+d}")
|
||||
else:
|
||||
dpg.set_value("est_flow", "Flow counts: (no Flow deck)")
|
||||
|
||||
def _refresh_console(self) -> None:
|
||||
version, lines = LOG_BUFFER.snapshot()
|
||||
if version == self._console_version:
|
||||
return
|
||||
self._console_version = version
|
||||
# Keep following the tail only if the user is already at the bottom.
|
||||
try:
|
||||
at_bottom = (dpg.get_y_scroll("console_child")
|
||||
>= dpg.get_y_scroll_max("console_child") - 2.0)
|
||||
except Exception:
|
||||
at_bottom = True
|
||||
dpg.set_value("console_text", "\n".join(lines))
|
||||
if at_bottom:
|
||||
dpg.set_y_scroll("console_child", -1.0)
|
||||
|
||||
def _refresh_widgets(self, state, armed: bool) -> None:
|
||||
dpg.set_value("status_text", state.message or state.status)
|
||||
dpg.set_value("vbat_text", f"{state.vbat:.2f} V" if state.vbat else "-- V")
|
||||
dpg.set_value("link_text", f"{state.link_quality:.0f}%")
|
||||
|
||||
connected = state.status == "connected"
|
||||
dpg.configure_item("connect_btn",
|
||||
label="Disconnect" if state.status in ("connected", "connecting")
|
||||
else "Connect")
|
||||
dpg.configure_item("arm_btn", enabled=connected,
|
||||
label="DISARM" if state.armed else "ARM")
|
||||
# Disabled when not connectable; green when ready to arm; yellow when armed.
|
||||
if not connected:
|
||||
dpg.bind_item_theme("arm_btn", 0)
|
||||
elif state.armed:
|
||||
dpg.bind_item_theme("arm_btn", self._armed_theme)
|
||||
else:
|
||||
dpg.bind_item_theme("arm_btn", self._arm_ready_theme)
|
||||
|
||||
for i, m in enumerate(self.motors):
|
||||
dpg.set_value(f"motor_out_{i}", f"out: {m.current:5.1f}% ({m.pwm})")
|
||||
|
||||
def _refresh_plots(self, now: float, state, armed: bool) -> None:
|
||||
# Only collect/scroll while armed; the trace freezes when disarmed.
|
||||
if not armed:
|
||||
self._plot_t0 = None
|
||||
return
|
||||
if self._plot_t0 is None:
|
||||
# Arming transition: restart the time axis from zero.
|
||||
self._plot_t0 = now
|
||||
self._plot_t.clear()
|
||||
self._plot_throttle.clear()
|
||||
self._plot_vbat.clear()
|
||||
|
||||
t = now - self._plot_t0
|
||||
avg = sum(m.current for m in self.motors) / NUM_MOTORS
|
||||
self._plot_t.append(t)
|
||||
self._plot_throttle.append(avg)
|
||||
self._plot_vbat.append(state.vbat)
|
||||
|
||||
# Drop history outside the rolling window.
|
||||
while self._plot_t and self._plot_t[0] < t - PLOT_WINDOW_S:
|
||||
self._plot_t.pop(0)
|
||||
self._plot_throttle.pop(0)
|
||||
self._plot_vbat.pop(0)
|
||||
|
||||
dpg.set_value("thr_series", [self._plot_t, self._plot_throttle])
|
||||
dpg.set_value("vbat_series", [self._plot_t, self._plot_vbat])
|
||||
dpg.set_axis_limits("thr_x", max(0, t - PLOT_WINDOW_S), max(PLOT_WINDOW_S, t))
|
||||
dpg.set_axis_limits("vbat_x", max(0, t - PLOT_WINDOW_S), max(PLOT_WINDOW_S, t))
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# Run loop
|
||||
# ------------------------------------------------------------------
|
||||
def run(self) -> None:
|
||||
self.build()
|
||||
try:
|
||||
while dpg.is_dearpygui_running():
|
||||
self.update()
|
||||
dpg.render_dearpygui_frame()
|
||||
finally:
|
||||
# Best-effort: never leave the bench armed.
|
||||
try:
|
||||
self.link.emergency_stop()
|
||||
self.link.disconnect()
|
||||
except Exception:
|
||||
pass
|
||||
dpg.destroy_context()
|
||||
|
||||
|
||||
def main() -> None:
|
||||
TestStandApp().run()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
453
test_stand/cf_link.py
Normal file
453
test_stand/cf_link.py
Normal file
@@ -0,0 +1,453 @@
|
||||
"""Crazyflie communication layer for the motor test stand.
|
||||
|
||||
Wraps cflib so the UI only deals with a small, thread-safe surface:
|
||||
connect/disconnect, arm/disarm, and writing raw motor PWM values.
|
||||
|
||||
Motor override is done through the firmware ``motorPowerSet`` param group:
|
||||
|
||||
motorPowerSet.enable = 0 normal flight control (no override)
|
||||
= 1 override each motor individually via m1..m4
|
||||
= 2 override all motors with the m1 value
|
||||
motorPowerSet.m1..m4 = 0..65535 raw PWM per motor
|
||||
|
||||
Current firmware arms through the supervisor via a platform arming request
|
||||
(``cf.platform.send_arming_request(True)``); the older ``system.forceArm``
|
||||
param no longer exists. ``motor.batCompensation = 0`` would disable battery
|
||||
compensation so a commanded PWM maps directly to output, for repeatable
|
||||
measurements. Both the arming request and those legacy params are sent
|
||||
best-effort, so firmware with either mechanism works.
|
||||
|
||||
All cflib callbacks run on cflib's own thread, so shared state is guarded by
|
||||
a lock and exposed through getters. Param writes are sent from the caller's
|
||||
thread, which cflib handles safely.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import logging
|
||||
import collections
|
||||
import threading
|
||||
from dataclasses import dataclass, field
|
||||
from typing import Callable, Optional
|
||||
|
||||
import cflib.crtp
|
||||
from cflib.crazyflie import Crazyflie
|
||||
from cflib.crazyflie.log import LogConfig
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
# Firmware console output is logged under this name so it's easy to spot.
|
||||
_CONSOLE_LOGGER = logging.getLogger("crazyflie.console")
|
||||
|
||||
PWM_MAX = 65535
|
||||
|
||||
# motorPowerSet.enable values
|
||||
ENABLE_OFF = 0
|
||||
ENABLE_INDIVIDUAL = 1
|
||||
ENABLE_ALL = 2
|
||||
|
||||
|
||||
class _StaleLogFilter(logging.Filter):
|
||||
"""Drop cflib's benign "no LogEntry to handle id=N" warnings.
|
||||
|
||||
Firmware streams log-data for a block id the host no longer tracks when a
|
||||
stale block is left over from a prior/parallel session. It's harmless and
|
||||
not actionable from here (power-cycle the Crazyflie to clear it), so we keep
|
||||
it out of the console instead of alarming the user.
|
||||
"""
|
||||
|
||||
def filter(self, record: logging.LogRecord) -> bool:
|
||||
return "no LogEntry to handle" not in record.getMessage()
|
||||
|
||||
|
||||
class LogBuffer(logging.Handler):
|
||||
"""A thread-safe ring buffer of formatted log lines for the UI console.
|
||||
|
||||
cflib logs from its own thread, so reads/writes are guarded by a lock. A
|
||||
monotonic version counter lets the UI cheaply detect when there's something
|
||||
new to render.
|
||||
"""
|
||||
|
||||
def __init__(self, capacity: int = 500) -> None:
|
||||
super().__init__()
|
||||
self._lines: collections.deque = collections.deque(maxlen=capacity)
|
||||
self._lock = threading.Lock()
|
||||
self._version = 0
|
||||
self.setFormatter(logging.Formatter(
|
||||
"%(asctime)s %(levelname).1s %(name)s: %(message)s", "%H:%M:%S"))
|
||||
|
||||
def emit(self, record: logging.LogRecord) -> None:
|
||||
try:
|
||||
line = self.format(record)
|
||||
except Exception:
|
||||
return
|
||||
with self._lock:
|
||||
self._lines.append(line)
|
||||
self._version += 1
|
||||
|
||||
def snapshot(self) -> tuple[int, list[str]]:
|
||||
with self._lock:
|
||||
return self._version, list(self._lines)
|
||||
|
||||
def clear(self) -> None:
|
||||
with self._lock:
|
||||
self._lines.clear()
|
||||
self._version += 1
|
||||
|
||||
|
||||
# Single shared console buffer, attached to the root logger once.
|
||||
LOG_BUFFER = LogBuffer()
|
||||
_LOGGING_READY = False
|
||||
|
||||
|
||||
def _setup_logging() -> None:
|
||||
"""Route cflib + app logs into LOG_BUFFER and the console (idempotent)."""
|
||||
global _LOGGING_READY
|
||||
if _LOGGING_READY:
|
||||
return
|
||||
logging.getLogger("cflib.crazyflie.log").addFilter(_StaleLogFilter())
|
||||
root = logging.getLogger()
|
||||
if root.level == logging.NOTSET or root.level > logging.INFO:
|
||||
root.setLevel(logging.INFO)
|
||||
LOG_BUFFER.setLevel(logging.INFO)
|
||||
root.addHandler(LOG_BUFFER)
|
||||
_LOGGING_READY = True
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class EstimatorState:
|
||||
"""Immutable snapshot of estimator pose + raw flow counts.
|
||||
|
||||
Published atomically: the cflib log callback builds a fresh instance and
|
||||
assigns it to a single attribute, which is atomic under the GIL, so the
|
||||
render thread can read it lock-free.
|
||||
"""
|
||||
|
||||
valid: bool = False # True once at least one sample has arrived
|
||||
has_flow: bool = False # True if a Flow deck supplied motion.* counts
|
||||
x: float = 0.0 # metres, origin at takeoff
|
||||
y: float = 0.0
|
||||
z: float = 0.0 # laser-ranger height
|
||||
yaw: float = 0.0 # degrees
|
||||
dx: int = 0 # raw flow counts since last sample
|
||||
dy: int = 0
|
||||
|
||||
|
||||
@dataclass
|
||||
class LinkState:
|
||||
"""Snapshot of link status, copied out under lock for the UI."""
|
||||
|
||||
status: str = "disconnected" # disconnected | connecting | connected | failed
|
||||
uri: Optional[str] = None
|
||||
message: str = ""
|
||||
armed: bool = False
|
||||
vbat: float = 0.0
|
||||
link_quality: float = 0.0
|
||||
missing_params: list = field(default_factory=list)
|
||||
|
||||
|
||||
class CrazyflieLink:
|
||||
"""Thread-safe wrapper around a single Crazyflie connection."""
|
||||
|
||||
def __init__(self) -> None:
|
||||
_setup_logging()
|
||||
cflib.crtp.init_drivers()
|
||||
self._cf = Crazyflie(rw_cache="./.cache")
|
||||
self._lock = threading.Lock()
|
||||
self._state = LinkState()
|
||||
self._log: Optional[LogConfig] = None
|
||||
self._est_log: Optional[LogConfig] = None
|
||||
self._est_has_flow = False
|
||||
# Lock-free snapshot: written by the cflib thread, read by the renderer.
|
||||
self._estimator = EstimatorState()
|
||||
self._console_buf = "" # partial firmware console line
|
||||
|
||||
self._cf.console.receivedChar.add_callback(self._on_console)
|
||||
self._cf.connected.add_callback(self._on_connected)
|
||||
self._cf.fully_connected.add_callback(self._on_fully_connected)
|
||||
self._cf.disconnected.add_callback(self._on_disconnected)
|
||||
self._cf.connection_failed.add_callback(self._on_connection_failed)
|
||||
self._cf.connection_lost.add_callback(self._on_connection_lost)
|
||||
# link_statistics is the current home for link quality; fall back to the
|
||||
# deprecated top-level callback on older cflib.
|
||||
link_stats = getattr(self._cf, "link_statistics", None)
|
||||
if link_stats is not None and hasattr(link_stats, "link_quality_updated"):
|
||||
link_stats.link_quality_updated.add_callback(self._on_link_quality)
|
||||
else: # pragma: no cover - older cflib
|
||||
self._cf.link_quality_updated.add_callback(self._on_link_quality)
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# Discovery / connection
|
||||
# ------------------------------------------------------------------
|
||||
@staticmethod
|
||||
def scan() -> list[str]:
|
||||
"""Return URIs of Crazyflies reachable on any installed interface."""
|
||||
try:
|
||||
return [entry[0] for entry in cflib.crtp.scan_interfaces()]
|
||||
except Exception as exc: # pragma: no cover - hardware dependent
|
||||
logger.warning("scan failed: %s", exc)
|
||||
return []
|
||||
|
||||
def connect(self, uri: str) -> None:
|
||||
logger.info("connecting to %s", uri)
|
||||
with self._lock:
|
||||
self._state = LinkState(status="connecting", uri=uri,
|
||||
message=f"connecting to {uri}...")
|
||||
self._cf.open_link(uri)
|
||||
|
||||
def disconnect(self) -> None:
|
||||
# Make sure nothing is left spinning before we drop the link.
|
||||
try:
|
||||
if self._state.status == "connected":
|
||||
self.disarm()
|
||||
finally:
|
||||
self._cf.close_link()
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# Arming / motor control
|
||||
# ------------------------------------------------------------------
|
||||
def arm(self, enable_mode: int) -> list[str]:
|
||||
"""Arm the motor override. Returns the list of *critical* missing params.
|
||||
|
||||
Only ``motorPowerSet.*`` is required. The legacy ``system.forceArm`` /
|
||||
``motor.batCompensation`` params don't exist on current firmware (the
|
||||
supervisor handles arming via a platform request instead), so they are
|
||||
set best-effort and their absence is not reported.
|
||||
"""
|
||||
missing: list[str] = []
|
||||
# Modern supervisor arming (current firmware).
|
||||
self._send_arming(True)
|
||||
# Legacy / optional params — best-effort, silent if absent.
|
||||
self._set_param("motor.batCompensation", 0)
|
||||
self._set_param("system.forceArm", 1)
|
||||
# Critical override params: report these if the firmware lacks them.
|
||||
for i in range(1, 5):
|
||||
self._set_param(f"motorPowerSet.m{i}", 0, missing)
|
||||
self._set_param("motorPowerSet.enable", enable_mode, missing)
|
||||
with self._lock:
|
||||
self._state.armed = True
|
||||
self._state.missing_params = missing
|
||||
mode_name = "all-linked" if enable_mode == ENABLE_ALL else "individual"
|
||||
logger.info("ARMED (%s mode)", mode_name)
|
||||
if missing:
|
||||
logger.warning("firmware missing motor params: %s", ", ".join(missing))
|
||||
return missing
|
||||
|
||||
def disarm(self) -> None:
|
||||
for i in range(1, 5):
|
||||
self._set_param(f"motorPowerSet.m{i}", 0)
|
||||
self._set_param("motorPowerSet.enable", ENABLE_OFF)
|
||||
self._set_param("system.forceArm", 0)
|
||||
self._send_arming(False)
|
||||
with self._lock:
|
||||
self._state.armed = False
|
||||
logger.info("disarmed")
|
||||
|
||||
def emergency_stop(self) -> None:
|
||||
"""Immediately cut all motors. Safe to call from any thread/state."""
|
||||
try:
|
||||
for i in range(1, 5):
|
||||
self._set_param(f"motorPowerSet.m{i}", 0)
|
||||
self._set_param("motorPowerSet.enable", ENABLE_OFF)
|
||||
self._set_param("system.forceArm", 0)
|
||||
self._send_arming(False)
|
||||
finally:
|
||||
with self._lock:
|
||||
self._state.armed = False
|
||||
self._state.message = "EMERGENCY STOP"
|
||||
logger.warning("EMERGENCY STOP")
|
||||
|
||||
def _send_arming(self, armed: bool) -> None:
|
||||
"""Send a supervisor arming request, best-effort (older fw lacks it)."""
|
||||
try:
|
||||
self._cf.platform.send_arming_request(armed)
|
||||
except Exception as exc: # pragma: no cover - hardware dependent
|
||||
logger.debug("arming request (%s) failed: %s", armed, exc)
|
||||
|
||||
def set_motor(self, index: int, pwm: int) -> None:
|
||||
"""Set a single motor (1..4) to a raw PWM value (0..65535)."""
|
||||
self._set_param(f"motorPowerSet.m{index}", _clamp_pwm(pwm))
|
||||
|
||||
def set_all(self, pwm: int) -> None:
|
||||
"""Set all motors at once (requires ENABLE_ALL mode)."""
|
||||
self._set_param("motorPowerSet.m1", _clamp_pwm(pwm))
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# State access
|
||||
# ------------------------------------------------------------------
|
||||
def snapshot(self) -> LinkState:
|
||||
with self._lock:
|
||||
return LinkState(
|
||||
status=self._state.status,
|
||||
uri=self._state.uri,
|
||||
message=self._state.message,
|
||||
armed=self._state.armed,
|
||||
vbat=self._state.vbat,
|
||||
link_quality=self._state.link_quality,
|
||||
missing_params=list(self._state.missing_params),
|
||||
)
|
||||
|
||||
@property
|
||||
def estimator(self) -> EstimatorState:
|
||||
"""Latest estimator snapshot (lock-free atomic read of one reference)."""
|
||||
return self._estimator
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# Internals
|
||||
# ------------------------------------------------------------------
|
||||
def _has_param(self, name: str) -> bool:
|
||||
# Use get_element(group, name) rather than get_element_by_complete_name:
|
||||
# the latter logs a noisy "Unable to find variable" warning on a miss,
|
||||
# and we probe optional params on purpose.
|
||||
try:
|
||||
group, var = name.split(".", 1)
|
||||
return self._cf.param.toc.get_element(group, var) is not None
|
||||
except Exception:
|
||||
return False
|
||||
|
||||
def _set_param(self, name: str, value, missing: Optional[list] = None) -> bool:
|
||||
if not self._has_param(name):
|
||||
if missing is not None:
|
||||
missing.append(name)
|
||||
return False
|
||||
try:
|
||||
self._cf.param.set_value(name, value)
|
||||
return True
|
||||
except Exception as exc: # pragma: no cover - hardware dependent
|
||||
logger.warning("failed to set %s=%s: %s", name, value, exc)
|
||||
return False
|
||||
|
||||
# --- cflib callbacks (run on cflib's thread) ----------------------
|
||||
def _on_console(self, text: str) -> None:
|
||||
# Firmware console arrives in chunks; emit one log line per newline.
|
||||
self._console_buf += text
|
||||
while "\n" in self._console_buf:
|
||||
line, self._console_buf = self._console_buf.split("\n", 1)
|
||||
line = line.rstrip("\r")
|
||||
if line:
|
||||
_CONSOLE_LOGGER.info(line)
|
||||
|
||||
def _on_connected(self, uri: str) -> None:
|
||||
with self._lock:
|
||||
self._state.message = f"connected to {uri}, downloading TOC..."
|
||||
|
||||
def _on_fully_connected(self, uri: str) -> None:
|
||||
with self._lock:
|
||||
self._state.status = "connected"
|
||||
self._state.uri = uri
|
||||
self._state.message = f"connected: {uri}"
|
||||
logger.info("fully connected: %s", uri)
|
||||
self._start_logging()
|
||||
self._start_estimator_logging()
|
||||
|
||||
def _on_disconnected(self, uri: str) -> None:
|
||||
self._stop_logging()
|
||||
self._stop_estimator_logging()
|
||||
self._estimator = EstimatorState() # reset to "no data"
|
||||
with self._lock:
|
||||
# Don't clobber a "failed" message with a routine disconnect.
|
||||
if self._state.status != "failed":
|
||||
self._state.status = "disconnected"
|
||||
self._state.message = "disconnected"
|
||||
self._state.armed = False
|
||||
self._state.vbat = 0.0
|
||||
self._state.link_quality = 0.0
|
||||
|
||||
def _on_connection_failed(self, uri: str, msg: str) -> None:
|
||||
with self._lock:
|
||||
self._state.status = "failed"
|
||||
self._state.message = f"connection failed: {msg}"
|
||||
self._state.armed = False
|
||||
|
||||
def _on_connection_lost(self, uri: str, msg: str) -> None:
|
||||
with self._lock:
|
||||
self._state.status = "failed"
|
||||
self._state.message = f"connection lost: {msg}"
|
||||
self._state.armed = False
|
||||
|
||||
def _on_link_quality(self, quality: float) -> None:
|
||||
with self._lock:
|
||||
self._state.link_quality = quality
|
||||
|
||||
# --- telemetry logging --------------------------------------------
|
||||
def _start_logging(self) -> None:
|
||||
# Never stack blocks: tear down any previous one first.
|
||||
self._stop_logging()
|
||||
lg = LogConfig(name="teststand", period_in_ms=200)
|
||||
try:
|
||||
lg.add_variable("pm.vbat", "float")
|
||||
self._cf.log.add_config(lg)
|
||||
lg.data_received_cb.add_callback(self._on_log_data)
|
||||
lg.start()
|
||||
self._log = lg
|
||||
except (KeyError, AttributeError) as exc:
|
||||
logger.warning("could not start battery logging: %s", exc)
|
||||
self._log = None
|
||||
|
||||
def _stop_logging(self) -> None:
|
||||
if self._log is not None:
|
||||
try:
|
||||
self._log.stop()
|
||||
self._log.delete()
|
||||
except Exception:
|
||||
pass
|
||||
self._log = None
|
||||
|
||||
def _on_log_data(self, timestamp, data, logconf) -> None:
|
||||
with self._lock:
|
||||
self._state.vbat = data.get("pm.vbat", self._state.vbat)
|
||||
|
||||
# --- estimator-state logging --------------------------------------
|
||||
def _start_estimator_logging(self) -> None:
|
||||
"""Start the estimator/pose block, best-effort.
|
||||
|
||||
Tries the full block (incl. Flow-deck motion.* counts) first; if those
|
||||
variables are absent (no deck) the whole block would be rejected, so we
|
||||
fall back to a pose-only block. Either way pose + yaw still stream.
|
||||
"""
|
||||
self._stop_estimator_logging()
|
||||
core = [("stateEstimate.x", "float"), ("stateEstimate.y", "float"),
|
||||
("stateEstimate.z", "float"), ("stabilizer.yaw", "float")]
|
||||
flow = [("motion.deltaX", "int16_t"), ("motion.deltaY", "int16_t")]
|
||||
for variables, has_flow in ((core + flow, True), (core, False)):
|
||||
lg = LogConfig(name="estimator", period_in_ms=200)
|
||||
for name, vtype in variables:
|
||||
lg.add_variable(name, vtype)
|
||||
try:
|
||||
self._cf.log.add_config(lg)
|
||||
lg.data_received_cb.add_callback(self._on_estimator_data)
|
||||
lg.start()
|
||||
self._est_log = lg
|
||||
self._est_has_flow = has_flow
|
||||
logger.info("estimator logging started (flow=%s)", has_flow)
|
||||
return
|
||||
except (KeyError, AttributeError) as exc:
|
||||
logger.info("estimator block %s unavailable: %s",
|
||||
"with flow" if has_flow else "pose-only", exc)
|
||||
self._est_log = None
|
||||
|
||||
def _stop_estimator_logging(self) -> None:
|
||||
if self._est_log is not None:
|
||||
try:
|
||||
self._est_log.stop()
|
||||
self._est_log.delete()
|
||||
except Exception:
|
||||
pass
|
||||
self._est_log = None
|
||||
|
||||
def _on_estimator_data(self, timestamp, data, logconf) -> None:
|
||||
# Build a fresh immutable snapshot and publish it atomically (single
|
||||
# reference assignment under the GIL — no lock needed).
|
||||
self._estimator = EstimatorState(
|
||||
valid=True,
|
||||
has_flow=self._est_has_flow,
|
||||
x=data.get("stateEstimate.x", 0.0),
|
||||
y=data.get("stateEstimate.y", 0.0),
|
||||
z=data.get("stateEstimate.z", 0.0),
|
||||
yaw=data.get("stabilizer.yaw", 0.0),
|
||||
dx=int(data.get("motion.deltaX", 0)),
|
||||
dy=int(data.get("motion.deltaY", 0)),
|
||||
)
|
||||
|
||||
|
||||
def _clamp_pwm(value) -> int:
|
||||
return max(0, min(PWM_MAX, int(value)))
|
||||
Reference in New Issue
Block a user