Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion .github/workflows/ci.yml
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
4 changes: 4 additions & 0 deletions AGENTS.md
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
19 changes: 18 additions & 1 deletion calibrate/constants.py
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
190 changes: 106 additions & 84 deletions calibrate/hw.py
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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,
Expand All @@ -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),
))


# ---------------------------------------------------------------------------
Expand Down Expand Up @@ -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):
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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:
Expand Down Expand Up @@ -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
Expand All @@ -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.")
Expand All @@ -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:
Expand All @@ -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.")
Expand Down
28 changes: 20 additions & 8 deletions calibrate/repl.py
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down Expand Up @@ -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.")


Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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

Expand Down
Loading
Loading