diff --git a/.gitignore b/.gitignore index 7fd44b3..261549e 100644 --- a/.gitignore +++ b/.gitignore @@ -18,8 +18,12 @@ env/ simulation/logs/ *.log -# One-off / diagnostic scripts (kept locally for reproducibility, not checked in by default) -tests/oneoff/ +# One-off / diagnostic scripts (kept locally for reproducibility, not checked in +# by default). Matched at any depth -- e.g. simulation/tests/oneoff/ is a stray +# legacy path that predates tests/oneoff/ moving to the repo-root tests/ package +# (see AGENTS.md repo layout); anchoring this to root only would silently leave +# that directory's scratch scripts untracked-but-visible to `git status`. +**/tests/oneoff/ # Generated flight report plots *.png diff --git a/AGENTS.md b/AGENTS.md index 33fb754..38e4015 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -118,12 +118,23 @@ Use the primary doc for each topic. Other docs should link, not restate. | Hardware assembly and components | `design/hardware.md` | `design/components.md`, `design/dshot.md`, `design/flap_sensor_bench.md` | | Testing taxonomy and Lua/Python test conventions | `design/testing.md` | `pyproject.toml` (`[tool.pytest.ini_options]`) | | Milestones and decisions history | `design/history.md` | this file (summary only) | +| MAVLink `*.mavlink.jsonl` log inspection (calibrate `run`, SITL stack tests) | `analysis/mavlink_jsonl_query.md` | `design/calibration.md` | Parameter-reference ownership note: - Canonical place for ArduPilot parameter defaults and inline explanations is `tests/sitl/copter-heli.parm`. - Canonical place for RAWES_* parameter defaults and inline explanations is `tests/sitl/rawes_common_defaults.parm`. - If a parameter explanation changes, update the owning `.parm` file first; other docs should link to it instead of duplicating bitmasks/tables. +## MAVLink Log Diagnosis (Agent Critical) + +For ANY problematic run that produced a `*.mavlink.jsonl` log (calibrate `run`, +SITL stack tests), use `analysis/mavlink_jsonl_query.py` as the first-line +diagnostic tool -- before writing one-off jsonl-parsing code or manually +grepping the raw file. See `analysis/mavlink_jsonl_query.md` for the full +interface (subcommands, filters, gotchas); do not duplicate its contents +here. If a diagnosis need doesn't fit an existing subcommand, prefer +extending the script (new subcommand/filter) over a standalone script. + ## Core Invariants (summary) - Frames: simulation physics runs NED world + FRD body. diff --git a/analysis/mavlink_jsonl_query.md b/analysis/mavlink_jsonl_query.md new file mode 100644 index 0000000..d9bdc67 --- /dev/null +++ b/analysis/mavlink_jsonl_query.md @@ -0,0 +1,109 @@ +# `mavlink_jsonl_query.py` -- MAVLink JSONL log query tool + +Canonical usage doc for `analysis/mavlink_jsonl_query.py`, the standard, +first-line tool for diagnosing any problematic run that produced a +`*.mavlink.jsonl` log (calibrate `run`, SITL stack tests). Prefer this tool +over writing one-off jsonl-parsing code or manually grepping the raw file. If +a diagnosis need doesn't fit an existing subcommand, prefer extending this +script (new subcommand/filter) over a standalone script -- keep this doc in +sync with any command/flag changes. + +Referenced from `AGENTS.md` (Documentation Ownership table + "MAVLink Log +Diagnosis" section) and `design/calibration.md`. + +## Log format + +Logs are written by `MavlinkLogWriter` (`groundstation/mavlink_log.py`) from +both `groundstation/gcs.py` (`RawesGCS.start_mavlog`) and the bench +calibration tool (`calibrate/run.py`). Every line is one JSON object: + +```json +{"_t_wall": , "_dir": "rx"|"tx", "mavpackettype": "", ...fields...} +``` + +`_t_wall` is wall-clock seconds; `_dir` is `"rx"` (from the FC) or `"tx"` (to +the FC). The tool adds a derived `t_rel` field (seconds since the first +message in the log) to every message before filtering/printing. + +## Subcommands + +``` +mavlink_jsonl_query.py types +mavlink_jsonl_query.py show [filters] [--fields a,b,c] [--json] [--limit N] +mavlink_jsonl_query.py count [filters] [--by FIELD] +mavlink_jsonl_query.py stats --type T --field F [filters] +mavlink_jsonl_query.py armed +mavlink_jsonl_query.py statustext [--since S] [--until S] [--min-severity N] +mavlink_jsonl_query.py nvf [--name RAWES_ARM] [--dir rx|tx] +mavlink_jsonl_query.py param [--id RAWES_MODE] +``` + +Run `--help` on any subcommand for the exact flags; this doc gives the intent +and gotchas for each. + +- **`types`** -- list every `mavpackettype` seen, split by direction, with + count and first/last `t_rel`. Always the right first command on an unknown + log: confirms whether a message type is present at all before chasing a + decode bug. A message type with count 0 (i.e. absent from this list) means + it was never requested/streamed from the FC -- not a decode/registration + bug in `groundstation/gcs.py`. +- **`show`** -- generic filtered dump of raw message fields, one line per + message. Use `--fields` to restrict columns, `--json` to emit raw JSON + lines (for piping into `jq`/Python), `--limit` to cap output. This is the + fallback for any message type without a dedicated subcommand; it does NOT + do any special-casing (e.g. no STATUSTEXT reassembly -- use the + `statustext` subcommand for that). +- **`count`** -- count matched messages, optionally grouped by a field + (`--by mavpackettype`, `--by mode`, etc). +- **`stats`** -- min/max/mean/median/stdev for one numeric field, e.g. + `--type ATTITUDE --field yaw`. +- **`armed`** -- timeline of HEARTBEAT armed-state/mode transitions (rx only, + excludes GCS self-heartbeats). Good for confirming arm/disarm timing and + flight-mode transitions without wading through every HEARTBEAT. +- **`statustext`** -- dump `gcs:send_text()`/STATUSTEXT messages in order, + **reassembled** from ArduPilot's wire-level chunking. See "STATUSTEXT + reassembly" below -- this is the one subcommand where NOT using the tool + (i.e. grepping the raw jsonl) actively produces misleading output. +- **`nvf`** -- dump the `NAMED_VALUE_FLOAT` stream, i.e. the ground<->Lua + `RAWES_*` interface. Filter with `--name RAWES_ARM` to follow one channel, + `--dir tx` to see only what the ground sent. +- **`param`** -- dump `PARAM_SET`/`PARAM_VALUE` events, filter with `--id`. + +## Common filters (`show`/`count`/`stats`) + +- `-t/--type TYPE` (repeatable; OR) -- e.g. `-t ATTITUDE -t AHRS2` +- `--dir {rx,tx}` +- `--since SEC` / `--until SEC` -- `t_rel` bounds +- `--eq FIELD=VALUE` (repeatable; AND) -- e.g. `--eq mavpackettype=STATUSTEXT` +- `--contains TEXT` -- substring match on the raw JSON line (case-insensitive) + +## STATUSTEXT reassembly (agent-critical) + +ArduPilot's `gcs:send_text()`/STATUSTEXT wire format caps the `text` field at +50 bytes. Any Lua `gcs:send_text()` call longer than that is split across +multiple STATUSTEXT messages that share a common non-zero `id` field, ordered +by `chunk_seq` (0, 1, 2, ...). `id == 0` is the ArduPilot sentinel for a +standalone, already-short single-chunk message -- those must never be merged +with each other even if several appear back-to-back. + +`mavlink_jsonl_query.py statustext` reassembles chunks sharing the same +non-zero `id` into a single logical line before printing. Do not use `show +-t STATUSTEXT` or grep the raw jsonl directly when reading Lua diagnostic +text -- both will print each ~50-byte fragment as its own line, sometimes +splitting a message mid-word (e.g. `"RAWES YIC capture: r=89.9 p=20.0 y=-175.3"` +can arrive on the wire as `"RAWES YIC capture: r=89.9 p=20.0 y=-17"` + +`"5.3"`), which is hard to read for both humans and AI and can look like log +corruption when it's just unreassembled chunking. + +## Example: diagnosing a "target never updates" bug + +``` +mavlink_jsonl_query.py types simulation/logs/calibrate/run_passive_....mavlink.jsonl +mavlink_jsonl_query.py statustext simulation/logs/calibrate/run_passive_....mavlink.jsonl +mavlink_jsonl_query.py show -t ATTITUDE_TARGET --fields q simulation/logs/calibrate/run_passive_....mavlink.jsonl +``` + +`types` confirms the message is actually present (vs. a missing stream +request); `statustext` surfaces Lua's own diagnostic text (e.g. `RAWES YIC +capture: ...`, `RAWES guided cmd: ...`) to compare what Lua *thinks* it +captured/sent against what the FC is actually reporting. diff --git a/analysis/mavlink_jsonl_query.py b/analysis/mavlink_jsonl_query.py index 0dba093..1808c65 100644 --- a/analysis/mavlink_jsonl_query.py +++ b/analysis/mavlink_jsonl_query.py @@ -8,10 +8,13 @@ {"_t_wall": , "_dir": "rx"|"tx", "mavpackettype": "", ...fields...} -This is the standard tool for ad-hoc inspection of these logs -- prefer -extending this script (new subcommand or filter) over writing one-off jsonl -parsing code. See mavlink_jsonl_query.md in this directory for usage -docs/examples; keep that file in sync with any command/flag changes. +This is the standard, first-line tool for diagnosing any problematic run that +has a `*.mavlink.jsonl` log (calibrate `run`, SITL stack tests) -- prefer it +over writing one-off jsonl parsing code or manually grepping the raw file, and +prefer extending this script (new subcommand or filter) over either of those. +See mavlink_jsonl_query.md in this directory for the canonical usage doc +(subcommand intent, gotchas, examples); keep that file in sync with any +command/flag changes. Usage summary (see `--help` on each subcommand for full options): mavlink_jsonl_query.py types @@ -229,19 +232,68 @@ def cmd_armed(args: argparse.Namespace) -> None: last_mode = mode +def _reassemble_statustext(rows: Iterable[dict]) -> Iterator[dict]: + """Reassemble multi-chunk STATUSTEXT messages into single logical lines. + + ArduPilot's gcs:send_text()/STATUSTEXT wire format caps `text` at 50 bytes; + longer messages are split across multiple STATUSTEXT messages that share a + common non-zero `id`, ordered by `chunk_seq` (0, 1, 2, ...). `id == 0` is + the ArduPilot sentinel for a standalone, already-short single-chunk + message -- those must NEVER be merged with each other even if several + appear back-to-back. Without this reassembly, `show`/raw dumps print each + ~50-char fragment as its own line/timestamp, which is hard for both humans + and AI to read (a single log line like "RAWES YIC capture: r=89.9 p=20.0 + y=-175.3" can be split mid-word across two lines). + """ + pending_id: "int | None" = None + pending_chunks: dict[int, str] = {} + pending_meta: "dict | None" = None + + def _flush() -> "dict | None": + if pending_meta is None: + return None + out = dict(pending_meta) + out["text"] = "".join(pending_chunks[k] for k in sorted(pending_chunks)) + return out + + for m in rows: + mid = m.get("id", 0) + text = m.get("text", "") + if isinstance(text, (bytes, bytearray)): + text = text.decode("utf-8", errors="replace") + text = text.split("\x00", 1)[0] + if mid == 0: + flushed = _flush() + if flushed is not None: + yield flushed + pending_id, pending_chunks, pending_meta = None, {}, None + out = dict(m) + out["text"] = text + yield out + continue + if mid != pending_id: + flushed = _flush() + if flushed is not None: + yield flushed + pending_id = mid + pending_chunks = {} + pending_meta = dict(m) + pending_chunks[m.get("chunk_seq", 0)] = text + flushed = _flush() + if flushed is not None: + yield flushed + + def cmd_statustext(args: argparse.Namespace) -> None: msgs = load(args.log) - for m in apply_filters(msgs, types=["STATUSTEXT"], direction="rx", - since=args.since, until=args.until): + rows = apply_filters(msgs, types=["STATUSTEXT"], direction="rx", + since=args.since, until=args.until) + for m in _reassemble_statustext(rows): sev = m.get("severity", -1) if args.min_severity is not None and sev > args.min_severity: # NOTE: lower severity number == more severe (MAVLink convention) continue - text = m.get("text", "") - if isinstance(text, (bytes, bytearray)): - text = text.decode("utf-8", errors="replace") - text = text.split("\x00", 1)[0] - print(f"[{m['t_rel']:>9.3f}s] {MAV_SEVERITY.get(sev, sev):<9} {text}") + print(f"[{m['t_rel']:>9.3f}s] {MAV_SEVERITY.get(sev, sev):<9} {m['text']}") def cmd_nvf(args: argparse.Namespace) -> None: diff --git a/calibrate.cmd b/calibrate.cmd index 58f3fda..b61723c 100644 --- a/calibrate.cmd +++ b/calibrate.cmd @@ -2,43 +2,9 @@ setlocal set "ROOT=%~dp0" -set "VENV=%ROOT%.venv" -set "PYTHON=%VENV%\Scripts\python.exe" -set "PIP=%VENV%\Scripts\pip.exe" -set "REQ=%ROOT%simulation\requirements.txt" -set "STAMP=%VENV%\.req_stamp" +set "PYTHON=%ROOT%.venv\Scripts\python.exe" -if not exist "%VENV%\Scripts\activate.bat" ( - echo Creating venv at %VENV% ... - python -m venv "%VENV%" - if errorlevel 1 ( - echo ERROR: Failed to create venv. Is Python 3 on PATH? - exit /b 1 - ) -) - -if not exist "%PYTHON%" ( - echo ERROR: venv python not found at %PYTHON% - exit /b 1 -) - -:: Only reinstall if requirements.txt changed since last install -for /f "delims=" %%H in ('certutil -hashfile "%REQ%" MD5 ^| findstr /v "hash"') do set REQ_HASH=%%H -set REQ_HASH=%REQ_HASH: =% - -set STAMP_HASH= -if exist "%STAMP%" ( - set /p STAMP_HASH=<"%STAMP%" -) - -if not "%REQ_HASH%"=="%STAMP_HASH%" ( - echo Installing/verifying requirements ... - "%PIP%" install -q -r "%REQ%" - if errorlevel 1 ( - echo ERROR: pip install failed. - exit /b 1 - ) - echo %REQ_HASH%>"%STAMP%" -) +call "%ROOT%setup.cmd" +if errorlevel 1 exit /b 1 "%PYTHON%" -m calibrate %* diff --git a/calibrate/constants.py b/calibrate/constants.py index 23f8102..20cb310 100644 --- a/calibrate/constants.py +++ b/calibrate/constants.py @@ -14,6 +14,7 @@ # Re-exported so submodules can do `from .constants import RawesGCS` etc. from groundstation.gcs import ( Attitude, + AttitudeQuaternion, EscTelemetry, PidTuning, BatteryStatus, diff --git a/calibrate/run.py b/calibrate/run.py index f3d85cc..61b5ec0 100644 --- a/calibrate/run.py +++ b/calibrate/run.py @@ -24,6 +24,7 @@ def getch(self): return b"" from .constants import ( Attitude, + AttitudeQuaternion, Heartbeat, EscTelemetry, PidTuning, @@ -313,6 +314,10 @@ def _run_observation(session: RawesGCS, mode_name: str, "mav_att_target_roll_deg", "mav_att_target_pitch_deg", "mav_att_target_yaw_deg", "mav_att_target_roll_rate_rads", "mav_att_target_pitch_rate_rads", "mav_att_target_yaw_rate_rads", "mav_att_target_thrust", + "mav_att_q_w", "mav_att_q_x", "mav_att_q_y", "mav_att_q_z", + "mav_att_target_q_w", "mav_att_target_q_x", "mav_att_target_q_y", "mav_att_target_q_z", + "mav_att_qerr_w", "mav_att_qerr_x", "mav_att_qerr_y", "mav_att_qerr_z", + "mav_att_qerr_deg", "mav_att_qerr_yaw_deg", "ch1_us", "ch2_us", "ch3_us", "ch4_us", "mav_servo1_us", "mav_servo2_us", "mav_servo3_us", "mav_servo9_us", "vbat_v", "current_a", @@ -329,7 +334,7 @@ def _run_observation(session: RawesGCS, mode_name: str, # Live table: H_YAW_TRIM output (out=trim), motor throttle command (u=YFF_U), # swashplate PWMs (s1..s3), GB4008 motor PWM with 5 s rolling average, rotor RPM. print_cols = ["t(s)", "armed", "yaw(d)", "yrate_d", "out", "u", - "s1", "s2", "s3", "mot", "mot~5s", "V", "A", "mRPM"] + "s1", "s2", "s3", "mot", "mot~5s", "qerr(d)", "qyaw(d)", "mRPM"] mot_window_s = 5.0 # rolling average window for motor (SERVO_MOTOR) PWM @@ -418,6 +423,8 @@ def key_handler(k: bytes) -> None: # no live key tuning for this mode "att_target_roll": None, "att_target_pitch": None, "att_target_yaw": None, "att_target_roll_rate": None, "att_target_pitch_rate": None, "att_target_yaw_rate": None, "att_target_thrust": None, + "att_q": None, # actual attitude quaternion (w, x, y, z) from ATTITUDE_QUATERNION + "att_target_q": None, # target attitude quaternion (w, x, y, z) from ATTITUDE_TARGET "ch1": None, "ch2": None, "ch3": None, "ch4": None, "s1": None, "s2": None, "s3": None, "smot": None, "smot_hist": [], @@ -454,6 +461,41 @@ def _quat_to_rpy_deg(q) -> tuple[float, float, float] | tuple[None, None, None]: yaw = math.degrees(math.atan2(siny_cosp, cosy_cosp)) return roll, pitch, yaw + def _quat_conj(q: tuple[float, float, float, float]) -> tuple[float, float, float, float]: + w, x, y, z = q + return (w, -x, -y, -z) + + def _quat_mul(a: tuple[float, float, float, float], + b: tuple[float, float, float, float]) -> tuple[float, float, float, float]: + aw, ax, ay, az = a + bw, bx, by, bz = b + return ( + aw * bw - ax * bx - ay * by - az * bz, + aw * bx + ax * bw + ay * bz - az * by, + aw * by - ax * bz + ay * bw + az * bx, + aw * bz + ax * by - ay * bx + az * bw, + ) + + def _quat_error(q_actual, q_target): + """Body-frame rotation error q_err = conj(q_actual) (x) q_target -- + i.e. the rotation that takes the actual attitude onto the target + attitude. Using the quaternion error directly (rather than + differencing the two Euler yaws) avoids the +-180 deg wraparound + ambiguity Euler subtraction has at the heading-lock boundary. + + Returns (qerr_w, qerr_x, qerr_y, qerr_z, total_err_deg, yaw_err_deg) + or a tuple of Nones if either input is missing. yaw_err_deg uses the + small-roll/pitch-error approximation 2*atan2(z, w), which is exact + when the error is a pure yaw rotation (the case for a yaw/heading + lock holding roll/pitch elsewhere) and near-exact otherwise.""" + if q_actual is None or q_target is None: + return (None,) * 6 + qe = _quat_mul(_quat_conj(q_actual), q_target) + w, x, y, z = qe + total_deg = math.degrees(2.0 * math.acos(max(-1.0, min(1.0, abs(w))))) + yaw_deg = math.degrees(2.0 * math.atan2(z, w)) + return w, x, y, z, total_deg, yaw_deg + def handle_msg(st, msg, t_rel): if isinstance(msg, Attitude): state["roll"] = math.degrees(msg.roll) @@ -462,6 +504,7 @@ def handle_msg(st, msg, t_rel): state["yaw_rate"] = msg.yawspeed # rad/s (mav_att_yaw_rate_rads) # Emit one CSV row per ATTITUDE message (typically 10-50 Hz) _erpm, _mech, _rotor = _rpm_triplet(state["erpm"]) + _qw, _qx, _qy, _qz, _qerr_deg, _qerr_yaw_deg = _quat_error(state["att_q"], state["att_target_q"]) return [ f"{t_rel:.4f}", int(st["armed"]), _fmt(state["roll"]), _fmt(state["pitch"]), _fmt(state["yaw"]), @@ -469,6 +512,10 @@ def handle_msg(st, msg, t_rel): _fmt(state["att_target_roll"]), _fmt(state["att_target_pitch"]), _fmt(state["att_target_yaw"]), _fmt(state["att_target_roll_rate"]), _fmt(state["att_target_pitch_rate"]), _fmt(state["att_target_yaw_rate"]), _fmt(state["att_target_thrust"]), + *(_fmt(v) for v in (state["att_q"] or (None, None, None, None))), + *(_fmt(v) for v in (state["att_target_q"] or (None, None, None, None))), + _fmt(_qw), _fmt(_qx), _fmt(_qy), _fmt(_qz), + _fmt(_qerr_deg), _fmt(_qerr_yaw_deg), state["ch1"], state["ch2"], state["ch3"], state["ch4"], state["s1"], state["s2"], state["s3"], state["smot"], _fmt(state["vbat"]), _fmt(state["curr"]), @@ -491,6 +538,10 @@ def handle_msg(st, msg, t_rel): state["att_target_pitch_rate"] = msg.body_pitch_rate state["att_target_yaw_rate"] = msg.body_yaw_rate state["att_target_thrust"] = msg.thrust + if msg.q is not None and len(msg.q) == 4: + state["att_target_q"] = tuple(float(v) for v in msg.q) + elif isinstance(msg, AttitudeQuaternion): + state["att_q"] = (msg.q1, msg.q2, msg.q3, msg.q4) elif isinstance(msg, RcChannels): state["ch1"] = msg.chan1_raw state["ch2"] = msg.chan2_raw @@ -593,6 +644,9 @@ def _fresh_or_stale(value, value_ts, fmt): mrpm_avg_s = None if state["mrpm_hist"]: mrpm_avg_s = f"{sum(v for _, v in state['mrpm_hist']) / len(state['mrpm_hist']):.0f}" + _, _, _, _, _qerr_deg, _qerr_yaw_deg = _quat_error(state["att_q"], state["att_target_q"]) + qerr_s = f"{_qerr_deg:+6.2f}" if _qerr_deg is not None else None + qyaw_s = f"{_qerr_yaw_deg:+6.2f}" if _qerr_yaw_deg is not None else None return [ f"{t_rel:.1f}", "YES" if st["armed"] else "no", @@ -601,8 +655,8 @@ def _fresh_or_stale(value, value_ts, fmt): out_s, i_s, state["s1"], state["s2"], state["s3"], state["smot"], mot_avg_s, - f"{state['vbat']:.2f}" if state["vbat"] is not None else None, - f"{state['curr']:.2f}" if state["curr"] is not None else None, + qerr_s, + qyaw_s, mrpm_avg_s, ] @@ -619,6 +673,32 @@ def _trim_streams() -> None: param2=200000.0, )) # 5 Hz + # ATTITUDE_QUATERNION (#31) isn't guaranteed to ride along with the + # legacy EXTRA1 REQUEST_DATA_STREAM group on every AP build, so request + # it explicitly at the same rate as ATTITUDE -- needed for the + # quaternion heading-lock deviation columns (mav_att_qerr_*). + 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_ATTITUDE_QUATERNION), + param2=40000.0, + )) # 25 Hz + + # ATTITUDE_TARGET (#83) -- the FC's telemetry echo of the active GUIDED + # angle target -- is likewise NOT part of the EXTRA1 stream group and + # was previously only listed in the msg_types receive filter with no + # request ever sent for it, so it never arrived (mav_att_target_q_*/ + # mav_att_qerr_* stayed empty even while a target was actively being + # held). Request it explicitly, same as ATTITUDE_QUATERNION above. + 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_ATTITUDE_TARGET), + param2=40000.0, + )) # 25 Hz + if not keep_rc: session.send_message(CommandLong( target_system=session._target_system, @@ -637,7 +717,7 @@ def _trim_streams() -> None: session, duration_s=duration, msg_types=["ATTITUDE", "RC_CHANNELS", "SERVO_OUTPUT_RAW", - "ATTITUDE_TARGET", "PID_TUNING", + "ATTITUDE_TARGET", "ATTITUDE_QUATERNION", "PID_TUNING", "HEARTBEAT", "STATUSTEXT", "BATTERY_STATUS", "SYS_STATUS", "NAMED_VALUE_FLOAT", _esc_telem_msg_for_channel(MOTOR_ESC_CHANNEL)[0]], diff --git a/design/calibration.md b/design/calibration.md index 673dfd7..3d52292 100644 --- a/design/calibration.md +++ b/design/calibration.md @@ -183,6 +183,31 @@ For `run`, the CSV now also captures: - Lua diagnostic NVFs: `YFF_*` and `OL_*` - `ATTITUDE_TARGET` state when emitted by ArduPilot - `PID_TUNING` state when emitted by ArduPilot +- Actual (`ATTITUDE_QUATERNION`) and target (`ATTITUDE_TARGET.q`) attitude + quaternions (`mav_att_q_*` / `mav_att_target_q_*`), plus the quaternion + attitude error (`mav_att_qerr_*`): `conj(q_actual) (x) q_target`, its total + rotation angle (`mav_att_qerr_deg`), and a yaw-only deviation + (`mav_att_qerr_yaw_deg`, via `2*atan2(z, w)`). The Lua heading/yaw lock is + implemented as a quaternion attitude target under the hood (AP's + `set_target_angle_and_rate_and_throttle` converts the Euler args to a + quaternion before handing off to the attitude controller), so comparing + quaternions directly avoids the +-180 deg wraparound ambiguity that + differencing the two Euler yaw columns has near the wrap boundary. + +Neither `ATTITUDE_QUATERNION` (#31) nor `ATTITUDE_TARGET` (#83) rides along +with the legacy `EXTRA1` `REQUEST_DATA_STREAM` group on ArduCopter -- `run` +explicitly requests both via `MAV_CMD_SET_MESSAGE_INTERVAL` at 25 Hz. Without +that explicit request, `mav_att_q_*`/`mav_att_target_q_*`/`mav_att_qerr_*` +stay empty even while a GUIDED angle target is actively held (verify with +`analysis/mavlink_jsonl_query.py types .mavlink.jsonl` -- if a message +type never appears at all, it's a missing stream/interval request, not a +decode bug; see `analysis/mavlink_jsonl_query.md` for full usage -- it is +the first-line tool for diagnosing any problematic run). `ATTITUDE_TARGET` +also only appears at all once the vehicle is +actually in `GUIDED`/`GUIDED_NOGPS` and Lua is driving an angle target (e.g. +`run passive --hold`, or `steady`/`pumping`) -- a bare `run passive` without +`--hold`/an IC-seeded IC still logs `mav_att_q_*` (actual attitude) but only +gets a non-empty target/`qerr` once the hold is engaged. `PID_TUNING` caveat: ArduPilot may suppress these messages when `GCS_PID_MASK=0`. calibrate requests `PID_TUNING`, but the FC must still be configured to emit it. diff --git a/design/flight_stack.md b/design/flight_stack.md index d210f8a..168b397 100644 --- a/design/flight_stack.md +++ b/design/flight_stack.md @@ -348,6 +348,7 @@ All other flight tunables (anchor position, slew rate, cyclic gains) are deliver | RAWES_RIC | rad | IC roll — part of the atomic passive IC seed (`RAWES_RIC`/`RAWES_PIC`/`RAWES_THR`). MODE_PASSIVE commands it as the GUIDED roll angle target. | | 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. | +| RAWES_YIC | rad, or the `RAWES_YIC_CAPTURE_SENTINEL` (-1000) | Fixed yaw target for MODE_PASSIVE, held as an absolute setpoint instead of capturing the (possibly spinning) AHRS yaw. Sending the sentinel instead captures the CURRENT roll/pitch/yaw all at once from AHRS (calibrate `run passive --hold`) — see §4.2b "IC seeding via capture" for the one-shot-per-boot commit gotcha. | **Named int inputs (ground → Lua, via `gcs.send_message(NamedValueInt(...))`, one-shot anchor location):** @@ -424,6 +425,31 @@ Until all three arrive, `run_passive_mode` returns early and emits no control-API traffic (no guided target writes, no arm/disarm). Once `_ic_seeded` latches, incremental updates to any of the three are accepted. +**IC seeding via capture (`RAWES_YIC` sentinel / calibrate `--hold`).** +Instead of ground-supplied `RAWES_RIC`/`RAWES_PIC` values, sending +`RAWES_YIC = RAWES_YIC_CAPTURE_SENTINEL` (-1000 rad, calibrate `run passive +--hold`) tells Lua to capture the **current** AHRS roll/pitch/yaw as the IC +seed instead — i.e. "hold exactly the attitude the vehicle is in right now" +(no `RAWES_RIC`/`RAWES_PIC` should be sent alongside it). This capture writes +`_ic_pending_roll_deg`/`_ic_pending_pitch_deg` (and `_passive_yaw_fixed_rad`) +AND commits directly to `_ic_roll_deg`/`_ic_pitch_deg` in the same tick — the +direct commit is required because `_ic_seeded` is Lua global state that +persists across `RAWES_MODE` transitions for the whole FC boot (it is never +reset on mode entry). If a `--hold` capture happens after passive has already +been seeded once earlier in the same boot (e.g. an earlier `--roll/--pitch` +run, or a prior `--hold`), relying only on the pending fields would leave +`_ic_roll_deg`/`_ic_pitch_deg` frozen at the stale prior value forever, since +the "already seeded" incremental-update branch only reacts to explicit +`RAWES_RIC`/`RAWES_PIC` NVFs, not to the sentinel-capture pending fields. This +previously caused the GUIDED angle target to stay at the *previous* run's +roll/pitch (e.g. 0/0) while the vehicle held a completely different actual +attitude, producing a large, non-decaying `mav_att_qerr_deg` in `calibrate run` +telemetry — diagnose this class of bug by comparing `RAWES YIC capture: r=... +p=... y=...` against subsequent `RAWES guided cmd: ... rpy=(...)` STATUSTEXT +lines (`analysis/mavlink_jsonl_query.py statustext`): if the commanded rpy +never converges to the captured r/p, the capture never reached the committed +fields. + **Per-tick command** (once seeded and in GUIDED): 1. Yaw is captured once from `ahrs:get_yaw_rad()` on the first ready tick diff --git a/design/sitl_testing.md b/design/sitl_testing.md index 102d86a..4159d8e 100644 --- a/design/sitl_testing.md +++ b/design/sitl_testing.md @@ -162,6 +162,7 @@ first — diagnosing from bad telemetry produces wrong conclusions. |------|---------| | Pump cycle diagnosis | `.venv/Scripts/python.exe analysis/pump_diagnosis.py --test test_pump_cycle_unified --bucket 1` | | Landing diagnosis | `.venv/Scripts/python.exe analysis/analyse_landing.py [--test test_landing_lua_sitl] [--bucket 2]` | +| Raw MAVLink inspection (STATUSTEXT, message presence, NVF/param events) | `.venv/Scripts/python.exe analysis/mavlink_jsonl_query.py ... /*.mavlink.jsonl` -- see [analysis/mavlink_jsonl_query.md](../analysis/mavlink_jsonl_query.md) | | Visualize result | `visualize.cmd simulation/logs//telemetry.csv` | | EKF gating reference | [design/EKF_GATING.md](EKF_GATING.md), [design/ekf_const_pos_mode.md](ekf_const_pos_mode.md) | diff --git a/groundstation/gcs.py b/groundstation/gcs.py index fba2a86..27dad1a 100644 --- a/groundstation/gcs.py +++ b/groundstation/gcs.py @@ -645,7 +645,11 @@ def send(self, mav) -> None: @dataclass(frozen=True) class SetAttitudeTarget: """MAVLink SET_ATTITUDE_TARGET (#82) -- mirrors - mavlink_set_attitude_target_t.""" + mavlink_set_attitude_target_t. + + Also used to decode the FC's ATTITUDE_TARGET (#83) telemetry echo of the + active attitude target (same field layout minus target_system/component). + `decode_message()` registers both wire message names against this class.""" MAVLINK_TYPE: ClassVar[str] = "SET_ATTITUDE_TARGET" target_system: int @@ -686,6 +690,32 @@ def decode(msg) -> "SetAttitudeTarget": ) +@dataclass(frozen=True) +class AttitudeQuaternion: + """MAVLink ATTITUDE_QUATERNION (#31) -- mirrors + mavlink_attitude_quaternion_t. q1..q4 are (w, x, y, z), NED body attitude.""" + MAVLINK_TYPE: ClassVar[str] = "ATTITUDE_QUATERNION" + + q1: float # w + q2: float # x + q3: float # y + q4: float # z + rollspeed: float = 0.0 # rad/s + pitchspeed: float = 0.0 # rad/s + yawspeed: float = 0.0 # rad/s + time_boot_ms: int = 0 + + @staticmethod + def decode(msg) -> "AttitudeQuaternion": + return AttitudeQuaternion( + q1=float(msg.q1), q2=float(msg.q2), q3=float(msg.q3), q4=float(msg.q4), + rollspeed=float(getattr(msg, "rollspeed", 0.0)), + pitchspeed=float(getattr(msg, "pitchspeed", 0.0)), + yawspeed=float(getattr(msg, "yawspeed", 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.""" @@ -863,10 +893,18 @@ def decode(msg) -> _DecodedMessageT: ... NamedValueFloat, NamedValueInt, SetAttitudeTarget, + AttitudeQuaternion, PidTuning, ) } _MESSAGE_CLASS_BY_TYPE.update({name: EscTelemetry for name in _ESC_CHANNEL_BASE_BY_TYPE}) +# ATTITUDE_TARGET (#83) is the FC's telemetry echo of the active attitude +# target -- same fields as SET_ATTITUDE_TARGET (#82) minus target_system/ +# component, so it decodes to the same dataclass. Without this, ATTITUDE_TARGET +# messages fell through decode_message() unchanged (raw pymavlink object), so +# `isinstance(decoded, SetAttitudeTarget)` checks downstream never matched and +# attitude-target logging columns stayed empty. +_MESSAGE_CLASS_BY_TYPE["ATTITUDE_TARGET"] = SetAttitudeTarget def decode_message(msg): diff --git a/pump_lua.cmd b/pump_lua.cmd index 5b126c4..84a44ba 100644 --- a/pump_lua.cmd +++ b/pump_lua.cmd @@ -15,12 +15,8 @@ REM The aero model is passed to the test via the RAWES_AERO environment variable set "ROOT=%~dp0" set "PY=%ROOT%.venv\Scripts\python.exe" -if not exist "%PY%" ( - echo Python venv not found at: - echo %ROOT%.venv\Scripts\python.exe - echo Run setup.cmd or bash setup.sh to create the root venv. - exit /b 1 -) +call "%ROOT%setup.cmd" +if errorlevel 1 exit /b 1 REM Parse args: --novis is a flag; any other token selects the aero model. set "NOVIS=" diff --git a/scripts/rawes.lua b/scripts/rawes.lua index 681615a..c0f21e0 100644 --- a/scripts/rawes.lua +++ b/scripts/rawes.lua @@ -1145,6 +1145,18 @@ local function update() _nv_floats["RAWES_YIC"] = nil _ic_pending_roll_deg = math.deg(ahrs:get_roll_rad()) _ic_pending_pitch_deg = math.deg(ahrs:get_pitch_rad()) + -- Also commit directly to _ic_roll_deg/_ic_pitch_deg (not just + -- the *_pending_* staging fields). The "not _ic_seeded" atomic + -- commit below only copies pending -> committed ONCE, the first + -- time IC seeding happens after a Lua (re)load. _ic_seeded is + -- Lua global state that persists across RAWES_MODE transitions + -- within the same FC boot, so a second/later --hold capture in + -- the same boot would otherwise land only in the *_pending_* + -- fields and never reach send_guided_angle_rate_throttle(), + -- leaving the GUIDED target frozen at whatever roll/pitch was + -- committed earlier (e.g. 0/0 from a prior --roll/--pitch run). + _ic_roll_deg = _ic_pending_roll_deg + _ic_pitch_deg = _ic_pending_pitch_deg _passive_yaw_fixed_rad = ahrs:get_yaw_rad() gcs:send_text(6, string.format( "RAWES YIC capture: r=%.1f p=%.1f y=%.1f", diff --git a/setup.cmd b/setup.cmd index 2622bc4..4bd0670 100644 --- a/setup.cmd +++ b/setup.cmd @@ -7,6 +7,8 @@ set "VENV=%REPO%\.venv" set "PYTHON=%VENV%\Scripts\python.exe" set "REQS=%REPO%\simulation\requirements.txt" set "STAMP=%VENV%\Scripts\.requirements_hash" +set "PYPROJECT=%REPO%\pyproject.toml" +set "PKG_STAMP=%VENV%\Scripts\.editable_install_hash" :: Create venv if missing if exist "%PYTHON%" goto :check_reqs @@ -16,12 +18,20 @@ if exist "%VENV%" ( ) echo [INFO] Creating venv at %VENV% ... py -3 -m venv "%VENV%" +if errorlevel 1 ( + echo ERROR: Failed to create venv. Is Python 3 on PATH? + exit /b 1 +) "%PYTHON%" -m pip install --upgrade pip --quiet :check_reqs +if not exist "%PYTHON%" ( + echo ERROR: venv python not found at %PYTHON% + exit /b 1 +) if not exist "%REQS%" ( echo [WARN] requirements.txt not found -- skipping install - goto :done + goto :install_pkg ) :: Hash-gated install: only reinstall when requirements.txt changes. @@ -38,13 +48,49 @@ if exist "%STAMP%" set /p CACHED=<"%STAMP%" if "%DIGEST%"=="%CACHED%" ( echo [INFO] requirements.txt unchanged -- skipping pip install - goto :done + goto :install_pkg ) echo [INFO] Installing requirements ... "%PYTHON%" -m pip install -r "%REQS%" +if errorlevel 1 ( + echo ERROR: pip install -r failed. + exit /b 1 +) echo %DIGEST%>"%STAMP%" +:install_pkg +:: Hash-gated editable install: only reinstall when pyproject.toml changes. +:: The editable install is what makes `import simulation`/`groundstation`/etc. +:: work from any cwd or when a script is invoked by path (rather than via +:: `python -c` from repo root) -- but re-running pip install -e on every +:: invocation is unnecessary overhead once it's already registered. +if not exist "%PYPROJECT%" ( + echo [WARN] pyproject.toml not found -- skipping editable install + goto :done +) +for /f "skip=1 tokens=1" %%H in ('certutil -hashfile "%PYPROJECT%" SHA256 2^>nul') do ( + set "PKG_DIGEST=%%H" + goto :got_pkg_hash +) +:got_pkg_hash + +set "PKG_CACHED=" +if exist "%PKG_STAMP%" set /p PKG_CACHED=<"%PKG_STAMP%" + +if "%PKG_DIGEST%"=="%PKG_CACHED%" ( + echo [INFO] pyproject.toml unchanged -- skipping pip install -e + goto :done +) + +echo [INFO] Installing rawes package (editable) ... +"%PYTHON%" -m pip install -e "%REPO%" --no-deps --quiet +if errorlevel 1 ( + echo ERROR: pip install -e failed. + exit /b 1 +) +echo %PKG_DIGEST%>"%PKG_STAMP%" + :done echo [INFO] Done. "%PYTHON%" --version diff --git a/setup.sh b/setup.sh index fe26878..d3b041a 100644 --- a/setup.sh +++ b/setup.sh @@ -1,10 +1,11 @@ #!/usr/bin/env bash # # setup.sh -- RAWES one-time setup tasks. All subcommands are idempotent. -# -# Subcommands: +## Subcommands: # (no args) create or refresh the Windows venv at .venv (repo root) -# hash-gated: requirements.txt is re-installed only when changed. +# hash-gated: requirements.txt and the editable rawes package +# install are each re-installed only when their source +# (requirements.txt / pyproject.toml) changes. # build build rawes-sim runtime with ArduPilot (~30-60 min) # build-lite build rawes-sim runtime without ArduPilot (fast) # hw push canonical params to a real Pixhawk via MAVLink @@ -25,6 +26,8 @@ VENV="$REPO_DIR/.venv" PYTHON="$VENV/Scripts/python.exe" REQS="$SIM_DIR/requirements.txt" STAMP="$VENV/Scripts/.requirements_hash" +PYPROJECT="$REPO_DIR/pyproject.toml" +PKG_STAMP="$VENV/Scripts/.editable_install_hash" _winpath() { if command -v cygpath &>/dev/null; then @@ -65,6 +68,25 @@ _setup_venv() { fi fi + # Hash-gated editable install: only reinstall when pyproject.toml changes. + # The editable install is what makes `import simulation`/`groundstation`/etc. + # work from any cwd or when a script is invoked by path (rather than via + # `python -c` from repo root) -- but re-running pip install -e on every + # invocation is unnecessary overhead once it's already registered. + if [ ! -f "$PYPROJECT" ]; then + echo "[WARN] $PYPROJECT not found -- skipping editable install" + else + local pkg_digest + pkg_digest="$(sha256sum "$PYPROJECT" | awk '{print $1}')" + if [ -f "$PKG_STAMP" ] && [ "$(cat "$PKG_STAMP")" = "$pkg_digest" ]; then + echo "[INFO] pyproject.toml unchanged -- skipping pip install -e" + else + echo "[INFO] Installing rawes package (editable) ..." + "$PYTHON" -m pip install -e "$(_winpath "$REPO_DIR")" --no-deps --quiet + echo "$pkg_digest" > "$PKG_STAMP" + fi + fi + echo "[INFO] Done." "$PYTHON" --version } diff --git a/visualize.cmd b/visualize.cmd index c7e3c5b..e104ce9 100644 --- a/visualize.cmd +++ b/visualize.cmd @@ -10,11 +10,8 @@ if "%~1"=="" ( set "CSV=%~1" ) -if not exist "%PY%" ( - echo Python venv not found. Expected: - echo %ROOT%.venv\Scripts\python.exe - exit /b 1 -) +call "%ROOT%setup.cmd" +if errorlevel 1 exit /b 1 pushd "%ROOT%" >nul start "RAWES visualization" cmd /k ""%PY%" viz3d\visualize_3d.py "%CSV%""