From 3226ef97fe574f96455423101981783838023db8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Onur=20=C3=96zkan?= Date: Sat, 7 Nov 2026 18:12:11 +0300 Subject: [PATCH] refactor(firmware): remove the retired simulation workflow Remove the retired simulator workspace member, dependencies, workflow and documentation. Keep the shared flight and hardware crates. Validation: retirement prefix host check, command-dispatch tests and book build. --- .github/workflows/sitl-smoke.yml | 41 - Makefile | 13 +- docs/book.toml | 2 +- docs/src/SUMMARY.md | 1 - docs/src/architecture.md | 7 +- docs/src/configuration.md | 13 +- docs/src/development.md | 18 +- docs/src/errors.md | 2 +- docs/src/getting-started.md | 1 - docs/src/index.md | 10 +- docs/src/logs.md | 9 +- docs/src/reference.md | 1 - docs/src/sitl.md | 59 - firmware/.dockerignore | 1 - firmware/Cargo.lock | 19 - firmware/Cargo.toml | 1 - firmware/README.md | 7 +- firmware/scripts/report-test-coverage.sh | 1 - firmware/tests/hitl/.gitkeep | 0 firmware/tests/sitl/.gitkeep | 0 firmware/tools/sitl/CMakeLists.txt | 13 - firmware/tools/sitl/Cargo.toml | 28 - firmware/tools/sitl/Dockerfile | 43 - firmware/tools/sitl/Makefile | 34 - firmware/tools/sitl/compose.yaml | 33 - firmware/tools/sitl/e2e-smoke.sh | 39 - firmware/tools/sitl/gazebo-entrypoint.sh | 4 - firmware/tools/sitl/kaptan-sim | 589 ------- firmware/tools/sitl/src/config/logging.rs | 8 - firmware/tools/sitl/src/config/mod.rs | 18 - firmware/tools/sitl/src/config/runtime.rs | 47 - firmware/tools/sitl/src/config/store.rs | 126 -- firmware/tools/sitl/src/config/tests.rs | 81 - firmware/tools/sitl/src/kaptan_sitl_quad.cc | 650 -------- firmware/tools/sitl/src/main.rs | 1440 ----------------- firmware/tools/sitl/src/protocol.rs | 363 ----- firmware/tools/sitl/test_kaptan_sim.py | 107 -- firmware/tools/sitl/world-structure-test.py | 157 -- .../tools/sitl/worlds/kaptan-sitl-quad.sdf.in | 204 --- flake.nix | 8 +- 40 files changed, 22 insertions(+), 4176 deletions(-) delete mode 100644 .github/workflows/sitl-smoke.yml delete mode 100644 docs/src/sitl.md delete mode 100644 firmware/tests/hitl/.gitkeep delete mode 100644 firmware/tests/sitl/.gitkeep delete mode 100644 firmware/tools/sitl/CMakeLists.txt delete mode 100644 firmware/tools/sitl/Cargo.toml delete mode 100644 firmware/tools/sitl/Dockerfile delete mode 100644 firmware/tools/sitl/Makefile delete mode 100644 firmware/tools/sitl/compose.yaml delete mode 100755 firmware/tools/sitl/e2e-smoke.sh delete mode 100755 firmware/tools/sitl/gazebo-entrypoint.sh delete mode 100755 firmware/tools/sitl/kaptan-sim delete mode 100644 firmware/tools/sitl/src/config/logging.rs delete mode 100644 firmware/tools/sitl/src/config/mod.rs delete mode 100644 firmware/tools/sitl/src/config/runtime.rs delete mode 100644 firmware/tools/sitl/src/config/store.rs delete mode 100644 firmware/tools/sitl/src/config/tests.rs delete mode 100644 firmware/tools/sitl/src/kaptan_sitl_quad.cc delete mode 100644 firmware/tools/sitl/src/main.rs delete mode 100644 firmware/tools/sitl/src/protocol.rs delete mode 100644 firmware/tools/sitl/test_kaptan_sim.py delete mode 100644 firmware/tools/sitl/world-structure-test.py delete mode 100644 firmware/tools/sitl/worlds/kaptan-sitl-quad.sdf.in diff --git a/.github/workflows/sitl-smoke.yml b/.github/workflows/sitl-smoke.yml deleted file mode 100644 index 5644413..0000000 --- a/.github/workflows/sitl-smoke.yml +++ /dev/null @@ -1,41 +0,0 @@ -name: Host SITL smoke - -on: - pull_request: - push: - workflow_dispatch: - -permissions: - contents: read - -jobs: - smoke: - name: Deterministic Gazebo command path - runs-on: ubuntu-24.04 - timeout-minutes: 25 - steps: - - name: Check out source - uses: actions/checkout@11bd71901bbe5b1630ceea73d27597364c9af683 # v4.2.2 - - - name: Check host-SITL contracts - working-directory: firmware - run: | - python3 -m unittest tools/sitl/test_kaptan_sim.py - python3 tools/sitl/world-structure-test.py - cargo check --locked -p kaptan-mavlink --no-default-features - cargo check --locked -p kaptan-log --no-default-features - cargo check --locked -p kaptan-diagnostics-mavlink - cargo test --locked -p kaptan-log - cargo test --locked -p kaptan-diagnostics-mavlink - if rg -n '(^use log::|\blog::)' platforms/stm32f7/src/f722; then - echo 'the standard log facade is forbidden in F722 IRQ/panic paths' - exit 1 - fi - if rg -n 'max_level_(off|error|warn|info|debug|trace)' --glob 'Cargo.toml' --glob '*.rs' .; then - echo 'log/max_level_* removes records before Kaptan can observe them' - exit 1 - fi - cargo test --locked -p kaptan-sitl - - - name: Run Docker closed-loop smoke - run: firmware/tools/sitl/e2e-smoke.sh diff --git a/Makefile b/Makefile index 2679a0d..b134d2b 100644 --- a/Makefile +++ b/Makefile @@ -6,12 +6,11 @@ override MAKEFLAGS += --no-print-directory override FIRMWARE_DIR := firmware override DOCS_DIR := docs -override SITL_DIR := firmware/tools/sitl override CONFIGURATOR_DIR := firmware/tools/configurator override BOARD_DIRS := $(patsubst %/,%,$(wildcard firmware/boards/*/)) override COMPONENT_TARGETS := $(FIRMWARE_DIR) $(DOCS_DIR) firmware/boards $(BOARD_DIRS) \ - $(SITL_DIR) $(CONFIGURATOR_DIR) + $(CONFIGURATOR_DIR) override PRIMARY_GOAL := $(firstword $(MAKECMDGOALS)) override ROUTED_GOAL := $(filter $(PRIMARY_GOAL),$(COMPONENT_TARGETS)) @@ -57,7 +56,7 @@ help: ## Show the command interface (the default) ' nix-shell Enter the Nix development shell' \ ' build Build all firmware and documentation' \ ' fmt Check Rust, shell, Python, Nix, and TOML formatting' \ - ' test Run Rust, script, and SITL tests' \ + ' test Run Rust and script tests' \ ' pre-commit Run fmt, firmware clippy/test/docs, and mdBook checks' \ ' check Run pre-commit plus whitespace checks' \ ' clean Remove generated build artifacts' \ @@ -74,8 +73,6 @@ help: ## Show the command interface (the default) ' docs -- Pass explicit arguments to mdbook' \ ' firmware/tools/configurator -- ' \ ' Pass args to kaptan-configurator' \ - ' firmware/tools/sitl -- ' \ - ' Pass args to kaptan-sim' \ '' 'Examples' \ ' make firmware -- test --workspace --locked --offline' \ ' make firmware clippy' \ @@ -83,8 +80,7 @@ help: ## Show the command interface (the default) ' make firmware/boards/kakute-f7-mini-v3' \ ' make firmware/boards/kakute-f7-mini-v3 flash' \ ' make docs -- build' \ - ' make firmware/tools/configurator' \ - ' make firmware/tools/sitl -- move 5 3 1 --wait' + ' make firmware/tools/configurator' nix-shell: ## Enter the reproducible Nix development shell nix develop @@ -96,14 +92,13 @@ build: ## Build all firmware boards and documentation fmt: +@$(MAKE) -C $(FIRMWARE_DIR) fmt shfmt -d $$(rg --files -g '*.sh') - ruff format --check firmware/tools/sitl/kaptan-sim $$(rg --files -g '*.py') + ruff format --check $$(rg --files -g '*.py') nixfmt --check $$(rg --files -g '*.nix') taplo format --check $$(rg --files -g '*.toml') test: python3 -m unittest scripts/test-make-argv.py +@$(MAKE) -C $(FIRMWARE_DIR) test - +@$(MAKE) -C $(SITL_DIR) _check pre-commit: fmt test +@$(MAKE) -C $(FIRMWARE_DIR) clippy diff --git a/docs/book.toml b/docs/book.toml index 90a4b10..a1c3391 100644 --- a/docs/book.toml +++ b/docs/book.toml @@ -1,6 +1,6 @@ [book] title = "Kaptan documentation" -description = "Architecture, board bring-up, simulation, and contributor guidance for Kaptan." +description = "Architecture, board bring-up, and contributor guidance for Kaptan." authors = ["Kaptan contributors"] language = "en" src = "src" diff --git a/docs/src/SUMMARY.md b/docs/src/SUMMARY.md index 9b7a1ca..d7998c8 100644 --- a/docs/src/SUMMARY.md +++ b/docs/src/SUMMARY.md @@ -12,5 +12,4 @@ - [Boards](boards/index.md) - [Kakute F7 Mini V3](boards/kakute-f7-mini-v3.md) - [GEPRC GEP-F722-HD V2](boards/geprc-f722-v2.md) -- [Host SITL](sitl.md) - [Reference](reference.md) diff --git a/docs/src/architecture.md b/docs/src/architecture.md index 9d0bc74..1168d4f 100644 --- a/docs/src/architecture.md +++ b/docs/src/architecture.md @@ -55,10 +55,9 @@ one complete replacement candidate. The core distinguishes only immutable boot-active values from the latest committed values. A save cannot mutate controller or estimator state in flight. -Target-owned stores use private persistence formats. SITL atomically replaces a -bounded CBOR snapshot. F722 alternates compact `KCSP` snapshots between two -erase sectors, retaining only the old and replacement records across the commit -boundary. Neither format is exposed as the public KCFG file. +Target-owned stores use private persistence formats. F722 alternates compact +`KCSP` snapshots between two erase sectors, retaining only the old and +replacement records across the commit boundary. That format is not exposed as the public KCFG file. Motor allocation/mixer topology is immutable compiled vehicle capability data, never a scalar field or persistence member. F722 recovers operational settings before constructing consumers. Its local USB CDC-ACM and directly wired UART diff --git a/docs/src/configuration.md b/docs/src/configuration.md index 7352d6d..40e5e08 100644 --- a/docs/src/configuration.md +++ b/docs/src/configuration.md @@ -10,8 +10,6 @@ parameter catalog. | Deployment | Configuration consumed today | Remote writes | | --- | --- | --- | -| `kaptan-sitl serve --config FILE.cbor` | One validated Quad-X configuration at process construction. | No live file endpoint is attached yet. | -| `kaptan-sitl serve` | The validated factory Quad-X configuration. | No persistent store. | | STM32F722 image | Board factory settings, or the newest valid flash-journal snapshot. | `/config/user.kcfg` over local USB CDC-ACM or the directly wired UART while motor pins remain inhibited. | ## Web configurator @@ -171,16 +169,7 @@ be exposed safely. ## Private persistence -SITL and F722 use target-appropriate private persistence. Neither private -format is the public KCFG artifact. - -### SITL CBOR - -SITL stores one bounded CBOR snapshot keyed by stable `FieldId`. Saving writes -a same-directory temporary file, synchronizes it, and atomically renames it -over the configured file. The exact map rejects unknown, duplicate, and missing -fields, wrong schema or format, trailing input, invalid scalar encodings, and -complete-model validation failures. +F722 uses private persistence separate from the public KCFG artifact. ### F722 flash journal diff --git a/docs/src/development.md b/docs/src/development.md index 6860bc0..0aec768 100644 --- a/docs/src/development.md +++ b/docs/src/development.md @@ -10,8 +10,7 @@ directory path to that directory's Makefile. The standard `--` ends Make's own options. Every following argument uses the selected application's native command line. For example, `make firmware -- test --workspace --locked --offline` runs `cargo test ---workspace --locked --offline` from `firmware`, while -`make firmware/tools/sitl -- ready` runs `kaptan-sim ready`. Board paths are +--workspace --locked --offline` from `firmware`. Board paths are the complete build-and-package command, so they need no invented operation name. @@ -37,7 +36,7 @@ make firmware -- clippy \ `make firmware docs` builds Rust API documentation with warnings denied. `make docs` checks and builds the separate mdBook project. `make pre-commit` runs both documentation checks along with repository formatting, host and -embedded Clippy, Rust and script tests, and SITL structure tests. `make check` +embedded Clippy, Rust and script tests. `make check` adds a whitespace check. `make build` builds and packages release firmware and builds the mdBook project. @@ -53,16 +52,3 @@ The strict gate requires 100% line and branch coverage for `kaptan-math`, core package's coverage artifacts, unions LCOV source-line records, and uses full LLVM source spans to merge generic branch instantiations without lowering the gate. - -## Simulation acceptance - -```sh -cd firmware/tools/sitl -python3 world-structure-test.py -python3 -m unittest test_kaptan_sim.py -./e2e-smoke.sh -``` - -The Docker smoke is intentionally separate from host unit tests because it -builds the pinned base images, Rust binary, Gazebo plugin, and full loopback -plant. diff --git a/docs/src/errors.md b/docs/src/errors.md index 08f164e..0519906 100644 --- a/docs/src/errors.md +++ b/docs/src/errors.md @@ -79,7 +79,7 @@ different reportable error is already pending. The message kind itself identifies an error; no level is duplicated in the payload. Dynamic human-readable context belongs in `LOG`, while structured protocol failures remain in their owning response message. -- Control, navigation, safety, and simulator error types are derive-ready but +- Control, navigation, and safety error types are derive-ready but their live owners are not yet attached to a MAVLink worker. F722 still uses `panic-halt`; it does not claim a live panic frame or diff --git a/docs/src/getting-started.md b/docs/src/getting-started.md index 68fad69..02abb04 100644 --- a/docs/src/getting-started.md +++ b/docs/src/getting-started.md @@ -33,7 +33,6 @@ target-specific commands in [Development and verification](development.md). | `firmware/boards/` | Board-owned user schemas, factory values, immutable hardware declarations, and image composition roots. | | `firmware/platforms/stm32f7/` | Reviewed F722 RTIC platform, peripheral ownership, and shared linker layout. | | `firmware/libs/` | Typed math, estimation, control, safety, protocol, and driver crates. | -| `firmware/tools/sitl/` | Host-only closed-loop Gazebo simulation. | | `firmware/tools/configurator/` | Board-agnostic loopback web UI and serial bridge. | | `docs/` | This Markdown documentation site. | diff --git a/docs/src/index.md b/docs/src/index.md index 4e28879..a91fb35 100644 --- a/docs/src/index.md +++ b/docs/src/index.md @@ -2,8 +2,7 @@ Kaptan is an embedded Rust flight-controller project. This manual is the single source of project documentation: board support, safety boundaries, -simulation, development workflow, and design references live here as -Markdown. +development workflow, and design references live here as Markdown. > **Status:** the checked-in firmware is telemetry-only. It does not expose > physical motor authority, and this site does not make a physical-flight @@ -14,7 +13,6 @@ Markdown. - New contributors should read [Getting started](getting-started.md). - Firmware source and its Rust workspace begin in `firmware/`. - Board work belongs under [Boards](boards/index.md). -- The currently supported closed-loop simulation is [Host SITL](sitl.md). - [Configuration](configuration.md) explains target-described user settings and immutable F722 hardware policy. - [Logs](logs.md) and [Errors](errors.md) describe the custom MAVLink @@ -29,7 +27,7 @@ only defines the firmware workspace boundary. Keep pages in Markdown, add each page to `SUMMARY.md`, and place commands beside the assumptions and safety boundary they depend on. -The current configuration contract is target-described. A SITL Quad-X profile -and each F722 board expose only values consumed by that compiled target; -physical F722 hardware selection remains immutable board policy. No writable +The current configuration contract is target-described. Each F722 board exposes +only values consumed by that compiled target. Physical F722 hardware selection +remains immutable board policy. No writable snapshot can bypass an immutable safety decision. diff --git a/docs/src/logs.md b/docs/src/logs.md index 1a335a0..2d86f8a 100644 --- a/docs/src/logs.md +++ b/docs/src/logs.md @@ -78,11 +78,6 @@ named choices to the configurator, so the UI carries no hardcoded field list. Only the MAVLink adapter converts the five event levels to the typed Kaptan wire enum. -The log ingress is transport-neutral and unfiltered. The tested -`SitlConfigRuntime` composition applies the configured MAVLink policy when -fanning an event into that sink and proves `kaptan_log::info!` reaches a -generated `LOG` payload. -`kaptan-sitl serve` does not yet attach a file/diagnostic MAVLink link. F722 -images accept the unsigned Kaptan file service on local USB CDC-ACM and their -directly wired UART; that physical-link assumption does not extend to a shared +The log ingress is transport-neutral and unfiltered. F722 images accept the +unsigned Kaptan file service on local USB CDC-ACM and their directly wired UART; that physical-link assumption does not extend to a shared or remote MAVLink network. diff --git a/docs/src/reference.md b/docs/src/reference.md index 78e19ff..034b510 100644 --- a/docs/src/reference.md +++ b/docs/src/reference.md @@ -4,7 +4,6 @@ This page collects stable limits and points to the detailed contracts. | Surface | Current contract | | --- | --- | -| SITL file | One raw bounded CBOR configuration snapshot, replaced through a same-directory temporary file and atomic rename. | | F722 persistence | One bounded native-width scalar snapshot inside each slot of a power-loss-safe two-slot flash journal. | | Public config artifact | One canonical self-describing KCFG file containing generated metadata, factory values, and the complete committed candidate. | | File transfer | Custom `FILE_CONTROL` (51025) and `FILE_DATA` (51026), one owner-bound stop-and-wait transfer, 246-byte data chunks, CRC-32C. | diff --git a/docs/src/sitl.md b/docs/src/sitl.md deleted file mode 100644 index 4d4ed6f..0000000 --- a/docs/src/sitl.md +++ /dev/null @@ -1,59 +0,0 @@ -# Host SITL - -Host SITL is Kaptan's deterministic closed-loop simulation tier. Gazebo sends -simulated IMU and declared noisy local position/velocity observations over a -private loopback protocol. The Rust process runs the ESKF, safety supervisor, -trajectory, controller, and allocator, then returns four virtual motor -fractions to Gazebo. - -It is a pre-bench software gate, not a physical-flight validation. - -## Run the closed loop - -```sh -docker compose -f firmware/tools/sitl/compose.yaml up -d --build -make firmware/tools/sitl -- ready -make firmware/tools/sitl -- move 5 3 1 --wait -make firmware/tools/sitl -- land 0 0 --wait -docker compose -f firmware/tools/sitl/compose.yaml down -``` - -For the reproducible command-path gate, use: - -```sh -firmware/tools/sitl/e2e-smoke.sh -``` - -The smoke scenario verifies ascent, translation, descent, landing, flat-plane -proximity, and zero applied motor fractions after landing. Its reference world -contains only a visual landing pad; the pad has no collision geometry and -never contributes a controller or truth observation. - -Open , connect to -`ws://localhost:9002`, and leave authorization blank to view the world. - -## Quad-X profile - -SITL starts through the central `ConfigManager` lifecycle. Create and validate -a private host profile file with: - -```sh -kaptan-sitl config write-default kaptan-sitl.cbor -kaptan-sitl config validate kaptan-sitl.cbor -kaptan-sitl serve --config kaptan-sitl.cbor -``` - -The file selects the restart-only profile used to build the estimator, -controller, allocator envelope, safety supervisor, and MAVLink log-sink policy at -process start. It is local persistence and never appears on MAVLink. -`kaptan-sitl serve` retains -the central manager for that boot but does not yet open a MAVLink file/log -endpoint; see [Configuration](configuration.md) for the KCFG, file-transfer, -and safety boundaries. - -## Limits - -This model does not validate terrain, vision, flow/range sensing, wind, -vibration, battery/ESC behavior, sensor latency/jitter, actuator faults, or -physical touchdown sensing. Add those only as a reviewed sensor/contact model, -not as a display-only shortcut. diff --git a/firmware/.dockerignore b/firmware/.dockerignore index a309aca..c7f83c2 100644 --- a/firmware/.dockerignore +++ b/firmware/.dockerignore @@ -1,3 +1,2 @@ target .git -tools/hitl diff --git a/firmware/Cargo.lock b/firmware/Cargo.lock index fedf281..34e3cfb 100644 --- a/firmware/Cargo.lock +++ b/firmware/Cargo.lock @@ -374,25 +374,6 @@ dependencies = [ "kaptan-time", ] -[[package]] -name = "kaptan-sitl" -version = "0.1.0" -dependencies = [ - "kaptan-config", - "kaptan-control", - "kaptan-diagnostics-mavlink", - "kaptan-err-reportable", - "kaptan-flight-config", - "kaptan-flight-runtime", - "kaptan-log", - "kaptan-math", - "kaptan-mavlink", - "kaptan-navigation", - "kaptan-safety", - "kaptan-time", - "minicbor", -] - [[package]] name = "kaptan-spi" version = "0.1.0" diff --git a/firmware/Cargo.toml b/firmware/Cargo.toml index 2592dc8..74c66cd 100644 --- a/firmware/Cargo.toml +++ b/firmware/Cargo.toml @@ -26,7 +26,6 @@ members = [ "libs/kaptan-time", "libs/kaptan-tx", "platforms/stm32f7", - "tools/sitl", "tools/configurator", ] resolver = "2" diff --git a/firmware/README.md b/firmware/README.md index 1c8df4e..976d249 100644 --- a/firmware/README.md +++ b/firmware/README.md @@ -2,16 +2,15 @@ This directory is the Kaptan flight-controller firmware workspace. It owns the Rust workspace manifest, board declarations, platform implementation, reusable -firmware crates, simulator, firmware scripts, and firmware test fixtures. +firmware crates and firmware scripts. ```text firmware/ ├── boards/ immutable board declarations and linker layouts ├── libs/ reusable no_std firmware crates ├── platforms/ hardware platform composition -├── tools/ SITL and firmware development tooling -├── scripts/ packaging, analysis, and coverage helpers -└── tests/ firmware integration-test fixtures +├── tools/ firmware development tooling +└── scripts/ packaging, analysis, and coverage helpers ``` This workspace owns its Makefile. From this directory, list its commands with: diff --git a/firmware/scripts/report-test-coverage.sh b/firmware/scripts/report-test-coverage.sh index 62524f0..049c7a4 100755 --- a/firmware/scripts/report-test-coverage.sh +++ b/firmware/scripts/report-test-coverage.sh @@ -128,7 +128,6 @@ report_package kaptan-sensor /libs/kaptan-sensor/ report_package kaptan-spi /libs/kaptan-spi/ report_package kaptan-time /libs/kaptan-time/ report_package kaptan-tx /libs/kaptan-tx/ -report_package kaptan-sitl /tools/sitl/ echo "platform-stm32f7 target-only/N/A (run cargo check -p platform-stm32f7 --lib)" echo "kaptan-geprc-f722-v2 target-only/N/A (run its thumbv7em-none-eabihf check)" diff --git a/firmware/tests/hitl/.gitkeep b/firmware/tests/hitl/.gitkeep deleted file mode 100644 index e69de29..0000000 diff --git a/firmware/tests/sitl/.gitkeep b/firmware/tests/sitl/.gitkeep deleted file mode 100644 index e69de29..0000000 diff --git a/firmware/tools/sitl/CMakeLists.txt b/firmware/tools/sitl/CMakeLists.txt deleted file mode 100644 index 60487a6..0000000 --- a/firmware/tools/sitl/CMakeLists.txt +++ /dev/null @@ -1,13 +0,0 @@ -cmake_minimum_required(VERSION 3.22 FATAL_ERROR) -project(kaptan_sitl LANGUAGES CXX) - -# Gazebo Jetty deliberately supplies unversioned CMake package targets. The -# image installs only the Jetty (gz-sim 10) development ABI, so an accidental -# build against another Gazebo major fails at configuration time. -find_package(gz-cmake REQUIRED) -gz_find_package(gz-sim REQUIRED) - -add_library(kaptan_sitl_quad SHARED src/kaptan_sitl_quad.cc) -target_compile_features(kaptan_sitl_quad PRIVATE cxx_std_20) -target_compile_options(kaptan_sitl_quad PRIVATE -Wall -Wextra -Wpedantic -Werror) -target_link_libraries(kaptan_sitl_quad PRIVATE gz-sim::gz-sim) diff --git a/firmware/tools/sitl/Cargo.toml b/firmware/tools/sitl/Cargo.toml deleted file mode 100644 index 99f57f7..0000000 --- a/firmware/tools/sitl/Cargo.toml +++ /dev/null @@ -1,28 +0,0 @@ -[package] -name = "kaptan-sitl" -edition.workspace = true -license.workspace = true -rust-version.workspace = true -version.workspace = true - -[lints] -workspace = true - -[dependencies] -kaptan-err-reportable = { path = "../../libs/kaptan-err-reportable" } -kaptan-control = { path = "../../libs/kaptan-control" } -kaptan-config = { path = "../../libs/kaptan-config" } -kaptan-flight-config = { path = "../../libs/kaptan-flight-config" } -kaptan-flight-runtime = { path = "../../libs/kaptan-flight-runtime" } -kaptan-log = { path = "../../libs/kaptan-log" } -kaptan-math = { path = "../../libs/kaptan-math" } -kaptan-navigation = { path = "../../libs/kaptan-navigation", features = [ - "sitl", -] } -kaptan-safety = { path = "../../libs/kaptan-safety" } -kaptan-time = { path = "../../libs/kaptan-time" } -minicbor = "0.25" - -[dev-dependencies] -kaptan-diagnostics-mavlink = { path = "../../libs/kaptan-diagnostics-mavlink" } -kaptan-mavlink = { path = "../../libs/kaptan-mavlink", default-features = false } diff --git a/firmware/tools/sitl/Dockerfile b/firmware/tools/sitl/Dockerfile deleted file mode 100644 index 3016d3e..0000000 --- a/firmware/tools/sitl/Dockerfile +++ /dev/null @@ -1,43 +0,0 @@ -# syntax=docker/dockerfile:1.7 - -FROM rust@sha256:3914072ca0c3b8aad871db9169a651ccfce30cf58303e5d6f2db16d1d8a7e58f AS sitl-build -WORKDIR /opt/kaptan -COPY . . -# Select the pinned host compiler explicitly rather than inheriting the -# repository's embedded-target toolchain components. `--locked` makes the -# resolved source graph an explicit reviewable input to the image build. -RUN cargo +1.90.0 build --locked --release --package kaptan-sitl - -# Gazebo Jetty's supported amd64 runtime. The image is intentionally split so -# the C++ plugin and host control process share no source-time build hack. -FROM ubuntu@sha256:561618e2c15bf2397621dd04f96926663a3b5616c189cf7e38db7e82f5c538ea AS gazebo-runtime -SHELL ["/bin/bash", "-o", "pipefail", "-c"] -ENV DEBIAN_FRONTEND=noninteractive -RUN apt-get update \ - && apt-get install --no-install-recommends -y ca-certificates curl gnupg \ - && curl --fail --location --silent --show-error \ - https://packages.osrfoundation.org/gazebo.gpg \ - --output /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg \ - && echo "deb [arch=amd64 signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] https://packages.osrfoundation.org/gazebo/ubuntu-stable noble main" \ - > /etc/apt/sources.list.d/gazebo-stable.list \ - && apt-get update \ - && apt-get install --no-install-recommends -y gz-sim10-cli \ - && rm -rf /var/lib/apt/lists/* - -FROM gazebo-runtime AS gazebo-build -RUN apt-get update \ - && apt-get install --no-install-recommends -y cmake g++ ninja-build pkg-config libgz-sim10-dev \ - && rm -rf /var/lib/apt/lists/* -COPY tools/sitl /opt/kaptan/sitl -RUN cmake -G Ninja -S /opt/kaptan/sitl -B /opt/kaptan/sitl/plugin/build \ - && cmake --build /opt/kaptan/sitl/plugin/build --parallel - -FROM gazebo-runtime -ENV HOME=/tmp \ - GZ_SIM_SYSTEM_PLUGIN_PATH=/opt/kaptan/sitl/plugin/build \ - GZ_SIM_RESOURCE_PATH=/opt/kaptan/sitl/worlds -COPY tools/sitl /opt/kaptan/sitl -COPY --from=gazebo-build /opt/kaptan/sitl/plugin/build/libkaptan_sitl_quad.so \ - /opt/kaptan/sitl/plugin/build/libkaptan_sitl_quad.so -COPY --from=sitl-build /opt/kaptan/target/release/kaptan-sitl /usr/local/bin/kaptan-sitl -RUN chmod 0555 /opt/kaptan/sitl/gazebo-entrypoint.sh /opt/kaptan/sitl/kaptan-sim /usr/local/bin/kaptan-sitl diff --git a/firmware/tools/sitl/Makefile b/firmware/tools/sitl/Makefile deleted file mode 100644 index c65fd47..0000000 --- a/firmware/tools/sitl/Makefile +++ /dev/null @@ -1,34 +0,0 @@ -override SHELL := /bin/sh -override MAKEFLAGS += --no-print-directory - -.DEFAULT_GOAL := help -.DELETE_ON_ERROR: - -override PYTHON := python3 - -override PRIMARY_GOAL := $(firstword $(MAKECMDGOALS)) - -run: ## Run kaptan-sim with the remaining command-line arguments - @exec python3 ../../../scripts/make-argv.py exec run ./kaptan-sim - -ifeq ($(PRIMARY_GOAL),run) - -.PHONY: run - -.DEFAULT: - @: - -else - -.PHONY: help run _check - -help: ## Show simulation commands - @printf '%s\n' 'Simulation' '' \ - 'Application arguments are passed through the root Makefile:' \ - ' make firmware/tools/sitl -- move 5 3 1 --wait' - -_check: - $(PYTHON) world-structure-test.py - $(PYTHON) -m unittest test_kaptan_sim.py - -endif diff --git a/firmware/tools/sitl/compose.yaml b/firmware/tools/sitl/compose.yaml deleted file mode 100644 index 09e743c..0000000 --- a/firmware/tools/sitl/compose.yaml +++ /dev/null @@ -1,33 +0,0 @@ -services: - gazebo-sitl: - build: - context: ../.. - dockerfile: tools/sitl/Dockerfile - command: ["/opt/kaptan/sitl/gazebo-entrypoint.sh"] - depends_on: - sitl: - condition: service_started - environment: - HOME: /tmp - network_mode: host - read_only: true - tmpfs: - - /tmp:mode=1777,size=256m - cap_drop: ["ALL"] - security_opt: ["no-new-privileges:true"] - init: true - stop_grace_period: 5s - - sitl: - build: - context: ../.. - dockerfile: tools/sitl/Dockerfile - command: ["/usr/local/bin/kaptan-sitl", "serve"] - network_mode: host - read_only: true - tmpfs: - - /tmp:mode=1777,size=32m - cap_drop: ["ALL"] - security_opt: ["no-new-privileges:true"] - init: true - stop_grace_period: 2s diff --git a/firmware/tools/sitl/e2e-smoke.sh b/firmware/tools/sitl/e2e-smoke.sh deleted file mode 100755 index 75fa9a9..0000000 --- a/firmware/tools/sitl/e2e-smoke.sh +++ /dev/null @@ -1,39 +0,0 @@ -#!/usr/bin/env sh -# Host-only closed-loop acceptance scenario. It verifies the actual UDP lease, -# estimator, safety, trajectory, controller, allocator, and Gazebo loop across -# ascent, cross-axis translation, descent, and terminal landing. Every terminal -# phase holds independent, noise-free Gazebo truth; that recorder cannot feed -# the controller. -set -eu - -project_root=$(CDPATH= cd -- "$(dirname -- "$0")/../.." && pwd) -compose_file="$project_root/tools/sitl/compose.yaml" - -python3 "$project_root/tools/sitl/world-structure-test.py" - -cleanup() { - status=$? - trap - EXIT - # Preserve the first failing command's result, but retain enough evidence - # to distinguish a client/lease failure from a Gazebo or container start - # failure in CI. `down` otherwise removes the only useful diagnostics. - if [ "$status" -ne 0 ]; then - echo "SITL smoke failed (exit $status); compose state and recent logs follow:" >&2 - docker compose -f "$compose_file" ps >&2 || true - docker compose -f "$compose_file" logs --no-color --tail 250 >&2 || true - fi - docker compose -f "$compose_file" down --remove-orphans || true - exit "$status" -} -trap cleanup EXIT -trap 'exit 130' INT -trap 'exit 143' TERM - -docker compose -f "$compose_file" up -d --build -"$project_root/tools/sitl/kaptan-sim" ready --timeout 30 -"$project_root/tools/sitl/kaptan-sim" move 0 0 1.5 --wait --verify-truth --timeout 60 -"$project_root/tools/sitl/kaptan-sim" move 5 3 5 --wait --verify-truth --timeout 90 -"$project_root/tools/sitl/kaptan-sim" move -4 -2 2 --wait --verify-truth --timeout 90 -# Start the terminal operation while airborne and off the landing point. A -# `move ... 1` is an armed hover, not evidence of landing or zero motors. -"$project_root/tools/sitl/kaptan-sim" land 0 0 --wait --verify-truth --timeout 60 diff --git a/firmware/tools/sitl/gazebo-entrypoint.sh b/firmware/tools/sitl/gazebo-entrypoint.sh deleted file mode 100755 index 26d83ad..0000000 --- a/firmware/tools/sitl/gazebo-entrypoint.sh +++ /dev/null @@ -1,4 +0,0 @@ -#!/bin/sh -set -eu - -exec gz sim --force-version 10 -s -r -v 3 /opt/kaptan/sitl/worlds/kaptan-sitl-quad.sdf.in diff --git a/firmware/tools/sitl/kaptan-sim b/firmware/tools/sitl/kaptan-sim deleted file mode 100755 index 28e1121..0000000 --- a/firmware/tools/sitl/kaptan-sim +++ /dev/null @@ -1,589 +0,0 @@ -#!/usr/bin/env python3 -"""Small user client for the local Kaptan host-SITL process.""" - -from __future__ import annotations - -import argparse -import math -import secrets -import socket -import struct -import sys -import time - - -CONTROL = ("127.0.0.1", 14662) -TRUTH = ("127.0.0.1", 14663) -# The simulator lease identifies its one local user by UDP source address. -# Keep this source stable across separate CLI invocations so a completed move -# can be followed by the next command without a second, competing user. -USER_CONTROL = ("127.0.0.1", 14665) -USER_CLOCK = ("127.0.0.1", 14664) -VERSION = 1 -STATUS_QUERY = struct.Struct("<4sB3x") -STATUS = struct.Struct("<4sB3xQH2x") -GOAL = struct.Struct("<4sB3xI3f") -GOAL_KEEPALIVE = struct.Struct("<4sB3xI") -GOAL_STATUS = struct.Struct("<4sB3xIB3x") -CLOCK_PACKET = struct.Struct("<4sB3xQ") -# KST2 carries pose/velocity, launch-relative attitude, and applied motor -# fractions. It is a test-only stream; the Rust controller never binds TRUTH. -TRUTH_PACKET = struct.Struct("<4sB3xQI14f") - -ESTIMATOR_READY = 1 << 0 -IMU_FRESH = 1 << 1 -LOCAL_AIDING_FRESH = 1 << 2 -OUTPUT_PERMITTED = 1 << 3 -ACTUATOR_TRANSPORT = 1 << 4 -READY = ESTIMATOR_READY | IMU_FRESH | LOCAL_AIDING_FRESH | ACTUATOR_TRANSPORT -GOAL_KEEPALIVE_MICROSECONDS = 100_000 -GOAL_REJECTED = 0 -GOAL_ACTIVE = 1 -GOAL_SETTLED = 2 -GOAL_LANDED = 3 -# Independent truth-recorder assertion for the single, flat ground plane -# declared by the supplied world. This is not a physical contact detector and -# must not be carried into a flight target. -GROUND_PLANE_TOLERANCE_METRES = 0.10 - - -def arguments() -> argparse.Namespace: - parser = argparse.ArgumentParser(description=__doc__) - subcommands = parser.add_subparsers(dest="command", required=True) - ready = subcommands.add_parser( - "ready", help="wait for estimator and simulated sensors" - ) - ready.add_argument("--timeout", type=float, default=20.0) - move = subcommands.add_parser( - "move", help="command absolute local north/east/up metres" - ) - move.add_argument("north", type=float) - move.add_argument("east", type=float) - move.add_argument("up", type=float) - move.add_argument("--wait", action="store_true", help="wait for the settled result") - move.add_argument( - "--verify-truth", - action="store_true", - help="verify noise-free Gazebo truth on the test-only recorder port after settling", - ) - move.add_argument("--truth-position-tolerance", type=float, default=0.75) - move.add_argument("--truth-velocity-tolerance", type=float, default=0.50) - move.add_argument("--truth-attitude-tolerance-rad", type=float, default=0.35) - move.add_argument( - "--truth-hold-seconds", - type=float, - default=0.75, - help="require every independent truth sample to remain within tolerance for this interval", - ) - # A distant, bounded goal is intentionally slow: the default must not turn - # a valid long trajectory into a spurious client-side test failure. - move.add_argument("--timeout", type=float, default=180.0) - land = subcommands.add_parser( - "land", - help="descend to an explicit local landing point and revoke virtual motor authority", - ) - land.add_argument( - "north", type=float, help="absolute local north landing coordinate in metres" - ) - land.add_argument( - "east", type=float, help="absolute local east landing coordinate in metres" - ) - land.add_argument( - "--wait", action="store_true", help="wait for the terminal landed result" - ) - land.add_argument( - "--verify-truth", - action="store_true", - help="verify independent Gazebo ground truth and zero applied motor fractions after landing", - ) - land.add_argument("--truth-position-tolerance", type=float, default=0.75) - land.add_argument("--truth-velocity-tolerance", type=float, default=0.25) - land.add_argument("--truth-attitude-tolerance-rad", type=float, default=0.35) - land.add_argument("--truth-hold-seconds", type=float, default=0.75) - land.add_argument("--timeout", type=float, default=180.0) - return parser.parse_args() - - -def recv(socket_: socket.socket, deadline: float) -> bytes | None: - remaining = deadline - time.monotonic() - if remaining <= 0: - return None - socket_.settimeout(remaining) - try: - payload, sender = socket_.recvfrom(256) - except TimeoutError: - return None - return payload if sender[0] == "127.0.0.1" else None - - -def latest_simulation_time( - clock_socket: socket.socket, previous: int | None -) -> int | None: - """Drains Gazebo's test-user clock without trusting host elapsed time.""" - latest = previous - while True: - try: - payload, sender = clock_socket.recvfrom(64) - except BlockingIOError: - return latest - if sender[0] != "127.0.0.1" or len(payload) != CLOCK_PACKET.size: - continue - magic, version, simulation_time = CLOCK_PACKET.unpack(payload) - if magic != b"KSC1" or version != VERSION: - continue - if latest is not None and simulation_time < latest: - raise RuntimeError( - "Gazebo simulation clock reset; restart the command in the new epoch" - ) - latest = simulation_time - - -def status(socket_: socket.socket, deadline: float) -> int | None: - socket_.sendto(STATUS_QUERY.pack(b"KSQ1", VERSION), CONTROL) - payload = recv(socket_, deadline) - if payload is None or len(payload) != STATUS.size: - return None - magic, version, _, flags = STATUS.unpack(payload) - if magic != b"KSS1" or version != VERSION: - return None - return flags - - -def wait_ready(socket_: socket.socket, timeout: float) -> None: - deadline = time.monotonic() + timeout - latest = None - while time.monotonic() < deadline: - latest = status(socket_, min(deadline, time.monotonic() + 0.5)) - if latest is not None and latest & READY == READY: - return - if latest is None: - raise RuntimeError( - "host SITL is not running; run docker compose -f " - "firmware/tools/sitl/compose.yaml up -d --build from the repository root" - ) - names = ( - (ESTIMATOR_READY, "estimator initialization"), - (IMU_FRESH, "fresh simulator IMU"), - (LOCAL_AIDING_FRESH, "accepted simulator local-state aiding"), - (ACTUATOR_TRANSPORT, "virtual actuator transport"), - ) - missing = [name for bit, name in names if latest & bit != bit] - raise RuntimeError(f"SITL is waiting for: {', '.join(missing)}") - - -def truth_sample_within_tolerance( - position_error: float, - velocity: float, - attitude_error: float, - motors_stopped: bool, - ground_plane_error: float, - position_tolerance: float, - velocity_tolerance: float, - attitude_tolerance: float, - require_stopped_motors: bool, - require_ground_plane: bool, -) -> bool: - """Checks one independent truth sample against its declared contract.""" - return not ( - position_error > position_tolerance - or velocity > velocity_tolerance - or attitude_error > attitude_tolerance - or (require_stopped_motors and not motors_stopped) - or (require_ground_plane and ground_plane_error > GROUND_PLANE_TOLERANCE_METRES) - ) - - -def verify_truth( - north: float, - east: float, - up: float, - timeout: float, - position_tolerance: float, - velocity_tolerance: float, - attitude_tolerance: float, - hold_seconds: float, - require_stopped_motors: bool, - require_ground_plane: bool, - control_socket: socket.socket | None = None, - clock_socket: socket.socket | None = None, - keepalive: bytes | None = None, -) -> None: - with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as truth_socket: - truth_socket.bind(TRUTH) - acquisition_deadline = time.monotonic() + timeout - hold_deadline: int | None = None - samples = 0 - worst_position_error = 0.0 - worst_velocity = 0.0 - worst_attitude_error = 0.0 - worst_ground_plane_error = 0.0 - worst_motor_fraction = 0.0 - last_out_of_tolerance: str | None = None - simulation_time: int | None = None - next_keepalive_time: int | None = None - last_truth_time: int | None = None - while time.monotonic() < acquisition_deadline: - if clock_socket is not None: - simulation_time = latest_simulation_time(clock_socket, simulation_time) - if ( - keepalive is not None - and control_socket is not None - and simulation_time is not None - and ( - next_keepalive_time is None - or simulation_time >= next_keepalive_time - ) - ): - control_socket.sendto(keepalive, CONTROL) - next_keepalive_time = simulation_time + GOAL_KEEPALIVE_MICROSECONDS - payload = recv( - truth_socket, min(acquisition_deadline, time.monotonic() + 0.01) - ) - if payload is None or len(payload) != TRUTH_PACKET.size: - continue - ( - magic, - version, - truth_time, - _, - north_m, - east_m, - down_m, - north_v, - east_v, - down_v, - attitude_w, - attitude_x, - attitude_y, - attitude_z, - motor_1, - motor_2, - motor_3, - motor_4, - ) = TRUTH_PACKET.unpack(payload) - if magic != b"KST2" or version != VERSION: - continue - if last_truth_time is not None and truth_time < last_truth_time: - raise RuntimeError("Gazebo truth time regressed") - last_truth_time = truth_time - position_error = math.sqrt( - (north_m - north) ** 2 + (east_m - east) ** 2 + (down_m + up) ** 2 - ) - # `up` is converted to local-NED down above. A terminal land - # request explicitly sets it to zero, so this distinguishes the - # declared ground plane from a low-velocity armed hover. - ground_plane_error = abs(down_m + up) - velocity = math.sqrt(north_v**2 + east_v**2 + down_v**2) - quaternion_norm = math.sqrt( - attitude_w**2 + attitude_x**2 + attitude_y**2 + attitude_z**2 - ) - if not math.isfinite(quaternion_norm) or abs(quaternion_norm - 1.0) > 1e-3: - raise RuntimeError( - "Gazebo truth contained a non-unit attitude quaternion" - ) - attitude_error = 2.0 * math.acos( - min(1.0, abs(attitude_w) / quaternion_norm) - ) - motors = (motor_1, motor_2, motor_3, motor_4) - maximum_motor_fraction = max(abs(motor) for motor in motors) - motors_stopped = all(abs(motor) <= 0.01 for motor in motors) - if not truth_sample_within_tolerance( - position_error, - velocity, - attitude_error, - motors_stopped, - ground_plane_error, - position_tolerance, - velocity_tolerance, - attitude_tolerance, - require_stopped_motors, - require_ground_plane, - ): - # A lifecycle result says the controller's own settle rule was - # met. The independent plant may still be damping. Require one - # uninterrupted truth hold, rather than rejecting the first - # transient sample after that result. - hold_deadline = None - samples = 0 - last_out_of_tolerance = ( - "position_error=" - f"{position_error:.3f} m, velocity={velocity:.3f} m/s, " - f"attitude_error={attitude_error:.3f} rad, " - f"ground_plane_error={ground_plane_error:.3f} m, motors={motors}" - ) - continue - if hold_deadline is None: - # Use the truth record's own simulated timestamp. A separately - # drained clock record can be ahead of buffered truth packets, - # so it cannot prove an uninterrupted physical hold. - hold_deadline = truth_time + int(hold_seconds * 1_000_000) - worst_position_error = position_error - worst_velocity = velocity - worst_attitude_error = attitude_error - worst_ground_plane_error = ground_plane_error - worst_motor_fraction = maximum_motor_fraction - else: - worst_position_error = max(worst_position_error, position_error) - worst_velocity = max(worst_velocity, velocity) - worst_attitude_error = max(worst_attitude_error, attitude_error) - worst_ground_plane_error = max( - worst_ground_plane_error, ground_plane_error - ) - worst_motor_fraction = max(worst_motor_fraction, maximum_motor_fraction) - samples += 1 - if truth_time >= hold_deadline: - summary = ( - "truth verified: " - f"samples={samples}, position_error<={worst_position_error:.3f} m, " - f"velocity<={worst_velocity:.3f} m/s, " - f"attitude_error<={worst_attitude_error:.3f} rad" - ) - if require_ground_plane: - summary += f", ground_plane_error<={worst_ground_plane_error:.3f} m" - if require_stopped_motors: - summary += f", motor_fraction<={worst_motor_fraction:.3f}" - print(summary) - return - if last_out_of_tolerance is not None: - raise RuntimeError( - "Gazebo truth never held within tolerance before timeout: " - + last_out_of_tolerance - ) - raise RuntimeError("did not receive a Gazebo truth record") - - -def move( - socket_: socket.socket, - clock_socket: socket.socket, - north: float, - east: float, - up: float, - timeout: float, - wait: bool, - verify: bool, - position_tolerance: float, - velocity_tolerance: float, - attitude_tolerance: float, - hold_seconds: float, -) -> None: - if not all(math.isfinite(value) for value in (north, east, up)): - raise ValueError("goal coordinates must be finite") - wait_ready(socket_, timeout) - goal_id = secrets.randbelow(0xFFFF_FFFF) + 1 - goal = GOAL.pack(b"KSG1", VERSION, goal_id, north, east, up) - command_goal( - socket_, - clock_socket, - goal, - goal_id, - north, - east, - up, - timeout, - wait, - verify, - position_tolerance, - velocity_tolerance, - attitude_tolerance, - hold_seconds, - GOAL_SETTLED, - False, - "move", - ) - - -def land( - socket_: socket.socket, - clock_socket: socket.socket, - north: float, - east: float, - timeout: float, - wait: bool, - verify: bool, - position_tolerance: float, - velocity_tolerance: float, - attitude_tolerance: float, - hold_seconds: float, -) -> None: - if not all(math.isfinite(value) for value in (north, east)): - raise ValueError("landing coordinates must be finite") - wait_ready(socket_, timeout) - goal_id = secrets.randbelow(0xFFFF_FFFF) + 1 - landing = GOAL.pack(b"KSD1", VERSION, goal_id, north, east, 0.0) - command_goal( - socket_, - clock_socket, - landing, - goal_id, - north, - east, - 0.0, - timeout, - wait, - verify, - position_tolerance, - velocity_tolerance, - attitude_tolerance, - hold_seconds, - GOAL_LANDED, - True, - "land", - ) - - -def command_goal( - socket_: socket.socket, - clock_socket: socket.socket, - goal: bytes, - goal_id: int, - north: float, - east: float, - up: float, - timeout: float, - wait: bool, - verify: bool, - position_tolerance: float, - velocity_tolerance: float, - attitude_tolerance: float, - hold_seconds: float, - terminal_lifecycle: int, - require_stopped_motors: bool, - action: str, -) -> None: - keepalive = GOAL_KEEPALIVE.pack(b"KSK1", VERSION, goal_id) - deadline = time.monotonic() + timeout - simulation_time: int | None = None - next_send_time: int | None = None - active = False - while time.monotonic() < deadline: - simulation_time = latest_simulation_time(clock_socket, simulation_time) - if next_send_time is None or ( - simulation_time is not None and simulation_time >= next_send_time - ): - socket_.sendto(keepalive if active else goal, CONTROL) - if simulation_time is not None: - next_send_time = simulation_time + GOAL_KEEPALIVE_MICROSECONDS - payload = recv(socket_, min(deadline, time.monotonic() + 0.01)) - if payload is None or len(payload) != GOAL_STATUS.size: - continue - magic, version, received_id, lifecycle = GOAL_STATUS.unpack(payload) - if magic != b"KSL1" or version != VERSION or received_id != goal_id: - continue - if lifecycle == GOAL_REJECTED: - raise RuntimeError( - f"{action} was rejected because SITL was not estimator-ready" - ) - if lifecycle == GOAL_ACTIVE: - active = True - print(f"{action} active: north={north:g} m, east={east:g} m, up={up:g} m") - break - if not active: - raise RuntimeError("goal was not activated") - if not wait: - return - while time.monotonic() < deadline: - simulation_time = latest_simulation_time(clock_socket, simulation_time) - if simulation_time is not None and ( - next_send_time is None or simulation_time >= next_send_time - ): - socket_.sendto(keepalive, CONTROL) - next_send_time = simulation_time + GOAL_KEEPALIVE_MICROSECONDS - payload = recv(socket_, min(deadline, time.monotonic() + 0.01)) - if payload is None or len(payload) != GOAL_STATUS.size: - continue - magic, version, received_id, lifecycle = GOAL_STATUS.unpack(payload) - if ( - magic == b"KSL1" - and version == VERSION - and received_id == goal_id - and lifecycle == terminal_lifecycle - ): - print("landed" if terminal_lifecycle == GOAL_LANDED else "settled") - if verify: - verify_truth( - north, - east, - up, - min(5.0, timeout), - position_tolerance, - velocity_tolerance, - attitude_tolerance, - hold_seconds, - require_stopped_motors, - terminal_lifecycle == GOAL_LANDED, - None if require_stopped_motors else socket_, - clock_socket, - None if require_stopped_motors else keepalive, - ) - return - raise RuntimeError(f"{action} did not reach its terminal state before timeout") - - -def main() -> int: - values = arguments() - if values.timeout <= 0: - raise ValueError("timeout must be positive") - if values.command in ("move", "land"): - tolerances = ( - values.truth_position_tolerance <= 0 - or values.truth_velocity_tolerance <= 0 - or values.truth_attitude_tolerance_rad <= 0 - or values.truth_hold_seconds <= 0 - or not math.isfinite(values.truth_position_tolerance) - or not math.isfinite(values.truth_velocity_tolerance) - or not math.isfinite(values.truth_attitude_tolerance_rad) - or not math.isfinite(values.truth_hold_seconds) - ) - if tolerances: - raise ValueError( - "truth tolerances and hold duration must be finite and positive" - ) - with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as client: - client.bind(USER_CONTROL) - if values.command == "ready": - wait_ready(client, values.timeout) - print("ready") - elif values.command == "move": - with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as clock: - clock.bind(USER_CLOCK) - clock.setblocking(False) - move( - client, - clock, - values.north, - values.east, - values.up, - values.timeout, - values.wait, - values.verify_truth, - values.truth_position_tolerance, - values.truth_velocity_tolerance, - values.truth_attitude_tolerance_rad, - values.truth_hold_seconds, - ) - else: - with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as clock: - clock.bind(USER_CLOCK) - clock.setblocking(False) - land( - client, - clock, - values.north, - values.east, - values.timeout, - values.wait, - values.verify_truth, - values.truth_position_tolerance, - values.truth_velocity_tolerance, - values.truth_attitude_tolerance_rad, - values.truth_hold_seconds, - ) - return 0 - - -if __name__ == "__main__": - try: - raise SystemExit(main()) - except (OSError, RuntimeError, ValueError, struct.error) as error: - print(f"kaptan-sim: {error}", file=sys.stderr) - raise SystemExit(2) from error diff --git a/firmware/tools/sitl/src/config/logging.rs b/firmware/tools/sitl/src/config/logging.rs deleted file mode 100644 index deda233..0000000 --- a/firmware/tools/sitl/src/config/logging.rs +++ /dev/null @@ -1,8 +0,0 @@ -//! Host-side access to the common bounded log ingress. - -use kaptan_log::LogRecord; - -/// Visits one transport-neutral event for composition-level sink fanout. -pub(super) fn drain_one(consume: impl FnOnce(&LogRecord) -> R) -> Option { - kaptan_log::drain_one(consume) -} diff --git a/firmware/tools/sitl/src/config/mod.rs b/firmware/tools/sitl/src/config/mod.rs deleted file mode 100644 index 8f5f0ef..0000000 --- a/firmware/tools/sitl/src/config/mod.rs +++ /dev/null @@ -1,18 +0,0 @@ -//! Host-only configuration composition for SITL. -//! -//! This module is deliberately not a firmware storage backend. Flight targets -//! need a reviewed A/B journal and authority path; SITL uses a same-directory -//! file replacement to make startup and configuration tests reproducible. - -#[cfg(test)] -mod logging; -mod runtime; -mod store; - -#[cfg(test)] -mod tests; - -pub(super) use runtime::{SitlConfigRuntime, configuration_service}; -pub use store::{ - FileConfigStore, FileStoreError, default_profile, load_profile, write_default_profile, -}; diff --git a/firmware/tools/sitl/src/config/runtime.rs b/firmware/tools/sitl/src/config/runtime.rs deleted file mode 100644 index 0342175..0000000 --- a/firmware/tools/sitl/src/config/runtime.rs +++ /dev/null @@ -1,47 +0,0 @@ -//! Central SITL configuration composition. - -#[cfg(test)] -use kaptan_diagnostics_mavlink::encode_log; -use kaptan_flight_config::{CandidateProfile, VehicleCapabilities, VehicleProfile}; -use kaptan_flight_runtime::FlightRuntime; -#[cfg(test)] -use kaptan_log::TransportAdmission; -#[cfg(test)] -use kaptan_mavlink::generated::kaptan::LOG_DATA; - -use super::FileConfigStore; -#[cfg(test)] -use super::logging; - -/// SITL's immutable compiled capability contract. -pub(super) const CAPABILITIES: VehicleCapabilities = VehicleCapabilities::quad_x(); - -/// SITL composition root retaining the one profile manager. -pub struct SitlConfigRuntime { - runtime: FlightRuntime, -} - -impl SitlConfigRuntime { - /// Returns the immutable profile selected by the central manager at boot. - pub const fn active_profile(&self) -> VehicleProfile { - *self.runtime.configuration().active() - } - - /// Drains one standard `log` facade record and exposes the generated - /// diagnostic payload for the host's sole MAVLink link owner. This - /// host-only harness accepts the bounded handoff synchronously. - #[cfg(test)] - pub(super) fn pending_log_payload(&mut self) -> Option { - let runtime = &mut self.runtime; - let _ = logging::drain_one(|record| runtime.publish_log_event(*record)); - self.runtime - .submit_next_mavlink(|record| (TransportAdmission::Accepted, encode_log(record))) - } -} - -/// Builds the host-only central configuration composition. -pub fn configuration_service(store: FileConfigStore) -> Result { - let runtime = FlightRuntime::recover(CandidateProfile::factory(), CAPABILITIES, store) - .map_err(|error| format!("configuration recovery failed: {error:?}"))?; - Ok(SitlConfigRuntime { runtime }) -} diff --git a/firmware/tools/sitl/src/config/store.rs b/firmware/tools/sitl/src/config/store.rs deleted file mode 100644 index f08f946..0000000 --- a/firmware/tools/sitl/src/config/store.rs +++ /dev/null @@ -1,126 +0,0 @@ -//! Host-only persistence for a validated SITL profile. - -use std::{ - fs::{self, File}, - io::{Read, Write}, - path::Path, -}; - -use kaptan_config::ConfigStore; -use kaptan_err_reportable::KaptanError; -use kaptan_flight_config::{ - CandidateProfile, CborEncodeError, CborError, MAXIMUM_CANDIDATE_BYTES, VehicleProfile, - decode_candidate, encode_candidate, -}; - -use super::{configuration_service, runtime::CAPABILITIES}; - -/// Host file-store error. It intentionally makes no flash power-loss claim. -#[derive(Debug, KaptanError)] -pub enum FileStoreError { - /// Host filesystem operation failed. - #[kaptan_error(id = 12101)] - Io(std::io::Error), - /// The bounded file record did not decode and validate. - #[kaptan_error(id = 12102)] - Cbor(CborError), - /// The fixed snapshot buffer could not encode the complete candidate. - #[kaptan_error(id = 12103)] - Encode, - /// Factory-only startup deliberately has no durable configuration path. - #[kaptan_error(id = 12104)] - FactoryOnly, -} - -/// Same-directory atomic-replacement store for SITL only. -pub struct FileConfigStore { - path: Option, -} - -impl FileConfigStore { - /// Creates a host-only store at a fixed profile path. - pub fn new(path: impl Into) -> Self { - Self { - path: Some(path.into()), - } - } - - /// Creates a factory-only store for `kaptan-sitl serve` without - /// `--config`. Commits are rejected rather than silently discarded. - pub const fn factory_only() -> Self { - Self { path: None } - } -} - -impl ConfigStore for FileConfigStore { - type Error = FileStoreError; - - fn load(&mut self, _: &CandidateProfile) -> Result, Self::Error> { - let Some(path) = self.path.as_deref() else { - return Ok(None); - }; - let file = match File::open(path) { - Ok(file) => file, - Err(error) if error.kind() == std::io::ErrorKind::NotFound => return Ok(None), - Err(error) => return Err(FileStoreError::Io(error)), - }; - const MAXIMUM_FILE_BYTES: u64 = MAXIMUM_CANDIDATE_BYTES as u64; - if file.metadata().map_err(FileStoreError::Io)?.len() > MAXIMUM_FILE_BYTES { - return Err(FileStoreError::Cbor(CborError::TooLarge)); - } - let mut bytes = Vec::new(); - file.take(MAXIMUM_FILE_BYTES + 1) - .read_to_end(&mut bytes) - .map_err(FileStoreError::Io)?; - if bytes.len() as u64 > MAXIMUM_FILE_BYTES { - return Err(FileStoreError::Cbor(CborError::TooLarge)); - } - decode_candidate(&bytes, &CAPABILITIES) - .map(Some) - .map_err(FileStoreError::Cbor) - } - - fn persist(&mut self, candidate: &CandidateProfile) -> Result<(), Self::Error> { - let path = self.path.clone().ok_or(FileStoreError::FactoryOnly)?; - write_candidate(&path, *candidate) - } -} - -/// Returns the fully validated factory-active profile. -pub fn default_profile() -> Result { - Ok(configuration_service(FileConfigStore::factory_only())?.active_profile()) -} - -/// Loads one bounded, complete CBOR candidate and returns its active view. -pub fn load_profile(path: impl AsRef) -> Result { - if !path.as_ref().is_file() { - return Err("configuration file does not exist".to_owned()); - } - Ok(configuration_service(FileConfigStore::new(path.as_ref()))?.active_profile()) -} - -/// Writes one validated factory candidate using a same-directory replacement. -pub fn write_default_profile(path: impl AsRef) -> Result<(), String> { - let mut store = FileConfigStore::new(path.as_ref()); - store - .persist(&CandidateProfile::factory()) - .map_err(|error| error.to_string()) -} - -fn write_candidate(path: &Path, candidate: CandidateProfile) -> Result<(), FileStoreError> { - let mut writer = minicbor::encode::write::Cursor::new([0_u8; MAXIMUM_CANDIDATE_BYTES]); - encode_candidate(candidate, &CAPABILITIES, &mut writer).map_err(|error| match error { - CborEncodeError::InvalidProfile(violation) => { - FileStoreError::Cbor(CborError::InvalidProfile(violation)) - } - CborEncodeError::Encode(_) => FileStoreError::Encode, - })?; - let length = writer.position(); - let bytes = writer.into_inner(); - let temporary = path.with_extension("cbor.tmp"); - let mut file = File::create(&temporary).map_err(FileStoreError::Io)?; - file.write_all(&bytes[..length]) - .map_err(FileStoreError::Io)?; - file.sync_all().map_err(FileStoreError::Io)?; - fs::rename(&temporary, path).map_err(FileStoreError::Io) -} diff --git a/firmware/tools/sitl/src/config/tests.rs b/firmware/tools/sitl/src/config/tests.rs deleted file mode 100644 index 18da4ca..0000000 --- a/firmware/tools/sitl/src/config/tests.rs +++ /dev/null @@ -1,81 +0,0 @@ -use std::fs; - -use kaptan_config::{ConfigModel, ConfigStore}; -use kaptan_flight_config::{CandidateProfile, decode_candidate}; -use kaptan_mavlink::{ - MavlinkFrame, - encoder::MavlinkEncoder, - generated::kaptan::{LogLevel, MavMessage}, - receiver::IntegrityMavlinkReceiver, -}; - -use super::{FileConfigStore, configuration_service, runtime::CAPABILITIES}; - -fn parse_test_message(frame: &MavlinkFrame) -> Option { - let mut receiver = IntegrityMavlinkReceiver::<1>::new(); - for byte in frame.raw_bytes() { - receiver.receive_byte(*byte); - } - receiver.take_received()?.decode() -} - -#[test] -fn host_store_round_trips_a_complete_validated_snapshot() { - let path = std::env::temp_dir().join(format!( - "kaptan-config-store-{}-{}.cbor", - std::process::id(), - std::thread::current().name().unwrap_or("test") - )); - let temporary = path.with_extension("cbor.tmp"); - let _ = fs::remove_file(&path); - let _ = fs::remove_file(&temporary); - - let factory = CandidateProfile::factory(); - let mut store = FileConfigStore::new(&path); - assert!(store.load(&factory).unwrap().is_none()); - store.persist(&factory).unwrap(); - let snapshot = fs::read(&path).unwrap(); - let decoded = decode_candidate(&snapshot, &CAPABILITIES).unwrap(); - assert!(decoded.validate(&CAPABILITIES).is_ok()); - let candidate = store.load(&factory).unwrap().expect("persisted candidate"); - assert!(candidate.validate(&CAPABILITIES).is_ok()); - - store.persist(&factory).unwrap(); - let mut reopened = FileConfigStore::new(&path); - assert!(reopened.load(&factory).unwrap().is_some()); - let mut fresh = FileConfigStore::new(&path); - fresh.persist(&factory).unwrap(); - - fs::remove_file(&path).unwrap(); -} - -#[test] -fn standard_log_macro_reaches_the_generated_mavlink_log_payload() { - let path = std::env::temp_dir().join(format!( - "kaptan-sitl-log-{}-{}.cbor", - std::process::id(), - std::thread::current().name().unwrap_or("test") - )); - let _ = fs::remove_file(&path); - let mut service = configuration_service(FileConfigStore::new(&path)).unwrap(); - - kaptan_log::info!("SITL configuration service ready"); - let payload = service - .pending_log_payload() - .expect("info facade record must reach LOG"); - assert_eq!(payload.level, LogLevel::LOG_LEVEL_INFO); - let message = std::str::from_utf8(&payload.message[..usize::from(payload.message_length)]) - .expect("facade text remains UTF-8"); - assert_eq!(message, "SITL configuration service ready"); - let mut encoder = MavlinkEncoder::new(1, 1); - let frame = encoder.encode(&payload); - assert_eq!(frame.message_id(), 51_020); - assert!(matches!( - parse_test_message(&frame), - Some(MavMessage::LOG(data)) - if data.level == payload.level - && data.message_length == payload.message_length - && data.message == payload.message - )); - let _ = fs::remove_file(&path); -} diff --git a/firmware/tools/sitl/src/kaptan_sitl_quad.cc b/firmware/tools/sitl/src/kaptan_sitl_quad.cc deleted file mode 100644 index 2d90e79..0000000 --- a/firmware/tools/sitl/src/kaptan_sitl_quad.cc +++ /dev/null @@ -1,650 +0,0 @@ -// Deterministic, deliberately narrow Gazebo-side adapter for the Kaptan -// quad-X host-SITL transport contract. It is simulator tooling, not flight code. - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -namespace kaptan::sitl -{ -namespace -{ -constexpr std::uint8_t kPacketVersion = 1; -constexpr std::array kImuMagic{'K', 'S', 'I', '1'}; -constexpr std::array kLocalStateMagic{'K', 'S', 'V', '1'}; -constexpr std::array kClockMagic{'K', 'S', 'C', '1'}; -constexpr std::array kTruthMagic{'K', 'S', 'T', '2'}; -constexpr std::array kOutputMagic{'K', 'S', 'O', '1'}; -constexpr std::size_t kImuPacketSize = 44; -constexpr std::size_t kClockPacketSize = 16; -constexpr std::size_t kTruthPacketSize = 76; -constexpr std::size_t kOutputPacketSize = 40; -constexpr std::size_t kMaximumPacketsPerStep = 4; -constexpr std::size_t kQuadMotorCount = 4; - -struct QuadMotorGeometry -{ - gz::math::Vector3d offset_flu; - double reaction_torque_sign; -}; - -// This test model is FLU / ENU. It is not an electrical Kakute timer map: -// M1 front-right CW, M2 rear-right CCW, M3 rear-left CW, M4 front-left CCW. -constexpr std::array kQuadXGeometry{{ - {{0.17, -0.17, 0.0}, -1.0}, - {{-0.17, -0.17, 0.0}, 1.0}, - {{-0.17, 0.17, 0.0}, -1.0}, - {{0.17, 0.17, 0.0}, 1.0}, -}}; - -bool IsFiniteFraction(const float value) -{ - return std::isfinite(value) && value >= 0.0F && value <= 1.0F; -} - -bool IsZero(const float value) -{ - return value == 0.0F; -} - -void WriteU32(std::uint8_t *destination, const std::uint32_t value) -{ - for (std::size_t index = 0; index < sizeof(value); ++index) - { - destination[index] = static_cast(value >> (index * 8)); - } -} - -void WriteU64(std::uint8_t *destination, const std::uint64_t value) -{ - for (std::size_t index = 0; index < sizeof(value); ++index) - { - destination[index] = static_cast(value >> (index * 8)); - } -} - -std::uint32_t ReadU32(const std::uint8_t *source) -{ - std::uint32_t value = 0; - for (std::size_t index = 0; index < sizeof(value); ++index) - { - value |= static_cast(source[index]) << (index * 8); - } - return value; -} - -std::uint64_t ReadU64(const std::uint8_t *source) -{ - std::uint64_t value = 0; - for (std::size_t index = 0; index < sizeof(value); ++index) - { - value |= static_cast(source[index]) << (index * 8); - } - return value; -} - -void WriteF32(std::uint8_t *destination, const float value) -{ - WriteU32(destination, std::bit_cast(value)); -} - -float ReadF32(const std::uint8_t *source) -{ - return std::bit_cast(ReadU32(source)); -} - -bool SequenceAfter(const std::uint32_t next, const std::uint32_t previous) -{ - const std::uint32_t distance = next - previous; - return distance != 0 && distance <= UINT32_MAX / 2; -} - -class UdpSocket -{ - public: - UdpSocket() = default; - ~UdpSocket() - { - if (this->descriptor_ >= 0) - { - close(this->descriptor_); - } - } - - UdpSocket(const UdpSocket &) = delete; - UdpSocket &operator=(const UdpSocket &) = delete; - - bool BindLoopback(const std::uint16_t port) - { - this->descriptor_ = socket(AF_INET, SOCK_DGRAM, 0); - if (this->descriptor_ < 0) - { - return false; - } - const int flags = fcntl(this->descriptor_, F_GETFL, 0); - if (flags < 0 || fcntl(this->descriptor_, F_SETFL, flags | O_NONBLOCK) < 0) - { - return false; - } - sockaddr_in address{}; - address.sin_family = AF_INET; - address.sin_port = htons(port); - address.sin_addr.s_addr = htonl(INADDR_LOOPBACK); - return bind( - this->descriptor_, - reinterpret_cast(&address), - sizeof(address)) == 0; - } - - bool SendToLoopback( - const std::uint16_t port, const std::uint8_t *data, const std::size_t size) const - { - sockaddr_in address{}; - address.sin_family = AF_INET; - address.sin_port = htons(port); - address.sin_addr.s_addr = htonl(INADDR_LOOPBACK); - return sendto( - this->descriptor_, data, size, MSG_DONTWAIT, - reinterpret_cast(&address), sizeof(address)) == - static_cast(size); - } - - ssize_t Receive(std::uint8_t *data, const std::size_t size) const - { - sockaddr_in sender{}; - socklen_t sender_size = sizeof(sender); - const ssize_t received = recvfrom( - this->descriptor_, data, size, MSG_DONTWAIT, - reinterpret_cast(&sender), &sender_size); - if (received < 0 || sender.sin_addr.s_addr != htonl(INADDR_LOOPBACK)) - { - return -1; - } - return received; - } - - private: - int descriptor_{-1}; -}; - -struct OutputCommand -{ - std::uint64_t controller_time_usec{}; - std::uint32_t control_sequence{}; - bool permitted{}; - std::array motors{}; -}; - -bool DecodeOutput(const std::array &packet, - OutputCommand &output) -{ - if (!std::equal(kOutputMagic.begin(), kOutputMagic.end(), packet.begin()) || - packet[4] != kPacketVersion || packet[5] != 0 || packet[6] != 0 || packet[7] != 0) - { - return false; - } - output.controller_time_usec = ReadU64(packet.data() + 8); - output.control_sequence = ReadU32(packet.data() + 16); - if (packet[20] > 1 || packet[21] != 0 || packet[22] != 0 || packet[23] != 0) - { - return false; - } - output.permitted = packet[20] == 1; - for (std::size_t index = 0; index < output.motors.size(); ++index) - { - output.motors[index] = ReadF32(packet.data() + 24 + index * sizeof(float)); - if (!IsFiniteFraction(output.motors[index])) - { - return false; - } - } - return output.permitted || std::all_of(output.motors.begin(), output.motors.end(), IsZero); -} -} // namespace - -class Quad final : public gz::sim::System, - public gz::sim::ISystemConfigure, - public gz::sim::ISystemPreUpdate, - public gz::sim::ISystemPostUpdate, - public gz::sim::ISystemReset -{ - public: - Quad() : data_(std::make_unique()) {} - - void Configure(const gz::sim::Entity &entity, - const std::shared_ptr &sdf, - gz::sim::EntityComponentManager &ecm, - gz::sim::EventManager &) override; - - void PreUpdate(const gz::sim::UpdateInfo &info, - gz::sim::EntityComponentManager &ecm) override; - - void PostUpdate(const gz::sim::UpdateInfo &info, - const gz::sim::EntityComponentManager &) override; - - void Reset(const gz::sim::UpdateInfo &, - gz::sim::EntityComponentManager &) override; - - private: - struct Data - { - void OnImu(const gz::msgs::IMU &message) - { - std::lock_guard lock(this->imu_mutex); - this->latest_imu = message; - ++this->imu_generation; - if (!this->received_first_imu) - { - gzmsg << "Kaptan SITL quad received its first Gazebo IMU sample.\n"; - this->received_first_imu = true; - } - } - - gz::sim::Link body; - gz::transport::Node node; - UdpSocket socket; - std::uint16_t sensor_port{}; - std::uint16_t actuator_port{}; - std::uint16_t truth_port{}; - std::uint16_t user_clock_port{}; - std::chrono::microseconds actuator_timeout{}; - double maximum_motor_thrust_newtons{}; - double motor_time_constant_seconds{}; - std::normal_distribution local_position_noise{0.0, 0.0}; - std::normal_distribution local_velocity_noise{0.0, 0.0}; - std::mt19937 local_state_random{}; - double reaction_torque_per_newton_metre{}; - std::optional launch_origin_enu; - std::optional launch_attitude_enu; - std::mutex imu_mutex; - gz::msgs::IMU latest_imu; - std::uint64_t imu_generation{}; - std::uint64_t published_imu_generation{}; - bool received_first_imu{}; - std::optional output; - std::array applied_motor_fractions{}; - std::optional> previous_output_order; - // Actuator freshness belongs to the simulated vehicle timeline, never - // host scheduling time. That keeps timeout behavior deterministic when - // Gazebo is paused, stepped, or runs at a non-real-time factor. - std::optional last_output_sim_time; - }; - - std::unique_ptr data_; -}; - -void Quad::Configure(const gz::sim::Entity &entity, - const std::shared_ptr &sdf, - gz::sim::EntityComponentManager &ecm, - gz::sim::EventManager &) -{ - const gz::sim::Model model(entity); - if (!model.Valid(ecm)) - { - gzerr << "Kaptan SITL quad must be attached to a model.\n"; - return; - } - const auto body_entity = model.LinkByName(ecm, "base_link"); - if (body_entity == gz::sim::kNullEntity) - { - gzerr << "Kaptan SITL quad requires a base_link.\n"; - return; - } - this->data_->body = gz::sim::Link(body_entity); - this->data_->body.EnableVelocityChecks(ecm); - - const std::string imu_topic = sdf->Get("imu_topic", "").first; - const unsigned int sensor_port = sdf->Get("sensor_udp_port", 0U).first; - const unsigned int actuator_port = - sdf->Get("actuator_udp_port", 0U).first; - const unsigned int truth_port = sdf->Get("truth_udp_port", 0U).first; - const unsigned int user_clock_port = - sdf->Get("user_clock_udp_port", 0U).first; - const std::uint64_t timeout_usec = - sdf->Get("actuator_timeout_usec", 0).first; - this->data_->maximum_motor_thrust_newtons = - sdf->Get("maximum_motor_thrust_newtons", 0.0).first; - this->data_->motor_time_constant_seconds = - sdf->Get("motor_time_constant_seconds", 0.0).first; - const double local_position_noise_stddev = - sdf->Get("local_position_noise_stddev_m", -1.0).first; - const double local_velocity_noise_stddev = - sdf->Get("local_velocity_noise_stddev_mps", -1.0).first; - const unsigned int local_state_noise_seed = - sdf->Get("local_state_noise_seed", 0U).first; - this->data_->reaction_torque_per_newton_metre = - sdf->Get("reaction_torque_per_newton_metre", 0.0).first; - if (imu_topic.empty() || sensor_port == 0 || actuator_port == 0 || truth_port == 0 || - user_clock_port == 0 || - sensor_port > std::numeric_limits::max() || - actuator_port > std::numeric_limits::max() || - truth_port > std::numeric_limits::max() || - user_clock_port > std::numeric_limits::max() || - timeout_usec == 0 || !std::isfinite(this->data_->maximum_motor_thrust_newtons) || - this->data_->maximum_motor_thrust_newtons <= 0.0 || - !std::isfinite(this->data_->motor_time_constant_seconds) || - this->data_->motor_time_constant_seconds <= 0.0 || - !std::isfinite(local_position_noise_stddev) || local_position_noise_stddev < 0.0 || - !std::isfinite(local_velocity_noise_stddev) || local_velocity_noise_stddev < 0.0 || - !std::isfinite(this->data_->reaction_torque_per_newton_metre) || - this->data_->reaction_torque_per_newton_metre < 0.0) - { - gzerr << "Kaptan SITL quad has an invalid static transport or vehicle configuration.\n"; - return; - } - this->data_->sensor_port = static_cast(sensor_port); - this->data_->actuator_port = static_cast(actuator_port); - this->data_->truth_port = static_cast(truth_port); - this->data_->user_clock_port = static_cast(user_clock_port); - this->data_->actuator_timeout = std::chrono::microseconds(timeout_usec); - this->data_->local_position_noise = - std::normal_distribution(0.0, local_position_noise_stddev); - this->data_->local_velocity_noise = - std::normal_distribution(0.0, local_velocity_noise_stddev); - this->data_->local_state_random.seed(local_state_noise_seed); - if (!this->data_->socket.BindLoopback(this->data_->actuator_port)) - { - gzerr << "Kaptan SITL quad could not bind its loopback actuator socket.\n"; - return; - } - if (!this->data_->node.Subscribe(imu_topic, &Data::OnImu, this->data_.get())) - { - gzerr << "Kaptan SITL quad could not subscribe to its configured IMU topic.\n"; - } - else - { - gzmsg << "Kaptan SITL quad subscribed to Gazebo IMU topic [" << imu_topic << "].\n"; - } -} - -void Quad::PreUpdate(const gz::sim::UpdateInfo &info, - gz::sim::EntityComponentManager &ecm) -{ - if (info.paused || info.simTime.count() < 0 || !this->data_->body.Valid(ecm)) - { - return; - } - const auto simulation_time = - std::chrono::duration_cast(info.simTime); - for (std::size_t attempt = 0; attempt < kMaximumPacketsPerStep; ++attempt) - { - std::array packet{}; - if (this->data_->socket.Receive(packet.data(), packet.size()) != - static_cast(packet.size())) - { - break; - } - OutputCommand command{}; - if (!DecodeOutput(packet, command)) - { - continue; - } - if (!command.permitted) - { - this->data_->output = command; - continue; - } - if (this->data_->previous_output_order.has_value() && - (command.controller_time_usec <= this->data_->previous_output_order->first || - !SequenceAfter(command.control_sequence, - this->data_->previous_output_order->second))) - { - continue; - } - this->data_->previous_output_order = - std::pair{command.controller_time_usec, command.control_sequence}; - this->data_->last_output_sim_time = simulation_time; - this->data_->output = command; - } - - const bool command_is_fresh = - this->data_->output.has_value() && this->data_->output->permitted && - this->data_->last_output_sim_time.has_value() && - simulation_time >= *this->data_->last_output_sim_time && - simulation_time - *this->data_->last_output_sim_time <= this->data_->actuator_timeout; - const auto pose = this->data_->body.WorldPose(ecm); - if (!pose.has_value()) - { - return; - } - const double dt_seconds = std::chrono::duration(info.dt).count(); - if (!std::isfinite(dt_seconds) || dt_seconds <= 0.0) - { - return; - } - const double alpha = 1.0 - std::exp(-dt_seconds / this->data_->motor_time_constant_seconds); - const auto desired_motors = command_is_fresh - ? this->data_->output->motors - : std::array{}; - const gz::math::Vector3d thrust_axis_world = pose->Rot().RotateVector({0.0, 0.0, 1.0}); - for (std::size_t index = 0; index < kQuadMotorCount; ++index) - { - this->data_->applied_motor_fractions[index] += - alpha * (static_cast(desired_motors[index]) - - this->data_->applied_motor_fractions[index]); - const double thrust = - this->data_->applied_motor_fractions[index] * - this->data_->maximum_motor_thrust_newtons; - this->data_->body.AddWorldWrench( - ecm, thrust_axis_world * thrust, - thrust_axis_world * - (kQuadXGeometry[index].reaction_torque_sign * thrust * - this->data_->reaction_torque_per_newton_metre), - kQuadXGeometry[index].offset_flu); - } -} - -void Quad::PostUpdate(const gz::sim::UpdateInfo &info, - const gz::sim::EntityComponentManager &_ecm) -{ - if (info.paused || info.simTime.count() < 0) - { - return; - } - // This is deliberately emitted for every unpaused Gazebo step, even when - // an IMU sample is absent. The host runtime uses it as its sole freshness - // and lease time base; host scheduling cannot advance safety state. - const auto send_clock = [this, &info]() { - std::array clock_packet{}; - std::copy(kClockMagic.begin(), kClockMagic.end(), clock_packet.begin()); - clock_packet[4] = kPacketVersion; - WriteU64(clock_packet.data() + 8, static_cast( - std::chrono::duration_cast( - info.simTime) - .count())); - (void)this->data_->socket.SendToLoopback( - this->data_->sensor_port, clock_packet.data(), clock_packet.size()); - (void)this->data_->socket.SendToLoopback( - this->data_->user_clock_port, clock_packet.data(), clock_packet.size()); - }; - gz::msgs::IMU imu; - { - std::lock_guard lock(this->data_->imu_mutex); - if (this->data_->imu_generation == this->data_->published_imu_generation) - { - send_clock(); - return; - } - imu = this->data_->latest_imu; - this->data_->published_imu_generation = this->data_->imu_generation; - } - - // Gazebo's standard body sensor convention is FLU. Kaptan's is FRD, so the - // single explicit boundary conversion is x, -y, -z for both vectors. - std::array packet{}; - std::copy(kImuMagic.begin(), kImuMagic.end(), packet.begin()); - packet[4] = kPacketVersion; - WriteU64(packet.data() + 8, static_cast( - std::chrono::duration_cast( - info.simTime) - .count())); - WriteU32(packet.data() + 16, static_cast(this->data_->imu_generation)); - const std::array frd{ - static_cast(imu.linear_acceleration().x()), - -static_cast(imu.linear_acceleration().y()), - -static_cast(imu.linear_acceleration().z()), - static_cast(imu.angular_velocity().x()), - -static_cast(imu.angular_velocity().y()), - -static_cast(imu.angular_velocity().z()), - }; - if (!std::all_of(frd.begin(), frd.end(), [](const float value) { return std::isfinite(value); })) - { - send_clock(); - return; - } - for (std::size_t index = 0; index < frd.size(); ++index) - { - WriteF32(packet.data() + 20 + index * sizeof(float), frd[index]); - } - (void)this->data_->socket.SendToLoopback( - this->data_->sensor_port, packet.data(), packet.size()); - - const auto pose = this->data_->body.WorldPose(_ecm); - const auto velocity = this->data_->body.WorldLinearVelocity(_ecm); - if (!pose.has_value() || !velocity.has_value()) - { - send_clock(); - return; - } - if (!this->data_->launch_origin_enu.has_value()) - { - this->data_->launch_origin_enu = pose->Pos(); - this->data_->launch_attitude_enu = pose->Rot(); - } - // This fixed test vehicle is yawed so ENU world +Y/+X/-Z are respectively - // its launch-local NED north/east/down axes. The conversion happens once at - // the Gazebo boundary; all flight code receives only explicit NED SI data. - const auto &origin = *this->data_->launch_origin_enu; - const std::array local_ned{ - static_cast(pose->Pos().Y() - origin.Y() + - this->data_->local_position_noise(this->data_->local_state_random)), - static_cast(pose->Pos().X() - origin.X() + - this->data_->local_position_noise(this->data_->local_state_random)), - static_cast(origin.Z() - pose->Pos().Z() + - this->data_->local_position_noise(this->data_->local_state_random)), - static_cast(velocity->Y() + - this->data_->local_velocity_noise(this->data_->local_state_random)), - static_cast(velocity->X() + - this->data_->local_velocity_noise(this->data_->local_state_random)), - static_cast(-velocity->Z() + - this->data_->local_velocity_noise(this->data_->local_state_random)), - }; - if (!std::all_of( - local_ned.begin(), local_ned.end(), - [](const float value) { return std::isfinite(value); })) - { - send_clock(); - return; - } - std::array state_packet{}; - std::copy(kLocalStateMagic.begin(), kLocalStateMagic.end(), state_packet.begin()); - state_packet[4] = kPacketVersion; - WriteU64(state_packet.data() + 8, static_cast( - std::chrono::duration_cast( - info.simTime) - .count())); - WriteU32(state_packet.data() + 16, static_cast(this->data_->imu_generation)); - for (std::size_t index = 0; index < local_ned.size(); ++index) - { - WriteF32(state_packet.data() + 20 + index * sizeof(float), local_ned[index]); - } - (void)this->data_->socket.SendToLoopback( - this->data_->sensor_port, state_packet.data(), state_packet.size()); - - // This packet is intentionally sent to a separate test-only port. The - // controller never binds that port, so these noise-free values cannot form a - // parallel control path or mask estimator behavior. The quaternion is the - // body attitude relative to launch; its rotation magnitude is frame - // invariant, which permits the test client to verify attitude without - // duplicating the ENU/FLU-to-NED/FRD conversion path. - const auto launch_attitude = *this->data_->launch_attitude_enu; - const auto relative_attitude = launch_attitude.Inverse() * pose->Rot(); - const std::array truth_ned{ - static_cast(pose->Pos().Y() - origin.Y()), - static_cast(pose->Pos().X() - origin.X()), - static_cast(origin.Z() - pose->Pos().Z()), - static_cast(velocity->Y()), - static_cast(velocity->X()), - static_cast(-velocity->Z()), - static_cast(relative_attitude.W()), - static_cast(relative_attitude.X()), - static_cast(relative_attitude.Y()), - static_cast(relative_attitude.Z()), - static_cast(this->data_->applied_motor_fractions[0]), - static_cast(this->data_->applied_motor_fractions[1]), - static_cast(this->data_->applied_motor_fractions[2]), - static_cast(this->data_->applied_motor_fractions[3]), - }; - if (!std::all_of( - truth_ned.begin(), truth_ned.end(), - [](const float value) { return std::isfinite(value); })) - { - send_clock(); - return; - } - std::array truth_packet{}; - std::copy(kTruthMagic.begin(), kTruthMagic.end(), truth_packet.begin()); - truth_packet[4] = kPacketVersion; - WriteU64(truth_packet.data() + 8, static_cast( - std::chrono::duration_cast( - info.simTime) - .count())); - WriteU32(truth_packet.data() + 16, static_cast(this->data_->imu_generation)); - for (std::size_t index = 0; index < truth_ned.size(); ++index) - { - WriteF32(truth_packet.data() + 20 + index * sizeof(float), truth_ned[index]); - } - (void)this->data_->socket.SendToLoopback( - this->data_->truth_port, truth_packet.data(), truth_packet.size()); - send_clock(); -} - -void Quad::Reset(const gz::sim::UpdateInfo &, - gz::sim::EntityComponentManager &) -{ - this->data_->output.reset(); - this->data_->applied_motor_fractions = {}; - this->data_->previous_output_order.reset(); - this->data_->last_output_sim_time.reset(); - this->data_->launch_origin_enu.reset(); - this->data_->launch_attitude_enu.reset(); - std::lock_guard lock(this->data_->imu_mutex); - this->data_->published_imu_generation = this->data_->imu_generation; -} - -} // namespace kaptan::sitl - -GZ_ADD_PLUGIN(kaptan::sitl::Quad, - gz::sim::System, - kaptan::sitl::Quad::ISystemConfigure, - kaptan::sitl::Quad::ISystemPreUpdate, - kaptan::sitl::Quad::ISystemPostUpdate, - kaptan::sitl::Quad::ISystemReset) -GZ_ADD_PLUGIN_ALIAS(kaptan::sitl::Quad, "kaptan::sitl::Quad") diff --git a/firmware/tools/sitl/src/main.rs b/firmware/tools/sitl/src/main.rs deleted file mode 100644 index 2aff44c..0000000 --- a/firmware/tools/sitl/src/main.rs +++ /dev/null @@ -1,1440 +0,0 @@ -//! Host-only closed-loop simulator executable. -//! -//! The process owns the same portable estimator and control primitives used by -//! Kaptan. Gazebo may supply simulated measurements, but only the ESKF nominal -//! state reaches the controller; simulator pose/velocity never has a parallel -//! controller path. - -/// Host-only persisted configuration adapter and custom-service integration. -pub mod config; -mod protocol; - -use std::{ - env, - io::ErrorKind, - net::{SocketAddr, UdpSocket}, - process::ExitCode, - thread, - time::Duration, -}; - -use kaptan_control::{ - ActuatorLimits, Allocator, BodyRate, BodyWrench, HeadingNed, KinematicLimits, MixerOutput, - PidConfig, PrecomputedMixer, RatePid, ThrustEnvelope, Trajectory, TranslationalControlConfig, - TranslationalController, TranslationalEstimate, force_to_attitude, -}; -use kaptan_math::{Frd, Vector3}; -use kaptan_navigation::sitl::{ - EstimatorConfig, ImuNoise, ImuObservation, InertialEskf, IntegrityConfig, StationaryInitializer, -}; -use kaptan_navigation::{ - AccelerationFrd, AngularRateFrd, CalibrationRevision, FrameId, - LocalPositionVelocityObservation, ObservationContext, ObservationSequence, PositionNed, - ScalarVariance, SourceId, Validity, VelocityNed, -}; -use kaptan_safety::{ActuatorPermit, SafetyConfig, SafetyInputs, SafetySupervisor}; -use kaptan_time::Timestamp; - -#[cfg(test)] -use crate::config::default_profile; -use crate::config::{ - FileConfigStore, SitlConfigRuntime, configuration_service, load_profile, write_default_profile, -}; -use kaptan_flight_config::VehicleProfile; - -use protocol::{ - ACTUATOR_PORT, CLOCK_MAGIC, CONTROL_PORT, GoalKeepalivePacket, GoalLifecycle, GoalPacket, - GoalStatusPacket, IMU_MAGIC, LOCAL_STATE_MAGIC, SENSOR_PORT, STATUS_ACTUATOR_TRANSPORT, - STATUS_ESTIMATOR_READY, STATUS_IMU_FRESH, STATUS_LOCAL_AIDING_FRESH, STATUS_OUTPUT_PERMITTED, -}; - -const SOURCE_IMU: SourceId = SourceId::new(1); -const SOURCE_LOCAL_STATE: SourceId = SourceId::new(2); -const CALIBRATION: CalibrationRevision = CalibrationRevision::new(1); -/// Bound either ingress lane so a fast simulator clock cannot indefinitely -/// delay command lease processing, and a command flood cannot delay sensor -/// ordering and safety supervision. -const MAXIMUM_INGRESS_DATAGRAMS_PER_TURN: usize = 32; - -fn main() -> ExitCode { - let mut arguments = env::args().skip(1); - match arguments.next().as_deref() { - Some("serve") => match serve_configuration(&mut arguments).and_then(serve) { - Ok(()) => ExitCode::SUCCESS, - Err(error) => { - eprintln!("kaptan-sitl: {error}"); - ExitCode::FAILURE - } - }, - Some("config") => match config_command(&mut arguments) { - Ok(()) => ExitCode::SUCCESS, - Err(error) => { - eprintln!("kaptan-sitl: {error}"); - ExitCode::FAILURE - } - }, - _ => { - eprintln!( - "usage: kaptan-sitl serve [--config PROFILE.cbor]\n kaptan-sitl config write-default PROFILE.cbor\n kaptan-sitl config validate PROFILE.cbor" - ); - ExitCode::FAILURE - } - } -} - -fn serve_configuration( - arguments: &mut impl Iterator, -) -> Result { - match (arguments.next().as_deref(), arguments.next()) { - (None, None) => configuration_service(FileConfigStore::factory_only()), - (Some("--config"), Some(path)) => configuration_service(FileConfigStore::new(path)), - _ => Err("usage: kaptan-sitl serve [--config PROFILE.cbor]".to_owned()), - } -} - -fn config_command(arguments: &mut impl Iterator) -> Result<(), String> { - match ( - arguments.next().as_deref(), - arguments.next(), - arguments.next(), - ) { - (Some("write-default"), Some(path), None) => write_default_profile(path), - (Some("validate"), Some(path), None) => { - let _profile = load_profile(path)?; - eprintln!("kaptan-sitl: versioned CBOR configuration candidate is valid"); - Ok(()) - } - _ => Err( - "usage: kaptan-sitl config write-default PROFILE.cbor | validate PROFILE.cbor" - .to_owned(), - ), - } -} - -fn serve(configuration: SitlConfigRuntime) -> Result<(), String> { - let sensor = bind_loopback(SENSOR_PORT)?; - let control = bind_loopback(CONTROL_PORT)?; - let actuator = UdpSocket::bind(("127.0.0.1", 0)).map_err(|error| error.to_string())?; - sensor - .set_nonblocking(true) - .map_err(|error| error.to_string())?; - control - .set_nonblocking(true) - .map_err(|error| error.to_string())?; - let mut runtime = SitlRuntime::from_configuration(configuration)?; - eprintln!( - "kaptan-sitl: boot configuration selected; listening for Gazebo sensors on 127.0.0.1:{SENSOR_PORT} and commands on 127.0.0.1:{CONTROL_PORT}", - ); - - loop { - let mut made_progress = false; - for _ in 0..MAXIMUM_INGRESS_DATAGRAMS_PER_TURN { - let Some((packet, sender)) = receive(&sensor)? else { - break; - }; - made_progress = true; - if !protocol::localhost(sender) { - continue; - } - runtime.accept_sensor(&packet, &actuator, &control)?; - } - for _ in 0..MAXIMUM_INGRESS_DATAGRAMS_PER_TURN { - let Some((packet, sender)) = receive(&control)? else { - break; - }; - made_progress = true; - if !protocol::localhost(sender) { - continue; - } - runtime.accept_control(&packet, sender, &control)?; - } - if !made_progress { - // Freshness, lease expiry, and safety supervision advance only - // when Gazebo sends an explicit simulation clock packet. Host - // scheduling must not change the simulated vehicle behavior. - thread::sleep(Duration::from_millis(1)); - } - } -} - -fn bind_loopback(port: u16) -> Result { - UdpSocket::bind(("127.0.0.1", port)).map_err(|error| { - format!("cannot bind 127.0.0.1:{port}; another simulator may still be running: {error}") - }) -} - -fn receive(socket: &UdpSocket) -> Result, SocketAddr)>, String> { - let mut buffer = [0_u8; 256]; - match socket.recv_from(&mut buffer) { - Ok((length, sender)) => Ok(Some((buffer[..length].to_vec(), sender))), - Err(error) if error.kind() == ErrorKind::WouldBlock => Ok(None), - Err(error) => Err(error.to_string()), - } -} - -/// Private host-SITL datagram boundary. Production uses only loopback UDP; -/// the trait lets unit tests observe the exact encoded records without opening -/// a host socket or introducing another actuator path. -trait DatagramSink { - fn send_datagram(&self, bytes: &[u8], destination: SocketAddr) -> Result<(), String>; -} - -impl DatagramSink for UdpSocket { - fn send_datagram(&self, bytes: &[u8], destination: SocketAddr) -> Result<(), String> { - self.send_to(bytes, destination) - .map(|_| ()) - .map_err(|error| error.to_string()) - } -} - -struct SitlRuntime { - profile: VehicleProfile, - initializer: StationaryInitializer<100>, - estimator: Option, - controller: QuadController, - trajectory: Option, - safety: SafetySupervisor, - active_goal: Option, - landing_goal: bool, - goal_settled: bool, - goal_client: Option, - output_sequence: u32, - simulation_time: Option, - last_imu_time: Option, - last_imu_sequence: Option, - last_local_state_time: Option, - last_local_state_sequence: Option, - last_command_time: Option, - pending_control_step: Option<(u64, Option)>, - last_rate: BodyRate, -} - -#[derive(Clone, Copy, Debug, PartialEq)] -enum ClockWork { - Ignore, - Supervise, - Control(Option), -} - -impl SitlRuntime { - #[cfg(test)] - fn new() -> Result { - Self::from_configuration(configuration_service(FileConfigStore::factory_only())?) - } - - fn from_configuration(configuration: SitlConfigRuntime) -> Result { - let profile = configuration.active_profile(); - Ok(Self { - profile, - initializer: StationaryInitializer::new(), - estimator: None, - controller: QuadController::new(profile)?, - trajectory: None, - safety: SafetySupervisor::new( - SafetyConfig::new(profile.safety().prearm_dwell_microseconds()) - .map_err(|error| format!("invalid SITL safety policy: {error:?}"))?, - ), - active_goal: None, - landing_goal: false, - goal_settled: false, - goal_client: None, - output_sequence: 0, - simulation_time: None, - last_imu_time: None, - last_imu_sequence: None, - last_local_state_time: None, - last_local_state_sequence: None, - last_command_time: None, - pending_control_step: None, - last_rate: BodyRate::radians_per_second(0.0, 0.0, 0.0), - }) - } - - fn accept_sensor( - &mut self, - bytes: &[u8], - actuator: &A, - control: &C, - ) -> Result<(), String> { - if bytes.len() >= 4 && bytes[..4] == IMU_MAGIC { - if let Ok(packet) = protocol::decode_imu(bytes) { - self.accept_imu(packet, actuator)?; - } - } else if bytes.len() >= 4 - && bytes[..4] == LOCAL_STATE_MAGIC - && let Ok(packet) = protocol::decode_local_state(bytes) - { - self.accept_local_state(packet); - } else if bytes.len() >= 4 - && bytes[..4] == CLOCK_MAGIC - && let Ok(packet) = protocol::decode_clock(bytes) - { - self.accept_clock(packet, actuator, control)?; - } - Ok(()) - } - - fn accept_control( - &mut self, - bytes: &[u8], - sender: SocketAddr, - socket: &C, - ) -> Result<(), String> { - if let Some(now) = self.simulation_time { - self.expire_stale_goal(now); - } - if protocol::is_status_query(bytes) { - socket.send_datagram( - &protocol::encode_status(self.current_time(), self.status_flags()), - sender, - )?; - return Ok(()); - } - if let Ok(keepalive) = protocol::decode_goal_keepalive(bytes) { - self.renew_goal(keepalive, sender); - return Ok(()); - } - if let Ok(goal) = protocol::decode_landing(bytes) { - return self.start_goal(goal, true, sender, socket); - } - let Ok(goal) = protocol::decode_goal(bytes) else { - return Ok(()); - }; - self.start_goal(goal, false, sender, socket) - } - - fn start_goal( - &mut self, - goal: GoalPacket, - landing: bool, - sender: SocketAddr, - socket: &C, - ) -> Result<(), String> { - let Some(now) = self.simulation_time else { - return self.send_goal_status_to( - socket, - sender, - GoalStatusPacket { - goal_id: goal.goal_id, - lifecycle: GoalLifecycle::Rejected, - }, - ); - }; - if let Some(active) = self.active_goal { - if self.goal_client != Some(sender) { - return self.send_goal_status_to( - socket, - sender, - GoalStatusPacket { - goal_id: goal.goal_id, - lifecycle: GoalLifecycle::Rejected, - }, - ); - } - if active == goal && self.landing_goal == landing { - self.last_command_time = self.simulation_time; - return self.send_goal_status_to( - socket, - sender, - GoalStatusPacket { - goal_id: goal.goal_id, - lifecycle: if self.goal_settled { - GoalLifecycle::Settled - } else { - GoalLifecycle::Active - }, - }, - ); - } - if active.goal_id == goal.goal_id { - return self.send_goal_status_to( - socket, - sender, - GoalStatusPacket { - goal_id: goal.goal_id, - lifecycle: GoalLifecycle::Rejected, - }, - ); - } - } - let Some(estimator) = self.estimator else { - return self.send_goal_status_to( - socket, - sender, - GoalStatusPacket { - goal_id: goal.goal_id, - lifecycle: GoalLifecycle::Rejected, - }, - ); - }; - let health = estimator.health(estimator.time()); - if !health.initialized - || !health.finite - || !health.capabilities.local_position - || !health.capabilities.local_velocity - { - return self.send_goal_status_to( - socket, - sender, - GoalStatusPacket { - goal_id: goal.goal_id, - lifecycle: GoalLifecycle::Rejected, - }, - ); - } - let initial_position = estimator.state().position().vector(); - let target = Vector3::new(goal.north_metres, goal.east_metres, -goal.up_metres); - let limits = flight_limits(); - match self.trajectory.as_mut() { - Some(trajectory) => trajectory - .set_goal(target) - .map_err(|error| format!("invalid goal: {error:?}"))?, - None => { - let mut trajectory = Trajectory::new(initial_position, limits) - .map_err(|error| format!("invalid trajectory configuration: {error:?}"))?; - trajectory - .set_goal(target) - .map_err(|error| format!("invalid goal: {error:?}"))?; - self.trajectory = Some(trajectory); - } - } - self.active_goal = Some(goal); - self.landing_goal = landing; - self.goal_settled = false; - self.goal_client = Some(sender); - self.last_command_time = Some(now); - let _ = self - .safety - .request_arm(Timestamp::from_microseconds(now), self.safety_inputs(now)); - self.send_goal_status_to( - socket, - sender, - GoalStatusPacket { - goal_id: goal.goal_id, - lifecycle: GoalLifecycle::Active, - }, - ) - } - - fn renew_goal(&mut self, keepalive: GoalKeepalivePacket, sender: SocketAddr) { - if self.goal_client == Some(sender) - && self - .active_goal - .is_some_and(|goal| goal.goal_id == keepalive.goal_id) - { - self.last_command_time = self.simulation_time; - } - } - - fn accept_imu( - &mut self, - packet: protocol::ImuPacket, - actuator: &A, - ) -> Result<(), String> { - let reset_epoch = !strictly_after( - packet.time_microseconds, - packet.sequence, - self.last_imu_time, - self.last_imu_sequence, - ); - if reset_epoch { - self.reset_for_simulator_epoch(); - } - let step_seconds = if reset_epoch { - None - } else { - control_step_seconds( - self.last_imu_time, - packet.time_microseconds, - self.profile.timing().maximum_imu_interval_microseconds(), - ) - }; - if self.estimator.is_some() && self.last_imu_time.is_some() && step_seconds.is_none() { - self.reset_for_simulator_epoch(); - } - self.last_imu_time = Some(packet.time_microseconds); - self.last_imu_sequence = Some(packet.sequence); - self.last_rate = BodyRate::radians_per_second( - packet.angular_rate_frd_rad_s[0], - packet.angular_rate_frd_rad_s[1], - packet.angular_rate_frd_rad_s[2], - ); - let observation = imu_observation(packet)?; - if let Some(estimator) = self.estimator.as_mut() { - if estimator.propagate(observation).is_err() { - self.reset_for_simulator_epoch(); - self.send_inhibited_output(actuator, packet.time_microseconds)?; - return Ok(()); - } - } else if self.initializer.push(observation) && self.initializer.is_complete() { - self.estimator = core::mem::take(&mut self.initializer).finish( - observation.context.measurement_time, - estimator_config(self.profile.timing().maximum_imu_interval_microseconds()), - IntegrityConfig::default(), - 0.0, - ); - } - self.pending_control_step = Some((packet.time_microseconds, step_seconds)); - // An invalid propagation already sent a zero command above. Control - // advances only when the matching explicit simulation clock arrives. - Ok(()) - } - - fn accept_local_state(&mut self, packet: protocol::LocalStatePacket) { - if !strictly_after( - packet.time_microseconds, - packet.sequence, - self.last_local_state_time, - self.last_local_state_sequence, - ) { - return; - } - self.last_local_state_time = Some(packet.time_microseconds); - self.last_local_state_sequence = Some(packet.sequence); - let Some(estimator) = self.estimator.as_mut() else { - return; - }; - let Some(position_variance) = - ScalarVariance::new(self.profile.aiding().position_variance_m2()) - else { - return; - }; - let Some(velocity_variance) = ScalarVariance::new( - self.profile - .aiding() - .velocity_variance_m2_per_second_squared(), - ) else { - return; - }; - let observation = LocalPositionVelocityObservation { - context: ObservationContext::new( - Timestamp::from_microseconds(packet.time_microseconds), - Timestamp::from_microseconds(packet.time_microseconds), - SOURCE_LOCAL_STATE, - FrameId::LOCAL_NED, - CALIBRATION, - u8::MAX, - Validity::Valid, - ObservationSequence::new(packet.sequence), - ), - position: PositionNed::metres( - packet.position_ned_metres[0], - packet.position_ned_metres[1], - packet.position_ned_metres[2], - ), - velocity: VelocityNed::metres_per_second( - packet.velocity_ned_mps[0], - packet.velocity_ned_mps[1], - packet.velocity_ned_mps[2], - ), - position_variance, - velocity_variance, - }; - let _ = - estimator.fuse_local_position_velocity(observation, SOURCE_LOCAL_STATE, CALIBRATION); - } - - /// Advances all non-estimator supervision from Gazebo's monotonic - /// simulation clock. IMU packets schedule control, but they cannot advance - /// leases or freshness themselves: the matching clock record is required. - fn accept_clock( - &mut self, - packet: protocol::ClockPacket, - actuator: &A, - control: &C, - ) -> Result<(), String> { - match self.advance_simulation_clock(packet.time_microseconds) { - ClockWork::Ignore => Ok(()), - ClockWork::Control(step_seconds) => { - self.control(packet.time_microseconds, step_seconds, actuator, control) - } - ClockWork::Supervise => self.supervise_clock(packet.time_microseconds, actuator), - } - } - - fn advance_simulation_clock(&mut self, simulation_time: u64) -> ClockWork { - match self.simulation_time { - Some(previous) if simulation_time == previous => return ClockWork::Ignore, - Some(previous) if simulation_time < previous => { - self.reset_for_simulator_epoch(); - } - _ => {} - } - self.simulation_time = Some(simulation_time); - - match self.pending_control_step { - Some((imu_time, step_seconds)) if imu_time == simulation_time => { - self.pending_control_step = None; - ClockWork::Control(step_seconds) - } - Some((imu_time, _)) if imu_time < simulation_time => { - // The required clock tick was lost or reordered. Never use a - // stale measurement interval to advance the controller. - self.pending_control_step = None; - ClockWork::Supervise - } - _ => ClockWork::Supervise, - } - } - - fn control( - &mut self, - simulation_time: u64, - step_seconds: Option, - actuator: &A, - control: &C, - ) -> Result<(), String> { - self.expire_stale_goal(simulation_time); - let inputs = self.safety_inputs(simulation_time); - let decision = self - .safety - .evaluate(Timestamp::from_microseconds(simulation_time), inputs); - let permitted = match decision.permit() { - Some(permit) if self.safety.permit_is_current(permit) => { - let motors = match (self.estimator, self.trajectory.as_mut(), step_seconds) { - (Some(estimator), Some(trajectory), Some(dt_seconds)) => trajectory - .update(dt_seconds) - .map_err(|_| ()) - .and_then(|reference| { - self.controller - .update(estimator, reference, self.last_rate, dt_seconds) - .map_err(|_| ()) - }), - _ => Ok([0.0; 4]), - }; - match motors { - Ok(motors) => { - self.send_armed_output(actuator, simulation_time, permit, motors)? - } - Err(()) => { - self.abort_goal(); - self.send_inhibited_output(actuator, simulation_time)?; - false - } - } - } - _ => { - self.controller.reset(); - self.send_inhibited_output(actuator, simulation_time)?; - false - } - }; - if permitted - && !self.goal_settled - && self.goal_is_settled() - && let Some(goal) = self.active_goal - { - let lifecycle = if self.landing_goal { - GoalLifecycle::Landed - } else { - GoalLifecycle::Settled - }; - self.send_goal_status( - control, - GoalStatusPacket { - goal_id: goal.goal_id, - lifecycle, - }, - )?; - self.goal_settled = true; - if self.landing_goal { - // Landing is terminal. Revoke authority in the same IMU-timed - // control cycle that reports it; a client cannot keep motors - // active merely by renewing the preceding trajectory lease. - self.abort_goal(); - self.send_inhibited_output(actuator, simulation_time)?; - } - } - Ok(()) - } - - /// Evaluates freshness on every explicit simulation clock tick, including - /// ticks where an IMU sample was absent. Wall-clock idling does nothing. - fn supervise_clock( - &mut self, - simulation_time: u64, - actuator: &A, - ) -> Result<(), String> { - self.expire_stale_goal(simulation_time); - let inputs = self.safety_inputs(simulation_time); - let decision = self - .safety - .evaluate(Timestamp::from_microseconds(simulation_time), inputs); - if decision.permit().is_none() { - self.controller.reset(); - self.send_inhibited_output(actuator, simulation_time)?; - } - Ok(()) - } - - fn safety_inputs(&self, now: u64) -> SafetyInputs { - let estimator_healthy = self.estimator.is_some_and(|estimator| { - let health = estimator.health(estimator.time()); - health.initialized - && health.finite - && health.capabilities.attitude - && health.capabilities.local_position - && health.capabilities.local_velocity - }) && fresh( - self.last_local_state_time, - now, - self.profile.timing().local_state_freshness_microseconds(), - ); - SafetyInputs { - estimator_healthy, - imu_fresh: fresh( - self.last_imu_time, - now, - self.profile.timing().imu_freshness_microseconds(), - ), - link_fresh: fresh( - self.last_command_time, - now, - self.profile.timing().command_link_freshness_microseconds(), - ), - actuator_healthy: true, - } - } - - fn expire_stale_goal(&mut self, now: u64) { - if self.active_goal.is_some() - && !fresh( - self.last_command_time, - now, - self.profile.timing().command_link_freshness_microseconds(), - ) - { - self.abort_goal(); - } - } - - fn abort_goal(&mut self) { - self.safety.request_disarm(); - self.trajectory = None; - self.active_goal = None; - self.landing_goal = false; - self.goal_settled = false; - self.goal_client = None; - self.last_command_time = None; - self.controller.reset(); - } - - /// Revalidates the supervisor token at the sole nonzero-output boundary. - fn send_armed_output( - &mut self, - actuator: &A, - simulation_time: u64, - permit: &ActuatorPermit, - motors: [f32; 4], - ) -> Result { - if !self.safety.permit_is_current(permit) { - self.send_inhibited_output(actuator, simulation_time)?; - return Ok(false); - } - self.send_wire_output(actuator, simulation_time, true, motors)?; - Ok(true) - } - - /// Sends an explicit zero, inhibited output without requiring a permit. - fn send_inhibited_output( - &mut self, - actuator: &A, - simulation_time: u64, - ) -> Result<(), String> { - self.send_wire_output(actuator, simulation_time, false, [0.0; 4]) - } - - fn send_wire_output( - &mut self, - actuator: &A, - simulation_time: u64, - permitted: bool, - motors: [f32; 4], - ) -> Result<(), String> { - let output = - protocol::encode_output(simulation_time, self.output_sequence, permitted, motors) - .map_err(|error| format!("refusing invalid motor output: {error:?}"))?; - self.output_sequence = self.output_sequence.wrapping_add(1); - actuator.send_datagram(&output, SocketAddr::from(([127, 0, 0, 1], ACTUATOR_PORT))) - } - - fn status_flags(&self) -> u16 { - let now = self.current_time(); - let mut flags = STATUS_ACTUATOR_TRANSPORT; - if fresh( - self.last_imu_time, - now, - self.profile.timing().imu_freshness_microseconds(), - ) { - flags |= STATUS_IMU_FRESH; - } - if fresh( - self.last_local_state_time, - now, - self.profile.timing().local_state_freshness_microseconds(), - ) { - flags |= STATUS_LOCAL_AIDING_FRESH; - } - if self.estimator.is_some_and(|estimator| { - let health = estimator.health(estimator.time()); - health.initialized - && health.finite - && health.capabilities.local_position - && health.capabilities.local_velocity - }) { - flags |= STATUS_ESTIMATOR_READY; - } - if self.safety.state() == kaptan_safety::SafetyState::Armed { - flags |= STATUS_OUTPUT_PERMITTED; - } - flags - } - - fn current_time(&self) -> u64 { - self.simulation_time.unwrap_or(0) - } - - fn send_goal_status( - &self, - socket: &C, - status: GoalStatusPacket, - ) -> Result<(), String> { - let Some(client) = self.goal_client else { - return Ok(()); - }; - self.send_goal_status_to(socket, client, status) - } - - fn send_goal_status_to( - &self, - socket: &C, - client: SocketAddr, - status: GoalStatusPacket, - ) -> Result<(), String> { - socket.send_datagram(&protocol::encode_goal_status(status), client) - } - - fn reset_for_simulator_epoch(&mut self) { - self.initializer = StationaryInitializer::new(); - self.estimator = None; - self.abort_goal(); - self.safety.reset_time_epoch(); - self.simulation_time = None; - self.last_imu_time = None; - self.last_imu_sequence = None; - self.last_local_state_time = None; - self.last_local_state_sequence = None; - self.last_command_time = None; - self.pending_control_step = None; - self.last_rate = BodyRate::radians_per_second(0.0, 0.0, 0.0); - self.controller.reset(); - } - - fn goal_is_settled(&self) -> bool { - let (Some(estimator), Some(trajectory)) = (self.estimator, self.trajectory.as_ref()) else { - return false; - }; - if !trajectory.is_complete(0.05, 0.05) { - return false; - } - let state = estimator.state(); - let error = trajectory.goal().sub(state.position().vector()); - error.norm_squared() <= 0.25 * 0.25 - && state.velocity().vector().norm_squared() <= 0.25 * 0.25 - } -} - -struct QuadController { - translational: TranslationalController, - thrust: ThrustEnvelope, - rate: RatePid, - allocator: Allocator<4>, -} - -impl QuadController { - fn new(config: VehicleProfile) -> Result { - let control = config.control(); - let thrust_config = config.thrust(); - let rate_pid = config.rate_pid(); - let actuator_limits = config.actuator_limits(); - let translational = TranslationalController::new(TranslationalControlConfig { - position_gain_per_second_squared: control.position_gain_per_second_squared(), - velocity_gain_per_second: control.velocity_gain_per_second(), - maximum_horizontal_acceleration_mps2: control.maximum_horizontal_acceleration_mps2(), - maximum_upward_acceleration_mps2: control.maximum_upward_acceleration_mps2(), - maximum_downward_acceleration_mps2: control.maximum_downward_acceleration_mps2(), - }) - .map_err(|error| format!("invalid translational controller: {error:?}"))?; - let thrust = ThrustEnvelope { - mass_kg: thrust_config.mass_kg(), - maximum_collective_thrust_newtons: thrust_config.maximum_collective_thrust_newtons(), - maximum_tilt_tangent: thrust_config.maximum_tilt_tangent(), - }; - thrust - .validate() - .map_err(|error| format!("invalid thrust envelope: {error:?}"))?; - let rate_config = PidConfig { - proportional: rate_pid.proportional(), - integral: rate_pid.integral(), - derivative: rate_pid.derivative(), - output_limit: rate_pid.output_limit(), - integral_limit: rate_pid.integral_limit(), - }; - let limits = ActuatorLimits { - minimum: actuator_limits.minimum(), - maximum: actuator_limits.maximum(), - slew_per_second: actuator_limits.slew_per_second(), - }; - let mixer = PrecomputedMixer::new([ - MixerOutput { - coefficients: config.mixer()[0], - limits, - }, - MixerOutput { - coefficients: config.mixer()[1], - limits, - }, - MixerOutput { - coefficients: config.mixer()[2], - limits, - }, - MixerOutput { - coefficients: config.mixer()[3], - limits, - }, - ]); - let allocator = - Allocator::new(mixer).map_err(|error| format!("invalid mixer: {error:?}"))?; - Ok(Self { - translational, - thrust, - rate: RatePid::new(rate_config, rate_config, rate_config), - allocator, - }) - } - - fn update( - &mut self, - estimator: InertialEskf, - reference: kaptan_control::KinematicReference, - measured_rate: BodyRate, - dt_seconds: f32, - ) -> Result<[f32; 4], String> { - let state = estimator.state(); - let estimate = TranslationalEstimate { - position: state.position().vector(), - velocity: state.velocity().vector(), - }; - let acceleration = self - .translational - .update(reference, estimate) - .map_err(|error| format!("translational control rejected estimate: {error:?}"))?; - let (force, force_limited) = self - .thrust - .force_for_acceleration(acceleration, 9.806_65) - .map_err(|error| format!("thrust envelope rejected command: {error:?}"))?; - let heading_vector = state - .attitude() - .body_to_ned(Vector3::::new(1.0, 0.0, 0.0)); - let heading = HeadingNed::new(heading_vector.x(), heading_vector.y()) - .ok_or_else(|| "estimator produced a vertical/invalid heading".to_owned())?; - let target_attitude = force_to_attitude(force, heading) - .map_err(|error| format!("force-to-attitude failed: {error:?}"))?; - let error = state - .attitude() - .inverse() - .compose(target_attitude) - .map_err(|error| format!("attitude composition failed: {error:?}"))?; - let direction = if error.w() < 0.0 { -1.0 } else { 1.0 }; - let target_rate = BodyRate::radians_per_second( - direction * 2.5 * error.x(), - direction * 2.5 * error.y(), - direction * 1.0 * error.z(), - ); - let torque = self - .rate - .update(target_rate, measured_rate, dt_seconds) - .map_err(|error| format!("rate controller failed: {error:?}"))?; - let collective = force.vector().norm() / self.thrust.maximum_collective_thrust_newtons; - let allocation = self - .allocator - .allocate( - BodyWrench { - roll: torque.x(), - pitch: torque.y(), - yaw: torque.z(), - collective, - }, - dt_seconds, - ) - .map_err(|error| format!("mixer failed: {error:?}"))?; - if force_limited || allocation.saturated { - self.rate.reset_integrators(); - } - Ok(allocation.commands) - } - - fn reset(&mut self) { - self.rate.reset(); - self.allocator.reset_commands(); - } -} - -fn imu_observation(packet: protocol::ImuPacket) -> Result { - let Some(gyro_rate) = ScalarVariance::new(2.5e-5) else { - return Err("invalid static gyro variance".to_owned()); - }; - let Some(acceleration) = ScalarVariance::new(2.5e-3) else { - return Err("invalid static acceleration variance".to_owned()); - }; - Ok(ImuObservation { - context: ObservationContext::new( - Timestamp::from_microseconds(packet.time_microseconds), - Timestamp::from_microseconds(packet.time_microseconds), - SOURCE_IMU, - FrameId::BODY_FRD, - CALIBRATION, - u8::MAX, - Validity::Valid, - ObservationSequence::new(packet.sequence), - ), - acceleration: AccelerationFrd::metres_per_second_squared( - packet.acceleration_frd_mps2[0], - packet.acceleration_frd_mps2[1], - packet.acceleration_frd_mps2[2], - ), - angular_rate: AngularRateFrd::radians_per_second( - packet.angular_rate_frd_rad_s[0], - packet.angular_rate_frd_rad_s[1], - packet.angular_rate_frd_rad_s[2], - ), - noise: ImuNoise { - gyro_rate, - acceleration, - }, - }) -} - -fn estimator_config(maximum_imu_interval_microseconds: u64) -> EstimatorConfig { - EstimatorConfig { - maximum_imu_interval_microseconds, - ..EstimatorConfig::default() - } -} - -fn control_step_seconds(previous: Option, current: u64, maximum_interval: u64) -> Option { - let elapsed = current.checked_sub(previous?)?; - if elapsed == 0 || elapsed > maximum_interval { - return None; - } - Some(elapsed as f32 / 1_000_000.0) -} - -fn flight_limits() -> KinematicLimits { - KinematicLimits { - maximum_horizontal_speed_mps: 3.0, - maximum_vertical_speed_mps: 1.5, - maximum_horizontal_acceleration_mps2: 2.0, - maximum_vertical_acceleration_mps2: 1.5, - } -} - -fn fresh(measurement_time: Option, now: u64, limit_microseconds: u64) -> bool { - measurement_time - .and_then(|measurement_time| now.checked_sub(measurement_time)) - .is_some_and(|age| age <= limit_microseconds) -} - -fn strictly_after( - time: u64, - sequence: u32, - previous_time: Option, - previous_sequence: Option, -) -> bool { - match (previous_time, previous_sequence) { - (Some(previous_time), Some(previous_sequence)) => { - time > previous_time - && sequence.wrapping_sub(previous_sequence) != 0 - && sequence.wrapping_sub(previous_sequence) <= u32::MAX / 2 - } - _ => true, - } -} - -#[cfg(test)] -mod tests { - use super::*; - use core::cell::RefCell; - use kaptan_config::{ConfigModel, ConfigStore, ConfigValue, FieldId}; - - #[derive(Default)] - struct RecordingSink { - records: RefCell, SocketAddr)>>, - } - - impl RecordingSink { - fn records(&self) -> Vec<(Vec, SocketAddr)> { - self.records.borrow().clone() - } - } - - impl DatagramSink for RecordingSink { - fn send_datagram(&self, bytes: &[u8], destination: SocketAddr) -> Result<(), String> { - self.records - .borrow_mut() - .push((bytes.to_vec(), destination)); - Ok(()) - } - } - - fn initialized_estimator() -> InertialEskf { - let initializer = StationaryInitializer::<4>::new(); - let imu = imu_observation(protocol::ImuPacket { - time_microseconds: 10, - sequence: 1, - acceleration_frd_mps2: [0.0, 0.0, -9.806_65], - angular_rate_frd_rad_s: [0.0; 3], - }) - .unwrap(); - let mut initializer = initializer; - for sequence in 1..=4 { - initializer.push(ImuObservation { - context: ObservationContext::new( - Timestamp::from_microseconds(sequence * 10_000), - Timestamp::from_microseconds(sequence * 10_000), - SOURCE_IMU, - FrameId::BODY_FRD, - CALIBRATION, - u8::MAX, - Validity::Valid, - ObservationSequence::new(sequence as u32), - ), - ..imu - }); - } - initializer - .finish( - Timestamp::from_microseconds(40_000), - estimator_config( - default_profile() - .unwrap() - .timing() - .maximum_imu_interval_microseconds(), - ), - IntegrityConfig::default(), - 0.0, - ) - .unwrap() - } - - #[test] - fn sequence_order() { - assert!(strictly_after(2, 0, Some(1), Some(u32::MAX))); - assert!(!strictly_after(2, 7, Some(2), Some(6))); - assert!(!strictly_after(1, 8, Some(2), Some(7))); - } - - #[test] - fn serve_configuration_retains_the_persisted_profile_that_builds_consumers() { - let path = std::env::temp_dir().join(format!( - "kaptan-sitl-serve-config-{}-{}.cbor", - std::process::id(), - std::thread::current().name().unwrap_or("test") - )); - let _ = std::fs::remove_file(&path); - let mut candidate = kaptan_flight_config::CandidateProfile::factory(); - candidate - .set_field(FieldId::new(13), ConfigValue::F32(1.7)) - .unwrap(); - FileConfigStore::new(&path).persist(&candidate).unwrap(); - - let mut arguments = - vec!["--config".to_owned(), path.to_string_lossy().into_owned()].into_iter(); - let configuration = serve_configuration(&mut arguments).unwrap(); - assert_eq!(configuration.active_profile().thrust().mass_kg(), 1.7); - let runtime = SitlRuntime::from_configuration(configuration).unwrap(); - assert_eq!(runtime.profile.thrust().mass_kg(), 1.7); - assert_eq!(runtime.controller.thrust.mass_kg, 1.7); - let _ = std::fs::remove_file(&path); - } - - #[test] - fn hover_output() { - let mut controller = QuadController::new(default_profile().unwrap()).unwrap(); - let filter = initialized_estimator(); - let outputs = controller - .update( - filter, - kaptan_control::KinematicReference { - position: Vector3::zero(), - velocity: Vector3::zero(), - acceleration: Vector3::zero(), - }, - BodyRate::radians_per_second(0.0, 0.0, 0.0), - 0.01, - ) - .unwrap(); - assert!( - outputs - .iter() - .all(|output| *output >= 0.0 && *output <= 1.0) - ); - } - - #[test] - fn settled_landing_reports_landed_then_emits_inhibited_zero_output() { - let now = default_profile() - .unwrap() - .safety() - .prearm_dwell_microseconds(); - let mut runtime = SitlRuntime::new().unwrap(); - runtime.estimator = Some(initialized_estimator()); - runtime.accept_local_state(protocol::LocalStatePacket { - time_microseconds: 40_000, - sequence: 1, - position_ned_metres: [0.0; 3], - velocity_ned_mps: [0.0; 3], - }); - runtime.simulation_time = Some(now); - runtime.last_imu_time = Some(now); - runtime.last_local_state_time = Some(now); - runtime.last_command_time = Some(now); - runtime.active_goal = Some(GoalPacket { - goal_id: 17, - north_metres: 0.0, - east_metres: 0.0, - up_metres: 0.0, - }); - runtime.landing_goal = true; - runtime.goal_client = Some(SocketAddr::from(([127, 0, 0, 1], 15_001))); - runtime.trajectory = Some(Trajectory::new(Vector3::zero(), flight_limits()).unwrap()); - let healthy = runtime.safety_inputs(now); - runtime - .safety - .request_arm(Timestamp::from_microseconds(0), healthy); - assert_eq!( - runtime - .safety - .evaluate(Timestamp::from_microseconds(now), healthy) - .state(), - kaptan_safety::SafetyState::Armed - ); - - let datagrams = RecordingSink::default(); - runtime - .control(now, Some(0.01), &datagrams, &datagrams) - .unwrap(); - - let records = datagrams.records(); - assert_eq!(records.len(), 3); - assert_eq!( - records[0].1, - SocketAddr::from(([127, 0, 0, 1], ACTUATOR_PORT)) - ); - assert_eq!(&records[0].0[..4], &protocol::OUTPUT_MAGIC); - assert_eq!(records[0].0[20], 1); - assert_eq!(records[1].1, SocketAddr::from(([127, 0, 0, 1], 15_001))); - assert_eq!(&records[1].0[..4], &protocol::GOAL_STATUS_MAGIC); - assert_eq!( - u32::from_le_bytes(records[1].0[8..12].try_into().unwrap()), - 17 - ); - assert_eq!(records[1].0[12], GoalLifecycle::Landed as u8); - assert_eq!( - records[2].1, - SocketAddr::from(([127, 0, 0, 1], ACTUATOR_PORT)) - ); - assert_eq!(&records[2].0[..4], &protocol::OUTPUT_MAGIC); - assert_eq!(records[2].0[20], 0); - assert_eq!(&records[2].0[24..], &[0_u8; 16]); - assert_eq!(runtime.safety.state(), kaptan_safety::SafetyState::Disarmed); - assert!(runtime.active_goal.is_none()); - assert!(runtime.trajectory.is_none()); - } - - #[test] - fn control_step_tracks_imu_time() { - let maximum_interval = default_profile() - .unwrap() - .timing() - .maximum_imu_interval_microseconds(); - assert!(control_step_seconds(None, 10_000, maximum_interval).is_none()); - assert!(control_step_seconds(Some(10_000), 10_000, maximum_interval).is_none()); - assert!(control_step_seconds(Some(10_000), 9_999, maximum_interval).is_none()); - assert!(control_step_seconds(Some(10_000), 60_001, maximum_interval).is_none()); - assert!( - (control_step_seconds(Some(10_000), 20_000, maximum_interval).unwrap() - 0.01).abs() - < f32::EPSILON - ); - assert!( - (control_step_seconds(Some(10_000), 60_000, maximum_interval).unwrap() - 0.05).abs() - < f32::EPSILON - ); - } - - #[test] - fn lease_expiry_clears_authority() { - let mut runtime = SitlRuntime::new().unwrap(); - runtime.simulation_time = Some(1_000_000); - let client = SocketAddr::from(([127, 0, 0, 1], 15_001)); - runtime.active_goal = Some(GoalPacket { - goal_id: 7, - north_metres: 1.0, - east_metres: 2.0, - up_metres: 3.0, - }); - runtime.goal_client = Some(client); - runtime.goal_settled = true; - runtime.trajectory = Some(Trajectory::new(Vector3::zero(), flight_limits()).unwrap()); - runtime.last_command_time = Some(1_000_000); - runtime.safety.request_arm( - Timestamp::from_microseconds(0), - SafetyInputs { - estimator_healthy: true, - imu_fresh: true, - link_fresh: true, - actuator_healthy: true, - }, - ); - let healthy = SafetyInputs { - estimator_healthy: true, - imu_fresh: true, - link_fresh: true, - actuator_healthy: true, - }; - runtime.safety.evaluate( - Timestamp::from_microseconds( - default_profile() - .unwrap() - .safety() - .prearm_dwell_microseconds(), - ), - healthy, - ); - - runtime.expire_stale_goal( - 1_000_000 - + default_profile() - .unwrap() - .timing() - .command_link_freshness_microseconds(), - ); - assert!(runtime.active_goal.is_some()); - - runtime.expire_stale_goal( - 1_000_001 - + default_profile() - .unwrap() - .timing() - .command_link_freshness_microseconds(), - ); - - assert!(runtime.active_goal.is_none()); - assert!(runtime.goal_client.is_none()); - assert!(!runtime.goal_settled); - assert!(runtime.trajectory.is_none()); - assert!(runtime.last_command_time.is_none()); - assert_eq!(runtime.safety.state(), kaptan_safety::SafetyState::Disarmed); - runtime.renew_goal(GoalKeepalivePacket { goal_id: 7 }, client); - assert!(runtime.last_command_time.is_none()); - } - - #[test] - fn freshness_uses_only_simulation_timestamps() { - let freshness = default_profile() - .unwrap() - .timing() - .imu_freshness_microseconds(); - assert!(fresh(Some(10_000), 60_000, freshness)); - assert!(!fresh(Some(10_000), 60_001, freshness)); - assert!(!fresh(Some(60_001), 60_000, freshness)); - assert!(!fresh(None, 60_000, freshness)); - } - - #[test] - fn clock_regression_resets_the_simulator_epoch() { - let mut runtime = SitlRuntime::new().unwrap(); - runtime.simulation_time = Some(20_000); - runtime.last_imu_time = Some(20_000); - runtime.last_local_state_time = Some(20_000); - runtime.last_command_time = Some(20_000); - runtime.pending_control_step = Some((20_000, Some(0.01))); - runtime.active_goal = Some(GoalPacket { - goal_id: 7, - north_metres: 0.0, - east_metres: 0.0, - up_metres: 1.0, - }); - assert_eq!( - runtime.advance_simulation_clock(10_000), - ClockWork::Supervise - ); - - assert_eq!(runtime.simulation_time, Some(10_000)); - assert!(runtime.estimator.is_none()); - assert!(runtime.active_goal.is_none()); - assert!(runtime.last_imu_time.is_none()); - assert!(runtime.last_local_state_time.is_none()); - assert!(runtime.last_command_time.is_none()); - assert!(runtime.pending_control_step.is_none()); - assert_eq!(runtime.safety.state(), kaptan_safety::SafetyState::Disarmed); - } - - #[test] - fn only_the_matching_clock_tick_releases_an_imu_control_step() { - let mut runtime = SitlRuntime::new().unwrap(); - runtime.pending_control_step = Some((20_000, Some(0.01))); - - assert_eq!( - runtime.advance_simulation_clock(17_500), - ClockWork::Supervise - ); - assert_eq!(runtime.pending_control_step, Some((20_000, Some(0.01)))); - assert_eq!( - runtime.advance_simulation_clock(20_000), - ClockWork::Control(Some(0.01)) - ); - assert!(runtime.pending_control_step.is_none()); - } - - #[test] - fn simulator_epoch_reset_accepts_a_new_safety_clock_epoch() { - let mut runtime = SitlRuntime::new().unwrap(); - let healthy = SafetyInputs { - estimator_healthy: true, - imu_fresh: true, - link_fresh: true, - actuator_healthy: true, - }; - runtime - .safety - .request_arm(Timestamp::from_microseconds(1_000), healthy); - assert_eq!( - runtime - .safety - .evaluate( - Timestamp::from_microseconds( - 1_000 - + default_profile() - .unwrap() - .safety() - .prearm_dwell_microseconds(), - ), - healthy, - ) - .state(), - kaptan_safety::SafetyState::Armed - ); - - runtime.reset_for_simulator_epoch(); - - assert_eq!(runtime.safety.state(), kaptan_safety::SafetyState::Disarmed); - assert_eq!( - runtime - .safety - .request_arm(Timestamp::from_microseconds(1), healthy) - .state(), - kaptan_safety::SafetyState::PreArm - ); - } -} diff --git a/firmware/tools/sitl/src/protocol.rs b/firmware/tools/sitl/src/protocol.rs deleted file mode 100644 index 69f4088..0000000 --- a/firmware/tools/sitl/src/protocol.rs +++ /dev/null @@ -1,363 +0,0 @@ -//! Fixed-size local UDP records for Kaptan's host-only simulator harness. -//! -//! This transport is deliberately not MAVLink and not part of Kaptan's flight -//! dialect. It is confined to one host network namespace and is versioned so -//! malformed simulator traffic never becomes estimator or motor authority. - -use kaptan_err_reportable::KaptanError; -use std::net::SocketAddr; - -pub const SENSOR_PORT: u16 = 14_660; -pub const ACTUATOR_PORT: u16 = 14_661; -pub const CONTROL_PORT: u16 = 14_662; -pub const VERSION: u8 = 1; -pub const IMU_MAGIC: [u8; 4] = *b"KSI1"; -pub const LOCAL_STATE_MAGIC: [u8; 4] = *b"KSV1"; -/// Gazebo simulation time, sent once for every unpaused simulation step. -/// This is the sole time base for host-SITL freshness and command leases. -pub const CLOCK_MAGIC: [u8; 4] = *b"KSC1"; -pub const OUTPUT_MAGIC: [u8; 4] = *b"KSO1"; -pub const GOAL_MAGIC: [u8; 4] = *b"KSG1"; -/// A host-SITL-only request to descend to a declared local landing point and -/// revoke motor authority once the landing point has settled. -pub const LAND_MAGIC: [u8; 4] = *b"KSD1"; -pub const GOAL_KEEPALIVE_MAGIC: [u8; 4] = *b"KSK1"; -pub const STATUS_QUERY_MAGIC: [u8; 4] = *b"KSQ1"; -pub const STATUS_MAGIC: [u8; 4] = *b"KSS1"; -pub const GOAL_STATUS_MAGIC: [u8; 4] = *b"KSL1"; - -pub const SENSOR_PACKET_SIZE: usize = 44; -pub const CLOCK_PACKET_SIZE: usize = 16; -pub const OUTPUT_PACKET_SIZE: usize = 40; -pub const GOAL_PACKET_SIZE: usize = 24; -pub const GOAL_KEEPALIVE_PACKET_SIZE: usize = 12; -pub const STATUS_QUERY_PACKET_SIZE: usize = 8; -pub const STATUS_PACKET_SIZE: usize = 20; -pub const GOAL_STATUS_PACKET_SIZE: usize = 16; - -pub const STATUS_ESTIMATOR_READY: u16 = 1 << 0; -pub const STATUS_IMU_FRESH: u16 = 1 << 1; -pub const STATUS_LOCAL_AIDING_FRESH: u16 = 1 << 2; -pub const STATUS_OUTPUT_PERMITTED: u16 = 1 << 3; -pub const STATUS_ACTUATOR_TRANSPORT: u16 = 1 << 4; -#[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)] -pub enum PacketError { - #[kaptan_error(id = 12001)] - Length, - #[kaptan_error(id = 12002)] - Version, - #[kaptan_error(id = 12003)] - NonFinite, -} - -#[derive(Clone, Copy, Debug, PartialEq)] -pub struct ImuPacket { - pub time_microseconds: u64, - pub sequence: u32, - pub acceleration_frd_mps2: [f32; 3], - pub angular_rate_frd_rad_s: [f32; 3], -} - -#[derive(Clone, Copy, Debug, PartialEq)] -pub struct LocalStatePacket { - pub time_microseconds: u64, - pub sequence: u32, - pub position_ned_metres: [f32; 3], - pub velocity_ned_mps: [f32; 3], -} - -#[derive(Clone, Copy, Debug, Eq, PartialEq)] -pub struct ClockPacket { - pub time_microseconds: u64, -} - -#[derive(Clone, Copy, Debug, PartialEq)] -pub struct GoalPacket { - pub goal_id: u32, - pub north_metres: f32, - pub east_metres: f32, - pub up_metres: f32, -} - -/// Renews the command lease for one accepted goal without modifying it. -#[derive(Clone, Copy, Debug, Eq, PartialEq)] -pub struct GoalKeepalivePacket { - pub goal_id: u32, -} - -#[derive(Clone, Copy, Debug, Eq, PartialEq)] -#[repr(u8)] -pub enum GoalLifecycle { - Rejected = 0, - Active = 1, - Settled = 2, - Landed = 3, -} - -#[derive(Clone, Copy, Debug, Eq, PartialEq)] -pub struct GoalStatusPacket { - pub goal_id: u32, - pub lifecycle: GoalLifecycle, -} - -pub fn decode_imu(bytes: &[u8]) -> Result { - let (time_microseconds, sequence, values) = decode_sensor(bytes, IMU_MAGIC)?; - Ok(ImuPacket { - time_microseconds, - sequence, - acceleration_frd_mps2: [values[0], values[1], values[2]], - angular_rate_frd_rad_s: [values[3], values[4], values[5]], - }) -} - -pub fn decode_local_state(bytes: &[u8]) -> Result { - let (time_microseconds, sequence, values) = decode_sensor(bytes, LOCAL_STATE_MAGIC)?; - Ok(LocalStatePacket { - time_microseconds, - sequence, - position_ned_metres: [values[0], values[1], values[2]], - velocity_ned_mps: [values[3], values[4], values[5]], - }) -} - -pub fn decode_clock(bytes: &[u8]) -> Result { - if bytes.len() != CLOCK_PACKET_SIZE { - return Err(PacketError::Length); - } - if bytes[0..4] != CLOCK_MAGIC || bytes[4] != VERSION || bytes[5..8] != [0; 3] { - return Err(PacketError::Version); - } - Ok(ClockPacket { - time_microseconds: read_u64(&bytes[8..16]).ok_or(PacketError::Length)?, - }) -} - -pub fn decode_goal(bytes: &[u8]) -> Result { - decode_goal_with_magic(bytes, GOAL_MAGIC, false) -} - -/// Decodes a host-SITL landing request. Landing always targets the declared -/// local ground plane (`up == 0`), rather than silently treating an arbitrary -/// move goal as a landing operation. -pub fn decode_landing(bytes: &[u8]) -> Result { - decode_goal_with_magic(bytes, LAND_MAGIC, true) -} - -fn decode_goal_with_magic( - bytes: &[u8], - magic: [u8; 4], - require_ground_plane: bool, -) -> Result { - if bytes.len() != GOAL_PACKET_SIZE { - return Err(PacketError::Length); - } - if bytes[0..4] != magic || bytes[4] != VERSION || bytes[5..8] != [0; 3] { - return Err(PacketError::Version); - } - let goal = GoalPacket { - goal_id: read_u32(&bytes[8..12]).ok_or(PacketError::Length)?, - north_metres: read_f32(&bytes[12..16]).ok_or(PacketError::Length)?, - east_metres: read_f32(&bytes[16..20]).ok_or(PacketError::Length)?, - up_metres: read_f32(&bytes[20..24]).ok_or(PacketError::Length)?, - }; - if goal.goal_id == 0 - || !goal.north_metres.is_finite() - || !goal.east_metres.is_finite() - || !goal.up_metres.is_finite() - || (require_ground_plane && goal.up_metres != 0.0) - { - return Err(PacketError::NonFinite); - } - Ok(goal) -} - -pub fn decode_goal_keepalive(bytes: &[u8]) -> Result { - if bytes.len() != GOAL_KEEPALIVE_PACKET_SIZE { - return Err(PacketError::Length); - } - if bytes[0..4] != GOAL_KEEPALIVE_MAGIC || bytes[4] != VERSION || bytes[5..8] != [0; 3] { - return Err(PacketError::Version); - } - let goal_id = read_u32(&bytes[8..12]).ok_or(PacketError::Length)?; - if goal_id == 0 { - return Err(PacketError::NonFinite); - } - Ok(GoalKeepalivePacket { goal_id }) -} - -pub fn is_status_query(bytes: &[u8]) -> bool { - bytes.len() == STATUS_QUERY_PACKET_SIZE - && bytes[0..4] == STATUS_QUERY_MAGIC - && bytes[4] == VERSION - && bytes[5..8] == [0; 3] -} - -pub fn encode_output( - time_microseconds: u64, - sequence: u32, - permitted: bool, - motors: [f32; 4], -) -> Result<[u8; OUTPUT_PACKET_SIZE], PacketError> { - if motors - .iter() - .any(|motor| !motor.is_finite() || !(0.0..=1.0).contains(motor)) - || (!permitted && motors.iter().any(|motor| *motor != 0.0)) - { - return Err(PacketError::NonFinite); - } - let mut bytes = [0_u8; OUTPUT_PACKET_SIZE]; - bytes[0..4].copy_from_slice(&OUTPUT_MAGIC); - bytes[4] = VERSION; - bytes[8..16].copy_from_slice(&time_microseconds.to_le_bytes()); - bytes[16..20].copy_from_slice(&sequence.to_le_bytes()); - bytes[20] = u8::from(permitted); - for (index, motor) in motors.iter().enumerate() { - bytes[24 + index * 4..28 + index * 4].copy_from_slice(&motor.to_le_bytes()); - } - Ok(bytes) -} - -pub fn encode_status(time_microseconds: u64, flags: u16) -> [u8; STATUS_PACKET_SIZE] { - let mut bytes = [0_u8; STATUS_PACKET_SIZE]; - bytes[0..4].copy_from_slice(&STATUS_MAGIC); - bytes[4] = VERSION; - bytes[8..16].copy_from_slice(&time_microseconds.to_le_bytes()); - bytes[16..18].copy_from_slice(&flags.to_le_bytes()); - bytes -} - -pub fn encode_goal_status(status: GoalStatusPacket) -> [u8; GOAL_STATUS_PACKET_SIZE] { - let mut bytes = [0_u8; GOAL_STATUS_PACKET_SIZE]; - bytes[0..4].copy_from_slice(&GOAL_STATUS_MAGIC); - bytes[4] = VERSION; - bytes[8..12].copy_from_slice(&status.goal_id.to_le_bytes()); - bytes[12] = status.lifecycle as u8; - bytes -} - -pub fn localhost(sender: SocketAddr) -> bool { - sender.ip().is_loopback() -} - -fn decode_sensor(bytes: &[u8], magic: [u8; 4]) -> Result<(u64, u32, [f32; 6]), PacketError> { - if bytes.len() != SENSOR_PACKET_SIZE { - return Err(PacketError::Length); - } - if bytes[0..4] != magic || bytes[4] != VERSION || bytes[5..8] != [0; 3] { - return Err(PacketError::Version); - } - let mut values = [0.0; 6]; - for (index, value) in values.iter_mut().enumerate() { - *value = read_f32(&bytes[20 + index * 4..24 + index * 4]).ok_or(PacketError::Length)?; - } - if values.iter().any(|value| !value.is_finite()) { - return Err(PacketError::NonFinite); - } - Ok(( - read_u64(&bytes[8..16]).ok_or(PacketError::Length)?, - read_u32(&bytes[16..20]).ok_or(PacketError::Length)?, - values, - )) -} - -fn read_u32(bytes: &[u8]) -> Option { - Some(u32::from_le_bytes(bytes.try_into().ok()?)) -} - -fn read_u64(bytes: &[u8]) -> Option { - Some(u64::from_le_bytes(bytes.try_into().ok()?)) -} - -fn read_f32(bytes: &[u8]) -> Option { - Some(f32::from_le_bytes(bytes.try_into().ok()?)) -} - -#[cfg(test)] -mod tests { - use super::*; - - fn sensor(magic: [u8; 4]) -> [u8; SENSOR_PACKET_SIZE] { - let mut bytes = [0_u8; SENSOR_PACKET_SIZE]; - bytes[..4].copy_from_slice(&magic); - bytes[4] = VERSION; - bytes[8..16].copy_from_slice(&123_u64.to_le_bytes()); - bytes[16..20].copy_from_slice(&9_u32.to_le_bytes()); - for index in 0..6 { - bytes[20 + index * 4..24 + index * 4] - .copy_from_slice(&(index as f32 + 0.5).to_le_bytes()); - } - bytes - } - - #[test] - fn sensor_records_are_finite() { - let packet = decode_imu(&sensor(IMU_MAGIC)).unwrap(); - assert_eq!(packet.time_microseconds, 123); - assert_eq!(packet.acceleration_frd_mps2, [0.5, 1.5, 2.5]); - assert_eq!(packet.angular_rate_frd_rad_s, [3.5, 4.5, 5.5]); - assert_eq!( - decode_imu(&sensor(LOCAL_STATE_MAGIC)), - Err(PacketError::Version) - ); - } - - #[test] - fn simulator_clock_has_no_host_receipt_time() { - let mut packet = [0_u8; CLOCK_PACKET_SIZE]; - packet[..4].copy_from_slice(&CLOCK_MAGIC); - packet[4] = VERSION; - packet[8..16].copy_from_slice(&987_654_u64.to_le_bytes()); - assert_eq!( - decode_clock(&packet), - Ok(ClockPacket { - time_microseconds: 987_654, - }) - ); - packet[4] = VERSION + 1; - assert_eq!(decode_clock(&packet), Err(PacketError::Version)); - } - - #[test] - fn inhibited_output() { - assert!(encode_output(1, 2, true, [0.1, 0.2, 0.3, 0.4]).is_ok()); - assert_eq!( - encode_output(1, 2, false, [0.1, 0.0, 0.0, 0.0]), - Err(PacketError::NonFinite) - ); - } - - #[test] - fn goal_keepalive_validation() { - let mut packet = [0_u8; GOAL_KEEPALIVE_PACKET_SIZE]; - packet[..4].copy_from_slice(&GOAL_KEEPALIVE_MAGIC); - packet[4] = VERSION; - packet[8..12].copy_from_slice(&7_u32.to_le_bytes()); - assert_eq!( - decode_goal_keepalive(&packet), - Ok(GoalKeepalivePacket { goal_id: 7 }) - ); - packet[8..12].copy_from_slice(&0_u32.to_le_bytes()); - assert_eq!(decode_goal_keepalive(&packet), Err(PacketError::NonFinite)); - } - - #[test] - fn landing_is_an_explicit_ground_plane_command() { - let mut packet = [0_u8; GOAL_PACKET_SIZE]; - packet[..4].copy_from_slice(&LAND_MAGIC); - packet[4] = VERSION; - packet[8..12].copy_from_slice(&7_u32.to_le_bytes()); - packet[12..16].copy_from_slice(&1.0_f32.to_le_bytes()); - packet[16..20].copy_from_slice(&(-2.0_f32).to_le_bytes()); - assert_eq!( - decode_landing(&packet), - Ok(GoalPacket { - goal_id: 7, - north_metres: 1.0, - east_metres: -2.0, - up_metres: 0.0, - }) - ); - packet[20..24].copy_from_slice(&0.1_f32.to_le_bytes()); - assert_eq!(decode_landing(&packet), Err(PacketError::NonFinite)); - } -} diff --git a/firmware/tools/sitl/test_kaptan_sim.py b/firmware/tools/sitl/test_kaptan_sim.py deleted file mode 100644 index c7d29f1..0000000 --- a/firmware/tools/sitl/test_kaptan_sim.py +++ /dev/null @@ -1,107 +0,0 @@ -"""Regression tests for host-SITL command-path truth assertions.""" - -import importlib.machinery -import importlib.util -from pathlib import Path -import unittest -from unittest import mock - - -def load_client_module(): - path = Path(__file__).with_name("kaptan-sim") - loader = importlib.machinery.SourceFileLoader("kaptan_sim", str(path)) - spec = importlib.util.spec_from_loader(loader.name, loader) - assert spec is not None - module = importlib.util.module_from_spec(spec) - loader.exec_module(module) - return module - - -class TruthAcceptanceTests(unittest.TestCase): - @classmethod - def setUpClass(cls): - cls.client = load_client_module() - - def acceptable(self, *, motors_stopped=True, ground_plane_error=0.0, landing=True): - return self.client.truth_sample_within_tolerance( - position_error=0.01, - velocity=0.01, - attitude_error=0.01, - motors_stopped=motors_stopped, - ground_plane_error=ground_plane_error, - position_tolerance=0.75, - velocity_tolerance=0.25, - attitude_tolerance=0.35, - require_stopped_motors=landing, - require_ground_plane=landing, - ) - - def test_move_truth_contract_allows_an_armed_hover(self): - self.assertTrue( - self.acceptable(motors_stopped=False, ground_plane_error=1.0, landing=False) - ) - - def test_landing_truth_contract_requires_zero_motors_and_ground_plane(self): - self.assertFalse(self.acceptable(motors_stopped=False)) - self.assertFalse( - self.acceptable( - ground_plane_error=self.client.GROUND_PLANE_TOLERANCE_METRES + 0.001 - ) - ) - self.assertTrue( - self.acceptable( - ground_plane_error=self.client.GROUND_PLANE_TOLERANCE_METRES - ) - ) - - def test_land_builds_the_terminal_ground_plane_command(self): - control_socket = object() - clock_socket = object() - with ( - mock.patch.object(self.client, "wait_ready") as wait_ready, - mock.patch.object(self.client, "command_goal") as command_goal, - ): - self.client.land( - control_socket, - clock_socket, - 1.0, - -2.0, - 60.0, - True, - True, - 0.75, - 0.25, - 0.35, - 0.75, - ) - - wait_ready.assert_called_once_with(control_socket, 60.0) - arguments = command_goal.call_args.args - self.assertEqual(arguments[0:2], (control_socket, clock_socket)) - magic, version, goal_id, north, east, up = self.client.GOAL.unpack(arguments[2]) - self.assertEqual( - (magic, version, north, east, up), (b"KSD1", 1, 1.0, -2.0, 0.0) - ) - self.assertEqual(arguments[3], goal_id) - self.assertEqual( - arguments[4:], - ( - 1.0, - -2.0, - 0.0, - 60.0, - True, - True, - 0.75, - 0.25, - 0.35, - 0.75, - self.client.GOAL_LANDED, - True, - "land", - ), - ) - - -if __name__ == "__main__": - unittest.main() diff --git a/firmware/tools/sitl/world-structure-test.py b/firmware/tools/sitl/world-structure-test.py deleted file mode 100644 index feef870..0000000 --- a/firmware/tools/sitl/world-structure-test.py +++ /dev/null @@ -1,157 +0,0 @@ -#!/usr/bin/env python3 -"""Structural regression checks for Kaptan's deterministic Gazebo SITL world. - -This is deliberately an SDF/model-contract check, not a dynamics substitute. -It protects the reviewed boundary that the map is visual-only while the flat -ground and four landing feet provide the sole touchdown geometry. -""" - -from __future__ import annotations - -from pathlib import Path -import sys -import xml.etree.ElementTree as xml - - -WORLD = Path(__file__).with_name("worlds") / "kaptan-sitl-quad.sdf.in" - - -def fail(message: str) -> None: - raise AssertionError(message) - - -def model(root: xml.Element, name: str) -> xml.Element: - found = root.find(f"./world/model[@name='{name}']") - if found is None: - fail(f"missing model {name!r}") - return found - - -def named(parent: xml.Element, tag: str, name: str) -> xml.Element: - found = parent.find(f"./{tag}[@name='{name}']") - if found is None: - fail(f"missing {tag} {name!r}") - return found - - -def pose(element: xml.Element) -> tuple[float, ...]: - text = element.findtext("pose") - if text is None: - fail(f"{element.tag} {element.get('name')!r} has no pose") - values = tuple(float(value) for value in text.split()) - if len(values) != 6: - fail(f"{element.tag} {element.get('name')!r} pose does not have six fields") - return values - - -def cylinder(element: xml.Element) -> tuple[float, float]: - radius = element.findtext("geometry/cylinder/radius") - length = element.findtext("geometry/cylinder/length") - if radius is None or length is None: - fail(f"{element.tag} {element.get('name')!r} is not a cylinder") - return float(radius), float(length) - - -def vector(element: xml.Element, path: str, size: int) -> tuple[float, ...]: - text = element.findtext(path) - if text is None: - fail(f"{element.tag} {element.get('name')!r} has no {path!r}") - values = tuple(float(value) for value in text.split()) - if len(values) != size: - fail(f"{element.tag} {element.get('name')!r} {path!r} has {len(values)} fields") - return values - - -def check_world() -> None: - root = xml.parse(WORLD).getroot() - world = root.find("./world") - if world is None: - fail("missing world") - model_names = { - model_element.get("name") for model_element in world.findall("model") - } - if model_names != {"ground", "sitl_reference_map", "kaptan_sitl_quad"}: - fail( - "the declared flat-world model may contain only ground, the visual reference map, " - f"and the quad; found {sorted(model_names)!r}" - ) - - ground = model(root, "ground") - ground_collisions = ground.findall(".//collision") - if [collision.get("name") for collision in ground_collisions] != [ - "ground_collision" - ]: - fail("ground must retain exactly one flat collision plane") - if ground_collisions[0].find("geometry/plane") is None: - fail("ground collision must remain a flat plane") - if vector(ground_collisions[0], "geometry/plane/normal", 3) != (0.0, 0.0, 1.0): - fail("ground plane must remain horizontal with world +Z normal") - if vector(ground_collisions[0], "geometry/plane/size", 2) != (100.0, 100.0): - fail("ground plane must retain its declared 100 m square contact area") - - reference_map = model(root, "sitl_reference_map") - if reference_map.findall(".//collision"): - fail("reference map must remain visual-only and cannot add obstacle physics") - if reference_map.findall(".//transparency"): - fail( - "reference map must not use unsupported SDF transparency material children" - ) - reference_link = named(reference_map, "link", "reference_link") - actual_marks = {visual.get("name") for visual in reference_link.findall("visual")} - if actual_marks != {"landing_pad"}: - fail( - f"reference map may contain only the landing pad; found {sorted(actual_marks)!r}" - ) - - quad = model(root, "kaptan_sitl_quad") - if pose(quad)[:3] != (0.0, 0.0, 0.14): - fail("quad base_link must start in its reviewed landing-foot rest pose") - base_link = named(quad, "link", "base_link") - body_collision = named(base_link, "collision", "body_collision") - if vector(body_collision, "geometry/box/size", 3) != (0.30, 0.30, 0.08): - fail("quad body collision must retain its reviewed clearance geometry") - foot_positions = { - "front_right": (0.12, -0.12), - "rear_right": (-0.12, -0.12), - "rear_left": (-0.12, 0.12), - "front_left": (0.12, 0.12), - } - for foot, expected_xy in foot_positions.items(): - collision = named(base_link, "collision", f"landing_foot_{foot}_collision") - visual = named(base_link, "visual", f"landing_foot_{foot}_visual") - if pose(collision)[:3] != (*expected_xy, -0.10): - fail(f"landing foot {foot!r} has an unexpected contact pose") - if pose(visual) != pose(collision): - fail(f"landing foot {foot!r} visual must match its collision geometry") - if cylinder(collision) != (0.022, 0.08) or cylinder(visual) != (0.022, 0.08): - fail(f"landing foot {foot!r} must retain its reviewed cylinder dimensions") - - expected_quad_collisions = { - "body_collision", - *(f"landing_foot_{foot}_collision" for foot in foot_positions), - } - actual_quad_collisions = { - collision.get("name") for collision in base_link.findall("collision") - } - if actual_quad_collisions != expected_quad_collisions: - fail( - "quad collision geometry must remain its body and four declared landing feet; " - f"found {sorted(actual_quad_collisions)!r}" - ) - - sensors = quad.findall(".//sensor") - if [(sensor.get("name"), sensor.get("type")) for sensor in sensors] != [ - ("imu", "imu") - ]: - fail( - "this local-state SITL world must not imply unmodeled visual/range/flow sensors" - ) - - -if __name__ == "__main__": - try: - check_world() - except (AssertionError, OSError, xml.ParseError, ValueError) as error: - print(f"SITL world structure check failed: {error}", file=sys.stderr) - sys.exit(1) - print("SITL world structure check passed") diff --git a/firmware/tools/sitl/worlds/kaptan-sitl-quad.sdf.in b/firmware/tools/sitl/worlds/kaptan-sitl-quad.sdf.in deleted file mode 100644 index 83434e5..0000000 --- a/firmware/tools/sitl/worlds/kaptan-sitl-quad.sdf.in +++ /dev/null @@ -1,204 +0,0 @@ - - - - 0 0 -9.80665 - - 0.0025 - 1.0 - 400 - - - - - - - ogre2 - - - - 9002 - 30 - - - - true - 0 0 10 0 0 0 - 0.8 0.8 0.8 1 - 0.2 0.2 0.2 1 - -0.5 0.1 -0.9 - - - 0.35 0.38 0.42 1 - 0.08 0.11 0.16 1 - true - - - - - true - - - 0 0 0.002 0 0 0 - 1.250.004 - - 0.03 0.20 0.36 10.05 0.36 0.65 1 - 0.01 0.08 0.14 1 - - - - - - - true - - - 0 0 1100 100 - - - 0 0 1100 100 - 0.2 0.2 0.2 10.4 0.4 0.4 1 - - - - - - - - - 0 0 0.14 0 0 1.5707963268 - - - 1.2 - - 0.0210.0210.039 - 000 - - - - 0.30 0.30 0.08 - - - 0.30 0.30 0.08 - 0.05 0.1 0.2 10.1 0.2 0.5 1 - - - 0.12 -0.12 -0.10 0 0 0 - 0.0220.08 - - - 0.12 -0.12 -0.10 0 0 0 - 0.0220.08 - 0.07 0.07 0.08 10.12 0.12 0.14 1 - - - -0.12 -0.12 -0.10 0 0 0 - 0.0220.08 - - - -0.12 -0.12 -0.10 0 0 0 - 0.0220.08 - 0.07 0.07 0.08 10.12 0.12 0.14 1 - - - -0.12 0.12 -0.10 0 0 0 - 0.0220.08 - - - -0.12 0.12 -0.10 0 0 0 - 0.0220.08 - 0.07 0.07 0.08 10.12 0.12 0.14 1 - - - 0.12 0.12 -0.10 0 0 0 - 0.0220.08 - - - 0.12 0.12 -0.10 0 0 0 - 0.0220.08 - 0.07 0.07 0.08 10.12 0.12 0.14 1 - - - 0.085 -0.085 0 0 0 -0.7853981634 - 0.30 0.025 0.02 - - - -0.085 -0.085 0 0 0 0.7853981634 - 0.30 0.025 0.02 - - - -0.085 0.085 0 0 0 -0.7853981634 - 0.30 0.025 0.02 - - - 0.085 0.085 0 0 0 0.7853981634 - 0.30 0.025 0.02 - - - 0.17 -0.17 0.05 0 0 0 - 0.100.01 - 0.7 0.1 0.1 10.7 0.1 0.1 1 - - - -0.17 -0.17 0.05 0 0 0 - 0.100.01 - 0.1 0.7 0.1 10.1 0.7 0.1 1 - - - -0.17 0.17 0.05 0 0 0 - 0.100.01 - 0.1 0.1 0.7 10.1 0.1 0.7 1 - - - 0.17 0.17 0.05 0 0 0 - 0.100.01 - 0.7 0.7 0.1 10.7 0.7 0.1 1 - - - true - 100 - /kaptan/sitl/imu - - - 00.002 - 00.002 - 00.002 - - - 00.03 - 00.03 - 00.03 - - - - - - /kaptan/sitl/imu - 14660 - 14661 - 14663 - 14664 - 25000 - 4.5 - 0.03 - 0.10 - 0.05 - 18181 - 0.012 - - - - diff --git a/flake.nix b/flake.nix index 35772f9..f2689d4 100644 --- a/flake.nix +++ b/flake.nix @@ -17,12 +17,6 @@ system: let pkgs = nixpkgs.legacyPackages.${system}; - python = pkgs.python3.withPackages ( - pythonPackages: with pythonPackages; [ - pymavlink - pyserial - ] - ); in { default = pkgs.mkShell { @@ -35,7 +29,7 @@ pkgs.shfmt pkgs.nixfmt pkgs.taplo - python + pkgs.python3 ]; }; }