diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 12a1f73..0c111c6 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -23,7 +23,7 @@ jobs: - name: Set up Python uses: actions/setup-python@v5 with: - python-version: "3.11" + python-version: "3.12" cache: "pip" cache-dependency-path: simulation/requirements.txt diff --git a/AGENTS.md b/AGENTS.md index b815588..33fb754 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -189,6 +189,10 @@ BLHeli backend and drives output 9 as plain PWM — see `design/sitl_testing.md` exist only to work around circular imports are a sign of bad architecture — fix the circular dependency by refactoring (e.g. extract a shared module, invert the dependency) rather than papering over it with a local import. +- Python 3.12+ is the project floor and should be treated as the baseline. Prefer modern Python + features when they improve clarity and reduce boilerplate (for example `match`/`case`, `X | Y` + union types, and 3.12 generic/type-alias syntax) rather than avoiding them for backward- + compatibility with older interpreters. - `/tmp` is NOT one shared filesystem on this box — Git Bash and WSL2 (used for `docker`/`test.sh stack`) each have their own separate `/tmp`, and native Windows executables invoked from Git Bash can't resolve `/tmp/...` paths at all. Full diff --git a/calibrate/constants.py b/calibrate/constants.py index 4769dcf..23f8102 100644 --- a/calibrate/constants.py +++ b/calibrate/constants.py @@ -12,7 +12,24 @@ import simulation # Re-exported so submodules can do `from .constants import RawesGCS` etc. -from groundstation.gcs import RawesGCS, WallClock +from groundstation.gcs import ( + Attitude, + EscTelemetry, + PidTuning, + BatteryStatus, + RawesGCS, + RcChannels, + SetAttitudeTarget, + SysStatus, + WallClock, + CommandAck, + Heartbeat, + NamedValueFloat, + CommandLong, + decode_message, + RequestDataStream, + StatusText, +) from simulation.param_defaults import load_ap_params from simulation.servo_pwm import (SWASH_PWM_MIN, SWASH_PWM_NEUTRAL, SWASH_PWM_MAX, MOTOR_PWM_MIN, MOTOR_PWM_MAX) diff --git a/calibrate/hw.py b/calibrate/hw.py index ad7792c..f19fa05 100644 --- a/calibrate/hw.py +++ b/calibrate/hw.py @@ -6,11 +6,23 @@ import math import time +from typing import cast from pymavlink import mavutil +from groundstation.gcs import MavConnectionLike from .constants import ( RawesGCS, + CommandAck, + EscTelemetry, + PidTuning, + RequestDataStream, + RcChannels, + CommandLong, + decode_message, + Heartbeat, + SetAttitudeTarget, + StatusText, GB4008_KV, GB4008_POLE_PAIRS, GB4008_KT, GB4008_GEAR_RATIO, SERVO_S1, SERVO_S2, SERVO_S3, SERVO_MOTOR, MOTOR_OFF_US, MOTOR_FULL_US, MOTOR_ESC_CHANNEL, @@ -47,6 +59,12 @@ def _esc_telem_msg_for_channel(channel: int) -> "tuple[str, int]": def _esc_erpm(msg, channel: int) -> "float | None": """eRPM for 1-based output `channel` from an ESC_TELEMETRY_* msg, else None.""" + if isinstance(msg, EscTelemetry): + base = msg.first_channel + idx = channel - base + if msg.rpm is None or not (0 <= idx < len(msg.rpm)): + return None + return msg.rpm[idx] info = _ESC_TELEM_MSGS.get(msg.get_type()) if info is None: return None @@ -119,15 +137,14 @@ def _norm_to_pwm(v: float) -> int: def _send_set_servo(session: RawesGCS, instance: int, pwm: int) -> None: """Send MAV_CMD_DO_SET_SERVO (works while disarmed).""" - session._mav.mav.command_long_send( - session._target_system, - session._target_component, - mavutil.mavlink.MAV_CMD_DO_SET_SERVO, - 0, # confirmation - float(instance), - float(pwm), - 0, 0, 0, 0, 0, - ) + session.send_message(CommandLong( + target_system=session._target_system, + target_component=session._target_component, + command=mavutil.mavlink.MAV_CMD_DO_SET_SERVO, + confirmation=0, + param1=float(instance), + param2=float(pwm), + )) def _send_motor_test(session: RawesGCS, instance: int, @@ -139,17 +156,16 @@ def _send_motor_test(session: RawesGCS, instance: int, throttle_pct : 0-100 (MOTOR_TEST_THROTTLE_PERCENT = 0) timeout_s : test duration; 0 = run until next command """ - session._mav.mav.command_long_send( - session._target_system, - session._target_component, - mavutil.mavlink.MAV_CMD_DO_MOTOR_TEST, - 0, - float(instance), # param1: motor instance - 0.0, # param2: throttle type 0 = PERCENT - float(throttle_pct), # param3: throttle value - float(timeout_s), # param4: test duration [s] - 0, 0, 0, - ) + session.send_message(CommandLong( + target_system=session._target_system, + target_component=session._target_component, + command=mavutil.mavlink.MAV_CMD_DO_MOTOR_TEST, + confirmation=0, + param1=float(instance), + param2=0.0, + param3=float(throttle_pct), + param4=float(timeout_s), + )) # --------------------------------------------------------------------------- @@ -226,7 +242,12 @@ def _print_status(session: RawesGCS) -> None: print(f"\n{sep}") print("SERVO OUTPUTS") print(sep) - session.request_stream(mavutil.mavlink.MAV_DATA_STREAM_RC_CHANNELS, 10) + session.send_message(RequestDataStream( + target_system=session._target_system, + target_component=session._target_component, + req_stream_id=mavutil.mavlink.MAV_DATA_STREAM_RC_CHANNELS, + req_message_rate=10, + )) srv = session._recv(type="SERVO_OUTPUT_RAW", blocking=True, timeout=2.0) if srv: for i in range(1, 13): @@ -358,9 +379,9 @@ def _restart_scripting(session: RawesGCS) -> None: def _probe_port(port: str, baud: int, timeout: float) -> tuple: """Try one port at one baud. Returns (ok, sysid) — closes connection before returning.""" - conn = None + conn: MavConnectionLike | None = None try: - conn = mavutil.mavlink_connection(port, baud=baud, autoreconnect=False) + conn = cast(MavConnectionLike, mavutil.mavlink_connection(port, baud=baud, autoreconnect=False)) hb = conn.wait_heartbeat(timeout=timeout) if hb: return True, conn.target_system @@ -447,7 +468,13 @@ def _monitor_esc(session: RawesGCS, duration: float = 10.0) -> None: esc_name, esc_id = _esc_telem_msg_for_channel(MOTOR_ESC_CHANNEL) idx = (MOTOR_ESC_CHANNEL - 1) % 4 - session.set_message_interval(esc_id, 100000) # 10 Hz + session.send_message(CommandLong( + target_system=session._target_system, + target_component=session._target_component, + command=mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL, + param1=float(esc_id), + param2=100000.0, + )) # 10 Hz deadline = time.monotonic() + duration last_print = 0.0 try: @@ -495,14 +522,14 @@ def _arm(session: RawesGCS, force: bool = False, """ print(" Sending arm command ...") param2 = 21196.0 if force else 0.0 - session._mav.mav.command_long_send( - session._target_system, session._target_component, - mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, - 0, - 1.0, # param1: 1 = arm - param2, # param2: 21196 = force-arm - 0, 0, 0, 0, 0, - ) + session.send_message(CommandLong( + target_system=session._target_system, + target_component=session._target_component, + command=mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, + confirmation=0, + param1=1.0, + param2=param2, + )) deadline = time.monotonic() + timeout armed = False @@ -513,20 +540,20 @@ def _arm(session: RawesGCS, force: bool = False, ) if msg is None: continue - t = msg.get_type() - if t == "STATUSTEXT": - print(f" [FC] {msg.text.rstrip()}") - elif t == "COMMAND_ACK" and msg.command == mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM: - if msg.result == mavutil.mavlink.MAV_RESULT_ACCEPTED: - print(" Arm command accepted -- waiting for armed heartbeat ...") - elif msg.result == mavutil.mavlink.MAV_RESULT_DENIED: - print(" [FAIL] Arm denied -- check pre-arm messages above") - return False - elif t == "HEARTBEAT": - if bool(msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED): - print(" [OK] Vehicle armed.") - armed = True - break + match decode_message(msg): + case StatusText(text=text): + print(f" [FC] {text}") + case CommandAck(command=command, result=result) if command == mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM: + if result == mavutil.mavlink.MAV_RESULT_ACCEPTED: + print(" Arm command accepted -- waiting for armed heartbeat ...") + elif result == mavutil.mavlink.MAV_RESULT_DENIED: + print(" [FAIL] Arm denied -- check pre-arm messages above") + return False + case Heartbeat(base_mode=base_mode): + if bool(base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED): + print(" [OK] Vehicle armed.") + armed = True + break if not armed: print(" [FAIL] Arm timed out.") @@ -544,14 +571,14 @@ def _disarm(session: RawesGCS, timeout: float = 10.0, """Send disarm command. Returns True if vehicle confirms disarmed.""" print(" Sending disarm command ...") param2 = 21196.0 if force else 0.0 - session._mav.mav.command_long_send( - session._target_system, session._target_component, - mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, - 0, - 0.0, # param1: 0 = disarm - param2, # param2: 21196 = force-disarm - 0, 0, 0, 0, 0, - ) + session.send_message(CommandLong( + target_system=session._target_system, + target_component=session._target_component, + command=mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, + confirmation=0, + param1=0.0, + param2=param2, + )) deadline = time.monotonic() + timeout ack_seen = False while time.monotonic() < deadline: @@ -561,36 +588,31 @@ def _disarm(session: RawesGCS, timeout: float = 10.0, ) if msg is None: continue - - t = msg.get_type() - if t == "STATUSTEXT": - print(f" [FC] {msg.text.rstrip()}") - continue - - if t == "COMMAND_ACK" and msg.command == mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM: - ack_seen = True - result = int(msg.result) - if result == mavutil.mavlink.MAV_RESULT_ACCEPTED: - print(" Disarm command accepted -- waiting for disarmed heartbeat ...") - elif result == mavutil.mavlink.MAV_RESULT_DENIED: - print(" [FAIL] Disarm denied by FC.") - return False - elif result == mavutil.mavlink.MAV_RESULT_TEMPORARILY_REJECTED: - print(" [FAIL] Disarm temporarily rejected by FC.") - return False - elif result == mavutil.mavlink.MAV_RESULT_UNSUPPORTED: - print(" [FAIL] Disarm unsupported by FC.") - return False - elif result == mavutil.mavlink.MAV_RESULT_FAILED: - print(" [FAIL] Disarm failed on FC.") - return False - continue - - if t == "HEARTBEAT": - armed = bool(msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) - if not armed: - print(" [OK] Vehicle disarmed.") - return True + match decode_message(msg): + case StatusText(text=text): + print(f" [FC] {text}") + continue + case CommandAck(command=command, result=result) if command == mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM: + ack_seen = True + if result == mavutil.mavlink.MAV_RESULT_ACCEPTED: + print(" Disarm command accepted -- waiting for disarmed heartbeat ...") + elif result == mavutil.mavlink.MAV_RESULT_DENIED: + print(" [FAIL] Disarm denied by FC.") + return False + elif result == mavutil.mavlink.MAV_RESULT_TEMPORARILY_REJECTED: + print(" [FAIL] Disarm temporarily rejected by FC.") + return False + elif result == mavutil.mavlink.MAV_RESULT_UNSUPPORTED: + print(" [FAIL] Disarm unsupported by FC.") + return False + elif result == mavutil.mavlink.MAV_RESULT_FAILED: + print(" [FAIL] Disarm failed on FC.") + return False + continue + case Heartbeat(base_mode=base_mode): + if not bool(base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED): + print(" [OK] Vehicle disarmed.") + return True if ack_seen: print(" [FAIL] Disarm timed out waiting for disarmed heartbeat.") diff --git a/calibrate/repl.py b/calibrate/repl.py index 28a28e1..880f7e6 100644 --- a/calibrate/repl.py +++ b/calibrate/repl.py @@ -13,7 +13,7 @@ from pymavlink import mavutil from .constants import ( - RawesGCS, WallClock, + RawesGCS, WallClock, CommandLong, RequestDataStream, SERVO_S1, SERVO_S2, SERVO_S3, SERVO_MOTOR, MOTOR_OFF_US, MOTOR_FULL_US, MOTOR_ESC_CHANNEL, SWASH_SERVOS, @@ -222,11 +222,13 @@ def _cmd_ping(args: list[str]) -> None: def _cmd_reboot(session: RawesGCS) -> None: print(" Sending reboot command ...") - session._mav.mav.command_long_send( - session._target_system, session._target_component, - mavutil.mavlink.MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, - 0, 1, 0, 0, 0, 0, 0, 0, - ) + session.send_message(CommandLong( + target_system=session._target_system, + target_component=session._target_component, + command=mavutil.mavlink.MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, + confirmation=0, + param1=1, + )) print(" Pixhawk rebooting -- reconnect in ~5 s.") @@ -347,7 +349,12 @@ def _factors(az): print(" tlat > 0 = roll right; tlon > 0 = nose-DOWN disk; col > 0 = positive thrust") print() # Live PWMs - session.request_stream(mavutil.mavlink.MAV_DATA_STREAM_RC_CHANNELS, 10) + session.send_message(RequestDataStream( + target_system=session._target_system, + target_component=session._target_component, + req_stream_id=mavutil.mavlink.MAV_DATA_STREAM_RC_CHANNELS, + req_message_rate=10, + )) srv = session._recv(type="SERVO_OUTPUT_RAW", blocking=True, timeout=2.0) if srv: s1 = getattr(srv, "servo1_raw", 0) @@ -823,7 +830,12 @@ def _connect(port: "str | None", baud: int) -> RawesGCS: session.connect(timeout=15.0) print(f"Connected: sysid={session._target_system} compid={session._target_component}") session.start_heartbeat() - session.request_stream(mavutil.mavlink.MAV_DATA_STREAM_RAW_CONTROLLER, 10) + session.send_message(RequestDataStream( + target_system=session._target_system, + target_component=session._target_component, + req_stream_id=mavutil.mavlink.MAV_DATA_STREAM_RAW_CONTROLLER, + req_message_rate=10, + )) _refresh_pole_pairs(session) return session diff --git a/calibrate/run.py b/calibrate/run.py index af7ef84..f3d85cc 100644 --- a/calibrate/run.py +++ b/calibrate/run.py @@ -10,6 +10,7 @@ from datetime import datetime, timezone from pymavlink import mavutil +from groundstation.gcs import decode_message # msvcrt is Windows stdlib -- used for non-blocking ESC-key abort. # Falls back to a stub on non-Windows so the rest of the module still imports. @@ -22,7 +23,17 @@ def getch(self): return b"" msvcrt = _MsvcrtStub() # type: ignore[assignment] from .constants import ( + Attitude, + Heartbeat, + EscTelemetry, + PidTuning, + RcChannels, RawesGCS, + NamedValueFloat, + CommandLong, + RequestDataStream, + SetAttitudeTarget, + StatusText, SERVO_MOTOR, MOTOR_OFF_US, MOTOR_ESC_CHANNEL, _ESC_TELEM_MSGS, _RUN_MODES, _TRIM_NVF, _IC_TRIM_KEYS, _PASSIVE_IC_THRUST, @@ -56,10 +67,11 @@ def _wait_for_armed(session: RawesGCS, timeout_s: float = 15.0) -> bool: blocking=True, timeout=0.5) if msg is None: continue - if msg.get_type() == "STATUSTEXT": - print(f" [FC] {msg.text.rstrip()}") - elif msg.get_type() == "HEARTBEAT": - if bool(msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED): + decoded = decode_message(msg) + if isinstance(decoded, StatusText): + print(f" [FC] {decoded.text}") + elif isinstance(decoded, Heartbeat): + if bool(decoded.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED): return True return False @@ -178,7 +190,12 @@ def _observation_loop(session: RawesGCS, *, Returns (n_rows, aborted). """ for s_id, hz in streams: - session.request_stream(s_id, hz) + session.send_message(RequestDataStream( + target_system=session._target_system, + target_component=session._target_component, + req_stream_id=s_id, + req_message_rate=hz, + )) if setup_hook is not None: setup_hook() @@ -214,15 +231,16 @@ def _observation_loop(session: RawesGCS, *, if on_tick is not None: on_tick(t_rel) if msg is not None: - if msg.get_type() == "HEARTBEAT": - state["armed"] = bool(msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) - elif msg.get_type() == "STATUSTEXT": + decoded = decode_message(msg) + if isinstance(decoded, Heartbeat): + state["armed"] = bool(decoded.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) + elif isinstance(decoded, StatusText): if not suppress_status: - text = msg.text.rstrip("\x00").strip() + text = decoded.text if text: state["pending_text"].append(text) else: - row = handle_msg(state, msg, t_rel) + row = handle_msg(state, decoded, t_rel) if row is not None and log is not None: log.row(row) if t_rel - last_print >= print_period_s: @@ -260,9 +278,9 @@ def _tick(t_rel: float) -> None: return last_step[0] = idx tlon_d, tlat_d, thr_d, label = steps[idx] - session.send_named_float("RAWES_TLN", math.radians(tlon_d)) - session.send_named_float("RAWES_TLT", math.radians(tlat_d)) - session.send_named_float("RAWES_THR", float(thr_d)) + session.send_message(NamedValueFloat("RAWES_TLN", math.radians(tlon_d))) + session.send_message(NamedValueFloat("RAWES_TLT", math.radians(tlat_d))) + session.send_message(NamedValueFloat("RAWES_THR", float(thr_d))) print(f" [{t_rel:6.1f}s] osc {idx+1}/{len(steps)} " f"tlon={tlon_d:+5.1f} tlat={tlat_d:+5.1f} thr={thr_d:.3f} " f"({label})") @@ -437,8 +455,7 @@ def _quat_to_rpy_deg(q) -> tuple[float, float, float] | tuple[None, None, None]: return roll, pitch, yaw def handle_msg(st, msg, t_rel): - mt = msg.get_type() - if mt == "ATTITUDE": + if isinstance(msg, Attitude): state["roll"] = math.degrees(msg.roll) state["pitch"] = math.degrees(msg.pitch) state["yaw"] = math.degrees(msg.yaw) @@ -465,21 +482,21 @@ def handle_msg(st, msg, t_rel): _fmt(state["pid_yaw_des"]), _fmt(state["pid_yaw_ach"]), _fmt(state["pid_yaw_ff"]), _fmt(state["pid_yaw_p"]), _fmt(state["pid_yaw_i"]), _fmt(state["pid_yaw_d"]), _fmt(_erpm), _fmt(_mech), _fmt(_rotor), ] - elif mt == "ATTITUDE_TARGET": - att_r, att_p, att_y = _quat_to_rpy_deg(getattr(msg, "q", None)) + if isinstance(msg, SetAttitudeTarget): + att_r, att_p, att_y = _quat_to_rpy_deg(msg.q) state["att_target_roll"] = att_r state["att_target_pitch"] = att_p state["att_target_yaw"] = att_y - state["att_target_roll_rate"] = getattr(msg, "body_roll_rate", None) - state["att_target_pitch_rate"] = getattr(msg, "body_pitch_rate", None) - state["att_target_yaw_rate"] = getattr(msg, "body_yaw_rate", None) - state["att_target_thrust"] = getattr(msg, "thrust", None) - elif mt == "RC_CHANNELS": - state["ch1"] = getattr(msg, "chan1_raw", None) - state["ch2"] = getattr(msg, "chan2_raw", None) - state["ch3"] = getattr(msg, "chan3_raw", None) - state["ch4"] = getattr(msg, "chan4_raw", None) - elif mt == "SERVO_OUTPUT_RAW": + state["att_target_roll_rate"] = msg.body_roll_rate + state["att_target_pitch_rate"] = msg.body_pitch_rate + state["att_target_yaw_rate"] = msg.body_yaw_rate + state["att_target_thrust"] = msg.thrust + elif isinstance(msg, RcChannels): + state["ch1"] = msg.chan1_raw + state["ch2"] = msg.chan2_raw + state["ch3"] = msg.chan3_raw + state["ch4"] = msg.chan4_raw + elif hasattr(msg, "servo1_raw"): state["s1"] = getattr(msg, "servo1_raw", None) state["s2"] = getattr(msg, "servo2_raw", None) state["s3"] = getattr(msg, "servo3_raw", None) @@ -490,18 +507,18 @@ def handle_msg(st, msg, t_rel): cutoff = t_rel - mot_window_s while state["smot_hist"] and state["smot_hist"][0][0] < cutoff: state["smot_hist"].pop(0) - elif mt == "BATTERY_STATUS": + elif hasattr(msg, "voltages"): cells = [v for v in msg.voltages if v != 65535] if cells: state["vbat"] = sum(cells) / 1000.0 if msg.current_battery >= 0: state["curr"] = msg.current_battery / 100.0 - elif mt == "SYS_STATUS": + elif hasattr(msg, "voltage_battery"): if state["vbat"] is None and msg.voltage_battery != 65535: state["vbat"] = msg.voltage_battery / 1000.0 if state["curr"] is None and msg.current_battery >= 0: state["curr"] = msg.current_battery / 100.0 - elif mt in _ESC_TELEM_MSGS: + elif isinstance(msg, EscTelemetry): erpm = _esc_erpm(msg, MOTOR_ESC_CHANNEL) if erpm is not None: state["erpm"] = erpm @@ -511,9 +528,8 @@ def handle_msg(st, msg, t_rel): cutoff = t_rel - mot_window_s while state["mrpm_hist"] and state["mrpm_hist"][0][0] < cutoff: state["mrpm_hist"].pop(0) - elif mt == "NAMED_VALUE_FLOAT": - nm = msg.name.rstrip("\x00").strip() if isinstance(msg.name, str) else \ - msg.name.decode("ascii", errors="replace").rstrip("\x00").strip() + elif isinstance(msg, NamedValueFloat): + nm = msg.name if nm == "YFF_T": state["yff_t"] = float(msg.value) state["yff_t_ts"] = t_rel @@ -545,17 +561,17 @@ def handle_msg(st, msg, t_rel): state["ol_col"] = float(msg.value) elif nm == "OL_TEN": state["ol_ten"] = float(msg.value) - elif mt == "PID_TUNING": - axis = int(getattr(msg, "axis", -1)) + elif isinstance(msg, PidTuning): + axis = msg.axis # ArduPilot emits PID_TUNING axis as 1=roll, 2=pitch, 3=yaw, 4=accelz. prefix = {1: "pid_roll", 2: "pid_pitch", 3: "pid_yaw"}.get(axis) if prefix is not None: - state[f"{prefix}_des"] = getattr(msg, "desired", None) - state[f"{prefix}_ach"] = getattr(msg, "achieved", None) - state[f"{prefix}_ff"] = getattr(msg, "FF", None) - state[f"{prefix}_p"] = getattr(msg, "P", None) - state[f"{prefix}_i"] = getattr(msg, "I", None) - state[f"{prefix}_d"] = getattr(msg, "D", None) + state[f"{prefix}_des"] = msg.desired + state[f"{prefix}_ach"] = msg.achieved + state[f"{prefix}_ff"] = msg.FF + state[f"{prefix}_p"] = msg.P + state[f"{prefix}_i"] = msg.I + state[f"{prefix}_d"] = msg.D return None def render_row(st, t_rel): @@ -595,11 +611,22 @@ def _trim_streams() -> None: # over the RC_CHANNELS stream (which we keep for SERVO_OUTPUT_RAW). # Also request the motor's ESC telemetry (bidir DShot RPM) at 5 Hz. _esc_name, _esc_id = _esc_telem_msg_for_channel(MOTOR_ESC_CHANNEL) - session.set_message_interval(_esc_id, 200000) # 5 Hz + session.send_message(CommandLong( + target_system=session._target_system, + target_component=session._target_component, + command=mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL, + param1=float(_esc_id), + param2=200000.0, + )) # 5 Hz if not keep_rc: - session.set_message_interval( - mavutil.mavlink.MAVLINK_MSG_ID_RC_CHANNELS, -1) + session.send_message(CommandLong( + target_system=session._target_system, + target_component=session._target_component, + command=mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL, + param1=float(mavutil.mavlink.MAVLINK_MSG_ID_RC_CHANNELS), + param2=-1.0, + )) print(" Stream trim: RC_CHANNELS off, AHRS2 off, EXTENDED_STATUS 1 Hz " f"(use --rc to keep RC_CHANNELS); {_esc_name} 5 Hz") else: @@ -632,9 +659,25 @@ def _trim_streams() -> None: ) # Restore telemetry the trim disabled (best-effort; resets on FC reboot). if not keep_rc: - session.set_message_interval(mavutil.mavlink.MAVLINK_MSG_ID_RC_CHANNELS, 0) - session.request_stream(mavutil.mavlink.MAV_DATA_STREAM_EXTENDED_STATUS, 2) - session.request_stream(mavutil.mavlink.MAV_DATA_STREAM_EXTRA3, 2) + session.send_message(CommandLong( + target_system=session._target_system, + target_component=session._target_component, + command=mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL, + param1=float(mavutil.mavlink.MAVLINK_MSG_ID_RC_CHANNELS), + param2=0.0, + )) + session.send_message(RequestDataStream( + target_system=session._target_system, + target_component=session._target_component, + req_stream_id=mavutil.mavlink.MAV_DATA_STREAM_EXTENDED_STATUS, + req_message_rate=2, + )) + session.send_message(RequestDataStream( + target_system=session._target_system, + target_component=session._target_component, + req_stream_id=mavutil.mavlink.MAV_DATA_STREAM_EXTRA3, + req_message_rate=2, + )) # Report final H_YAW_TRIM. _tv = session.get_param("H_YAW_TRIM") @@ -755,7 +798,7 @@ def _cmd_run(session: RawesGCS, args: list[str]) -> None: if cfg.get("ic_seed"): thr = float(trim.get("thr", _PASSIVE_IC_THRUST)) print(" Seeding IC:") - session.send_named_float("RAWES_THR", thr) + session.send_message(NamedValueFloat("RAWES_THR", thr)) print(f" RAWES_THR = {thr:.3f} (thrust [0..1])") if flags.get("--hold"): @@ -767,7 +810,7 @@ def _cmd_run(session: RawesGCS, args: list[str]) -> None: print(" [WARN] --hold ignores --roll/--pitch (captures current AHRS attitude instead)") if "--yaw" in flags: print(" [WARN] --hold ignores --yaw (captures current AHRS attitude instead)") - session.send_named_float("RAWES_YIC", _RAWES_YIC_CAPTURE_SENTINEL) + session.send_message(NamedValueFloat("RAWES_YIC", _RAWES_YIC_CAPTURE_SENTINEL)) print(f" RAWES_YIC = {_RAWES_YIC_CAPTURE_SENTINEL:.1f} (capture current roll/pitch/yaw)") else: roll_deg = float(flags.get("--roll", 0.0)) @@ -775,14 +818,14 @@ def _cmd_run(session: RawesGCS, args: list[str]) -> None: # RAWES IC seed commits atomically only after THR+RIC+PIC all arrive. # Always send roll/pitch (default 0 deg) so PASSIVE does not stall at # "ic=waiting" when only --trim thr is provided. - session.send_named_float("RAWES_RIC", math.radians(roll_deg)) + session.send_message(NamedValueFloat("RAWES_RIC", math.radians(roll_deg))) print(f" RAWES_RIC = {roll_deg:+7.3f} deg ({math.radians(roll_deg):+.4f} rad)") - session.send_named_float("RAWES_PIC", math.radians(pitch_deg)) + session.send_message(NamedValueFloat("RAWES_PIC", math.radians(pitch_deg))) print(f" RAWES_PIC = {pitch_deg:+7.3f} deg ({math.radians(pitch_deg):+.4f} rad)") if "--yaw" in flags: yaw_deg = float(flags["--yaw"]) - session.send_named_float("RAWES_YIC", math.radians(yaw_deg)) + session.send_message(NamedValueFloat("RAWES_YIC", math.radians(yaw_deg))) print(f" RAWES_YIC = {yaw_deg:+7.3f} deg ({math.radians(yaw_deg):+.4f} rad)") # thr was consumed by the IC seed -- don't re-send it via the trim block. trim.pop("thr", None) @@ -793,7 +836,7 @@ def _cmd_run(session: RawesGCS, args: list[str]) -> None: print(" Sending trim NVFs (deg -> rad on the wire):") for k, v_deg in trim.items(): v_rad = math.radians(float(v_deg)) - session.send_named_float(_TRIM_NVF[k], v_rad) + session.send_message(NamedValueFloat(_TRIM_NVF[k], v_rad)) print(f" {_TRIM_NVF[k]} = {v_deg:+7.3f} deg ({v_rad:+.4f} rad)") # Arm. Passive mode keeps channel 4 under AP/Lua tail ownership, so skip diff --git a/calibrate/watch.py b/calibrate/watch.py index e683f7b..a5d41f1 100644 --- a/calibrate/watch.py +++ b/calibrate/watch.py @@ -10,7 +10,14 @@ from pymavlink import mavutil from .constants import ( + Attitude, + BatteryStatus, + EscTelemetry, RawesGCS, + CommandLong, + SysStatus, + StatusText, + decode_message, MOTOR_ESC_CHANNEL, SERVO_MOTOR, _WATCH_STREAMS, ) @@ -83,10 +90,10 @@ def _watch_servos(session, duration, log): state = {f"s{i}": None for i in range(1, 9)} def handle(st, msg, t_rel): - mt = msg.get_type() - if mt == "SERVO_OUTPUT_RAW": + decoded = decode_message(msg) + if hasattr(decoded, "servo1_raw"): for i in range(1, 9): - state[f"s{i}"] = getattr(msg, f"servo{i}_raw", None) + state[f"s{i}"] = getattr(decoded, f"servo{i}_raw", None) return [f"{t_rel:.4f}"] + [state[f"s{i}"] for i in range(1, 9)] return None @@ -112,12 +119,13 @@ def _watch_esc(session, duration, log): state = {"erpm": None, "volt": None, "curr": None, "temp": None} def handle(st, msg, t_rel): - if msg.get_type() != esc_name: + decoded = decode_message(msg) + if not isinstance(decoded, EscTelemetry) or decoded.message_name != esc_name: return None - erpm = _esc_erpm(msg, MOTOR_ESC_CHANNEL) - volt = msg.voltage[idx] / 100.0 if hasattr(msg, "voltage") else None - curr = msg.current[idx] / 100.0 if hasattr(msg, "current") else None - temp = msg.temperature[idx] if hasattr(msg, "temperature") else None + erpm = _esc_erpm(decoded, MOTOR_ESC_CHANNEL) + volt = decoded.voltage[idx] / 100.0 if idx < len(decoded.voltage) else None + curr = decoded.current[idx] / 100.0 if idx < len(decoded.current) else None + temp = decoded.temperature[idx] if idx < len(decoded.temperature) else None state["erpm"], state["volt"], state["curr"], state["temp"] = \ erpm, volt, curr, temp _e, mech, rotor = _rpm_triplet(erpm) @@ -137,7 +145,13 @@ def render(st, t_rel): ] def _req_esc() -> None: - session.set_message_interval(esc_id, 100000) # 10 Hz + session.send_message(CommandLong( + target_system=session._target_system, + target_component=session._target_component, + command=mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL, + param1=float(esc_id), + param2=100000.0, + )) # 10 Hz _observation_loop( session, duration_s=duration, @@ -157,8 +171,9 @@ def _watch_text(session, duration, log): def handle(st, msg, t_rel): # STATUSTEXTs are handled by the engine and surfaced via state["pending_text"] # We still log them here so the CSV captures everything. - if msg.get_type() == "STATUSTEXT": - text = msg.text.rstrip("\x00").strip() + decoded = decode_message(msg) + if isinstance(decoded, StatusText): + text = decoded.text if text: return [f"{t_rel:.4f}", int(getattr(msg, "severity", 6)), text] return None @@ -185,13 +200,14 @@ def _watch_attitude(session, duration, log): "wx": None, "wy": None, "wz": None} def handle(st, msg, t_rel): - if msg.get_type() == "ATTITUDE": - state["roll"] = math.degrees(msg.roll) - state["pitch"] = math.degrees(msg.pitch) - state["yaw"] = math.degrees(msg.yaw) - state["wx"] = msg.rollspeed - state["wy"] = msg.pitchspeed - state["wz"] = msg.yawspeed + decoded = decode_message(msg) + if isinstance(decoded, Attitude): + state["roll"] = math.degrees(decoded.roll) + state["pitch"] = math.degrees(decoded.pitch) + state["yaw"] = math.degrees(decoded.yaw) + state["wx"] = decoded.rollspeed + state["wy"] = decoded.pitchspeed + state["wz"] = decoded.yawspeed return [f"{t_rel:.4f}", f"{state['roll']:.3f}", f"{state['pitch']:.3f}", f"{state['yaw']:.3f}", f"{state['wx']:.4f}", f"{state['wy']:.4f}", f"{state['wz']:.4f}"] @@ -222,20 +238,20 @@ def _watch_power(session, duration, log): state = {"vbat": None, "curr": None} def handle(st, msg, t_rel): - mt = msg.get_type() - if mt == "BATTERY_STATUS": - cells = [v for v in msg.voltages if v != 65535] - if cells: - state["vbat"] = sum(cells) / 1000.0 - if msg.current_battery >= 0: - state["curr"] = msg.current_battery / 100.0 - elif mt == "SYS_STATUS": - if state["vbat"] is None and msg.voltage_battery != 65535: - state["vbat"] = msg.voltage_battery / 1000.0 - if state["curr"] is None and msg.current_battery >= 0: - state["curr"] = msg.current_battery / 100.0 - else: - return None + match decode_message(msg): + case BatteryStatus(voltages=voltages, current_battery=current_battery): + cells = [v for v in voltages if v != 65535] + if cells: + state["vbat"] = sum(cells) / 1000.0 + if current_battery >= 0: + state["curr"] = current_battery / 100.0 + case SysStatus(voltage_battery=voltage_battery, current_battery=current_battery): + if state["vbat"] is None and voltage_battery != 65535: + state["vbat"] = voltage_battery / 1000.0 + if state["curr"] is None and current_battery >= 0: + state["curr"] = current_battery / 100.0 + case _: + return None power = state["vbat"] * state["curr"] if (state["vbat"] is not None and state["curr"] is not None) else None return [f"{t_rel:.4f}", _fmt(state["vbat"]), _fmt(state["curr"]), _fmt(power)] diff --git a/design/flight_stack.md b/design/flight_stack.md index eca9aa9..82f2536 100644 --- a/design/flight_stack.md +++ b/design/flight_stack.md @@ -337,7 +337,7 @@ Sections 4.2–4.5 give the per-mode detail and gain values. All other flight tunables (anchor position, slew rate, cyclic gains) are delivered as NAMED_VALUE_FLOATs — see table below. -**Named float inputs (ground → Lua, via `gcs.send_named_float`):** +**Named float inputs (ground → Lua, via `gcs.send_message(NamedValueFloat(...))`):** | Name | Value | Purpose | |---|---|---| @@ -349,7 +349,7 @@ All other flight tunables (anchor position, slew rate, cyclic gains) are deliver | RAWES_PIC | rad | IC pitch — part of the atomic IC seed. MODE_PASSIVE commands it as the GUIDED pitch angle target. | | RAWES_THR | [0..1] | IC thrust — part of the atomic IC seed. MODE_PASSIVE maps it directly to GUIDED throttle to preserve rotor RPM during kinematic. | -**Named int inputs (ground → Lua, via `gcs.send_named_int`, one-shot anchor location):** +**Named int inputs (ground → Lua, via `gcs.send_message(NamedValueInt(...))`, one-shot anchor location):** | Name | Value | Purpose | |---|---|---| @@ -1063,7 +1063,7 @@ level first. | `simulation/torque_model.py` | Hub yaw kinematics: `HubParams`, `HubState`, `step()`, `equilibrium_throttle()` | | `simulation/mediator_torque.py` | Standalone torque SITL mediator | | `simulation/comms.py` | `VirtualComms` (simtest-only comms link) | -| `groundstation/gcs.py` | `RawesGCS` MAVLink client: arm, mode, params, `send_named_float` | +| `groundstation/gcs.py` | `RawesGCS` MAVLink client: arm, mode, params, `send_message`, message dataclasses (`NamedValueFloat`, ...) | | `simulation/sensor.py` | `PhysicalSensor` — honest NED sensors (accel, gyro, vel) | | `analysis/analyse_run.py` | Post-run report: physics + EKF/GPS + attitude per time bucket | | `analysis/analyse_landing.py` | Landing diagnosis: alt/vz/winch/tension/collective per bucket | diff --git a/design/sitl_flight_timeline.md b/design/sitl_flight_timeline.md index f8593f0..dcc899c 100644 --- a/design/sitl_flight_timeline.md +++ b/design/sitl_flight_timeline.md @@ -85,7 +85,7 @@ Use this as the compact reference table for this run. | 0.000 | observed | Mediator startup and 60 s kinematic profile start | `events.jsonl: startup`, `events.jsonl: kinematic_config.total_s=60` | | 8.860-8.960 | observed | EKF yaw aligned and GPS reaches 3D fix (`fix_type=6`) | `STATUSTEXT "EKF3 IMU0 yaw aligned"`, `GPS_RAW_INT.fix_type=6` | | 14.000 (nominal) | scheduled | Arm gate reached in fixture | `conftest.py _arm_at_sim_s=14.0` | -| 14.000+ (nominal) | scheduled | PASSIVE IC seeds scheduled, including tilt targets (`RAWES_RIC/PIC`) | `conftest.py send_named_float RAWES_THR/RIC/PIC`, `RAWES_MODE=3` | +| 14.000+ (nominal) | scheduled | PASSIVE IC seeds scheduled, including tilt targets (`RAWES_RIC/PIC`) | `conftest.py gcs.send_message(NamedValueFloat(...)) RAWES_THR/RIC/PIC`, `RAWES_MODE=3` | | 24.077 | observed | EKF starts GPS aiding | `STATUSTEXT "EKF3 IMU0 is using GPS"` | | 26.748 | observed | Tilt target first visible at AP interface | `telemetry.csv: |mav_att_target_pitch_deg| >= 1` | | 26.848 | observed | Physical tilt response begins (real attitude moves) | `telemetry.csv: |mav_att_pitch_deg| >= 5` | diff --git a/design/testing.md b/design/testing.md index a770141..59a19de 100644 --- a/design/testing.md +++ b/design/testing.md @@ -300,10 +300,10 @@ for i in range(max_steps): sim._update_fn() ``` -**Sending substates in stack tests** (via `gcs.send_named_float`): +**Sending substates in stack tests** (via `gcs.send_message(NamedValueFloat(...))`): ```python -gcs.send_named_float("RAWES_SUB", LAND_FINAL_DROP) +gcs.send_message(NamedValueFloat("RAWES_SUB", LAND_FINAL_DROP)) ``` **`_nv_floats` reset:** Lua resets its internal `_nv_floats` table to `{}` on every mode diff --git a/groundstation/gcs.py b/groundstation/gcs.py index 286975a..fba2a86 100644 --- a/groundstation/gcs.py +++ b/groundstation/gcs.py @@ -24,7 +24,9 @@ import math import threading import time +from dataclasses import dataclass from pathlib import Path +from typing import Any, ClassVar, Protocol, TypeVar, cast from pymavlink import mavutil @@ -32,6 +34,24 @@ log = logging.getLogger(__name__) + +class MavSenderLike(Protocol): + def __getattr__(self, name: str) -> Any: ... + + def set_send_callback(self, callback) -> None: ... + + +class MavConnectionLike(Protocol): + mav: MavSenderLike + target_system: int + target_component: int + + def recv_match(self, *args, **kwargs) -> Any: ... + + def wait_heartbeat(self, *args, **kwargs) -> Any: ... + + def close(self) -> None: ... + # ArduCopter custom mode numbers STABILIZE = 0 ACRO = 1 @@ -136,6 +156,740 @@ def now_ms(self) -> int: return int((time.monotonic() - self._t0) * 1000) +# ========================================================================== +# MAVLink message dataclasses +# ========================================================================== +# One dataclass per MAVLink message this codebase sends and/or receives. +# Field names/types intentionally mirror the generated C++ struct ArduPilot +# itself uses (mavlink/include/mavlink/v2.0/common/mavlink_msg_.h, +# e.g. `mavlink_attitude_t`) -- these are the same names the MAVLink XML +# defines, so they also match pymavlink's message attributes 1:1. This is +# additive: existing call sites (RawesGCS methods, observe() callbacks) are +# unchanged for now and can adopt these incrementally. +# +# MAVLINK_TYPE mirrors msg.get_type() (e.g. "ATTITUDE") for future dispatch. +# decode(msg) parses a raw pymavlink message into the dataclass; send(mav) +# serializes the dataclass back out over a pymavlink connection. Messages +# this codebase only ever receives get decode() only; messages it only ever +# sends get send() only. + +def _decode_mavlink_name(msg) -> str: + """Decode a 10/16-byte null-padded name field (bytes or str) into a str. + + Shared by NamedValueFloat/NamedValueInt (name, 10 bytes) and would also + suit ParamValue/ParamSet (param_id, 16 bytes) if/when those adopt decode(). + """ + raw_name = getattr(msg, "name", "") + name = ( + raw_name.decode("ascii", errors="ignore") + if isinstance(raw_name, bytes) else str(raw_name) + ) + return name.rstrip("\x00").strip() + + +@dataclass(frozen=True) +class StatusText: + """MAVLink STATUSTEXT (#253) -- mirrors mavlink_statustext_t.""" + MAVLINK_TYPE: ClassVar[str] = "STATUSTEXT" + + text: str + severity: int = 0 # MAV_SEVERITY + + @staticmethod + def decode(msg) -> "StatusText": + return StatusText( + text=str(msg.text).rstrip("\x00").strip(), + severity=int(getattr(msg, "severity", 0)), + ) + + +@dataclass(frozen=True) +class Attitude: + """MAVLink ATTITUDE (#30) -- mirrors mavlink_attitude_t.""" + MAVLINK_TYPE: ClassVar[str] = "ATTITUDE" + + roll: float # rad + pitch: float # rad + yaw: float # rad + rollspeed: float # rad/s + pitchspeed: float # rad/s + yawspeed: float # rad/s + time_boot_ms: int = 0 + + @staticmethod + def decode(msg) -> "Attitude": + return Attitude( + roll=float(msg.roll), + pitch=float(msg.pitch), + yaw=float(msg.yaw), + rollspeed=float(msg.rollspeed), + pitchspeed=float(msg.pitchspeed), + yawspeed=float(msg.yawspeed), + time_boot_ms=int(getattr(msg, "time_boot_ms", 0)), + ) + + +@dataclass(frozen=True) +class LocalPositionNed: + """MAVLink LOCAL_POSITION_NED (#32) -- mirrors mavlink_local_position_ned_t.""" + MAVLINK_TYPE: ClassVar[str] = "LOCAL_POSITION_NED" + + x: float # m, North + y: float # m, East + z: float # m, Down + vx: float = 0.0 # m/s + vy: float = 0.0 # m/s + vz: float = 0.0 # m/s + time_boot_ms: int = 0 + + @staticmethod + def decode(msg) -> "LocalPositionNed": + return LocalPositionNed( + x=float(msg.x), y=float(msg.y), z=float(msg.z), + vx=float(getattr(msg, "vx", 0.0)), + vy=float(getattr(msg, "vy", 0.0)), + vz=float(getattr(msg, "vz", 0.0)), + time_boot_ms=int(getattr(msg, "time_boot_ms", 0)), + ) + + +@dataclass(frozen=True) +class GlobalPositionInt: + """MAVLink GLOBAL_POSITION_INT (#33) -- mirrors mavlink_global_position_int_t.""" + MAVLINK_TYPE: ClassVar[str] = "GLOBAL_POSITION_INT" + + lat: int # degE7 + lon: int # degE7 + alt: int # mm, AMSL + relative_alt: int # mm, above home + vx: int = 0 # cm/s + vy: int = 0 # cm/s + vz: int = 0 # cm/s + hdg: int = 0 # cdeg + time_boot_ms: int = 0 + + @staticmethod + def decode(msg) -> "GlobalPositionInt": + return GlobalPositionInt( + lat=int(msg.lat), lon=int(msg.lon), alt=int(msg.alt), + relative_alt=int(msg.relative_alt), + vx=int(getattr(msg, "vx", 0)), + vy=int(getattr(msg, "vy", 0)), + vz=int(getattr(msg, "vz", 0)), + hdg=int(getattr(msg, "hdg", 0)), + time_boot_ms=int(getattr(msg, "time_boot_ms", 0)), + ) + + +@dataclass(frozen=True) +class EkfStatusReport: + """MAVLink EKF_STATUS_REPORT (#193) -- mirrors mavlink_ekf_status_report_t.""" + MAVLINK_TYPE: ClassVar[str] = "EKF_STATUS_REPORT" + + flags: int # EKF_STATUS_FLAGS bitmask + velocity_variance: float = 0.0 + pos_horiz_variance: float = 0.0 + pos_vert_variance: float = 0.0 + compass_variance: float = 0.0 + terrain_alt_variance: float = 0.0 + + @staticmethod + def decode(msg) -> "EkfStatusReport": + return EkfStatusReport( + flags=int(msg.flags), + velocity_variance=float(getattr(msg, "velocity_variance", 0.0)), + pos_horiz_variance=float(getattr(msg, "pos_horiz_variance", 0.0)), + pos_vert_variance=float(getattr(msg, "pos_vert_variance", 0.0)), + compass_variance=float(getattr(msg, "compass_variance", 0.0)), + terrain_alt_variance=float(getattr(msg, "terrain_alt_variance", 0.0)), + ) + + +@dataclass(frozen=True) +class BatteryStatus: + """MAVLink BATTERY_STATUS (#147) -- mirrors mavlink_battery_status_t.""" + MAVLINK_TYPE: ClassVar[str] = "BATTERY_STATUS" + + current_battery: int = -1 + battery_remaining: int = -1 + voltages: tuple[int, ...] = () + + @staticmethod + def decode(msg) -> "BatteryStatus": + return BatteryStatus( + current_battery=int(getattr(msg, "current_battery", -1)), + battery_remaining=int(getattr(msg, "battery_remaining", -1)), + voltages=tuple(int(v) for v in getattr(msg, "voltages", ())), + ) + + +@dataclass(frozen=True) +class SysStatus: + """MAVLink SYS_STATUS (#1) -- mirrors mavlink_sys_status_t.""" + MAVLINK_TYPE: ClassVar[str] = "SYS_STATUS" + + onboard_control_sensors_present: int = 0 + onboard_control_sensors_enabled: int = 0 + onboard_control_sensors_health: int = 0 + load: int = 0 + voltage_battery: int = 65535 + current_battery: int = -1 + battery_remaining: int = -1 + + @staticmethod + def decode(msg) -> "SysStatus": + return SysStatus( + onboard_control_sensors_present=int(getattr(msg, "onboard_control_sensors_present", 0)), + onboard_control_sensors_enabled=int(getattr(msg, "onboard_control_sensors_enabled", 0)), + onboard_control_sensors_health=int(getattr(msg, "onboard_control_sensors_health", 0)), + load=int(getattr(msg, "load", 0)), + voltage_battery=int(getattr(msg, "voltage_battery", 65535)), + current_battery=int(getattr(msg, "current_battery", -1)), + battery_remaining=int(getattr(msg, "battery_remaining", -1)), + ) + + +@dataclass(frozen=True) +class GpsRawInt: + """MAVLink GPS_RAW_INT (#24) -- mirrors mavlink_gps_raw_int_t.""" + MAVLINK_TYPE: ClassVar[str] = "GPS_RAW_INT" + + fix_type: int = 0 + satellites_visible: int = 0 + eph: int = 65535 + epv: int = 65535 + lat: int = 0 + lon: int = 0 + alt: int = 0 + + @staticmethod + def decode(msg) -> "GpsRawInt": + return GpsRawInt( + fix_type=int(getattr(msg, "fix_type", 0)), + satellites_visible=int(getattr(msg, "satellites_visible", 0)), + eph=int(getattr(msg, "eph", 65535)), + epv=int(getattr(msg, "epv", 65535)), + lat=int(getattr(msg, "lat", 0)), + lon=int(getattr(msg, "lon", 0)), + alt=int(getattr(msg, "alt", 0)), + ) + + +@dataclass(frozen=True) +class PowerStatus: + """MAVLink POWER_STATUS (#125) -- mirrors mavlink_power_status_t.""" + MAVLINK_TYPE: ClassVar[str] = "POWER_STATUS" + + Vcc: int = 0 + Vservo: int = 0 + flags: int = 0 + + @staticmethod + def decode(msg) -> "PowerStatus": + return PowerStatus( + Vcc=int(getattr(msg, "Vcc", 0)), + Vservo=int(getattr(msg, "Vservo", 0)), + flags=int(getattr(msg, "flags", 0)), + ) + + +@dataclass(frozen=True) +class MemInfo: + """MAVLink MEMINFO (#152) -- mirrors mavlink_meminfo_t.""" + MAVLINK_TYPE: ClassVar[str] = "MEMINFO" + + freemem: int = 0 + freemem32: int | None = None + + @staticmethod + def decode(msg) -> "MemInfo": + freemem32 = getattr(msg, "freemem32", None) + return MemInfo( + freemem=int(getattr(msg, "freemem", 0)), + freemem32=int(freemem32) if freemem32 is not None else None, + ) + + +@dataclass(frozen=True) +class McuStatus: + """MAVLink MCU_STATUS (#11039) -- mirrors mavlink_mcu_status_t.""" + MAVLINK_TYPE: ClassVar[str] = "MCU_STATUS" + + MCU_temperature: int = 0 + MCU_voltage: int = 0 + MCU_voltage_min: int = 0 + MCU_voltage_max: int = 0 + + @staticmethod + def decode(msg) -> "McuStatus": + return McuStatus( + MCU_temperature=int(getattr(msg, "MCU_temperature", 0)), + MCU_voltage=int(getattr(msg, "MCU_voltage", 0)), + MCU_voltage_min=int(getattr(msg, "MCU_voltage_min", 0)), + MCU_voltage_max=int(getattr(msg, "MCU_voltage_max", 0)), + ) + + +@dataclass(frozen=True) +class ServoOutputRaw: + """MAVLink SERVO_OUTPUT_RAW (#36) -- mirrors mavlink_servo_output_raw_t.""" + MAVLINK_TYPE: ClassVar[str] = "SERVO_OUTPUT_RAW" + + servo1_raw: int = 0 + servo2_raw: int = 0 + servo3_raw: int = 0 + servo4_raw: int = 0 + servo5_raw: int = 0 + servo6_raw: int = 0 + servo7_raw: int = 0 + servo8_raw: int = 0 + servo9_raw: int = 0 + servo10_raw: int = 0 + servo11_raw: int = 0 + servo12_raw: int = 0 + servo13_raw: int = 0 + servo14_raw: int = 0 + servo15_raw: int = 0 + servo16_raw: int = 0 + port: int = 0 + time_usec: int = 0 + + @staticmethod + def decode(msg) -> "ServoOutputRaw": + return ServoOutputRaw(**{ + f: int(getattr(msg, f, 0)) + for f in ( + "servo1_raw", "servo2_raw", "servo3_raw", "servo4_raw", + "servo5_raw", "servo6_raw", "servo7_raw", "servo8_raw", + "servo9_raw", "servo10_raw", "servo11_raw", "servo12_raw", + "servo13_raw", "servo14_raw", "servo15_raw", "servo16_raw", + "port", "time_usec", + ) + }) + + +@dataclass(frozen=True) +class ParamValue: + """MAVLink PARAM_VALUE (#22) -- mirrors mavlink_param_value_t.""" + MAVLINK_TYPE: ClassVar[str] = "PARAM_VALUE" + + param_id: str + param_value: float + param_type: int = 0 # MAV_PARAM_TYPE + param_count: int = 0 + param_index: int = 0 + + @staticmethod + def decode(msg) -> "ParamValue": + return ParamValue( + param_id=str(msg.param_id).rstrip("\x00").strip(), + param_value=float(msg.param_value), + param_type=int(getattr(msg, "param_type", 0)), + param_count=int(getattr(msg, "param_count", 0)), + param_index=int(getattr(msg, "param_index", 0)), + ) + + +@dataclass(frozen=True) +class CommandAck: + """MAVLink COMMAND_ACK (#77) -- mirrors mavlink_command_ack_t.""" + MAVLINK_TYPE: ClassVar[str] = "COMMAND_ACK" + + command: int # MAV_CMD + result: int # MAV_RESULT + + @staticmethod + def decode(msg) -> "CommandAck": + return CommandAck(command=int(msg.command), result=int(msg.result)) + + +@dataclass(frozen=True) +class Heartbeat: + """MAVLink HEARTBEAT (#0) -- mirrors mavlink_heartbeat_t.""" + MAVLINK_TYPE: ClassVar[str] = "HEARTBEAT" + + type: int # MAV_TYPE + autopilot: int # MAV_AUTOPILOT + base_mode: int # MAV_MODE_FLAG bitmask + custom_mode: int + system_status: int # MAV_STATE + mavlink_version: int = 3 + + @staticmethod + def decode(msg) -> "Heartbeat": + return Heartbeat( + type=int(msg.type), + autopilot=int(msg.autopilot), + base_mode=int(msg.base_mode), + custom_mode=int(msg.custom_mode), + system_status=int(msg.system_status), + mavlink_version=int(getattr(msg, "mavlink_version", 3)), + ) + + def send(self, mav) -> None: + mav.mav.heartbeat_send( + self.type, self.autopilot, self.base_mode, + self.custom_mode, self.system_status, + ) + + +@dataclass(frozen=True) +class NamedValueFloat: + """MAVLink NAMED_VALUE_FLOAT (#251) -- mirrors mavlink_named_value_float_t.""" + MAVLINK_TYPE: ClassVar[str] = "NAMED_VALUE_FLOAT" + + name: str + value: float + time_boot_ms: int = 0 + + @staticmethod + def decode(msg) -> "NamedValueFloat": + return NamedValueFloat( + name=_decode_mavlink_name(msg), + value=float(getattr(msg, "value", float("nan"))), + time_boot_ms=int(getattr(msg, "time_boot_ms", 0)), + ) + + def send(self, mav) -> None: + name_b = self.name.encode("ascii")[:10].ljust(10, b"\x00") + mav.mav.named_value_float_send(self.time_boot_ms, name_b, float(self.value)) + + +@dataclass(frozen=True) +class NamedValueInt: + """MAVLink NAMED_VALUE_INT (#252) -- mirrors mavlink_named_value_int_t.""" + MAVLINK_TYPE: ClassVar[str] = "NAMED_VALUE_INT" + + name: str + value: int + time_boot_ms: int = 0 + + @staticmethod + def decode(msg) -> "NamedValueInt": + return NamedValueInt( + name=_decode_mavlink_name(msg), + value=int(getattr(msg, "value", 0)), + time_boot_ms=int(getattr(msg, "time_boot_ms", 0)), + ) + + def send(self, mav) -> None: + name_b = self.name.encode("ascii")[:10].ljust(10, b"\x00") + mav.mav.named_value_int_send(self.time_boot_ms, name_b, int(self.value)) + + +@dataclass(frozen=True) +class CommandLong: + """MAVLink COMMAND_LONG (#76) -- mirrors mavlink_command_long_t. + + Generic command envelope used by arm/disarm (MAV_CMD_COMPONENT_ARM_DISARM), + set_mode (MAV_CMD_DO_SET_MODE), and set_message_interval + (MAV_CMD_SET_MESSAGE_INTERVAL) -- the command field selects behaviour. + """ + MAVLINK_TYPE: ClassVar[str] = "COMMAND_LONG" + + target_system: int + target_component: int + command: int # MAV_CMD + confirmation: int = 0 + param1: float = 0.0 + param2: float = 0.0 + param3: float = 0.0 + param4: float = 0.0 + param5: float = 0.0 + param6: float = 0.0 + param7: float = 0.0 + + def send(self, mav) -> None: + mav.mav.command_long_send( + self.target_system, self.target_component, + self.command, self.confirmation, + self.param1, self.param2, self.param3, + self.param4, self.param5, self.param6, self.param7, + ) + + +@dataclass(frozen=True) +class SetPositionTargetLocalNed: + """MAVLink SET_POSITION_TARGET_LOCAL_NED (#84) -- mirrors + mavlink_set_position_target_local_ned_t.""" + MAVLINK_TYPE: ClassVar[str] = "SET_POSITION_TARGET_LOCAL_NED" + + target_system: int + target_component: int + coordinate_frame: int + type_mask: int + x: float = 0.0 + y: float = 0.0 + z: float = 0.0 + vx: float = 0.0 + vy: float = 0.0 + vz: float = 0.0 + afx: float = 0.0 + afy: float = 0.0 + afz: float = 0.0 + yaw: float = 0.0 + yaw_rate: float = 0.0 + time_boot_ms: int = 0 + + def send(self, mav) -> None: + mav.mav.set_position_target_local_ned_send( + self.time_boot_ms, self.target_system, self.target_component, + self.coordinate_frame, self.type_mask, + self.x, self.y, self.z, + self.vx, self.vy, self.vz, + self.afx, self.afy, self.afz, + self.yaw, self.yaw_rate, + ) + + +@dataclass(frozen=True) +class SetAttitudeTarget: + """MAVLink SET_ATTITUDE_TARGET (#82) -- mirrors + mavlink_set_attitude_target_t.""" + MAVLINK_TYPE: ClassVar[str] = "SET_ATTITUDE_TARGET" + + target_system: int + target_component: int + type_mask: int + q: list[float] + body_roll_rate: float = 0.0 + body_pitch_rate: float = 0.0 + body_yaw_rate: float = 0.0 + thrust: float = 0.0 + time_boot_ms: int = 0 + + def send(self, mav) -> None: + mav.mav.set_attitude_target_send( + self.time_boot_ms, + self.target_system, + self.target_component, + self.type_mask, + self.q, + self.body_roll_rate, + self.body_pitch_rate, + self.body_yaw_rate, + self.thrust, + ) + + @staticmethod + def decode(msg) -> "SetAttitudeTarget": + return SetAttitudeTarget( + target_system=int(getattr(msg, "target_system", 0)), + target_component=int(getattr(msg, "target_component", 0)), + type_mask=int(getattr(msg, "type_mask", 0)), + q=list(getattr(msg, "q", ()) or ()), + body_roll_rate=float(getattr(msg, "body_roll_rate", 0.0)), + body_pitch_rate=float(getattr(msg, "body_pitch_rate", 0.0)), + body_yaw_rate=float(getattr(msg, "body_yaw_rate", 0.0)), + thrust=float(getattr(msg, "thrust", 0.0)), + time_boot_ms=int(getattr(msg, "time_boot_ms", 0)), + ) + + +@dataclass(frozen=True) +class RcChannels: + """MAVLink RC_CHANNELS (#65) -- mirrors mavlink_rc_channels_t.""" + MAVLINK_TYPE: ClassVar[str] = "RC_CHANNELS" + + chan1_raw: int | None = None + chan2_raw: int | None = None + chan3_raw: int | None = None + chan4_raw: int | None = None + + @staticmethod + def decode(msg) -> "RcChannels": + return RcChannels( + chan1_raw=getattr(msg, "chan1_raw", None), + chan2_raw=getattr(msg, "chan2_raw", None), + chan3_raw=getattr(msg, "chan3_raw", None), + chan4_raw=getattr(msg, "chan4_raw", None), + ) + + +@dataclass(frozen=True) +class PidTuning: + """MAVLink PID_TUNING (#194) -- mirrors mavlink_pid_tuning_t.""" + MAVLINK_TYPE: ClassVar[str] = "PID_TUNING" + + axis: int = -1 + desired: float | None = None + achieved: float | None = None + FF: float | None = None + P: float | None = None + I: float | None = None + D: float | None = None + PDmod: float | None = None + SRate: float | None = None + + @staticmethod + def decode(msg) -> "PidTuning": + return PidTuning( + axis=int(getattr(msg, "axis", -1)), + desired=getattr(msg, "desired", None), + achieved=getattr(msg, "achieved", None), + FF=getattr(msg, "FF", None), + P=getattr(msg, "P", None), + I=getattr(msg, "I", None), + D=getattr(msg, "D", None), + PDmod=getattr(msg, "PDmod", None), + SRate=getattr(msg, "SRate", None), + ) + + +_ESC_CHANNEL_BASE_BY_TYPE = { + "ESC_TELEMETRY_1_TO_4": 1, + "ESC_TELEMETRY_5_TO_8": 5, + "ESC_TELEMETRY_9_TO_12": 9, +} + + +@dataclass(frozen=True) +class EscTelemetry: + """MAVLink ESC_TELEMETRY_* messages -- common decoded shape.""" + + message_name: str + first_channel: int + rpm: tuple[int, ...] = () + voltage: tuple[int, ...] = () + current: tuple[int, ...] = () + temperature: tuple[int, ...] = () + + @staticmethod + def decode(msg) -> "EscTelemetry": + name = msg.get_type() + return EscTelemetry( + message_name=name, + first_channel=_ESC_CHANNEL_BASE_BY_TYPE.get(name, 0), + rpm=tuple(int(v) for v in getattr(msg, "rpm", ())), + voltage=tuple(int(v) for v in getattr(msg, "voltage", ())), + current=tuple(int(v) for v in getattr(msg, "current", ())), + temperature=tuple(int(v) for v in getattr(msg, "temperature", ())), + ) + + +@dataclass(frozen=True) +class ParamSet: + """MAVLink PARAM_SET (#23) -- mirrors mavlink_param_set_t.""" + MAVLINK_TYPE: ClassVar[str] = "PARAM_SET" + + target_system: int + target_component: int + param_id: str + param_value: float + param_type: int # MAV_PARAM_TYPE + + def send(self, mav) -> None: + mav.mav.param_set_send( + self.target_system, self.target_component, + self.param_id.encode("utf-8"), float(self.param_value), self.param_type, + ) + + +@dataclass(frozen=True) +class ParamRequestRead: + """MAVLink PARAM_REQUEST_READ (#20) -- mirrors mavlink_param_request_read_t.""" + MAVLINK_TYPE: ClassVar[str] = "PARAM_REQUEST_READ" + + target_system: int + target_component: int + param_id: str + param_index: int = -1 + + def send(self, mav) -> None: + mav.mav.param_request_read_send( + self.target_system, self.target_component, + self.param_id.encode("utf-8"), self.param_index, + ) + + +@dataclass(frozen=True) +class ParamRequestList: + """MAVLink PARAM_REQUEST_LIST (#21) -- mirrors mavlink_param_request_list_t.""" + MAVLINK_TYPE: ClassVar[str] = "PARAM_REQUEST_LIST" + + target_system: int + target_component: int + + def send(self, mav) -> None: + mav.mav.param_request_list_send(self.target_system, self.target_component) + + +@dataclass(frozen=True) +class RequestDataStream: + """MAVLink REQUEST_DATA_STREAM (#66) -- mirrors mavlink_request_data_stream_t.""" + MAVLINK_TYPE: ClassVar[str] = "REQUEST_DATA_STREAM" + + target_system: int + target_component: int + req_stream_id: int # MAV_DATA_STREAM + req_message_rate: int # Hz + start_stop: int = 1 # 1 = start, 0 = stop + + def send(self, mav) -> None: + mav.mav.request_data_stream_send( + self.target_system, self.target_component, + self.req_stream_id, self.req_message_rate, self.start_stop, + ) + + +_DecodedMessageT = TypeVar("_DecodedMessageT", covariant=True) + + +class _DecodableMessageClass(Protocol[_DecodedMessageT]): + MAVLINK_TYPE: str + + @staticmethod + def decode(msg) -> _DecodedMessageT: ... + +_MESSAGE_CLASS_BY_TYPE: dict[str, object] = { + cls.MAVLINK_TYPE: cls + for cls in ( + StatusText, + Attitude, + LocalPositionNed, + GlobalPositionInt, + EkfStatusReport, + BatteryStatus, + SysStatus, + GpsRawInt, + PowerStatus, + MemInfo, + McuStatus, + RcChannels, + ServoOutputRaw, + ParamValue, + CommandAck, + Heartbeat, + NamedValueFloat, + NamedValueInt, + SetAttitudeTarget, + PidTuning, + ) +} +_MESSAGE_CLASS_BY_TYPE.update({name: EscTelemetry for name in _ESC_CHANNEL_BASE_BY_TYPE}) + + +def decode_message(msg): + """Decode a raw pymavlink message into a registered dataclass when possible. + + Returns the original object unchanged when this module has no dataclass + wrapper for the message type yet. + """ + cls = _MESSAGE_CLASS_BY_TYPE.get(msg.get_type()) + if cls is None: + return msg + return cast(_DecodableMessageClass[object], cls).decode(msg) + + +def decode_as(msg, message_cls: "_DecodableMessageClass[_DecodedMessageT]") -> "_DecodedMessageT": + """Decode *msg* as *message_cls*, with a type check on msg.get_type().""" + if msg.get_type() != message_cls.MAVLINK_TYPE: + raise TypeError( + f"Expected {message_cls.MAVLINK_TYPE}, got {msg.get_type()}" + ) + return message_cls.decode(msg) + + class RawesGCS: """ Minimal MAVLink GCS client for RAWES SITL control. @@ -167,7 +921,7 @@ def __init__( self._address = address self._source_system = source_system self._baud = baud - self._mav = None + self._mav: MavConnectionLike | None = None self._target_system = 1 self._target_component = 1 self._watchdog = watchdog # nullary callable; raises if process is dead @@ -325,6 +1079,10 @@ def _recv( timeout : float Maximum wall-clock seconds to wait (only used when blocking=True). """ + if self._mav is None: + raise RuntimeError("RawesGCS is not connected") + + mav = self._mav type_set = None if type is not None: type_set = set(type) if isinstance(type, list) else {type} @@ -333,7 +1091,7 @@ def _recv( # Drain all currently available messages from the network into the # internal buffer without blocking. while True: - msg = self._mav.recv_match(blocking=False) + msg = mav.recv_match(blocking=False) if msg is None: break self._recv_buf.append(msg) @@ -371,11 +1129,11 @@ def connect_nowait(self) -> None: Target system/component default to 1/1 and are corrected automatically when the first HEARTBEAT is processed by _recv(). """ - self._mav = mavutil.mavlink_connection( + self._mav = cast(MavConnectionLike, mavutil.mavlink_connection( self._address, baud=self._baud, source_system=self._source_system, - ) + )) self._register_send_logger() log.info("GCS socket open (no heartbeat wait) — target sys=%d comp=%d", self._target_system, self._target_component) @@ -406,11 +1164,11 @@ def connect(self, timeout: float = 30.0, watchdog=None) -> None: # ECONNTIMEDOUT can happen in Docker when no process holds the port; # it is caught the same way. try: - self._mav = mavutil.mavlink_connection( + self._mav = cast(MavConnectionLike, mavutil.mavlink_connection( self._address, baud=self._baud, source_system=self._source_system, - ) + )) except Exception as exc: log.debug("Connect attempt failed: %s", exc) if _wd is not None: @@ -495,11 +1253,13 @@ def _heartbeat_worker(self, rate_hz: float) -> None: interval = 1.0 / rate_hz while not self._hb_stop.wait(interval): try: - self._mav.mav.heartbeat_send( - mavutil.mavlink.MAV_TYPE_GCS, - mavutil.mavlink.MAV_AUTOPILOT_INVALID, - 0, 0, 0, - ) + self.send_message(Heartbeat( + type=mavutil.mavlink.MAV_TYPE_GCS, + autopilot=mavutil.mavlink.MAV_AUTOPILOT_INVALID, + base_mode=0, + custom_mode=0, + system_status=0, + )) except Exception: pass @@ -525,7 +1285,6 @@ def set_param( with a PARAM_REQUEST_READ and retries the set up to *retries* times. Returns True if the parameter is confirmed at the requested value. """ - name_bytes = name.encode("utf-8") param_type = ( mavutil.mavlink.MAV_PARAM_TYPE_INT32 if isinstance(value, int) @@ -536,13 +1295,13 @@ def set_param( if attempt > 0: log.debug("set_param %s retry %d/%d", name, attempt, retries - 1) - self._mav.mav.param_set_send( - self._target_system, - self._target_component, - name_bytes, - float(value), - param_type, - ) + self.send_message(ParamSet( + target_system=self._target_system, + target_component=self._target_component, + param_id=name, + param_value=float(value), + param_type=param_type, + )) deadline = self.sim_now() + timeout while self.sim_now() < deadline: @@ -551,23 +1310,24 @@ def set_param( ) if msg is None: continue - pid = msg.param_id.rstrip("\x00") + pv = ParamValue.decode(msg) + pid = pv.param_id log.debug( "PARAM_VALUE received: %s = %g (looking for %s)", - pid, msg.param_value, name, + pid, pv.param_value, name, ) if pid == name: - log.info("Param %-20s = %g", name, msg.param_value) + log.info("Param %-20s = %g", name, pv.param_value) return True # ACK not received — read back to check if the value was applied silently log.debug("No ACK for %s — verifying via PARAM_REQUEST_READ", name) - self._mav.mav.param_request_read_send( - self._target_system, - self._target_component, - name_bytes, - -1, - ) + self.send_message(ParamRequestRead( + target_system=self._target_system, + target_component=self._target_component, + param_id=name, + param_index=-1, + )) verify_deadline = self.sim_now() + 2.0 while self.sim_now() < verify_deadline: msg = self._recv( @@ -575,17 +1335,18 @@ def set_param( ) if msg is None: continue - pid = msg.param_id.rstrip("\x00") + pv = ParamValue.decode(msg) + pid = pv.param_id if pid == name: - if msg.param_value == float(value): + if pv.param_value == float(value): log.info( "Param %-20s = %g (set confirmed via readback)", - name, msg.param_value, + name, pv.param_value, ) return True log.debug( "Param %s readback = %g (wanted %g) — will retry set", - name, msg.param_value, value, + name, pv.param_value, value, ) break @@ -598,22 +1359,22 @@ def get_param(self, name: str, timeout: float = 5.0) -> float | None: Returns the float value, or None if no response within *timeout*. """ - name_bytes = name.encode("utf-8") - self._mav.mav.param_request_read_send( - self._target_system, - self._target_component, - name_bytes, - -1, - ) + self.send_message(ParamRequestRead( + target_system=self._target_system, + target_component=self._target_component, + param_id=name, + param_index=-1, + )) deadline = self.sim_now() + timeout while self.sim_now() < deadline: msg = self._recv(type="PARAM_VALUE", blocking=True, timeout=1.0) if msg is None: continue - pid = msg.param_id.rstrip("\x00") + pv = ParamValue.decode(msg) + pid = pv.param_id if pid == name: - log.debug("get_param %s = %g", name, msg.param_value) - return float(msg.param_value) + log.debug("get_param %s = %g", name, pv.param_value) + return float(pv.param_value) log.warning("get_param %s: no response within %.1f s", name, timeout) return None @@ -623,10 +1384,10 @@ def fetch_all_params(self, timeout: float = 30.0) -> "dict[str, float]": Returns a dict mapping param name to value. Completes when the full param_count is received or *timeout* expires (whichever comes first). """ - self._mav.mav.param_request_list_send( - self._target_system, - self._target_component, - ) + self.send_message(ParamRequestList( + target_system=self._target_system, + target_component=self._target_component, + )) params: "dict[str, float]" = {} total: "int | None" = None deadline = self.sim_now() + timeout @@ -636,10 +1397,11 @@ def fetch_all_params(self, timeout: float = 30.0) -> "dict[str, float]": if total is not None and len(params) >= total: break continue - pid = msg.param_id.rstrip("\x00") - params[pid] = float(msg.param_value) + pv = ParamValue.decode(msg) + pid = pv.param_id + params[pid] = float(pv.param_value) if total is None: - total = msg.param_count + total = pv.param_count if len(params) >= total: break log.debug("fetch_all_params: got %d/%s params", len(params), total) @@ -666,20 +1428,19 @@ def wait_ekf_attitude(self, timeout: float = 45.0) -> bool: ) if msg is None: continue - mt = msg.get_type() - if mt == "STATUSTEXT": - log.info("[t=%.1f] EKF wait STATUSTEXT: %s", - self.sim_now(), msg.text.rstrip("\x00").strip()) - elif mt == "EKF_STATUS_REPORT": - log.debug("EKF_STATUS flags=0x%04x", msg.flags) - elif mt == "ATTITUDE": - r = math.degrees(msg.roll) - p = math.degrees(msg.pitch) - y = math.degrees(msg.yaw) - if all(math.isfinite(v) for v in (r, p, y)): - log.info("[t=%.1f] EKF attitude ready rpy=(%.1f, %.1f, %.1f)deg", - self.sim_now(), r, p, y) - return True + match decode_message(msg): + case StatusText(text=text): + log.info("[t=%.1f] EKF wait STATUSTEXT: %s", self.sim_now(), text) + case EkfStatusReport(flags=flags): + log.debug("EKF_STATUS flags=0x%04x", flags) + case Attitude() as att: + r = math.degrees(att.roll) + p = math.degrees(att.pitch) + y = math.degrees(att.yaw) + if all(math.isfinite(v) for v in (r, p, y)): + log.info("[t=%.1f] EKF attitude ready rpy=(%.1f, %.1f, %.1f)deg", + self.sim_now(), r, p, y) + return True return False def wait_ekf_ok(self, timeout: float = 60.0) -> None: @@ -705,10 +1466,11 @@ def wait_ekf_ok(self, timeout: float = 60.0) -> None: ) if msg is None: continue - if (msg.flags & NEEDED) == NEEDED: - log.info("[t=%.1f] EKF healthy (flags=0x%04x)", self.sim_now(), msg.flags) + ekf = EkfStatusReport.decode(msg) + if (ekf.flags & NEEDED) == NEEDED: + log.info("[t=%.1f] EKF healthy (flags=0x%04x)", self.sim_now(), ekf.flags) return - log.debug("EKF not ready yet (flags=0x%04x)", msg.flags) + log.debug("EKF not ready yet (flags=0x%04x)", ekf.flags) raise TimeoutError(f"EKF not healthy after {timeout:.0f}s") # ------------------------------------------------------------------ @@ -734,14 +1496,14 @@ def arm( """ param2 = 21196.0 if force else 0.0 log.info("Sending arm command (force=%s) …", force) - self._mav.mav.command_long_send( - self._target_system, - self._target_component, - mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, - 0, # confirmation - 1, # param1: 1 = arm - param2, 0, 0, 0, 0, 0, - ) + self.send_message(CommandLong( + target_system=self._target_system, + target_component=self._target_component, + command=mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, + confirmation=0, + param1=1, + param2=param2, + )) deadline = self.sim_now() + timeout t_last_arm_send = self.sim_now() _poll = 0.5 @@ -752,52 +1514,42 @@ def arm( ) if msg is None: continue - - if msg.get_type() == "STATUSTEXT": - log.warning("[t=%.1f] STATUSTEXT during arm: %s", - self.sim_now(), msg.text.rstrip("\x00").strip()) - continue - - if msg.get_type() == "COMMAND_ACK": - if msg.command == mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM: - if msg.result == mavutil.mavlink.MAV_RESULT_ACCEPTED: - log.info("[t=%.1f] Arm command ACCEPTED — waiting for armed heartbeat ...", - self.sim_now()) - elif msg.result in ( - mavutil.mavlink.MAV_RESULT_TEMPORARILY_REJECTED, - mavutil.mavlink.MAV_RESULT_FAILED, - ): - # Transient pre-arm failure (e.g. interlock not yet registered, - # EKF still converging). ArduPilot returns FAILED (=4) for - # pre-arm check failures, TEMPORARILY_REJECTED (=1) for busy. - # Sleep 1 s sim-time then resend; do NOT raise. - log.info( - "[t=%.1f] Arm rejected (result=%d) — retrying", - self.sim_now(), msg.result, - ) - self.sim_sleep(1.0) - self._mav.mav.command_long_send( - self._target_system, - self._target_component, - mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, - 0, 1, param2, 0, 0, 0, 0, 0, - ) - t_last_arm_send = self.sim_now() - else: - raise RuntimeError( - f"Arm rejected by vehicle (result={msg.result})" - ) - elif msg.command != mavutil.mavlink.MAV_CMD_DO_SET_MODE: - log.debug("COMMAND_ACK for cmd=%d result=%d (not arm)", - msg.command, msg.result) - - if msg.get_type() == "HEARTBEAT": - armed = bool(msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) - log.info("[t=%.1f] HEARTBEAT: sysid=%d base_mode=0x%02x armed=%s custom_mode=%d", - self.sim_now(), msg.get_srcSystem(), msg.base_mode, armed, msg.custom_mode) - if armed: - log.info("[t=%.1f] Vehicle is armed.", self.sim_now()) - return + match decode_message(msg): + case StatusText(text=text): + log.warning("[t=%.1f] STATUSTEXT during arm: %s", self.sim_now(), text) + continue + case CommandAck(command=command, result=result): + if command == mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM: + if result == mavutil.mavlink.MAV_RESULT_ACCEPTED: + log.info("[t=%.1f] Arm command ACCEPTED — waiting for armed heartbeat ...", + self.sim_now()) + elif result in ( + mavutil.mavlink.MAV_RESULT_TEMPORARILY_REJECTED, + mavutil.mavlink.MAV_RESULT_FAILED, + ): + log.info("[t=%.1f] Arm rejected (result=%d) — retrying", + self.sim_now(), result) + self.sim_sleep(1.0) + self.send_message(CommandLong( + target_system=self._target_system, + target_component=self._target_component, + command=mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, + confirmation=0, + param1=1, + param2=param2, + )) + t_last_arm_send = self.sim_now() + else: + raise RuntimeError(f"Arm rejected by vehicle (result={result})") + elif command != mavutil.mavlink.MAV_CMD_DO_SET_MODE: + log.debug("COMMAND_ACK for cmd=%d result=%d (not arm)", command, result) + case Heartbeat() as hb: + armed = bool(hb.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) + log.info("[t=%.1f] HEARTBEAT: sysid=%d base_mode=0x%02x armed=%s custom_mode=%d", + self.sim_now(), msg.get_srcSystem(), hb.base_mode, armed, hb.custom_mode) + if armed: + log.info("[t=%.1f] Vehicle is armed.", self.sim_now()) + return raise TimeoutError(f"Vehicle did not confirm armed within {timeout:.0f}s") @@ -813,14 +1565,14 @@ def disarm(self, timeout: float = 15.0, force: bool = True) -> None: """ param2 = 21196.0 if force else 0.0 log.info("Sending disarm command (force=%s) …", force) - self._mav.mav.command_long_send( - self._target_system, - self._target_component, - mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, - 0, # confirmation - 0, # param1: 0 = disarm - param2, 0, 0, 0, 0, 0, - ) + self.send_message(CommandLong( + target_system=self._target_system, + target_component=self._target_component, + command=mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, + confirmation=0, + param1=0, + param2=param2, + )) deadline = self.sim_now() + timeout while self.sim_now() < deadline: msg = self._recv( @@ -829,15 +1581,14 @@ def disarm(self, timeout: float = 15.0, force: bool = True) -> None: ) if msg is None: continue - if msg.get_type() == "STATUSTEXT": - log.info("[t=%.1f] STATUSTEXT during disarm: %s", - self.sim_now(), msg.text.rstrip("\x00").strip()) - continue - if msg.get_type() == "HEARTBEAT": - armed = bool(msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) - if not armed: - log.info("[t=%.1f] Vehicle is disarmed.", self.sim_now()) - return + match decode_message(msg): + case StatusText(text=text): + log.info("[t=%.1f] STATUSTEXT during disarm: %s", self.sim_now(), text) + continue + case Heartbeat(base_mode=base_mode): + if not bool(base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED): + log.info("[t=%.1f] Vehicle is disarmed.", self.sim_now()) + return raise TimeoutError(f"Vehicle did not confirm disarmed within {timeout:.0f}s") # ------------------------------------------------------------------ @@ -856,15 +1607,14 @@ def set_mode( """ log.info("Setting mode %d …", mode_id) t_last_send = self.sim_now() - self._mav.mav.command_long_send( - self._target_system, - self._target_component, - mavutil.mavlink.MAV_CMD_DO_SET_MODE, - 0, - mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, - float(mode_id), - 0, 0, 0, 0, 0, - ) + self.send_message(CommandLong( + target_system=self._target_system, + target_component=self._target_component, + command=mavutil.mavlink.MAV_CMD_DO_SET_MODE, + confirmation=0, + param1=mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, + param2=float(mode_id), + )) deadline = self.sim_now() + timeout _poll = 0.5 while self.sim_now() < deadline: @@ -875,42 +1625,33 @@ def set_mode( ) if msg is None: continue - t = msg.get_type() - if t == "HEARTBEAT": - if msg.custom_mode == mode_id: - log.info("[t=%.1f] Mode confirmed: %d", self.sim_now(), mode_id) - return - log.debug("Heartbeat custom_mode=%d (waiting for %d)", msg.custom_mode, mode_id) - elif t == "COMMAND_ACK": - if msg.command == mavutil.mavlink.MAV_CMD_DO_SET_MODE: - if msg.result == mavutil.mavlink.MAV_RESULT_ACCEPTED: + match decode_message(msg): + case Heartbeat(custom_mode=custom_mode): + if custom_mode == mode_id: + log.info("[t=%.1f] Mode confirmed: %d", self.sim_now(), mode_id) + return + log.debug("Heartbeat custom_mode=%d (waiting for %d)", custom_mode, mode_id) + case CommandAck(command=command, result=result) if command == mavutil.mavlink.MAV_CMD_DO_SET_MODE: + if result == mavutil.mavlink.MAV_RESULT_ACCEPTED: log.debug("MAV_CMD_DO_SET_MODE accepted, waiting for heartbeat confirmation") - elif msg.result == mavutil.mavlink.MAV_RESULT_FAILED: - # Transient rejection (e.g. EKF not yet providing position). - # Retry after 1 s sim-time. - log.debug( - "Mode %d rejected (result=%d) — retrying", - mode_id, msg.result, - ) + elif result == mavutil.mavlink.MAV_RESULT_FAILED: + log.debug("Mode %d rejected (result=%d) — retrying", mode_id, result) self.sim_sleep(1.0) - self._mav.mav.command_long_send( - self._target_system, - self._target_component, - mavutil.mavlink.MAV_CMD_DO_SET_MODE, - 0, - mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, - float(mode_id), - 0, 0, 0, 0, 0, - ) + self.send_message(CommandLong( + target_system=self._target_system, + target_component=self._target_component, + command=mavutil.mavlink.MAV_CMD_DO_SET_MODE, + confirmation=0, + param1=mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, + param2=float(mode_id), + )) t_last_send = self.sim_now() else: raise RuntimeError( - f"Mode change rejected by vehicle " - f"(mode={mode_id}, result={msg.result})" + f"Mode change rejected by vehicle (mode={mode_id}, result={result})" ) - elif t == "STATUSTEXT": - log.warning("[t=%.1f] STATUSTEXT during mode set: %s", - self.sim_now(), msg.text.rstrip("\x00").strip()) + case StatusText(text=text): + log.warning("[t=%.1f] STATUSTEXT during mode set: %s", self.sim_now(), text) raise TimeoutError(f"Mode {mode_id} not confirmed within {timeout:.0f}s") # ------------------------------------------------------------------ @@ -935,17 +1676,18 @@ def send_position_target_ned( north, east, down : float Target position [m] yaw : float Target yaw [rad], default 0 (ignored in mask) """ - self._mav.mav.set_position_target_local_ned_send( - 0, # time_boot_ms - self._target_system, - self._target_component, - mavutil.mavlink.MAV_FRAME_LOCAL_NED, - _POS_ONLY_MASK, - north, east, down, # position - 0.0, 0.0, 0.0, # velocity (ignored) - 0.0, 0.0, 0.0, # acceleration (ignored) - yaw, 0.0, # yaw, yaw_rate (ignored) - ) + self.send_message(SetPositionTargetLocalNed( + target_system=self._target_system, + target_component=self._target_component, + coordinate_frame=mavutil.mavlink.MAV_FRAME_LOCAL_NED, + type_mask=_POS_ONLY_MASK, + x=north, + y=east, + z=down, + yaw=yaw, + yaw_rate=0.0, + time_boot_ms=0, + )) log.info( "Position target sent: N=%.1f E=%.1f D=%.1f m", north, east, down ) @@ -965,13 +1707,13 @@ def request_stream(self, stream_id: int, rate_hz: int) -> None: rate_hz : int Requested message rate in Hz. Call once; ArduPilot sustains the rate. """ - self._mav.mav.request_data_stream_send( - self._target_system, - self._target_component, - stream_id, - rate_hz, - 1, # 1 = start streaming - ) + self.send_message(RequestDataStream( + target_system=self._target_system, + target_component=self._target_component, + req_stream_id=stream_id, + req_message_rate=rate_hz, + start_stop=1, + )) log.debug("Requested stream id=%d at %d Hz", stream_id, rate_hz) def set_message_interval(self, msg_id: int, interval_us: int) -> None: @@ -990,15 +1732,14 @@ def set_message_interval(self, msg_id: int, interval_us: int) -> None: Interval between messages in microseconds. ``-1`` disables the message; ``0`` restores the stream/param default rate. """ - self._mav.mav.command_long_send( - self._target_system, - self._target_component, - mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL, - 0, - float(msg_id), - float(interval_us), - 0, 0, 0, 0, 0, - ) + self.send_message(CommandLong( + target_system=self._target_system, + target_component=self._target_component, + command=mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL, + confirmation=0, + param1=float(msg_id), + param2=float(interval_us), + )) log.debug("SET_MESSAGE_INTERVAL id=%d interval_us=%d", msg_id, interval_us) def send_rc_override(self, channels: dict[int, int]) -> None: @@ -1013,51 +1754,35 @@ def send_rc_override(self, channels: dict[int, int]) -> None: "rawes.lua may hold CH8 override." ) - def send_named_float(self, name: str, value: float) -> None: - """Send a NAMED_VALUE_FLOAT MAVLink message to the vehicle. + def send_message(self, msg) -> None: + """Send any MAVLink message dataclass (NamedValueFloat, NamedValueInt, + Heartbeat, CommandLong, ...) that defines a `send(mav)` method. - ArduPilot Lua scripts that have called mavlink.register_rx_msgid(251) - will receive this message via mavlink.receive_chan() on their next tick. - - Parameters - ---------- - name : str - Message name, up to 10 ASCII characters (truncated + null-padded). - value : float - Floating-point value carried by the message. + Replaces the old per-message send_named_float()/send_named_int() + wrappers -- construct the dataclass and pass it here instead: + gcs.send_message(NamedValueFloat("RAWES_THR", 0.5)) + gcs.send_message(NamedValueInt("RAWES_LAT", lat_e7)) """ - name_b = name.encode("ascii")[:10].ljust(10, b"\x00") - self._mav.mav.named_value_float_send( - 0, # time_boot_ms — not meaningful for GCS-to-vehicle messages - name_b, - float(value), - ) - log.debug("NAMED_VALUE_FLOAT sent: %s=%.4g", name, value) - - def send_named_int(self, name: str, value: int) -> None: - """Send a NAMED_VALUE_INT MAVLink message to the vehicle. + msg.send(self._mav) + log.debug("%s sent: %r", type(msg).__name__, msg) - ArduPilot Lua scripts that have called mavlink.register_rx_msgid(252) - will receive this message via mavlink.receive_chan() on their next tick. - - Use this instead of send_named_float() whenever the value must keep - exact int32 precision (e.g. lat/lon in degrees*1e7) -- a float32 NVF - loses precision for large-magnitude values like absolute latitude. - - Parameters - ---------- - name : str - Message name, up to 10 ASCII characters (truncated + null-padded). - value : int - Signed 32-bit integer value carried by the message. - """ - name_b = name.encode("ascii")[:10].ljust(10, b"\x00") - self._mav.mav.named_value_int_send( - 0, # time_boot_ms — not meaningful for GCS-to-vehicle messages - name_b, - int(value), + def recv_decoded( + self, + message_cls: "_DecodableMessageClass[_DecodedMessageT]", + *, + blocking: bool = True, + timeout: float = 1.0, + ) -> "_DecodedMessageT | None": + """Receive one message of *message_cls* and decode it immediately.""" + msg = self._recv( + type=message_cls.MAVLINK_TYPE, + blocking=blocking, + timeout=timeout, ) - log.debug("NAMED_VALUE_INT sent: %s=%d", name, value) + if msg is None: + return None + return message_cls.decode(msg) + # ------------------------------------------------------------------ # Telemetry receive @@ -1073,7 +1798,8 @@ def recv_local_position( type="LOCAL_POSITION_NED", blocking=True, timeout=timeout ) if msg: - return (msg.x, msg.y, msg.z) + pos = LocalPositionNed.decode(msg) + return (pos.x, pos.y, pos.z) return None def recv_local_position_latest(self) -> tuple[float, float, float] | None: @@ -1087,7 +1813,7 @@ def recv_local_position_latest(self) -> tuple[float, float, float] | None: msg = self._recv(type="LOCAL_POSITION_NED", blocking=False) if msg is None: break - latest = msg + latest = LocalPositionNed.decode(msg) if latest is not None: return (latest.x, latest.y, latest.z) return None @@ -1100,5 +1826,6 @@ def recv_attitude( type="ATTITUDE", blocking=True, timeout=timeout ) if msg: - return (msg.roll, msg.pitch, msg.yaw) + att = Attitude.decode(msg) + return (att.roll, att.pitch, att.yaw) return None diff --git a/groundstation/rawes_modes.py b/groundstation/rawes_modes.py index ff4a707..6b4fd16 100644 --- a/groundstation/rawes_modes.py +++ b/groundstation/rawes_modes.py @@ -14,15 +14,18 @@ Usage ----- + from groundstation.gcs import NamedValueFloat from groundstation.rawes_modes import MODE_STEADY, PUMP_REEL_OUT, send_anchor_ned - gcs.set_param("RAWES_MODE", MODE_STEADY) # set mode (pumping runs in steady) - gcs.send_named_float("RAWES_SUB", PUMP_REEL_OUT) # set substate - send_anchor_ned(gcs, 0.0, 0.0, 0.0) # anchor at the mock/SITL origin + gcs.set_param("RAWES_MODE", MODE_STEADY) # set mode (pumping runs in steady) + gcs.send_message(NamedValueFloat("RAWES_SUB", PUMP_REEL_OUT)) # set substate + send_anchor_ned(gcs, 0.0, 0.0, 0.0) # anchor at the mock/SITL origin """ import math +from groundstation.gcs import NamedValueInt + # ── Mode numbers (RAWES_MODE script-generated param; 0=none 1=steady 3=passive 4=landing) ── MODE_NONE = 0 # script passive: no control-channel overrides (CH8 interlock hold still applies while armed) @@ -86,9 +89,15 @@ def anchor_ned_to_gps(dn_m: float, de_m: float, dd_m: float) -> tuple[int, int, def send_anchor_ned(sim, dn_m: float, de_m: float, dd_m: float) -> None: """Send the anchor at NED offset (dn_m, de_m, dd_m) from MOCK_ORIGIN_* via NAMED_VALUE_INT. - `sim` must expose `send_named_int(name, value)` (RawesLua harness / gcs.py). + `sim` may expose `send_message(NamedValueInt(...))` or the older + `send_named_int(name, value)` compatibility shim. """ lat_e7, lon_e7, alt_cm = anchor_ned_to_gps(dn_m, de_m, dd_m) + if hasattr(sim, "send_message"): + sim.send_message(NamedValueInt(NV_ANCHOR_LAT_KEY, lat_e7)) + sim.send_message(NamedValueInt(NV_ANCHOR_LON_KEY, lon_e7)) + sim.send_message(NamedValueInt(NV_ANCHOR_ALT_KEY, alt_cm)) + return sim.send_named_int(NV_ANCHOR_LAT_KEY, lat_e7) sim.send_named_int(NV_ANCHOR_LON_KEY, lon_e7) sim.send_named_int(NV_ANCHOR_ALT_KEY, alt_cm) diff --git a/groundstation/unified_ground.py b/groundstation/unified_ground.py index 9cc6c2a..236c74f 100644 --- a/groundstation/unified_ground.py +++ b/groundstation/unified_ground.py @@ -17,6 +17,7 @@ from __future__ import annotations +from groundstation.gcs import NamedValueFloat from groundstation.pumping_planner import TensionCommand _PHASE_TO_SUB: dict[str, int] = { @@ -50,11 +51,11 @@ def send(self, cmd: TensionCommand, dt: float) -> None: class GcsComms(NvComms): """Sends TensionCommand via MAVLink NAMED_VALUE_FLOAT (SITL stack tests). - gcs: object with send_named_float(name: str, value: float) — e.g. RawesGCS. + gcs: object with send_message(msg) — e.g. RawesGCS. """ def __init__(self, gcs) -> None: self._gcs = gcs def send_nv(self, name: str, value: float) -> None: - self._gcs.send_named_float(name, value) + self._gcs.send_message(NamedValueFloat(name, value)) diff --git a/pyproject.toml b/pyproject.toml index 1686e08..67e2fd7 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -6,7 +6,7 @@ build-backend = "setuptools.build_meta" name = "rawes" version = "0.1.0" description = "RAWES tethered autorotating rotor kite -- simulation, flight-stack, and analysis tooling" -requires-python = ">=3.10" +requires-python = ">=3.12" # Runtime dependencies are NOT declared here on purpose: simulation/requirements.txt # (installed via setup.cmd/setup.sh, hash-gated) remains the single source of truth diff --git a/scripts/query_hardware.py b/scripts/query_hardware.py index a419675..2b6b404 100644 --- a/scripts/query_hardware.py +++ b/scripts/query_hardware.py @@ -7,29 +7,53 @@ """ from __future__ import annotations import datetime, math, os, struct, sys, threading, time +from typing import cast from pymavlink import mavutil +from groundstation.gcs import ( + Attitude, + EkfStatusReport, + GpsRawInt, + Heartbeat, + MavConnectionLike, + McuStatus, + MemInfo, + ParamRequestRead, + ParamValue, + PowerStatus, + RequestDataStream, + ServoOutputRaw, + SysStatus, +) PORT = os.environ.get("RAWES_HIL_PORT", "COM4") BAUD = int(os.environ.get("RAWES_HIL_BAUD", "115200")) OUT = os.path.join(os.path.dirname(__file__), "..", "..", "hardware", "hardware.md") -mav = mavutil.mavlink_connection(PORT, baud=BAUD, source_system=255) +mav = cast(MavConnectionLike, mavutil.mavlink_connection(PORT, baud=BAUD, source_system=255)) mav.wait_heartbeat(timeout=15) print(f"Connected sysid={mav.target_system}") def _hb(): for _ in range(120): try: - mav.mav.heartbeat_send( - mavutil.mavlink.MAV_TYPE_GCS, - mavutil.mavlink.MAV_AUTOPILOT_INVALID, 0, 0, 0) + Heartbeat( + type=mavutil.mavlink.MAV_TYPE_GCS, + autopilot=mavutil.mavlink.MAV_AUTOPILOT_INVALID, + base_mode=0, + custom_mode=0, + system_status=0, + ).send(mav) except Exception: break time.sleep(1.0) threading.Thread(target=_hb, daemon=True).start() -mav.mav.request_data_stream_send( - mav.target_system, mav.target_component, - mavutil.mavlink.MAV_DATA_STREAM_ALL, 4, 1) +RequestDataStream( + target_system=mav.target_system, + target_component=mav.target_component, + req_stream_id=mavutil.mavlink.MAV_DATA_STREAM_ALL, + req_message_rate=4, + start_stop=1, +).send(mav) time.sleep(1.5) def recv_param(name, timeout=8.0): @@ -37,12 +61,18 @@ def recv_param(name, timeout=8.0): last = 0.0 while time.monotonic() < deadline: if time.monotonic() - last > 2.0: - mav.mav.param_request_read_send( - mav.target_system, mav.target_component, name.encode(), -1) + ParamRequestRead( + target_system=mav.target_system, + target_component=mav.target_component, + param_id=name, + param_index=-1, + ).send(mav) last = time.monotonic() msg = mav.recv_match(type="PARAM_VALUE", blocking=True, timeout=0.5) - if msg and msg.param_id.rstrip("\x00") == name: - return float(msg.param_value) + if msg: + pv = ParamValue.decode(msg) + if pv.param_id == name: + return float(pv.param_value) return None lines = [] @@ -87,13 +117,14 @@ def L(s=""): lines.append(s) L("## System") L() if hb_msg: + hb = Heartbeat.decode(hb_msg) L("| Field | Value |") L("|-------|-------|") - L(f"| Autopilot | {hb_msg.autopilot} (3=ArduPilot) |") - L(f"| MAV type | {hb_msg.type} (4=Helicopter) |") - L(f"| Base mode | 0x{hb_msg.base_mode:02X} |") - L(f"| System status | {hb_msg.system_status} |") - L(f"| MAVLink version | {hb_msg.mavlink_version} |") + L(f"| Autopilot | {hb.autopilot} (3=ArduPilot) |") + L(f"| MAV type | {hb.type} (4=Helicopter) |") + L(f"| Base mode | 0x{hb.base_mode:02X} |") + L(f"| System status | {hb.system_status} |") + L(f"| MAVLink version | {hb.mavlink_version} |") L() # --- Sensor health --- @@ -101,6 +132,7 @@ def L(s=""): lines.append(s) L("## Sensor Health") L() if ss: + ss_d = SysStatus.decode(ss) sensors = [ (0x000001, "3D Gyro"), (0x000002, "3D Accel"), @@ -118,16 +150,16 @@ def L(s=""): lines.append(s) L("| Sensor | Present | Enabled | Healthy |") L("|--------|---------|---------|---------|") for bit, name in sensors: - pres = bool(ss.onboard_control_sensors_present & bit) - enab = bool(ss.onboard_control_sensors_enabled & bit) - hlth = bool(ss.onboard_control_sensors_health & bit) + pres = bool(ss_d.onboard_control_sensors_present & bit) + enab = bool(ss_d.onboard_control_sensors_enabled & bit) + hlth = bool(ss_d.onboard_control_sensors_health & bit) if pres: L(f"| {name} | yes | {'yes' if enab else 'no'} | {'**NO**' if not hlth else 'yes'} |") L() - L(f"CPU load: {ss.load/10.0:.1f}% " - f"Battery voltage: {ss.voltage_battery/1000.0:.2f} V " - f"Current: {ss.current_battery/100.0:.2f} A " - f"Remaining: {ss.battery_remaining}%") + L(f"CPU load: {ss_d.load/10.0:.1f}% " + f"Battery voltage: {ss_d.voltage_battery/1000.0:.2f} V " + f"Current: {ss_d.current_battery/100.0:.2f} A " + f"Remaining: {ss_d.battery_remaining}%") L() # --- Power --- @@ -135,11 +167,12 @@ def L(s=""): lines.append(s) L("## Power Rails") L() if pw: + pw_d = PowerStatus.decode(pw) L("| Rail | Voltage |") L("|------|---------|") - L(f"| Vcc (5V) | {pw.Vcc/1000.0:.3f} V |") - L(f"| Vservo | {pw.Vservo/1000.0:.3f} V |") - flags = pw.flags + L(f"| Vcc (5V) | {pw_d.Vcc/1000.0:.3f} V |") + L(f"| Vservo | {pw_d.Vservo/1000.0:.3f} V |") + flags = pw_d.flags L() L("| Flag | State |") L("|------|-------|") @@ -153,11 +186,12 @@ def L(s=""): lines.append(s) L("## Memory") L() if mi: + mi_d = MemInfo.decode(mi) L("| Field | Value |") L("|-------|-------|") - L(f"| Free RAM | {mi.freemem} bytes |") - if hasattr(mi, "freemem32"): - L(f"| Free RAM (32-bit) | {mi.freemem32} bytes |") + L(f"| Free RAM | {mi_d.freemem} bytes |") + if mi_d.freemem32 is not None: + L(f"| Free RAM (32-bit) | {mi_d.freemem32} bytes |") L() # --- MCU --- @@ -165,12 +199,13 @@ def L(s=""): lines.append(s) L("## MCU") L() if mcu: + mcu_d = McuStatus.decode(mcu) L("| Field | Value |") L("|-------|-------|") - L(f"| Temperature | {mcu.MCU_temperature/100.0:.1f} deg C |") - L(f"| Voltage | {mcu.MCU_voltage/1000.0:.3f} V |") - L(f"| Voltage min | {mcu.MCU_voltage_min/1000.0:.3f} V |") - L(f"| Voltage max | {mcu.MCU_voltage_max/1000.0:.3f} V |") + L(f"| Temperature | {mcu_d.MCU_temperature/100.0:.1f} deg C |") + L(f"| Voltage | {mcu_d.MCU_voltage/1000.0:.3f} V |") + L(f"| Voltage min | {mcu_d.MCU_voltage_min/1000.0:.3f} V |") + L(f"| Voltage max | {mcu_d.MCU_voltage_max/1000.0:.3f} V |") L() # --- GPS --- @@ -178,18 +213,19 @@ def L(s=""): lines.append(s) L("## GPS") L() if gps: + gps_d = GpsRawInt.decode(gps) fix_map = {0:"No GPS", 1:"No fix", 2:"2D fix", 3:"3D fix", 4:"DGPS", 5:"RTK float", 6:"RTK fixed"} L("| Field | Value |") L("|-------|-------|") - L(f"| Fix type | {fix_map.get(gps.fix_type, str(gps.fix_type))} |") - L(f"| Satellites visible | {gps.satellites_visible} |") - L(f"| HDOP | {gps.eph/100.0:.2f} |") - L(f"| VDOP | {gps.epv/100.0:.2f} |") - if gps.fix_type >= 3: - L(f"| Latitude | {gps.lat/1e7:.6f} deg |") - L(f"| Longitude | {gps.lon/1e7:.6f} deg |") - L(f"| Altitude | {gps.alt/1000.0:.1f} m MSL |") + L(f"| Fix type | {fix_map.get(gps_d.fix_type, str(gps_d.fix_type))} |") + L(f"| Satellites visible | {gps_d.satellites_visible} |") + L(f"| HDOP | {gps_d.eph/100.0:.2f} |") + L(f"| VDOP | {gps_d.epv/100.0:.2f} |") + if gps_d.fix_type >= 3: + L(f"| Latitude | {gps_d.lat/1e7:.6f} deg |") + L(f"| Longitude | {gps_d.lon/1e7:.6f} deg |") + L(f"| Altitude | {gps_d.alt/1000.0:.1f} m MSL |") L() # --- EKF --- @@ -197,19 +233,20 @@ def L(s=""): lines.append(s) L("## EKF Status") L() if ekf: + ekf_d = EkfStatusReport.decode(ekf) L("| Field | Value |") L("|-------|-------|") - L(f"| Flags | 0x{ekf.flags:04X} |") - L(f"| Attitude initialised | {bool(ekf.flags & 0x0001)} |") - L(f"| Velocity horiz valid | {bool(ekf.flags & 0x0002)} |") - L(f"| Pos horiz rel valid | {bool(ekf.flags & 0x0004)} |") - L(f"| Pos horiz abs valid | {bool(ekf.flags & 0x0008)} |") - L(f"| Pos vert abs valid | {bool(ekf.flags & 0x0010)} |") - L(f"| Velocity variance | {ekf.velocity_variance:.4f} |") - L(f"| Pos horiz variance | {ekf.pos_horiz_variance:.4f} |") - L(f"| Pos vert variance | {ekf.pos_vert_variance:.4f} |") - L(f"| Compass variance | {ekf.compass_variance:.4f} |") - L(f"| Terrain alt variance | {ekf.terrain_alt_variance:.4f} |") + L(f"| Flags | 0x{ekf_d.flags:04X} |") + L(f"| Attitude initialised | {bool(ekf_d.flags & 0x0001)} |") + L(f"| Velocity horiz valid | {bool(ekf_d.flags & 0x0002)} |") + L(f"| Pos horiz rel valid | {bool(ekf_d.flags & 0x0004)} |") + L(f"| Pos horiz abs valid | {bool(ekf_d.flags & 0x0008)} |") + L(f"| Pos vert abs valid | {bool(ekf_d.flags & 0x0010)} |") + L(f"| Velocity variance | {ekf_d.velocity_variance:.4f} |") + L(f"| Pos horiz variance | {ekf_d.pos_horiz_variance:.4f} |") + L(f"| Pos vert variance | {ekf_d.pos_vert_variance:.4f} |") + L(f"| Compass variance | {ekf_d.compass_variance:.4f} |") + L(f"| Terrain alt variance | {ekf_d.terrain_alt_variance:.4f} |") L() # --- Attitude --- @@ -217,14 +254,15 @@ def L(s=""): lines.append(s) L("## Attitude (bench, stationary)") L() if att: + att_d = Attitude.decode(att) L("| Axis | Value |") L("|------|-------|") - L(f"| Roll | {math.degrees(att.roll):.1f} deg |") - L(f"| Pitch | {math.degrees(att.pitch):.1f} deg |") - L(f"| Yaw | {math.degrees(att.yaw):.1f} deg |") - L(f"| Roll rate | {math.degrees(att.rollspeed):.2f} deg/s |") - L(f"| Pitch rate | {math.degrees(att.pitchspeed):.2f} deg/s |") - L(f"| Yaw rate | {math.degrees(att.yawspeed):.2f} deg/s |") + L(f"| Roll | {math.degrees(att_d.roll):.1f} deg |") + L(f"| Pitch | {math.degrees(att_d.pitch):.1f} deg |") + L(f"| Yaw | {math.degrees(att_d.yaw):.1f} deg |") + L(f"| Roll rate | {math.degrees(att_d.rollspeed):.2f} deg/s |") + L(f"| Pitch rate | {math.degrees(att_d.pitchspeed):.2f} deg/s |") + L(f"| Yaw rate | {math.degrees(att_d.yawspeed):.2f} deg/s |") L() # --- IMU --- @@ -244,11 +282,12 @@ def L(s=""): lines.append(s) L("## Servo Outputs (current)") L() if srv: + srv_d = ServoOutputRaw.decode(srv) L("| Channel | PWM (us) |") L("|---------|----------|") for i in range(1, 17): attr = f"servo{i}_raw" - val = getattr(srv, attr, None) + val = getattr(srv_d, attr, None) if val is not None and val != 0: L(f"| {i} | {val} |") L() diff --git a/scripts/rawes.lua b/scripts/rawes.lua index f35dbd2..681615a 100644 --- a/scripts/rawes.lua +++ b/scripts/rawes.lua @@ -416,7 +416,8 @@ local _diag_nvf_keys = { "OL_RSP", "OL_PSP", "OL_YSP", -- outer-loop commanded body rates "OL_RER", "OL_PER", "OL_YER", -- body-rate tracking errors "OL_AP", "OL_AI", "OL_AD", "OL_COL", -- altitude PID terms + commanded thrust - "OL_TEN" -- ramped tension feedforward [N] + "OL_TEN", -- ramped tension feedforward [N] + "ANCH_N", "ANCH_E", "ANCH_D" -- resolved anchor NED offset from EKF origin [m] } local function _diag_set(name, value) @@ -850,6 +851,18 @@ local function run_flight() _diag_set("OL_AD", alt_d) _diag_set("OL_COL", _last_thrust) + -- ANCH_N/E/D: the resolved anchor NED offset from the EKF origin (see + -- _try_resolve_anchor()). pos_ned itself is already telemetered via the + -- standard LOCAL_POSITION_NED mavlink message, so only the anchor offset + -- needs a new channel -- post-run log analysis subtracts this from + -- LOCAL_POSITION_NED and compares against the physics-truth pos_x/pos_y/ + -- pos_z in telemetry.csv to verify the onboard anchor resolution tracks + -- the real position (see design/sitl_testing.md, "Anchor-relative + -- position cross-check"). + _diag_set("ANCH_N", anch:x()) + _diag_set("ANCH_E", anch:y()) + _diag_set("ANCH_D", anch:z()) + -- Diagnostic log (every ~5 s at 50 Hz) if _diag % 250 == 1 then local sub_info = "" diff --git a/simulation/mediator.py b/simulation/mediator.py index 14653d7..a494d17 100644 --- a/simulation/mediator.py +++ b/simulation/mediator.py @@ -38,6 +38,7 @@ from pathlib import Path import numpy as np +from groundstation.gcs import Attitude, CommandLong, LocalPositionNed, NamedValueFloat, PidTuning, RequestDataStream, ServoOutputRaw, SetAttitudeTarget, decode_message # Local modules (same directory) from simulation.physics_core import PhysicsCore @@ -275,15 +276,14 @@ def _mavlink_logger_worker(): def _request_interval(message_name: str, message_id: int, rate_hz: float) -> None: interval_us = max(1, int(round(1_000_000.0 / max(rate_hz, 1.0)))) - mav.mav.command_long_send( - target_sys, - target_comp, - mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL, - 0, - float(message_id), - float(interval_us), - 0.0, 0.0, 0.0, 0.0, 0.0, - ) + CommandLong( + target_system=target_sys, + target_component=target_comp, + command=mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL, + confirmation=0, + param1=float(message_id), + param2=float(interval_us), + ).send(mav) log.info( "Dedicated MAVLink link: requested %s at %.1f Hz", message_name, @@ -292,13 +292,13 @@ def _request_interval(message_name: str, message_id: int, rate_hz: float) -> Non try: # Use normal stream cadence on this dedicated link. - mav.mav.request_data_stream_send( - target_sys, - target_comp, - mavutil.mavlink.MAV_DATA_STREAM_ALL, - 10, - 1, - ) + RequestDataStream( + target_system=target_sys, + target_component=target_comp, + req_stream_id=mavutil.mavlink.MAV_DATA_STREAM_ALL, + req_message_rate=10, + start_stop=1, + ).send(mav) _request_interval( "ATTITUDE_TARGET", mavutil.mavlink.MAVLINK_MSG_ID_ATTITUDE_TARGET, @@ -396,54 +396,50 @@ def _request_interval(message_name: str, message_id: int, rate_hz: float) -> Non update_async_mavlink(fields) continue - if mtype == "ATTITUDE": - fields["mav_att_roll_deg"] = math.degrees(float(msg.roll)) - fields["mav_att_pitch_deg"] = math.degrees(float(msg.pitch)) - fields["mav_att_yaw_deg"] = math.degrees(float(msg.yaw)) - fields["mav_att_roll_rate_rads"] = float(getattr(msg, "rollspeed", float("nan"))) - fields["mav_att_pitch_rate_rads"] = float(getattr(msg, "pitchspeed", float("nan"))) - fields["mav_att_yaw_rate_rads"] = float(getattr(msg, "yawspeed", float("nan"))) - elif mtype == "ATTITUDE_TARGET": - r_deg, p_deg, y_deg = _quat_wxyz_to_rpy_deg(getattr(msg, "q", None)) + decoded = decode_message(msg) + if isinstance(decoded, Attitude): + att = decoded + fields["mav_att_roll_deg"] = math.degrees(att.roll) + fields["mav_att_pitch_deg"] = math.degrees(att.pitch) + fields["mav_att_yaw_deg"] = math.degrees(att.yaw) + fields["mav_att_roll_rate_rads"] = att.rollspeed + fields["mav_att_pitch_rate_rads"] = att.pitchspeed + fields["mav_att_yaw_rate_rads"] = att.yawspeed + elif isinstance(decoded, SetAttitudeTarget): + r_deg, p_deg, y_deg = _quat_wxyz_to_rpy_deg(decoded.q) fields["mav_att_target_roll_deg"] = r_deg fields["mav_att_target_pitch_deg"] = p_deg fields["mav_att_target_yaw_deg"] = y_deg - fields["mav_att_target_roll_rate_rads"] = float(getattr(msg, "body_roll_rate", float("nan"))) - fields["mav_att_target_pitch_rate_rads"] = float(getattr(msg, "body_pitch_rate", float("nan"))) - fields["mav_att_target_yaw_rate_rads"] = float(getattr(msg, "body_yaw_rate", float("nan"))) - elif mtype == "SERVO_OUTPUT_RAW": - fields["mav_servo1_us"] = float(getattr(msg, "servo1_raw", float("nan"))) - fields["mav_servo2_us"] = float(getattr(msg, "servo2_raw", float("nan"))) - fields["mav_servo3_us"] = float(getattr(msg, "servo3_raw", float("nan"))) - fields["mav_servo9_us"] = float(getattr(msg, "servo9_raw", float("nan"))) - elif mtype == "NAMED_VALUE_FLOAT": + fields["mav_att_target_roll_rate_rads"] = decoded.body_roll_rate + fields["mav_att_target_pitch_rate_rads"] = decoded.body_pitch_rate + fields["mav_att_target_yaw_rate_rads"] = decoded.body_yaw_rate + elif isinstance(decoded, ServoOutputRaw): + fields["mav_servo1_us"] = float(decoded.servo1_raw) + fields["mav_servo2_us"] = float(decoded.servo2_raw) + fields["mav_servo3_us"] = float(decoded.servo3_raw) + fields["mav_servo9_us"] = float(decoded.servo9_raw) + elif isinstance(decoded, NamedValueFloat): # Lua diagnostics are streamed as named floats. Keep only the # telemetry-relevant keys as latest-value async snapshot fields. - raw_name = getattr(msg, "name", "") - if isinstance(raw_name, bytes): - nvf_name = raw_name.decode("ascii", errors="ignore") - else: - nvf_name = str(raw_name) - nvf_name = nvf_name.rstrip("\x00").strip() - mapped = _nvf_key_map.get(nvf_name) + mapped = _nvf_key_map.get(decoded.name) if mapped is not None: - fields[mapped] = float(getattr(msg, "value", float("nan"))) - elif mtype == "LOCAL_POSITION_NED": - fields["ekf_pos_x"] = float(getattr(msg, "x", float("nan"))) - fields["ekf_pos_y"] = float(getattr(msg, "y", float("nan"))) - fields["ekf_pos_z"] = float(getattr(msg, "z", float("nan"))) - elif mtype == "PID_TUNING": + fields[mapped] = float(decoded.value) + elif isinstance(decoded, LocalPositionNed): + fields["ekf_pos_x"] = decoded.x + fields["ekf_pos_y"] = decoded.y + fields["ekf_pos_z"] = decoded.z + elif isinstance(decoded, PidTuning): # ArduPilot emits axis as 1=roll, 2=pitch, 3=yaw, 4=accelz. # Only roll/pitch/yaw have telemetry columns (ap_rate_pid_terms). - axis = int(getattr(msg, "axis", -1)) + axis = decoded.axis prefix = {1: "rate_roll", 2: "rate_pitch", 3: "rate_yaw"}.get(axis) if prefix is not None: - fields[f"{prefix}_p_contrib"] = float(getattr(msg, "P", float("nan"))) - fields[f"{prefix}_i_contrib"] = float(getattr(msg, "I", float("nan"))) - fields[f"{prefix}_d_contrib"] = float(getattr(msg, "D", float("nan"))) - fields[f"{prefix}_ff_contrib"] = float(getattr(msg, "FF", float("nan"))) - fields[f"{prefix}_pdmod"] = float(getattr(msg, "PDmod", float("nan"))) - fields[f"{prefix}_srate"] = float(getattr(msg, "SRate", float("nan"))) + fields[f"{prefix}_p_contrib"] = float(decoded.P if decoded.P is not None else float("nan")) + fields[f"{prefix}_i_contrib"] = float(decoded.I if decoded.I is not None else float("nan")) + fields[f"{prefix}_d_contrib"] = float(decoded.D if decoded.D is not None else float("nan")) + fields[f"{prefix}_ff_contrib"] = float(decoded.FF if decoded.FF is not None else float("nan")) + fields[f"{prefix}_pdmod"] = float(decoded.PDmod if decoded.PDmod is not None else float("nan")) + fields[f"{prefix}_srate"] = float(decoded.SRate if decoded.SRate is not None else float("nan")) if fields: update_async_mavlink(fields) diff --git a/simulation/rawes_lua_harness.py b/simulation/rawes_lua_harness.py index dd768b9..136a0a4 100644 --- a/simulation/rawes_lua_harness.py +++ b/simulation/rawes_lua_harness.py @@ -35,6 +35,8 @@ import numpy as np from lupa import lua54 +from groundstation.gcs import NamedValueFloat, NamedValueInt + # ── File paths ──────────────────────────────────────────────────────────────── _SIM_DIR = Path(__file__).resolve().parent @@ -432,22 +434,40 @@ def _send_named(self, name: str, value, msg_cls, msgid: int) -> None: lua_str = "".join(f"\\x{b:02x}" for b in raw) self._lua.execute(f'table.insert(_mock.mavlink_inbox, "{lua_str}")') - def send_named_float(self, name: str, value: float) -> None: - """Inject a NAMED_VALUE_FLOAT (msgid 251) into the Lua mavlink inbox.""" + def send_message(self, msg) -> None: + """Inject a supported MAVLink dataclass into the Lua mavlink inbox. + + Supported today: NamedValueFloat and NamedValueInt, which mirror the + same ground->Lua control channel used by RawesGCS in SITL. + """ from pymavlink import mavutil as _mu - self._send_named( - name, float(value), _mu.mavlink.MAVLink_named_value_float_message, 251 - ) + if isinstance(msg, NamedValueFloat): + self._send_named( + msg.name, + float(msg.value), + _mu.mavlink.MAVLink_named_value_float_message, + 251, + ) + return + if isinstance(msg, NamedValueInt): + self._send_named( + msg.name, + int(msg.value), + _mu.mavlink.MAVLink_named_value_int_message, + 252, + ) + return + raise TypeError(f"Unsupported RawesLua message type: {type(msg).__name__}") + + def send_named_float(self, name: str, value: float) -> None: + """Compatibility shim: inject a NAMED_VALUE_FLOAT into the Lua inbox.""" + self.send_message(NamedValueFloat(name, float(value))) def send_named_int(self, name: str, value: int) -> None: - """Inject a NAMED_VALUE_INT (msgid 252) into the Lua mavlink inbox. + """Compatibility shim: inject a NAMED_VALUE_INT into the Lua inbox. Used for the anchor lat/lon/alt (RAWES_LAT/LON/AAL), which are sent as int32 to preserve ArduPilot's own Location precision end-to-end. """ - from pymavlink import mavutil as _mu - - self._send_named( - name, int(value), _mu.mavlink.MAVLink_named_value_int_message, 252 - ) + self.send_message(NamedValueInt(name, int(value))) diff --git a/tests/hil/test_hil_smoke.py b/tests/hil/test_hil_smoke.py index 1d81b01..b0320c6 100644 --- a/tests/hil/test_hil_smoke.py +++ b/tests/hil/test_hil_smoke.py @@ -32,6 +32,7 @@ import pytest from pymavlink import mavutil +from groundstation.gcs import Attitude, Heartbeat, ParamRequestRead, ParamValue, RequestDataStream, SysStatus # --------------------------------------------------------------------------- # Fixture: serial connection @@ -97,14 +98,18 @@ def _recv_param(mav, name: str, timeout: float = 8.0) -> float | None: while time.monotonic() < deadline: now = time.monotonic() if now - last_request > 2.0: - mav.mav.param_request_read_send( - mav.target_system, mav.target_component, - name.encode(), -1, - ) + ParamRequestRead( + target_system=mav.target_system, + target_component=mav.target_component, + param_id=name, + param_index=-1, + ).send(mav) last_request = now msg = mav.recv_match(type="PARAM_VALUE", blocking=True, timeout=0.5) - if msg and msg.param_id.rstrip("\x00") == name: - return float(msg.param_value) + if msg: + pv = ParamValue.decode(msg) + if pv.param_id == name: + return float(pv.param_value) return None @@ -121,12 +126,13 @@ def test_heartbeat_is_ardupilot(hil_mav): """ msg = hil_mav.recv_match(type="HEARTBEAT", blocking=True, timeout=3.0) assert msg is not None, "No HEARTBEAT received within 3 s" + hb = Heartbeat.decode(msg) - assert msg.autopilot == mavutil.mavlink.MAV_AUTOPILOT_ARDUPILOTMEGA, ( - f"Expected ArduPilot autopilot (3), got {msg.autopilot}" + assert hb.autopilot == mavutil.mavlink.MAV_AUTOPILOT_ARDUPILOTMEGA, ( + f"Expected ArduPilot autopilot (3), got {hb.autopilot}" ) - assert msg.type == mavutil.mavlink.MAV_TYPE_HELICOPTER, ( - f"Expected MAV_TYPE_HELICOPTER (4), got {msg.type} " + assert hb.type == mavutil.mavlink.MAV_TYPE_HELICOPTER, ( + f"Expected MAV_TYPE_HELICOPTER (4), got {hb.type} " "(check FRAME_CLASS param -- should be heli frame)" ) @@ -139,17 +145,21 @@ def test_attitude_flowing(hil_mav): Bench test (Pixhawk stationary): |roll|, |pitch| < 30 deg is expected. """ # Request ATTITUDE stream if not already flowing - hil_mav.mav.request_data_stream_send( - hil_mav.target_system, hil_mav.target_component, - mavutil.mavlink.MAV_DATA_STREAM_EXTRA1, 4, 1, - ) + RequestDataStream( + target_system=hil_mav.target_system, + target_component=hil_mav.target_component, + req_stream_id=mavutil.mavlink.MAV_DATA_STREAM_EXTRA1, + req_message_rate=4, + start_stop=1, + ).send(hil_mav) msg = hil_mav.recv_match(type="ATTITUDE", blocking=True, timeout=5.0) assert msg is not None, "No ATTITUDE message received within 5 s" + att = Attitude.decode(msg) - roll_deg = math.degrees(msg.roll) - pitch_deg = math.degrees(msg.pitch) - yaw_deg = math.degrees(msg.yaw) + roll_deg = math.degrees(att.roll) + pitch_deg = math.degrees(att.pitch) + yaw_deg = math.degrees(att.yaw) assert all(math.isfinite(v) for v in (roll_deg, pitch_deg, yaw_deg)), ( f"ATTITUDE contains non-finite values: " @@ -168,31 +178,35 @@ def test_sys_status_imu_healthy(hil_mav): Confirms the Pixhawk's IMU hardware is detected and passing self-test. """ - hil_mav.mav.request_data_stream_send( - hil_mav.target_system, hil_mav.target_component, - mavutil.mavlink.MAV_DATA_STREAM_EXTENDED_STATUS, 2, 1, - ) + RequestDataStream( + target_system=hil_mav.target_system, + target_component=hil_mav.target_component, + req_stream_id=mavutil.mavlink.MAV_DATA_STREAM_EXTENDED_STATUS, + req_message_rate=2, + start_stop=1, + ).send(hil_mav) msg = hil_mav.recv_match(type="SYS_STATUS", blocking=True, timeout=5.0) assert msg is not None, "No SYS_STATUS message received within 5 s" + sys_status = SysStatus.decode(msg) IMU_SENSORS = ( mavutil.mavlink.MAV_SYS_STATUS_SENSOR_3D_GYRO | mavutil.mavlink.MAV_SYS_STATUS_SENSOR_3D_ACCEL ) - present = msg.onboard_control_sensors_present & IMU_SENSORS - enabled = msg.onboard_control_sensors_enabled & IMU_SENSORS - healthy = msg.onboard_control_sensors_health & IMU_SENSORS + present = sys_status.onboard_control_sensors_present & IMU_SENSORS + enabled = sys_status.onboard_control_sensors_enabled & IMU_SENSORS + healthy = sys_status.onboard_control_sensors_health & IMU_SENSORS assert present == IMU_SENSORS, ( - f"IMU sensors not present (present=0x{msg.onboard_control_sensors_present:08x})" + f"IMU sensors not present (present=0x{sys_status.onboard_control_sensors_present:08x})" ) assert enabled == IMU_SENSORS, ( - f"IMU sensors not enabled (enabled=0x{msg.onboard_control_sensors_enabled:08x})" + f"IMU sensors not enabled (enabled=0x{sys_status.onboard_control_sensors_enabled:08x})" ) assert healthy == IMU_SENSORS, ( - f"IMU sensors unhealthy (health=0x{msg.onboard_control_sensors_health:08x})" + f"IMU sensors unhealthy (health=0x{sys_status.onboard_control_sensors_health:08x})" ) @@ -205,7 +219,7 @@ def test_vehicle_is_disarmed(hil_mav): msg = hil_mav.recv_match(type="HEARTBEAT", blocking=True, timeout=3.0) assert msg is not None, "No HEARTBEAT received" - armed = bool(msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) + armed = bool(Heartbeat.decode(msg).base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) assert not armed, ( "Vehicle is ARMED during HIL smoke test -- disarm before running" ) diff --git a/tests/simtests/test_ground_liftoff.py b/tests/simtests/test_ground_liftoff.py index d6a9870..e67ff5f 100644 --- a/tests/simtests/test_ground_liftoff.py +++ b/tests/simtests/test_ground_liftoff.py @@ -31,6 +31,7 @@ from tests.simtests.simtest_runner import PhysicsRunner from tests.common.mock_ardupilot import MockArdupilot from simulation.rawes_lua_harness import RawesLua +from groundstation.gcs import NamedValueFloat from groundstation.rawes_modes import MODE_STEADY, send_anchor_ned from tests.simtests._rotor_helpers import load_default_rotor @@ -134,8 +135,8 @@ def test_ground_liftoff(): send_anchor_ned(sim, 0.0, 0.0, 0.0) # anchor at the mock EKF origin def _inject(s, r): - s.send_named_float("RAWES_TEN", TETHER_FORCE_N) - s.send_named_float("RAWES_ALT", TARGET_ALT_M) + s.send_message(NamedValueFloat("RAWES_TEN", TETHER_FORCE_N)) + s.send_message(NamedValueFloat("RAWES_ALT", TARGET_ALT_M)) for i in range(total_steps): t = i * DT diff --git a/tests/simtests/test_landing_lua.py b/tests/simtests/test_landing_lua.py index 5a6ecc3..2a87079 100644 --- a/tests/simtests/test_landing_lua.py +++ b/tests/simtests/test_landing_lua.py @@ -27,6 +27,7 @@ from tests.simtests.simtest_ic import load_ic from tests.simtests.simtest_runner import PhysicsRunner from tests.common.mock_ardupilot import MockArdupilot +from groundstation.gcs import NamedValueFloat from groundstation.landing_planner import LandingGroundController from simulation.rawes_lua_harness import RawesLua from groundstation.rawes_modes import MODE_LANDING, LAND_FINAL_DROP @@ -142,7 +143,7 @@ def _run_landing(log) -> dict: # ── Send final_drop substate to Lua once ────────────────────────── if cmd.phase == "final_drop" and not final_drop_sent: - sim.send_named_float("RAWES_SUB", LAND_FINAL_DROP) + sim.send_message(NamedValueFloat("RAWES_SUB", LAND_FINAL_DROP)) final_drop_sent = True t_final_start = t_sim diff --git a/tests/simtests/test_steady_flight_lua.py b/tests/simtests/test_steady_flight_lua.py index 4bb2913..f17a532 100644 --- a/tests/simtests/test_steady_flight_lua.py +++ b/tests/simtests/test_steady_flight_lua.py @@ -29,6 +29,7 @@ from simulation.simtest_log import BadEventLog from tests.simtests.simtest_runner import PhysicsRunner from tests.common.mock_ardupilot import MockArdupilot +from groundstation.gcs import NamedValueFloat from simulation.rawes_lua_harness import RawesLua from groundstation.rawes_modes import MODE_STEADY, send_anchor_ned from tests.simtests._rotor_helpers import load_default_rotor @@ -79,7 +80,7 @@ def _run_steady(log) -> dict: if i % LUA_EVERY == 0: lua.tick(t, runner, - inject=lambda s, r: s.send_named_float("RAWES_TEN", 300.0)) + inject=lambda s, r: s.send_message(NamedValueFloat("RAWES_TEN", 300.0))) dT = runner.tension_now - tension_target v_winch = max(-_WINCH_VMAX, min(_WINCH_VMAX, _WINCH_KP * dT)) diff --git a/tests/simtests/test_yaw_regulation_lua.py b/tests/simtests/test_yaw_regulation_lua.py index 9e59a2b..98d436f 100644 --- a/tests/simtests/test_yaw_regulation_lua.py +++ b/tests/simtests/test_yaw_regulation_lua.py @@ -37,6 +37,7 @@ import simulation.torque_model as _m from simulation.rawes_lua_harness import RawesLua +from groundstation.gcs import NamedValueFloat from groundstation.rawes_modes import MODE_PASSIVE # --------------------------------------------------------------------------- @@ -107,9 +108,9 @@ def test_yaw_regulation_lua(): ) # Seed IC so _ic_seeded becomes True on the first update() tick - sim.send_named_float("RAWES_THR", _IC_THRUST) - sim.send_named_float("RAWES_RIC", _IC_ROLL) - sim.send_named_float("RAWES_PIC", _IC_PITCH) + sim.send_message(NamedValueFloat("RAWES_THR", _IC_THRUST)) + sim.send_message(NamedValueFloat("RAWES_RIC", _IC_ROLL)) + sim.send_message(NamedValueFloat("RAWES_PIC", _IC_PITCH)) # --------------------------------------------------------------------------- # Closed-loop simulation diff --git a/tests/sitl/flight/conftest.py b/tests/sitl/flight/conftest.py index e47c5df..0025246 100644 --- a/tests/sitl/flight/conftest.py +++ b/tests/sitl/flight/conftest.py @@ -25,6 +25,7 @@ HOME_LON_DEG, HOME_ALT_M, ) +from groundstation.gcs import NamedValueFloat, NamedValueInt from simulation.ic import load_ic from simulation.torque_model import HubParams, equilibrium_throttle @@ -59,9 +60,9 @@ def _send_anchor_location(ctx) -> None: lat_e7 = round(HOME_LAT_DEG * 1e7) lon_e7 = round(HOME_LON_DEG * 1e7) alt_cm = round(anchor_alt_m * 100) - ctx.gcs.send_named_int("RAWES_LAT", lat_e7) - ctx.gcs.send_named_int("RAWES_LON", lon_e7) - ctx.gcs.send_named_int("RAWES_AAL", alt_cm) + ctx.gcs.send_message(NamedValueInt("RAWES_LAT", lat_e7)) + ctx.gcs.send_message(NamedValueInt("RAWES_LON", lon_e7)) + ctx.gcs.send_message(NamedValueInt("RAWES_AAL", alt_cm)) ctx.log.info( " anchor location sent via NVI (lat=%.7f lon=%.7f alt=%.1fm, " "home_alt_m=%.2f)", @@ -368,7 +369,7 @@ def _ic_trapezoid_stack(tmp_path, *, test_name, winch_cmd_port, run_ground_winch raise KeyError( "initial_state missing thrust seed: eq_thrust" ) - ctx.gcs.send_named_float("RAWES_THR", float(_ic_thrust)) + ctx.gcs.send_message(NamedValueFloat("RAWES_THR", float(_ic_thrust))) ctx.log.info("IC thrust: %.3f", _ic_thrust) # Seed passive IC roll/pitch via short NV names (10-char limit): @@ -381,8 +382,8 @@ def _ic_trapezoid_stack(tmp_path, *, test_name, winch_cmd_port, run_ground_winch _r22 = float(_R0[2][2]) _ic_roll_rad = math.atan2(_r21, _r22) _ic_pitch_rad = -math.asin(max(-1.0, min(1.0, _r20))) - ctx.gcs.send_named_float("RAWES_RIC", _ic_roll_rad) - ctx.gcs.send_named_float("RAWES_PIC", _ic_pitch_rad) + ctx.gcs.send_message(NamedValueFloat("RAWES_RIC", _ic_roll_rad)) + ctx.gcs.send_message(NamedValueFloat("RAWES_PIC", _ic_pitch_rad)) ctx.log.info( "IC passive attitude: roll=%+.2f deg pitch=%+.2f deg", math.degrees(_ic_roll_rad), math.degrees(_ic_pitch_rad), @@ -394,7 +395,7 @@ def _ic_trapezoid_stack(tmp_path, *, test_name, winch_cmd_port, run_ground_winch # reading, avoiding the ~2.5 m EKF vertical convergence-lag bias # present at capture time. _tension_eq = float(_ic["tension_eq_n"]) - ctx.gcs.send_named_float("RAWES_TEN", _tension_eq) + ctx.gcs.send_message(NamedValueFloat("RAWES_TEN", _tension_eq)) # Anchor location is a static constant, sent EXACTLY ONCE here # (after the IC seed values are queued) -- it does not depend on @@ -409,7 +410,7 @@ def _ic_trapezoid_stack(tmp_path, *, test_name, winch_cmd_port, run_ground_winch # needed). _alt_ic = float(ctx.home_alt_m) - ctx.gcs.send_named_float("RAWES_ALT", _alt_ic) + ctx.gcs.send_message(NamedValueFloat("RAWES_ALT", _alt_ic)) ctx.log.info("IC equilibrium tension: %.0f N target altitude: %.1f m", _tension_eq, _alt_ic) @@ -423,7 +424,7 @@ def _ic_trapezoid_stack(tmp_path, *, test_name, winch_cmd_port, run_ground_winch # step-input torque mismatch that spins the hub. See design/flight_stack.md # "Yaw observer in passive mode" and repo memory sitl-param-verify-and-yaw-ff.md. _yff_seed = equilibrium_throttle(float(_ic["omega_spin"]), HubParams()) - ctx.gcs.send_named_float("RAWES_YFF", _yff_seed) + ctx.gcs.send_message(NamedValueFloat("RAWES_YFF", _yff_seed)) ctx.log.info("IC yaw-trim equilibrium seed: %.3f", _yff_seed) ctx.wait_drain(timeout=1.0, label="post-param") ctx.wait_drain(timeout=0.5, label="post-col") diff --git a/tests/sitl/flight/test_lua_flight_ic_passive_sitl.py b/tests/sitl/flight/test_lua_flight_ic_passive_sitl.py index 3a37c1a..0871f87 100644 --- a/tests/sitl/flight/test_lua_flight_ic_passive_sitl.py +++ b/tests/sitl/flight/test_lua_flight_ic_passive_sitl.py @@ -30,6 +30,7 @@ get_arducopter_crash_info, observe, ) +from groundstation.gcs import NamedValueFloat, ServoOutputRaw, StatusText, decode_message # Timing _KINEMATIC_TIMEOUT_S = 60.0 @@ -90,10 +91,10 @@ def test_lua_flight_ic_passive_sitl(guided_nogps_armed_lua_full: StackContext): ic_roll_rad = math.atan2(r21, r22) ic_pitch_rad = -math.asin(max(-1.0, min(1.0, r20))) - gcs.send_named_float("RAWES_THR", thr_seed) - gcs.send_named_float("RAWES_TEN", ten_seed) - gcs.send_named_float("RAWES_RIC", ic_roll_rad) - gcs.send_named_float("RAWES_PIC", ic_pitch_rad) + gcs.send_message(NamedValueFloat("RAWES_THR", thr_seed)) + gcs.send_message(NamedValueFloat("RAWES_TEN", ten_seed)) + gcs.send_message(NamedValueFloat("RAWES_RIC", ic_roll_rad)) + gcs.send_message(NamedValueFloat("RAWES_PIC", ic_pitch_rad)) ok = gcs.set_param("RAWES_MODE", 3, timeout=5.0) log.info( "Release seeds: RAWES_THR=%.3f, RAWES_TEN=%.1f N, IC r/p=(%.2f, %.2f)deg, RAWES_MODE=3 ACK=%s", @@ -115,13 +116,13 @@ def test_lua_flight_ic_passive_sitl(guided_nogps_armed_lua_full: StackContext): def _handle(msg, t_rel): if msg is None: return None - mt = msg.get_type() - if mt == "SERVO_OUTPUT_RAW": - activity = abs(msg.servo1_raw - 1500) + abs(msg.servo2_raw - 1500) + decoded = decode_message(msg) + if isinstance(decoded, ServoOutputRaw): + activity = abs(decoded.servo1_raw - 1500) + abs(decoded.servo2_raw - 1500) if activity > state["max_cyclic"]: state["max_cyclic"] = activity - elif mt == "STATUSTEXT": - text = msg.text.rstrip("\x00").strip() + elif isinstance(decoded, StatusText): + text = decoded.text all_statustext.append(text) tl = text.lower() if "emergency yaw" in tl or "yaw reset" in tl: diff --git a/tests/sitl/flight/test_lua_flight_steady_sitl.py b/tests/sitl/flight/test_lua_flight_steady_sitl.py index 99fa3d4..40a1764 100644 --- a/tests/sitl/flight/test_lua_flight_steady_sitl.py +++ b/tests/sitl/flight/test_lua_flight_steady_sitl.py @@ -72,6 +72,7 @@ StackContext, dump_startup_diagnostics, observe, assert_no_mediator_criticals, get_arducopter_crash_info, ) +from groundstation.gcs import LocalPositionNed, NamedValueFloat, ServoOutputRaw, StatusText, decode_message from analysis.analyse_run import compute_steady_metrics, print_flight_report # -- Timing ------------------------------------------------------------------- @@ -92,6 +93,17 @@ _OMEGA_END_TOL_RAD_S = 0.5 # omega tolerance at kinematic exit [rad/s] _OMEGA_MONO_EPS_RAD_S = 0.15 # allow tiny sample jitter while checking monotonicity +# Anchor-relative position cross-check: rawes.lua resolves the ground-sent +# anchor lat/lon/alt into an EKF-local NED offset (ANCH_N/E/D, sent via NVF) +# and combines it with the standard LOCAL_POSITION_NED mavlink message to get +# its own view of "position relative to anchor". Compare that (post-run) +# against the physics-truth pos_x/pos_y/pos_z in telemetry.csv -- this is a +# sanity check on the onboard anchor resolution + EKF position, not an EKF +# accuracy benchmark, hence a generous tolerance (EKF/GPS noise + the ~0.5-1 m +# lat/lon->NED quantization noted in conftest._send_anchor_location()). +_MAX_ANCHOR_POS_ERR_M = 5.0 # m -- max allowed |onboard rel - physics truth| +_POS_XCHECK_MAX_DT_S = 1.0 # s -- max allowed gap when matching samples to telemetry rows + _POS_LOG_INTERVAL = 5.0 # s between periodic log lines # Temporary debug switch: keep Lua in MODE_PASSIVE after kinematic exit. @@ -149,10 +161,10 @@ def test_lua_flight_steady_sitl(guided_nogps_armed_lua_full: StackContext): ic_roll_rad = math.atan2(r21, r22) ic_pitch_rad = -math.asin(max(-1.0, min(1.0, r20))) - gcs.send_named_float("RAWES_THR", thr_seed) - gcs.send_named_float("RAWES_TEN", ten_seed) - gcs.send_named_float("RAWES_RIC", ic_roll_rad) - gcs.send_named_float("RAWES_PIC", ic_pitch_rad) + gcs.send_message(NamedValueFloat("RAWES_THR", thr_seed)) + gcs.send_message(NamedValueFloat("RAWES_TEN", ten_seed)) + gcs.send_message(NamedValueFloat("RAWES_RIC", ic_roll_rad)) + gcs.send_message(NamedValueFloat("RAWES_PIC", ic_pitch_rad)) ok = gcs.set_param("RAWES_MODE", 3, timeout=5.0) log.info( "Release seeds (IC-passive init): RAWES_THR=%.3f, RAWES_TEN=%.1f N, " @@ -179,7 +191,7 @@ def test_lua_flight_steady_sitl(guided_nogps_armed_lua_full: StackContext): # Send RAWES_ALT immediately after MODE_STEADY so Lua's capture uses the # physics IC altitude, not the EKF-reported altitude (which has a ~2.5 m # vertical bias). Mode enter clears _nv_floats, so this must arrive after. - gcs.send_named_float("RAWES_ALT", float(-ic["pos"][2])) + gcs.send_message(NamedValueFloat("RAWES_ALT", float(-ic["pos"][2]))) all_statustext = ctx.all_statustext lua_captured = False @@ -209,18 +221,20 @@ def test_lua_flight_steady_sitl(guided_nogps_armed_lua_full: StackContext): "ekf_yaw_reset": ekf_yaw_reset, "max_cyclic": max_cyclic_activity, "t_last_log": t_obs_start, + "anch": {"N": None, "E": None, "D": None}, + "pos_samples": [], # (t_abs, x, y, z) from LOCAL_POSITION_NED } def _handle(msg, t_rel): now = gcs.sim_now() if msg is not None: - mt = msg.get_type() - if mt == "SERVO_OUTPUT_RAW": - activity = abs(msg.servo1_raw - 1500) + abs(msg.servo2_raw - 1500) + decoded = decode_message(msg) + if isinstance(decoded, ServoOutputRaw): + activity = abs(decoded.servo1_raw - 1500) + abs(decoded.servo2_raw - 1500) if activity > state["max_cyclic"]: state["max_cyclic"] = activity - elif mt == "STATUSTEXT": - text = msg.text.rstrip("\x00").strip() + elif isinstance(decoded, StatusText): + text = decoded.text log.info("STATUSTEXT [t=%.1fs]: %s", t_rel, text) all_statustext.append(text) tl = text.lower() @@ -230,6 +244,12 @@ def _handle(msg, t_rel): if "emergency yaw" in tl or "yaw reset" in tl: state["ekf_yaw_reset"] = True log.warning("EKF yaw reset at t=%.1fs: %s", t_rel, text) + elif isinstance(decoded, NamedValueFloat): + name, value = decoded.name, decoded.value + if name in ("ANCH_N", "ANCH_E", "ANCH_D"): + state["anch"][name[-1]] = value + elif isinstance(decoded, LocalPositionNed): + state["pos_samples"].append((now, decoded.x, decoded.y, decoded.z)) if (not _DEBUG_KEEP_PASSIVE) and (not state["lua_captured"]) and now > t_capture_deadline: pytest.fail( @@ -249,12 +269,14 @@ def _handle(msg, t_rel): observe(ctx, _OBS_SECONDS, _handle, msg_types=["SERVO_OUTPUT_RAW", "STATUSTEXT", "ATTITUDE", - "LOCAL_POSITION_NED", "EKF_STATUS_REPORT"], + "LOCAL_POSITION_NED", "EKF_STATUS_REPORT", "NAMED_VALUE_FLOAT"], label="observation") lua_captured = state["lua_captured"] ekf_yaw_reset = state["ekf_yaw_reset"] max_cyclic_activity = state["max_cyclic"] + anch = state["anch"] + pos_samples = state["pos_samples"] log.info("Observation complete: max_activity=%d PWM", max_cyclic_activity) try: @@ -263,9 +285,10 @@ def _handle(msg, t_rel): # so we can assert before printing the full report. metrics = None _rows = [] - if ctx.telemetry_log.exists(): + telemetry_log = ctx.telemetry_log + if telemetry_log is not None and telemetry_log.exists(): from simulation.telemetry_csv import read_csv as _read_csv - _rows = _read_csv(ctx.telemetry_log) + _rows = _read_csv(telemetry_log) metrics = compute_steady_metrics(_rows, stable_alt_m=_STABLE_ALT_M) log.info( "Physics: min_alt=%.2f m stable=%.0f s floor_hits=%d" @@ -276,11 +299,52 @@ def _handle(msg, t_rel): else: log.warning("No telemetry CSV -- physics checks skipped") + # -- Anchor-relative position cross-check ------------------------------ + # Combine the onboard-resolved anchor (ANCH_N/E/D, via NVF) with the + # standard LOCAL_POSITION_NED samples to reconstruct the script's own + # view of "position relative to anchor", then compare against the + # physics-truth pos_x/pos_y/pos_z in telemetry.csv (see + # design/sitl_testing.md, "Anchor-relative position cross-check"). + anchor_pos_max_err = None + if _rows and pos_samples and all(v is not None for v in anch.values()): + truth_t = np.array([float(r.t_sim) for r in _rows]) + truth_pos = np.array([[float(r.pos_x), float(r.pos_y), float(r.pos_z)] for r in _rows]) + order = np.argsort(truth_t) + truth_t = truth_t[order] + truth_pos = truth_pos[order] + + worst = None + for t_abs, x, y, z in pos_samples: + rel = np.array([x - anch["N"], y - anch["E"], z - anch["D"]]) + idx = int(np.searchsorted(truth_t, t_abs)) + idx = min(max(idx, 0), len(truth_t) - 1) + if abs(truth_t[idx] - t_abs) > _POS_XCHECK_MAX_DT_S: + continue + err = float(np.linalg.norm(rel - truth_pos[idx])) + if worst is None or err > worst[0]: + worst = (err, t_abs, rel, truth_pos[idx]) + + if worst is not None: + anchor_pos_max_err = worst[0] + log.info( + "Anchor-relative position cross-check: max_err=%.2f m at t=%.1fs " + "(onboard rel=%s truth=%s) n_samples=%d", + worst[0], worst[1], worst[2], worst[3], len(pos_samples), + ) + else: + log.warning("Anchor-relative position cross-check: no matchable samples") + else: + log.warning( + "Anchor-relative position cross-check skipped " + "(anch=%s n_pos_samples=%d)", anch, len(pos_samples) + ) + # -- Assertions ------------------------------------------------------- log.info( "=== ASSERTION CHECK: captured=%s cyclic=%d yaw_reset=%s" " min_alt=%.2f mean_alt=%.1f max_alt=%.0f" - " max_tension=%.0f stable=%.0f floor_hits=%d ===", + " max_tension=%.0f stable=%.0f floor_hits=%d" + " anchor_pos_max_err=%s ===", lua_captured, max_cyclic_activity, ekf_yaw_reset, metrics.min_phys_alt if metrics else float("nan"), metrics.mean_alt if metrics else float("nan"), @@ -288,6 +352,7 @@ def _handle(msg, t_rel): metrics.max_tension if metrics else float("nan"), metrics.max_stable_s if metrics else float("nan"), metrics.floor_hits if metrics else -1, + f"{anchor_pos_max_err:.2f}" if anchor_pos_max_err is not None else "n/a", ) if not _DEBUG_KEEP_PASSIVE: @@ -296,6 +361,16 @@ def _handle(msg, t_rel): f"STATUSTEXT: {all_statustext}" ) + if anchor_pos_max_err is not None: + assert anchor_pos_max_err <= _MAX_ANCHOR_POS_ERR_M, ( + f"Onboard anchor-relative position diverged from physics truth: " + f"max_err={anchor_pos_max_err:.2f} m > {_MAX_ANCHOR_POS_ERR_M:.1f} m.\n" + "Checklist:\n" + " * RAWES_LAT/LON/AAL resolved correctly by _try_resolve_anchor()\n" + " * ANCH_N/E/D NVF telemetry matches conftest._send_anchor_location() intent\n" + " * EKF position (LOCAL_POSITION_NED) not diverging from physics truth" + ) + if metrics is not None: # Verify the single-release mechanism behavior in this SITL test: # over the final kinematic window, omega_rotor ramps to the IC @@ -383,9 +458,12 @@ def _handle(msg, t_rel): ) # -- Full physics report ---------------------------------------------- - _log_dir = ctx.telemetry_log.parent - if _log_dir.exists(): - print_flight_report(_log_dir) + if telemetry_log is not None: + _log_dir = telemetry_log.parent + if _log_dir.exists(): + print_flight_report(_log_dir) + else: + log.warning("No telemetry log path available -- flight report skipped") except Exception as exc: dump_startup_diagnostics(ctx) diff --git a/tests/sitl/flight/test_pumping_cycle_sitl.py b/tests/sitl/flight/test_pumping_cycle_sitl.py index 22bec30..bcbd9bd 100644 --- a/tests/sitl/flight/test_pumping_cycle_sitl.py +++ b/tests/sitl/flight/test_pumping_cycle_sitl.py @@ -44,6 +44,7 @@ from simulation.telemetry_csv import read_csv from groundstation.pumping_planner import PumpingGroundController from groundstation.unified_ground import _cmd_to_nv +from groundstation.gcs import NamedValueFloat, StatusText, decode_message from tests.simtests._rotor_helpers import load_default_rotor _ROTOR = load_default_rotor() @@ -124,10 +125,10 @@ def test_pumping_cycle_lua_sitl(guided_nogps_armed_pumping_lua: StackContext): ic_roll_rad = math.atan2(float(R0[2][1]), float(R0[2][2])) ic_pitch_rad = -math.asin(max(-1.0, min(1.0, float(R0[2][0])))) - gcs.send_named_float("RAWES_THR", thr_seed) - gcs.send_named_float("RAWES_TEN", ten_seed) - gcs.send_named_float("RAWES_RIC", ic_roll_rad) - gcs.send_named_float("RAWES_PIC", ic_pitch_rad) + gcs.send_message(NamedValueFloat("RAWES_THR", thr_seed)) + gcs.send_message(NamedValueFloat("RAWES_TEN", ten_seed)) + gcs.send_message(NamedValueFloat("RAWES_RIC", ic_roll_rad)) + gcs.send_message(NamedValueFloat("RAWES_PIC", ic_pitch_rad)) gcs.set_param("RAWES_MODE", 3, timeout=5.0) log.info(" Holding MODE_PASSIVE 10 s to settle before MODE_STEADY ...") gcs.sim_sleep(10.0) @@ -215,7 +216,7 @@ def test_pumping_cycle_lua_sitl(guided_nogps_armed_pumping_lua: StackContext): # Send NVF to Lua via GCS for name, value in _cmd_to_nv(cmd): - gcs.send_named_float(name, value) + gcs.send_message(NamedValueFloat(name, value)) # All cycles complete -> planner returns to hold (mirrors simtest). if planner.cycle_count >= N_CYCLES: @@ -227,8 +228,8 @@ def test_pumping_cycle_lua_sitl(guided_nogps_armed_pumping_lua: StackContext): type=["STATUSTEXT"], blocking=False, timeout=0.01, ) - if msg is not None and msg.get_type() == "STATUSTEXT": - text = msg.text.rstrip("\x00").strip() + if msg is not None and isinstance((decoded := decode_message(msg)), StatusText): + text = decoded.text all_statustext.append(text) log.info("STATUSTEXT: %s", text) if "RAWES steady: captured" in text: diff --git a/tests/sitl/stack_infra.py b/tests/sitl/stack_infra.py index 004f58e..0f3f95b 100644 --- a/tests/sitl/stack_infra.py +++ b/tests/sitl/stack_infra.py @@ -83,7 +83,23 @@ def _project_default_rotor(): ) from pymavlink import mavutil as _mavutil -from groundstation.gcs import GUIDED, GUIDED_NOGPS, STABILIZE, RawesGCS +from groundstation.gcs import ( + Attitude, + decode_message, + EkfStatusReport, + GlobalPositionInt, + GUIDED, + GUIDED_NOGPS, + LocalPositionNed, + ParamValue, + STABILIZE, + RawesGCS, + NamedValueFloat, + ParamRequestRead, + RequestDataStream, + StatusText, + SetAttitudeTarget, +) from simulation.mediator_events import MediatorEventLog from simulation.controller import make_hold_controller from simulation.ic import load_ic_dict, IC_JSON_PATH @@ -392,20 +408,20 @@ def _recv_one(recv_timeout: float) -> None: if msg is None: last_text[0] = None return - mtype = msg.get_type() - if mtype == "STATUSTEXT": - text = msg.text.rstrip("\x00").strip() + decoded = decode_message(msg) + if isinstance(decoded, StatusText): + text = decoded.text self.all_statustext.append(text) self.log.info("STATUSTEXT [%s]: %s", label, text) last_text[0] = text return - if mtype == "LOCAL_POSITION_NED": + if isinstance(decoded, LocalPositionNed): # Only the most recent sample is kept -- callers needing a # gated/finite sample poll self.last_local_position_ned via # `until` rather than accumulating history here. self.last_local_position_ned = ( - float(msg.x), float(msg.y), float(msg.z), - float(msg.vx), float(msg.vy), float(msg.vz), + decoded.x, decoded.y, decoded.z, + decoded.vx, decoded.vy, decoded.vz, ) last_text[0] = None @@ -999,17 +1015,17 @@ def _check_alive() -> None: | _mavutil.mavlink.ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE | _mavutil.mavlink.ATTITUDE_TARGET_TYPEMASK_THROTTLE_IGNORE ) - gcs._mav.mav.set_attitude_target_send( - 0, - gcs._target_system, - gcs._target_component, - mask, - q, - 0.0, - 0.0, - 0.0, - 0.0, - ) + gcs.send_message(SetAttitudeTarget( + target_system=gcs._target_system, + target_component=gcs._target_component, + type_mask=mask, + q=q, + body_roll_rate=0.0, + body_pitch_rate=0.0, + body_yaw_rate=0.0, + thrust=0.0, + time_boot_ms=0, + )) log.info( "[arm] Seeded SET_ATTITUDE_TARGET from EKF rpy=(%.2f, %.2f, %.2f) deg", math.degrees(roll), math.degrees(pitch), math.degrees(yaw), @@ -1031,7 +1047,7 @@ def _check_alive() -> None: # Optional Lua disarm timer (ms from now). This no longer controls arming. if armon_ms is not None and armon_ms > 0: - gcs.send_named_float("RAWES_ARM", float(armon_ms)) + gcs.send_message(NamedValueFloat("RAWES_ARM", float(armon_ms))) log.info("[arm] Sent RAWES_ARM disarm timer=%d ms", armon_ms) @@ -1101,23 +1117,35 @@ def _run_acro_setup( # Request telemetry streams so ArduPilot sends ATTITUDE, EKF, and position. # Without this, SITL often sends no messages on a plain TCP connection. log.info("[setup 1/6] Requesting telemetry streams ...") - gcs._mav.mav.request_data_stream_send( - gcs._target_system, gcs._target_component, - _mavutil.mavlink.MAV_DATA_STREAM_ALL, 10, 1, # all streams at 10 Hz - ) + gcs.send_message(RequestDataStream( + target_system=gcs._target_system, + target_component=gcs._target_component, + req_stream_id=_mavutil.mavlink.MAV_DATA_STREAM_ALL, + req_message_rate=10, + start_stop=1, + )) # Also send a targeted ATTITUDE request to be sure - gcs._mav.mav.request_data_stream_send( - gcs._target_system, gcs._target_component, - _mavutil.mavlink.MAV_DATA_STREAM_EXTRA1, 10, 1, # ATTITUDE - ) - gcs._mav.mav.request_data_stream_send( - gcs._target_system, gcs._target_component, - _mavutil.mavlink.MAV_DATA_STREAM_EXTENDED_STATUS, 5, 1, # EKF_STATUS - ) - gcs._mav.mav.request_data_stream_send( - gcs._target_system, gcs._target_component, - _mavutil.mavlink.MAV_DATA_STREAM_POSITION, 5, 1, # LOCAL_POSITION_NED - ) + gcs.send_message(RequestDataStream( + target_system=gcs._target_system, + target_component=gcs._target_component, + req_stream_id=_mavutil.mavlink.MAV_DATA_STREAM_EXTRA1, + req_message_rate=10, + start_stop=1, + )) + gcs.send_message(RequestDataStream( + target_system=gcs._target_system, + target_component=gcs._target_component, + req_stream_id=_mavutil.mavlink.MAV_DATA_STREAM_EXTENDED_STATUS, + req_message_rate=5, + start_stop=1, + )) + gcs.send_message(RequestDataStream( + target_system=gcs._target_system, + target_component=gcs._target_component, + req_stream_id=_mavutil.mavlink.MAV_DATA_STREAM_POSITION, + req_message_rate=5, + start_stop=1, + )) # ── 2. Param subsystem ──────────────────────────────────────────────────── log.info("[setup 2/6] Waiting for param subsystem ...") @@ -1167,11 +1195,11 @@ def _run_acro_setup( if msg is None: continue t_now = gcs.sim_now() - mt = msg.get_type() + decoded = decode_message(msg) - if mt == "STATUSTEXT": - text = msg.text.rstrip("\x00").strip() - sev = getattr(msg, "severity", "?") + if isinstance(decoded, StatusText): + text = decoded.text + sev = decoded.severity log.info("[setup 4/6] STATUSTEXT [sev=%s]: %s", sev, text) all_statustext.append(text) setup_samples.append({"t": t_now, "type": "STATUSTEXT", @@ -1185,12 +1213,12 @@ def _run_acro_setup( log.warning("[setup 4/6] GPS Glitch seen pre-arm — continuing " "(will be flagged as known issue only if arm fails).") - elif mt == "ATTITUDE": - att_seed_rpy = (float(msg.roll), float(msg.pitch), float(msg.yaw)) - r, p, y = math.degrees(msg.roll), math.degrees(msg.pitch), math.degrees(msg.yaw) - rr, pr, yr = (math.degrees(msg.rollspeed), - math.degrees(msg.pitchspeed), - math.degrees(msg.yawspeed)) + elif isinstance(decoded, Attitude): + att_seed_rpy = (decoded.roll, decoded.pitch, decoded.yaw) + r, p, y = math.degrees(decoded.roll), math.degrees(decoded.pitch), math.degrees(decoded.yaw) + rr, pr, yr = (math.degrees(decoded.rollspeed), + math.degrees(decoded.pitchspeed), + math.degrees(decoded.yawspeed)) log.info("[setup 4/6] ATTITUDE rpy=(%.2f°,%.2f°,%.2f°) " "rates=(%.2f,%.2f,%.2f)°/s", r, p, y, rr, pr, yr) setup_samples.append({"t": t_now, "type": "ATTITUDE", @@ -1207,13 +1235,13 @@ def _run_acro_setup( # so corrections stay valid even when velocity-derived yaw jumps # ~150° at kinematic end when the tether activates. if hasattr(ctx.controller, "set_equilibrium"): - ctx.controller.set_equilibrium(msg.roll, msg.pitch) + ctx.controller.set_equilibrium(decoded.roll, decoded.pitch) log.info("[setup 4/6] Equilibrium set: roll_eq=%.2f° pitch_eq=%.2f°", r, p) ekf_ok = ekf_att - elif mt == "EKF_STATUS_REPORT": - flags = msg.flags + elif isinstance(decoded, EkfStatusReport): + flags = decoded.flags log.info("[setup 4/6] EKF_STATUS flags=0x%04x vel_var=%.3f " "pos_var=%.3f hgt_var=%.3f", flags, @@ -1227,23 +1255,23 @@ def _run_acro_setup( t_ekf = t_now ekf_ok = ekf_att - elif mt == "LOCAL_POSITION_NED": + elif isinstance(decoded, LocalPositionNed): log.info("[setup 4/6] LOCAL_POSITION_NED N=%.2f E=%.2f D=%.2f " "vN=%.3f vE=%.3f vD=%.3f", - msg.x, msg.y, msg.z, msg.vx, msg.vy, msg.vz) + decoded.x, decoded.y, decoded.z, decoded.vx, decoded.vy, decoded.vz) setup_samples.append({"t": t_now, "type": "LOCAL_POSITION_NED", - "N": msg.x, "E": msg.y, "D": msg.z, - "vN": msg.vx, "vE": msg.vy, "vD": msg.vz}) + "N": decoded.x, "E": decoded.y, "D": decoded.z, + "vN": decoded.vx, "vE": decoded.vy, "vD": decoded.vz}) if not ekf_pos: log.info("[setup 4/6] LOCAL_POSITION_NED received — EKF has position.") ekf_pos = True # ekf_ok does not depend on ekf_pos — ATTITUDE alone is sufficient - elif mt == "GLOBAL_POSITION_INT": + elif isinstance(decoded, GlobalPositionInt): log.info("[setup 4/6] GLOBAL_POSITION_INT lat=%d lon=%d alt_mm=%d", - msg.lat, msg.lon, msg.alt) + decoded.lat, decoded.lon, decoded.alt) setup_samples.append({"t": t_now, "type": "GLOBAL_POSITION_INT", - "lat": msg.lat, "lon": msg.lon, "alt_mm": msg.alt}) + "lat": decoded.lat, "lon": decoded.lon, "alt_mm": decoded.alt}) # FAIL HARD — do not proceed with a broken EKF if not ekf_ok: @@ -1307,10 +1335,10 @@ def _run_acro_setup( blocking=True, timeout=0.2, ) if msg is not None: - mt = msg.get_type() - if mt == "STATUSTEXT": - text = msg.text.rstrip("\x00").strip() - sev = getattr(msg, "severity", "?") + decoded = decode_message(msg) + if isinstance(decoded, StatusText): + text = decoded.text + sev = decoded.severity log.info("[stabilise] STATUSTEXT [sev=%s]: %s", sev, text) all_statustext.append(text) # "tilt alignment complete" STATUSTEXT is the definitive signal @@ -1322,17 +1350,17 @@ def _run_acro_setup( ekf_att_ready = True log.info("[stabilise] EKF tilt alignment confirmed via STATUSTEXT " "-- proceeding to arm immediately.") - elif mt == "LOCAL_POSITION_NED": + elif isinstance(decoded, LocalPositionNed): log.info("[stabilise] LOCAL_POSITION_NED N=%.2f E=%.2f D=%.2f", - msg.x, msg.y, msg.z) + decoded.x, decoded.y, decoded.z) setup_samples.append({"t": gcs.sim_now(), "type": "LOCAL_POSITION_NED", - "N": msg.x, "E": msg.y, "D": msg.z, - "vN": msg.vx, "vE": msg.vy, "vD": msg.vz}) + "N": decoded.x, "E": decoded.y, "D": decoded.z, + "vN": decoded.vx, "vE": decoded.vy, "vD": decoded.vz}) if not ekf_pos: log.info("[stabilise] EKF GPS position fused (LOCAL_POSITION_NED).") ekf_pos = True - elif mt == "EKF_STATUS_REPORT": - flags = msg.flags + elif isinstance(decoded, EkfStatusReport): + flags = decoded.flags last_flags = flags log.info("[stabilise] EKF_STATUS flags=0x%04x", flags) if flags & _EKF_POS_FLAG: @@ -1362,8 +1390,8 @@ def _run_acro_setup( blocking=True, timeout=0.5, ) - if msg is not None and msg.get_type() == "STATUSTEXT": - text = msg.text.rstrip("\x00").strip() + if msg is not None and isinstance((decoded := decode_message(msg)), StatusText): + text = decoded.text all_statustext.append(text) log.info("[setup-delay] STATUSTEXT: %s", text) @@ -1596,9 +1624,10 @@ def wait_for_acro_stability(gcs, log, timeout: float = 5.0) -> bool: msg = gcs._recv(type="ATTITUDE", blocking=True, timeout=1.0) if msg is None: continue - r = math.degrees(msg.roll) - p = math.degrees(msg.pitch) - y = math.degrees(msg.yaw) + att = Attitude.decode(msg) + r = math.degrees(att.roll) + p = math.degrees(att.pitch) + y = math.degrees(att.yaw) if all(math.isfinite(v) for v in (r, p, y)): log.info("GUIDED_NOGPS stable: rpy=(%.2f°, %.2f°, %.2f°)", r, p, y) return True @@ -1613,8 +1642,9 @@ def drain_statustext(gcs, log) -> list[str]: msg = gcs._recv(type="STATUSTEXT", blocking=True, timeout=0.05) if msg is None: break - text = msg.text.rstrip("\x00").strip() - log.warning("STATUSTEXT [sev=%s] %s", getattr(msg, "severity", "?"), text) + st = StatusText.decode(msg) + text = st.text + log.warning("STATUSTEXT [sev=%s] %s", st.severity, text) texts.append(text) return texts @@ -1732,13 +1762,17 @@ def assert_no_mediator_criticals(mediator_log: Path) -> None: def _wait_params_ready(gcs, log, timeout: float = 15.0) -> None: deadline = gcs.sim_now() + timeout while gcs.sim_now() < deadline: - gcs._mav.mav.param_request_read_send( - gcs._target_system, gcs._target_component, b"SYSID_THISMAV", -1, - ) + gcs.send_message(ParamRequestRead( + target_system=gcs._target_system, + target_component=gcs._target_component, + param_id="SYSID_THISMAV", + param_index=-1, + )) msg = gcs._recv(type="PARAM_VALUE", blocking=True, timeout=1.0) if msg is not None: + pv = ParamValue.decode(msg) log.info("Param subsystem ready (%s = %g)", - msg.param_id.rstrip("\x00"), msg.param_value) + pv.param_id, pv.param_value) return log.debug("Waiting for param subsystem ...") raise TimeoutError( @@ -2086,20 +2120,38 @@ def _assert_alive() -> None: _assert_alive() log.info("GCS connected") - gcs.request_stream(_mavutil.mavlink.MAV_DATA_STREAM_EXTRA1, 10) - gcs.request_stream(_mavutil.mavlink.MAV_DATA_STREAM_EXTRA3, 2) - gcs.request_stream(_mavutil.mavlink.MAV_DATA_STREAM_RC_CHANNELS, 10) + gcs.send_message(RequestDataStream( + target_system=gcs._target_system, + target_component=gcs._target_component, + req_stream_id=_mavutil.mavlink.MAV_DATA_STREAM_EXTRA1, + req_message_rate=10, + )) + gcs.send_message(RequestDataStream( + target_system=gcs._target_system, + target_component=gcs._target_component, + req_stream_id=_mavutil.mavlink.MAV_DATA_STREAM_EXTRA3, + req_message_rate=2, + )) + gcs.send_message(RequestDataStream( + target_system=gcs._target_system, + target_component=gcs._target_component, + req_stream_id=_mavutil.mavlink.MAV_DATA_STREAM_RC_CHANNELS, + req_message_rate=10, + )) log.info("Waiting for param subsystem ...") deadline = gcs.sim_now() + 20.0 while gcs.sim_now() < deadline: _assert_alive() - gcs._mav.mav.param_request_read_send( - gcs._target_system, gcs._target_component, b"SYSID_THISMAV", -1, - ) + gcs.send_message(ParamRequestRead( + target_system=gcs._target_system, + target_component=gcs._target_component, + param_id="SYSID_THISMAV", + param_index=-1, + )) msg = gcs._recv(type="PARAM_VALUE", blocking=True, timeout=1.0) if msg is not None: - log.info("Param subsystem ready (SYSID_THISMAV=%g)", msg.param_value) + log.info("Param subsystem ready (SYSID_THISMAV=%g)", ParamValue.decode(msg).param_value) break else: pytest.fail("Param subsystem never responded within 20 s") @@ -2129,22 +2181,28 @@ def _assert_alive() -> None: if msg is None: continue now = gcs.sim_now() - if msg.get_type() == "STATUSTEXT": - text = msg.text.rstrip("\x00").strip() + decoded = decode_message(msg) + if isinstance(decoded, StatusText): + text = decoded.text log.info("SITL: %s", text) if "EKF3 active" in text or "EKF3 IMU" in text: - gcs.request_stream(_mavutil.mavlink.MAV_DATA_STREAM_EXTRA1, 10) + gcs.send_message(RequestDataStream( + target_system=gcs._target_system, + target_component=gcs._target_component, + req_stream_id=_mavutil.mavlink.MAV_DATA_STREAM_EXTRA1, + req_message_rate=10, + )) if "rawes" in text.lower() and "mode=" in text.lower(): log.info("Lua script confirmed loaded: %s", text) if "yaw alignment complete" in text.lower() and now - t_start >= _MIN_WAIT: ekf_ok = True break - elif msg.get_type() == "ATTITUDE": - if (all(math.isfinite(v) for v in (msg.roll, msg.pitch, msg.yaw)) + elif isinstance(decoded, Attitude): + if (all(math.isfinite(v) for v in (decoded.roll, decoded.pitch, decoded.yaw)) and now - t_start >= _MIN_WAIT): log.info( "EKF attitude ready rpy=(%.1f, %.1f, %.1f) deg", - math.degrees(msg.roll), math.degrees(msg.pitch), math.degrees(msg.yaw), + math.degrees(decoded.roll), math.degrees(decoded.pitch), math.degrees(decoded.yaw), ) ekf_ok = True break @@ -2170,11 +2228,11 @@ def _assert_alive() -> None: math.isfinite(v) for v in (_att.roll, _att.pitch, _att.yaw) ): _pre_arm_yaw = float(_att.yaw) - gcs.send_named_float("RAWES_THR", float(passive_thrust)) - gcs.send_named_float("RAWES_RIC", float(passive_roll_rad)) - gcs.send_named_float("RAWES_PIC", float(passive_pitch_rad)) + gcs.send_message(NamedValueFloat("RAWES_THR", float(passive_thrust))) + gcs.send_message(NamedValueFloat("RAWES_RIC", float(passive_roll_rad))) + gcs.send_message(NamedValueFloat("RAWES_PIC", float(passive_pitch_rad))) if passive_yaw_rad is not None: - gcs.send_named_float("RAWES_YIC", float(passive_yaw_rad)) + gcs.send_message(NamedValueFloat("RAWES_YIC", float(passive_yaw_rad))) _pre_arm_yaw = float(passive_yaw_rad) log.info( "PASSIVE seed: RAWES_THR=%.3f, RIC=%+.4f, PIC=%+.4f, YIC=%s; pre-arm yaw=%.1f deg", diff --git a/tests/sitl/torque/torque_test_utils.py b/tests/sitl/torque/torque_test_utils.py index 26ed345..30c7321 100644 --- a/tests/sitl/torque/torque_test_utils.py +++ b/tests/sitl/torque/torque_test_utils.py @@ -17,12 +17,14 @@ from __future__ import annotations import math +from pathlib import Path import pytest from simulation.telemetry_csv import TelRow, write_csv from simulation.mediator_events import MediatorEventLog from simulation.simtest_log import BadEventLog +from groundstation.gcs import Attitude, NamedValueFloat, ServoOutputRaw, StatusText, decode_message from tests.sitl.stack_infra import observe # noqa: E402 @@ -67,38 +69,32 @@ def run_observation_loop( def handle(msg, t_rel): if msg is None: return None - mt = msg.get_type() - if mt == "STATUSTEXT": - log.debug("SITL t=%.1fs: %s", t_rel, msg.text.rstrip("\x00").strip()) - elif mt == "SERVO_OUTPUT_RAW": - pwm[0] = yaw_motor_pwm_from_servo_output(msg, default=pwm[0]) - elif mt == "NAMED_VALUE_FLOAT": - raw_name = getattr(msg, "name", "") - if isinstance(raw_name, bytes): - name = raw_name.decode("ascii", errors="ignore") - else: - name = str(raw_name) - name = name.rstrip("\x00").strip() - mapped = nvf_map.get(name) - if mapped is not None: - nvf_latest[mapped] = float(getattr(msg, "value", float("nan"))) - elif mt == "ATTITUDE": - rows.append(TelRow( - t_sim=t_rel, phase="DYNAMIC", - rpy_roll=msg.roll, rpy_pitch=msg.pitch, rpy_yaw=msg.yaw, - omega_z=msg.yawspeed, omega_rotor=ctx.omega_rotor, - servo_mot_us=float(pwm[0]), - mav_nvf_yff_trim=float(nvf_latest["mav_nvf_yff_trim"]), - mav_nvf_yff_u=float(nvf_latest["mav_nvf_yff_u"]), - mav_nvf_yff_gz=float(nvf_latest["mav_nvf_yff_gz"]), - )) - if t_rel >= settle_s: - obs.append({"t": t_rel, "yaw": msg.yaw, "yaw_rate": msg.yawspeed}) - if len(obs) % log_interval_samples == 0: - log.info("t=%6.1f s psi=%+7.2f deg psi_dot=%+6.2f deg/s pwm=%d", - t_rel, math.degrees(msg.yaw), math.degrees(msg.yawspeed), pwm[0]) - if t_rel >= settle_s + observe_s: - return True + match decode_message(msg): + case StatusText(text=text): + log.debug("SITL t=%.1fs: %s", t_rel, text) + case ServoOutputRaw() as servo: + pwm[0] = yaw_motor_pwm_from_servo_output(servo, default=pwm[0]) + case NamedValueFloat(name=name, value=value): + mapped = nvf_map.get(name) + if mapped is not None: + nvf_latest[mapped] = value + case Attitude() as att: + rows.append(TelRow( + t_sim=t_rel, phase="DYNAMIC", + rpy_roll=att.roll, rpy_pitch=att.pitch, rpy_yaw=att.yaw, + omega_z=att.yawspeed, omega_rotor=ctx.omega_rotor, + servo_mot_us=float(pwm[0]), + mav_nvf_yff_trim=float(nvf_latest["mav_nvf_yff_trim"]), + mav_nvf_yff_u=float(nvf_latest["mav_nvf_yff_u"]), + mav_nvf_yff_gz=float(nvf_latest["mav_nvf_yff_gz"]), + )) + if t_rel >= settle_s: + obs.append({"t": t_rel, "yaw": att.yaw, "yaw_rate": att.yawspeed}) + if len(obs) % log_interval_samples == 0: + log.info("t=%6.1f s psi=%+7.2f deg psi_dot=%+6.2f deg/s pwm=%d", + t_rel, math.degrees(att.yaw), math.degrees(att.yawspeed), pwm[0]) + if t_rel >= settle_s + observe_s: + return True return None observe(ctx, settle_s + observe_s + timeout_margin_s, handle, diff --git a/tests/unit/test_armon_lua.py b/tests/unit/test_armon_lua.py index b4de789..5ec5c23 100644 --- a/tests/unit/test_armon_lua.py +++ b/tests/unit/test_armon_lua.py @@ -12,6 +12,7 @@ from simulation.rawes_lua_harness import RawesLua +from groundstation.gcs import NamedValueFloat # --------------------------------------------------------------------------- @@ -29,7 +30,7 @@ def _armon_deadline_ms(sim: RawesLua): def _send_arm(sim: RawesLua, ms: float): - sim.send_named_float("RAWES_ARM", ms) + sim.send_message(NamedValueFloat("RAWES_ARM", ms)) # --------------------------------------------------------------------------- diff --git a/tests/unit/test_math_lua.py b/tests/unit/test_math_lua.py index 6cc8050..2c8b325 100644 --- a/tests/unit/test_math_lua.py +++ b/tests/unit/test_math_lua.py @@ -21,6 +21,7 @@ import pytest import simulation +from groundstation.gcs import NamedValueFloat from simulation.controller import ( compute_bz_altitude_hold, ) @@ -317,8 +318,8 @@ def test_first_angle_command_is_close_to_ic_body_z(self, sim): send_anchor_ned(sim, -float(pos0[0]), -float(pos0[1]), -float(pos0[2])) sim.pos_ned = [0.0, 0.0, 0.0] - sim.send_named_float("RAWES_THR", float(ic["eq_thrust"])) - sim.send_named_float("RAWES_TEN", float(ic["tension_eq_n"])) + sim.send_message(NamedValueFloat("RAWES_THR", float(ic["eq_thrust"]))) + sim.send_message(NamedValueFloat("RAWES_TEN", float(ic["tension_eq_n"]))) # Capture needs one run_flight call; angle command appears on the next. sim.run(0.20) diff --git a/viz3d/visualize_torque.py b/viz3d/visualize_torque.py index 18d7a50..56c32f4 100644 --- a/viz3d/visualize_torque.py +++ b/viz3d/visualize_torque.py @@ -38,7 +38,7 @@ import sys import time from pathlib import Path -from typing import List, Optional +from typing import Any, List, Optional, Protocol, cast import numpy as np @@ -49,6 +49,43 @@ from simulation.mediator_torque import PROFILES as _MEDIATOR_PROFILES from simulation.torque_model import HubParams as _HubParams, equilibrium_throttle as _eq_throttle + +class _TextActorLike(Protocol): + def SetInput(self, text: str) -> None: ... + + def GetTextProperty(self) -> Any: ... + + +class _PlotterLike(Protocol): + title: str + camera_position: Any + window_size: tuple[int, int] + render_window: Any + + def add_mesh(self, *args, **kwargs) -> Any: ... + + def add_axes(self, *args, **kwargs) -> Any: ... + + def add_key_event(self, *args, **kwargs) -> Any: ... + + def add_text(self, *args, **kwargs) -> _TextActorLike: ... + + def set_background(self, *args, **kwargs) -> Any: ... + + def enable_anti_aliasing(self, *args, **kwargs) -> Any: ... + + def render(self, *args, **kwargs) -> Any: ... + + def show(self, *args, **kwargs) -> Any: ... + + def open_gif(self, *args, **kwargs) -> Any: ... + + def write_frame(self, *args, **kwargs) -> Any: ... + + def close(self, *args, **kwargs) -> Any: ... + + def update(self, *args, **kwargs) -> Any: ... + _MODEL_PARAMS = _HubParams() # default GB4008 params (same as mediator default) @@ -212,7 +249,7 @@ class EventLog: _COLOURS = {"info": "white", "warn": "yellow", "pass": "lime", "fail": "red"} - def __init__(self, pl: pv.Plotter, + def __init__(self, pl: _PlotterLike, settle_s: float, observe_s: float, threshold: float, omega_nom: float = 28.0) -> None: self.pl = pl @@ -348,7 +385,7 @@ class TorqueScene: update() only sets actor.user_matrix — no mesh objects created per frame. """ - def __init__(self, pl: pv.Plotter, + def __init__(self, pl: _PlotterLike, settle_s: float = 40.0, observe_s: float = 20.0, threshold_degs: float = 1.0) -> None: @@ -548,7 +585,7 @@ def play(frames: List[TorqueTelemetryFrame], f"result={meta.get('result','?')}" ) - pl = pv.Plotter(title=title, off_screen=(export is not None)) + pl = cast(_PlotterLike, pv.Plotter(title=title, off_screen=(export is not None))) pl.set_background((0.05, 0.05, 0.08)) # very dark blue-black pl.camera_position = [ (0.55, -0.75, 0.80), @@ -674,8 +711,8 @@ def _cb_minus(): speed[0] = max(speed[0] / 2, 0.125) # FPS counter + controls — pre-created, FPS updated in-place via SetInput pl.add_text("Space=play/pause =step +/-=speed N/B=next/prev file drag=orbit", position=(10, 10), font_size=7, color=(0.45, 0.45, 0.45)) - _fps_actor = pl.add_text("FPS --.-", position=(10, 28), - font_size=9, font="courier", color=(0.5, 0.9, 0.5)) + _fps_actor = cast(_TextActorLike, pl.add_text("FPS --.-", position=(10, 28), + font_size=9, font="courier", color=(0.5, 0.9, 0.5))) # File counter (top-centre) _w, _h = pl.window_size