feat(simulation): add ROS 2 swarm controller package

ROS 2 package for coordinating PX4 SIH multi-drone swarm missions via
XRCE-DDS. Includes:

- swarm_controller.py: orchestrates N drones through 6 phases
  (EV lock, arm/takeoff, grid hold, circle transition, circle hold,
  kamikaze dive) with DDS cross-talk filtering for multi-instance
  XRCE-DDS setups
- vision_provider.py: publishes noisy VehicleOdometry from ground
  truth for EKF2 external vision fusion (placeholder for non-GPS)
- swarm_launch.py: ROS 2 launch file for N vision providers + controller
- run_swarm.sh: Docker entrypoint that launches PX4 instances,
  XRCE-DDS agent, and ROS 2 nodes
- Dockerfile + build_docker.sh: containerized build with ROS 2 Jazzy,
  px4_msgs from PX4 source, and the swarm package

Validated with 4 drones: all phases complete, 4/4 impacts, ULogs
generated per instance (~16 MB each).

Signed-off-by: Ramon Roche <mrpollo@gmail.com>

.clang-tidy

@@ -150,10 +150,8 @@ Checks:          '*,
                   -readability-convert-member-functions-to-static,
                   -readability-make-member-function-const,
                   -bugprone-implicit-widening-of-multiplication-result,
-                  -bugprone-macro-parentheses,
                   -bugprone-multi-level-implicit-pointer-conversion,
                   -bugprone-signed-char-misuse,
-                  -bugprone-too-small-loop-variable,
                   -cppcoreguidelines-avoid-non-const-global-variables,
                   -cppcoreguidelines-use-default-member-init,
                   -hicpp-multiway-paths-covered,

.github/workflows/commit_checks.yml

@@ -0,0 +1,148 @@
+name: Commit Quality
+
+on:
+  pull_request:
+    types: [opened, edited, synchronize, reopened]
+
+permissions:
+  contents: read
+  pull-requests: write
+  issues: write
+
+concurrency:
+  group: ${{ github.workflow }}-${{ github.ref }}
+  cancel-in-progress: true
+
+env:
+  PR_NUMBER: ${{ github.event.pull_request.number }}
+  IS_FORK: ${{ github.event.pull_request.head.repo.full_name != github.repository }}
+
+jobs:
+  pr-title:
+    name: PR Title
+    runs-on: ubuntu-latest
+    steps:
+      - name: Checkout
+        uses: actions/checkout@v4
+        with:
+          sparse-checkout: Tools/ci
+          fetch-depth: 1
+
+      - name: Check PR title
+        id: check
+        run: |
+          python3 Tools/ci/check_pr_title.py "${{ github.event.pull_request.title }}" --markdown-file comment.md && rc=0 || rc=$?
+          echo "exit_code=$rc" >> "$GITHUB_OUTPUT"
+
+      - name: Post or clear comment
+        if: env.IS_FORK == 'false'
+        env:
+          GH_TOKEN: ${{ github.token }}
+        run: |
+          if [ "${{ steps.check.outputs.exit_code }}" != "0" ]; then
+            python3 Tools/ci/pr_comment.py --marker pr-title --pr "$PR_NUMBER" --result fail < comment.md
+          else
+            python3 Tools/ci/pr_comment.py --marker pr-title --pr "$PR_NUMBER" --result pass
+          fi
+
+      - name: Result
+        if: steps.check.outputs.exit_code != '0'
+        run: |
+          echo "::error::PR title does not follow conventional commits format. See the PR comment for details."
+          exit 1
+
+  commit-messages:
+    name: Commit Messages
+    runs-on: ubuntu-latest
+    steps:
+      - name: Checkout
+        uses: actions/checkout@v4
+        with:
+          sparse-checkout: Tools/ci
+          fetch-depth: 1
+
+      - name: Check commit messages
+        id: check
+        env:
+          GH_TOKEN: ${{ github.token }}
+        run: |
+          gh api \
+            "repos/${{ github.repository }}/pulls/${PR_NUMBER}/commits?per_page=100" \
+            | python3 Tools/ci/check_commit_messages.py --markdown-file comment.md && rc=0 || rc=$?
+          echo "exit_code=$rc" >> "$GITHUB_OUTPUT"
+          # Check for warnings (non-empty markdown on exit 0)
+          if [ "$rc" -eq 0 ] && [ -s comment.md ]; then
+            echo "has_warnings=true" >> "$GITHUB_OUTPUT"
+          else
+            echo "has_warnings=false" >> "$GITHUB_OUTPUT"
+          fi
+
+      - name: Post or clear comment
+        if: env.IS_FORK == 'false'
+        env:
+          GH_TOKEN: ${{ github.token }}
+        run: |
+          if [ "${{ steps.check.outputs.exit_code }}" != "0" ]; then
+            python3 Tools/ci/pr_comment.py --marker commit-msgs --pr "$PR_NUMBER" --result fail < comment.md
+          elif [ "${{ steps.check.outputs.has_warnings }}" == "true" ]; then
+            python3 Tools/ci/pr_comment.py --marker commit-msgs --pr "$PR_NUMBER" --result warn < comment.md
+          else
+            python3 Tools/ci/pr_comment.py --marker commit-msgs --pr "$PR_NUMBER" --result pass
+          fi
+
+      - name: Result
+        if: steps.check.outputs.exit_code != '0'
+        run: |
+          echo "::error::Commit message errors found. See the PR comment for details."
+          exit 1
+
+  pr-body:
+    name: PR Description
+    runs-on: ubuntu-latest
+    steps:
+      - name: Checkout
+        if: env.IS_FORK == 'false'
+        uses: actions/checkout@v4
+        with:
+          sparse-checkout: Tools/ci
+          fetch-depth: 1
+
+      - name: Check PR body
+        id: check
+        env:
+          PR_BODY: ${{ github.event.pull_request.body }}
+        run: |
+          message=""
+          if [ -z "$PR_BODY" ]; then
+            message="PR description is empty. Please add a summary of the changes."
+            echo "::warning::PR description is empty."
+          else
+            cleaned=$(echo "$PR_BODY" | sed 's/<!--.*-->//g' | tr -d '[:space:]')
+            if [ -z "$cleaned" ]; then
+              message="PR description contains only template comments. Please fill in the details."
+              echo "::warning::PR description contains only template comments."
+            fi
+          fi
+          echo "message=$message" >> "$GITHUB_OUTPUT"
+
+      - name: Post or clear comment
+        if: env.IS_FORK == 'false'
+        env:
+          GH_TOKEN: ${{ github.token }}
+        run: |
+          if [ -n "${{ steps.check.outputs.message }}" ]; then
+            printf '%s\n' \
+              "## PR Description (advisory)" \
+              "" \
+              "This is **not blocking**, but your PR description appears to be empty or incomplete." \
+              "" \
+              "${{ steps.check.outputs.message }}" \
+              "" \
+              "A good PR description helps reviewers understand what changed and why." \
+              "" \
+              "---" \
+              "*This comment will be automatically removed once the issue is resolved.*" \
+            | python3 Tools/ci/pr_comment.py --marker pr-body --pr "$PR_NUMBER" --result warn
+          else
+            python3 Tools/ci/pr_comment.py --marker pr-body --pr "$PR_NUMBER" --result pass
+          fi

.github/workflows/dev_container.yml

@@ -130,8 +130,8 @@ jobs:
           load: false
           push: ${{ startsWith(github.ref, 'refs/tags/') || (github.event_name == 'workflow_dispatch' && github.event.inputs.deploy_to_registry) }}
           provenance: false
-          cache-from: type=gha,version=1,scope=${{ matrix.arch }}
-          cache-to: type=gha,version=1,mode=max,scope=${{ matrix.arch }}
+          cache-from: type=gha,scope=${{ matrix.arch }}
+          cache-to: type=gha,mode=max,scope=${{ matrix.arch }}
 
   deploy:
     name: Deploy To Registry

.github/workflows/docs_crowdin_download.yml

@@ -34,13 +34,13 @@ jobs:
           upload_sources: false
           upload_translations: false
           download_translations: true
-          commit_message: New Crowdin translations - ${{ matrix.lc }}
+          commit_message: 'docs(i18n): PX4 guide translations (Crowdin) - ${{ matrix.lc }}'
           localization_branch_name: l10n_crowdin_docs_translations_${{ matrix.lc }}
           crowdin_branch_name: main
           create_pull_request: true
           pull_request_base_branch_name: 'main'
-          pull_request_title: New PX4 guide translations (Crowdin) - ${{ matrix.lc }}
-          pull_request_body: 'New PX4 guide Crowdin translations by [Crowdin GH Action](https://github.com/crowdin/github-action) for ${{ matrix.lc }}'
+          pull_request_title: 'docs(i18n): PX4 guide translations (Crowdin) - ${{ matrix.lc }}'
+          pull_request_body: 'docs(i18n): PX4 guide Crowdin translations by [Crowdin GH Action](https://github.com/crowdin/github-action) for ${{ matrix.lc }}'
           pull_request_labels: 'Documentation 📑'
           pull_request_reviewers: hamishwillee
           download_language: ${{ matrix.lc }}

CITATION.cff

@@ -0,0 +1,27 @@
+cff-version: 1.2.0
+title: "PX4 Autopilot"
+message: "If you use PX4 in your research, please cite it using this metadata."
+type: software
+authors:
+  - family-names: Meier
+    given-names: Lorenz
+  - name: "The PX4 Contributors"
+repository-code: "https://github.com/PX4/PX4-Autopilot"
+url: "https://px4.io"
+abstract: >-
+  PX4 is an open-source autopilot stack for drones and
+  unmanned vehicles. It supports multirotors, fixed-wing,
+  VTOL, rovers, and many more platforms. PX4 runs on both
+  RTOS and POSIX-compatible operating systems.
+keywords:
+  - autopilot
+  - drone
+  - uav
+  - flight-controller
+  - robotics
+  - ros2
+license: BSD-3-Clause
+identifiers:
+  - type: doi
+    value: "10.5281/zenodo.595432"
+    description: "Zenodo concept DOI (resolves to latest version)"

CONTRIBUTING.md

@@ -1,44 +1,170 @@
-# Contributing to PX4 Firmware
+# Contributing to PX4-Autopilot
 
-We follow the [Github flow](https://guides.github.com/introduction/flow/) development model.
+We follow the [GitHub flow](https://guides.github.com/introduction/flow/) development model.
 
-### Fork the project, then clone your repo
+## Fork the project, then clone your repo
 
-First [fork and clone](https://help.github.com/articles/fork-a-repo) the project project.
+First [fork and clone](https://help.github.com/articles/fork-a-repo) the project.
 
-### Create a feature branch
+## Create a feature branch
 
-*Always* branch off main for new features.
+Always branch off `main` for new features.
 
 ```
 git checkout -b mydescriptivebranchname
 ```
 
-### Edit and build the code
+## Edit and build the code
 
-The [developer guide](https://docs.px4.io/main/en/development/development.html) explains how to set up the development environment on Mac OS, Linux or Windows. Please take note of our [coding style](https://docs.px4.io/main/en/contribute/code.html) when editing files.
+The [developer guide](https://docs.px4.io/main/en/development/development.html) explains how to set up the development environment on Mac OS, Linux or Windows.
 
-### Commit your changes
+### Coding standards
 
-Always write descriptive commit messages and add a fixes or relates note to them with an [issue number](https://github.com/px4/Firmware/issues) (Github will link these then conveniently)
+All C/C++ code must follow the [PX4 coding style](https://docs.px4.io/main/en/contribute/code.html). Formatting is enforced by [astyle](http://astyle.sourceforge.net/) in CI (`make check_format`). Code quality checks run via [clang-tidy](https://clang.llvm.org/extra/clang-tidy/). Pull requests that fail either check will not be merged.
 
-**Example:**
+Python code is checked with [mypy](https://mypy-lang.org/) and [flake8](https://flake8.pycqa.org/).
+
+## Commit message convention
+
+PX4 uses [conventional commits](https://www.conventionalcommits.org/) for all commit messages and PR titles.
+
+### Format
 
 ```
-Change how the attitude controller works
-
-- Fixes rate feed forward
-- Allows a local body rate override
-
-Fixes issue #123
+type(scope): short description of the change
 ```
 
-### Test your changes
+| Part | Rule |
+|------|------|
+| **type** | Category of change (see types table below) |
+| **scope** | The module, driver, board, or area of PX4 affected |
+| **`!`** (optional) | Append before `:` to mark a breaking change |
+| **description** | What the change does, at least 5 characters, written in imperative form |
 
-Since we care about safety, we will regularly ask you for test results. Best is to do a test flight (or bench test where it applies) and upload the logfile from it (on the microSD card in the logs directory) to Google Drive or Dropbox and share the link.
+### Types
 
-### Push your changes
+| Type | Description |
+|------|-------------|
+| `feat` | A new feature |
+| `fix` | A bug fix |
+| `docs` | Documentation only changes |
+| `style` | Formatting, whitespace, no code change |
+| `refactor` | Code change that neither fixes a bug nor adds a feature |
+| `perf` | Performance improvement |
+| `test` | Adding or correcting tests |
+| `build` | Build system or external dependencies |
+| `ci` | CI configuration files and scripts |
+| `chore` | Other changes that don't modify src or test files |
+| `revert` | Reverts a previous commit |
 
-Push changes to your repo and send a [pull request](https://github.com/PX4/Firmware/compare/).
+### Scopes
+
+The scope identifies which part of PX4 is affected. Common scopes:
+
+| Scope | Area |
+|-------|------|
+| `ekf2` | Extended Kalman Filter (state estimation) |
+| `mavlink` | MAVLink messaging protocol |
+| `commander` | Commander and mode management |
+| `navigator` | Mission, RTL, Land, and other navigation modes |
+| `sensors` | Sensor drivers and processing |
+| `drivers` | Hardware drivers |
+| `boards/px4_fmu-v6x` | Board-specific changes (use the board name) |
+| `mc_att_control` | Multicopter attitude control |
+| `mc_pos_control` | Multicopter position control |
+| `fw_att_control` | Fixed-wing attitude control |
+| `vtol` | VTOL-specific logic |
+| `actuators` | Mixer and actuator output |
+| `battery` | Battery monitoring and estimation |
+| `logger` | On-board logging |
+| `param` | Parameter system |
+| `simulation` | SITL, Gazebo, SIH |
+| `ci` | Continuous integration and workflows |
+| `docs` | Documentation |
+| `build` | CMake, toolchain, build system |
+| `uorb` | Inter-module messaging |
+
+For changes spanning multiple subsystems, use the primary one affected. Look at the directory path of the files you changed to find the right scope: `src/modules/ekf2/` uses `ekf2`, `src/drivers/imu/` uses `drivers/imu`, `.github/workflows/` uses `ci`.
+
+### Breaking changes
+
+Append `!` before the colon to indicate a breaking change:
+
+```
+feat(ekf2)!: remove deprecated height fusion API
+```
+
+### Good commit messages
+
+```
+feat(ekf2): add height fusion timeout
+fix(mavlink): correct BATTERY_STATUS_V2 parsing
+refactor(navigator): simplify RTL altitude logic
+ci(workflows): migrate to reusable workflows
+docs(ekf2): update tuning guide
+feat(boards/px4_fmu-v6x)!: remove deprecated driver API
+perf(mc_rate_control): reduce loop latency
+```
+
+### Commits to avoid
+
+These will be flagged by CI and should be squashed or reworded before merging:
+
+```
+fix                                    # too vague, no type or scope
+update                                 # too vague, no type or scope
+ekf2: fix something                   # missing type prefix
+apply suggestions from code review     # squash into parent commit
+do make format                         # squash into parent commit
+WIP: trying something                  # not ready for main
+oops                                   # not descriptive
+```
+
+### PR titles
+
+The PR title follows the same `type(scope): description` format. This is enforced by CI and is especially important because the PR title becomes the commit message when a PR is squash-merged.
+
+### Merge policy
+
+Commits should be atomic and independently revertable. Squash at reviewer discretion for obvious cases (multiple WIP commits, messy review-response history). When your commits are clean and logical, they will be preserved as individual commits on `main`.
+
+### Cleaning up commits
+
+If CI flags your commit messages, you can fix them with an interactive rebase:
+
+```bash
+# Squash all commits into one:
+git rebase -i HEAD~N   # replace N with the number of commits
+# mark all commits except the first as 'squash' or 'fixup'
+# reword the remaining commit to follow the format
+git push --force-with-lease
+
+# Or reword specific commits:
+git rebase -i HEAD~N
+# mark the bad commits as 'reword'
+git push --force-with-lease
+```
+
+## Test your changes
+
+PX4 is safety-critical software. All contributions must include adequate testing where practical:
+
+- **New features** must include unit tests and/or integration tests that exercise the new functionality, where practical. Hardware-dependent changes that cannot be tested in SITL should include bench test or flight test evidence.
+- **Bug fixes** must include a regression test where practical. When automated testing is not feasible (hardware-specific issues, race conditions, etc.), provide a link to a flight log demonstrating the fix and the reproduction steps for the original bug.
+- **Reviewers** will verify that tests or test evidence exist before approving a pull request.
+
+### Types of tests
+
+| Test type | When to use | How to run |
+|-----------|-------------|------------|
+| **Unit tests** (gtest) | Module-level logic, math, parsing | `make tests` |
+| **SITL integration tests** (MAVSDK) | Flight behavior, failsafes, missions | `test/mavsdk_tests/` |
+| **Bench tests / flight logs** | Hardware-dependent changes | Upload logs to [Flight Review](https://logs.px4.io) |
+
+Since we care about safety, we will regularly ask you for test results. Best is to do a test flight (or bench test where it applies) and upload the log file from it (on the microSD card in the logs directory) to Google Drive or Dropbox and share the link.
+
+## Push your changes
+
+Push changes to your repo and send a [pull request](https://github.com/PX4/PX4-Autopilot/compare/).
 
 Make sure to provide some testing feedback and if possible the link to a flight log file. Upload flight log files to [Flight Review](http://logs.px4.io) and link the resulting report.

README.md

@@ -10,6 +10,7 @@
 
 <p align="center">
   <a href="https://github.com/PX4/PX4-Autopilot/releases"><img src="https://img.shields.io/github/release/PX4/PX4-Autopilot.svg" alt="Releases"></a>
+  <a href="https://www.bestpractices.dev/projects/6520"><img src="https://www.bestpractices.dev/projects/6520/badge" alt="OpenSSF Best Practices"></a>
   <a href="https://zenodo.org/badge/latestdoi/22634/PX4/PX4-Autopilot"><img src="https://zenodo.org/badge/22634/PX4/PX4-Autopilot.svg" alt="DOI"></a>
   <a href="https://github.com/PX4/PX4-Autopilot/actions/workflows/build_all_targets.yml"><img src="https://github.com/PX4/PX4-Autopilot/actions/workflows/build_all_targets.yml/badge.svg?branch=main" alt="Build Targets"></a>
   <a href="https://discord.gg/dronecode"><img src="https://discordapp.com/api/guilds/1022170275984457759/widget.png?style=shield" alt="Discord"></a>

ROMFS/px4fmu_common/init.d-posix/airframes/10046_sihsim_quadx_vision

@@ -0,0 +1,38 @@
+#!/bin/sh
+#
+# @name QuadrotorX SITL for SIH (Swarm)
+#
+# @type Quadrotor
+#
+# @maintainer Ramon Roche <mrpollo@gmail.com>
+#
+
+. ${R}etc/init.d/rc.mc_defaults
+
+PX4_SIMULATOR=${PX4_SIMULATOR:=sihsim}
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=quadx}
+
+# Simulated sensors: GPS + baro + mag
+param set-default SENS_EN_GPSSIM 1
+param set-default SENS_EN_BAROSIM 1
+param set-default SENS_EN_MAGSIM 1
+
+# Square quadrotor X PX4 numbering
+param set-default CA_ROTOR_COUNT 4
+param set-default CA_ROTOR0_PX 1
+param set-default CA_ROTOR0_PY 1
+param set-default CA_ROTOR1_PX -1
+param set-default CA_ROTOR1_PY -1
+param set-default CA_ROTOR2_PX 1
+param set-default CA_ROTOR2_PY -1
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX -1
+param set-default CA_ROTOR3_PY 1
+param set-default CA_ROTOR3_KM -0.05
+
+param set-default PWM_MAIN_FUNC1 101
+param set-default PWM_MAIN_FUNC2 102
+param set-default PWM_MAIN_FUNC3 103
+param set-default PWM_MAIN_FUNC4 104
+
+param set SIH_VEHICLE_TYPE 0

ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna

@@ -44,8 +44,6 @@ param set-default FW_T_SINK_MIN 3
 
 param set-default FW_W_EN 1
 
-param set-default FD_ESCS_EN 0
-
 param set-default MIS_TAKEOFF_ALT 30
 
 param set-default NAV_ACC_RAD 15

ROMFS/px4fmu_common/init.d-posix/airframes/4004_gz_standard_vtol

@@ -104,4 +104,3 @@ param set-default VT_FWD_THRUST_EN 4
 param set-default VT_PITCH_MIN -5
 param set-default VT_F_TRANS_THR 1
 param set-default VT_TYPE 2
-param set-default FD_ESCS_EN 0

ROMFS/px4fmu_common/init.d-posix/airframes/60002_gz_uuv_bluerov2_heavy

@@ -26,7 +26,6 @@ param set-default SENS_EN_GPSSIM 1
 param set-default SENS_EN_BAROSIM 1
 param set-default SENS_EN_MAGSIM 1
 param set-default COM_ARM_CHK_ESCS 0  # We don't have ESCs
-param set-default FD_ESCS_EN 0 # We don't have ESCs - but maybe we need this later?
 
 # Set proper failsafes
 param set-default COM_ACT_FAIL_ACT 0

ROMFS/px4fmu_common/init.d-posix/airframes/70000_gz_atmos

@@ -28,7 +28,6 @@ param set-default SIM_GZ_EN 1
 
 param set-default SENS_EN_MAGSIM 1
 param set-default COM_ARM_CHK_ESCS 0  # We don't have ESCs
-param set-default FD_ESCS_EN 0
 
 param set-default CA_AIRFRAME 14
 param set-default MAV_TYPE 45

ROMFS/px4fmu_common/init.d-posix/airframes/70001_gz_atmos_dual

@@ -28,7 +28,6 @@ param set-default SIM_GZ_EN 1
 
 param set-default SENS_EN_MAGSIM 1
 param set-default COM_ARM_CHK_ESCS 0  # We don't have ESCs
-param set-default FD_ESCS_EN 0
 
 param set-default CA_AIRFRAME 14
 param set-default MAV_TYPE 45

ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt

@@ -107,6 +107,7 @@ px4_add_romfs_files(
 	10043_sihsim_standard_vtol
 	10044_sihsim_hex
 	10045_sihsim_rover_ackermann
+	10046_sihsim_quadx_vision
 
 	17001_flightgear_tf-g1
 	17002_flightgear_tf-g2

ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlink

@@ -3,7 +3,6 @@
 
 udp_offboard_port_local=$((14580+px4_instance))
 udp_offboard_port_remote=$((14540+px4_instance))
-[ "$px4_instance" -gt 9 ] && udp_offboard_port_remote=14549 # use the same ports for more than 10 instances to avoid port overlaps
 udp_onboard_payload_port_local=$((14280+px4_instance))
 udp_onboard_payload_port_remote=$((14030+px4_instance))
 udp_onboard_gimbal_port_local=$((13030+px4_instance))

SECURITY.md

@@ -2,24 +2,40 @@
 
 ## Supported Versions
 
-The following is a list of versions the development team is currently supporting.
+The following versions receive security updates:
 
 | Version | Supported          |
 | ------- | ------------------ |
-| 1.4.x   | :white_check_mark: |
-| 1.3.3   | :white_check_mark: |
-| < 1.3   | :x:                |
+| 1.16.x  | :white_check_mark: |
+| < 1.16  | :x:                |
 
 ## Reporting a Vulnerability
 
-We currently only receive security vulnerability reports through GitHub.
+We receive security vulnerability reports through GitHub Security Advisories.
 
-To begin a report, please go to the top-level repository, for example, PX4/PX4-Autopilot,
-and click on the Security tab. If you are on mobile, click the ... dropdown menu, and then click Security.
+To begin a report, go to the [PX4/PX4-Autopilot](https://github.com/PX4/PX4-Autopilot) repository
+and click on the **Security** tab. If you are on mobile, click the **...** dropdown menu, then click **Security**.
 
-Click Report a Vulnerability to open the advisory form. Fill in the advisory details form.
-Make sure your title is descriptive, and the development team can find all of the relevant details needed
-to verify on the description box. We recommend you add as much data as possible. We welcome logs,
-screenshots, photos, and videos, anything that can help us verify and identify the issues being reported.
+Click **Report a Vulnerability** to open the advisory form. Fill in the advisory details form.
+Make sure your title is descriptive and the description contains all relevant details needed
+to verify the issue. We welcome logs, screenshots, photos, and videos.
 
-At the bottom of the form, click Submit report. The maintainer team will be notified and will get back to you ASAP.
+At the bottom of the form, click **Submit report**.
+
+## Response Process
+
+1. **Acknowledgment**: The maintainer team will acknowledge your report within **7 days**.
+2. **Triage**: We will assess severity and impact and communicate next steps.
+3. **Disclosure**: We coordinate disclosure with the reporter. We follow responsible disclosure practices and will credit reporters in the advisory unless they request anonymity.
+
+If you do not receive acknowledgment within 7 days, please follow up by emailing the [release managers](MAINTAINERS.md).
+
+## Secure Development Practices
+
+The PX4 development team applies the following practices to reduce security risk:
+
+- **Code review**: All changes require peer review before merging.
+- **Static analysis**: [clang-tidy](https://clang.llvm.org/extra/clang-tidy/) runs on every pull request with warnings treated as errors.
+- **Fuzzing**: A daily fuzzing pipeline using [Google fuzztest](https://github.com/google/fuzztest) tests MAVLink message handling and GNSS driver protocol parsing.
+- **Input validation**: All external inputs (MAVLink messages, RC signals, sensor data) are validated against expected ranges before use.
+- **Compiler hardening**: Builds use `-Wall -Werror`, stack protectors, and other hardening flags where supported by the target platform.

Tools/ci/check_commit_messages.py

@@ -0,0 +1,331 @@
+#!/usr/bin/env python3
+"""Validate commit messages in a PR against conventional commits format.
+
+Reads a JSON array of GitHub commit objects from stdin (as returned by the
+GitHub API's /pulls/{n}/commits endpoint) and checks each message for
+blocking errors and advisory warnings.
+
+With --markdown, outputs a formatted PR comment body instead of plain text.
+"""
+
+import json
+import sys
+
+from conventional_commits import (
+    EXEMPT_PREFIXES,
+    parse_header,
+)
+
+# Blocking: prefixes that indicate unsquashed fixup commits
+FIXUP_PREFIXES = ('fixup!', 'squash!', 'amend!')
+
+# Blocking: single-word throwaway messages (case-insensitive exact match)
+THROWAWAY_WORDS = frozenset({
+    'fix', 'fixed', 'fixes',
+    'update', 'updated', 'updates',
+    'test', 'tests', 'testing',
+    'tmp', 'temp',
+    'oops', 'wip',
+    'debug', 'cleanup',
+})
+
+# Blocking: debug session leftovers
+DEBUG_KEYWORDS = ('tmate',)
+
+# Warning: review-response messages (case-insensitive substring match)
+REVIEW_RESPONSE_PATTERNS = (
+    'address review',
+    'apply suggestions from code review',
+    'code review',
+)
+
+# Warning: formatter-only commits
+FORMATTER_PATTERNS = (
+    'do make format',
+    'make format',
+    'run formatter',
+    'apply format',
+)
+
+MIN_MESSAGE_LENGTH = 5
+
+
+def check_commit(message: str) -> tuple[list[str], list[str]]:
+    """Return (errors, warnings) for a single commit message."""
+    errors: list[str] = []
+    warnings: list[str] = []
+
+    first_line = message.split('\n', 1)[0].strip()
+    lower = first_line.lower()
+
+    # --- Blocking checks ---
+
+    for prefix in FIXUP_PREFIXES:
+        if lower.startswith(prefix):
+            errors.append(f'Unsquashed commit: starts with "{prefix}"')
+
+    if lower == 'wip' or lower.startswith('wip ') or lower.startswith('wip:'):
+        errors.append('WIP commit should not be merged')
+
+    if len(first_line) < MIN_MESSAGE_LENGTH:
+        errors.append(f'Message too short ({len(first_line)} chars, minimum {MIN_MESSAGE_LENGTH})')
+
+    if first_line.strip() and first_line.strip().lower() in THROWAWAY_WORDS:
+        errors.append(f'Single-word throwaway message: "{first_line.strip()}"')
+
+    for kw in DEBUG_KEYWORDS:
+        if kw in lower:
+            errors.append(f'Debug session leftover: contains "{kw}"')
+
+    # --- Warning checks ---
+
+    for pattern in REVIEW_RESPONSE_PATTERNS:
+        if pattern in lower:
+            warnings.append('Review-response commit')
+            break
+
+    for pattern in FORMATTER_PATTERNS:
+        if pattern in lower:
+            warnings.append('Formatter-only commit')
+            break
+
+    if not parse_header(first_line):
+        # Exempt merge commits
+        for prefix in EXEMPT_PREFIXES:
+            if first_line.startswith(prefix):
+                break
+        else:
+            warnings.append(
+                'Missing conventional commit format '
+                '(e.g. "feat(ekf2): add something")'
+            )
+
+    return errors, warnings
+
+
+def suggest_commit(message: str) -> str | None:
+    """Suggest how to fix a bad commit message."""
+    first_line = message.split('\n', 1)[0].strip()
+    lower = first_line.lower()
+
+    for prefix in FIXUP_PREFIXES:
+        if lower.startswith(prefix):
+            return 'Squash this into the commit it fixes'
+
+    if lower == 'wip' or lower.startswith('wip ') or lower.startswith('wip:'):
+        return 'Reword with a descriptive message (e.g. "feat(scope): what changed")'
+
+    if len(first_line) < MIN_MESSAGE_LENGTH:
+        return 'Reword with a descriptive message (e.g. "feat(ekf2): what changed")'
+
+    if first_line.strip().lower() in THROWAWAY_WORDS:
+        return 'Reword with a descriptive message (e.g. "fix(scope): what changed")'
+
+    return None
+
+
+def format_plain(data: list) -> tuple[bool, bool]:
+    """Print plain text output. Returns (has_blocking, has_warnings)."""
+    has_blocking = False
+    has_warnings = False
+
+    for commit in data:
+        sha = commit.get('sha', '?')[:10]
+        message = commit.get('commit', {}).get('message', '')
+        first_line = message.split('\n', 1)[0].strip()
+
+        errors, warnings = check_commit(message)
+
+        if errors or warnings:
+            print(f"\n  {sha}  {first_line}")
+
+        for err in errors:
+            print(f"    ERROR: {err}")
+            has_blocking = True
+
+        for warn in warnings:
+            print(f"    WARNING: {warn}")
+            has_warnings = True
+
+    if has_blocking:
+        print(
+            "\n"
+            "ERROR = must fix before merging (CI will block the PR)\n"
+            "WARNING = advisory, not blocking, but recommended to fix\n"
+            "\n"
+            "See the contributing guide for details:\n"
+            "  https://github.com/PX4/PX4-Autopilot/blob/main/CONTRIBUTING.md#commit-message-convention\n",
+        )
+
+    elif has_warnings:
+        print(
+            "\n"
+            "WARNING = advisory, not blocking, but recommended to fix\n"
+            "\n"
+            "See the contributing guide for details:\n"
+            "  https://github.com/PX4/PX4-Autopilot/blob/main/CONTRIBUTING.md#commit-message-convention\n",
+        )
+
+    return has_blocking, has_warnings
+
+
+def format_markdown_blocking(data: list) -> str:
+    """Format a blocking error markdown comment."""
+    error_groups: dict[str, list[str]] = {}
+    unique_commits: list[tuple[str, str, list[str], str]] = []
+
+    for commit in data:
+        sha = commit.get('sha', '?')[:10]
+        message = commit.get('commit', {}).get('message', '')
+        first_line = message.split('\n', 1)[0].strip()
+
+        errors, _ = check_commit(message)
+        if not errors:
+            continue
+
+        suggestion = suggest_commit(message) or ''
+        unique_commits.append((sha, first_line, errors, suggestion))
+
+        for err in errors:
+            error_groups.setdefault(err, []).append(sha)
+
+    lines = [
+        "## \u274c Commit messages need attention before merging",
+        "",
+    ]
+
+    has_large_group = any(len(shas) > 3 for shas in error_groups.values())
+
+    if has_large_group:
+        lines.extend([
+            "**Issues found:**",
+            "",
+        ])
+        for err_msg, shas in error_groups.items():
+            if len(shas) > 3:
+                lines.append(f"- **{len(shas)} commits**: {err_msg} "
+                             f"(`{shas[0]}`, `{shas[1]}`, ... `{shas[-1]}`)")
+            else:
+                sha_list = ', '.join(f'`{s}`' for s in shas)
+                lines.append(f"- {err_msg}: {sha_list}")
+
+        distinct_messages = {msg for _, msg, _, _ in unique_commits}
+        if len(distinct_messages) <= 5:
+            lines.extend(["", "**Affected commits:**", ""])
+            for sha, msg, errors, suggestion in unique_commits:
+                safe_msg = msg.replace('|', '\\|')
+                lines.append(f"- `{sha}` {safe_msg}")
+    else:
+        lines.extend([
+            "| Commit | Message | Issue | Suggested fix |",
+            "|--------|---------|-------|---------------|",
+        ])
+        for sha, msg, errors, suggestion in unique_commits:
+            issues = '; '.join(errors)
+            safe_msg = msg.replace('|', '\\|')
+            lines.append(f"| `{sha}` | {safe_msg} | {issues} | {suggestion} |")
+
+    lines.extend([
+        "",
+        "See [CONTRIBUTING.md](https://github.com/PX4/PX4-Autopilot/blob/main/CONTRIBUTING.md#commit-message-convention) "
+        "for how to clean up commits.",
+        "",
+        "---",
+        "*This comment will be automatically removed once the issues are resolved.*",
+    ])
+
+    return '\n'.join(lines)
+
+
+def format_markdown_advisory(data: list) -> str:
+    """Format an advisory warning markdown comment."""
+    lines = [
+        "## \U0001f4a1 Commit messages could be improved",
+        "",
+        "Not blocking, but these commit messages could use some cleanup.",
+        "",
+        "| Commit | Message | Suggestion |",
+        "|--------|---------|------------|",
+    ]
+
+    for commit in data:
+        sha = commit.get('sha', '?')[:10]
+        message = commit.get('commit', {}).get('message', '')
+        first_line = message.split('\n', 1)[0].strip()
+
+        _, warnings = check_commit(message)
+        if not warnings:
+            continue
+
+        suggestion = '; '.join(warnings)
+        safe_msg = first_line.replace('|', '\\|')
+        lines.append(f"| `{sha}` | {safe_msg} | {suggestion} |")
+
+    lines.extend([
+        "",
+        "See the [commit message convention](https://github.com/PX4/PX4-Autopilot/blob/main/CONTRIBUTING.md#commit-message-convention) "
+        "for details.",
+        "",
+        "---",
+        "*This comment will be automatically removed once the issues are resolved.*",
+    ])
+
+    return '\n'.join(lines)
+
+
+def main() -> None:
+    markdown_stdout = '--markdown' in sys.argv
+    markdown_file = None
+    for i, a in enumerate(sys.argv):
+        if a == '--markdown-file' and i + 1 < len(sys.argv):
+            markdown_file = sys.argv[i + 1]
+        elif a.startswith('--markdown-file='):
+            markdown_file = a.split('=', 1)[1]
+
+    try:
+        data = json.load(sys.stdin)
+    except json.JSONDecodeError as exc:
+        print(f"Failed to parse JSON input: {exc}", file=sys.stderr)
+        sys.exit(2)
+
+    if not isinstance(data, list):
+        print("Expected a JSON array of commit objects.", file=sys.stderr)
+        sys.exit(2)
+
+    # Always compute blocking/warning state
+    has_blocking = False
+    has_warnings = False
+    for commit in data:
+        message = commit.get('commit', {}).get('message', '')
+        errors, warnings = check_commit(message)
+        if errors:
+            has_blocking = True
+        if warnings:
+            has_warnings = True
+
+    # Generate markdown if needed
+    md = None
+    if has_blocking:
+        md = format_markdown_blocking(data)
+    elif has_warnings:
+        md = format_markdown_advisory(data)
+
+    if md:
+        if markdown_stdout:
+            print(md)
+        if markdown_file:
+            with open(markdown_file, 'w') as f:
+                f.write(md + '\n')
+    elif markdown_file:
+        with open(markdown_file, 'w') as f:
+            pass
+
+    # Plain text output to stderr for CI logs (always, unless --markdown only)
+    if not markdown_stdout:
+        has_blocking, _ = format_plain(data)
+
+    sys.exit(1 if has_blocking else 0)
+
+
+if __name__ == '__main__':
+    main()

Tools/ci/check_pr_title.py

@@ -0,0 +1,163 @@
+#!/usr/bin/env python3
+"""Validate that a PR title follows conventional commits format.
+
+Format: type(scope): description
+
+Can output plain text for CI logs or markdown for PR comments.
+"""
+
+import re
+import sys
+
+from conventional_commits import (
+    CONVENTIONAL_TYPES,
+    EXEMPT_PREFIXES,
+    parse_header,
+    suggest_scope,
+    suggest_type,
+)
+
+
+def suggest_title(title: str) -> str | None:
+    """Try to suggest a corrected title in conventional commits format."""
+    stripped = title.strip()
+
+    # Remove common bracket prefixes like [docs], [CI], etc.
+    bracket_match = re.match(r'^\[([^\]]+)\]\s*(.+)', stripped)
+    if bracket_match:
+        prefix = bracket_match.group(1).strip().lower()
+        rest = bracket_match.group(2).strip()
+        rest = re.sub(r'^[\-:]\s*', '', rest).strip()
+        if len(rest) >= 5:
+            # Try to map bracket content to a type
+            commit_type = prefix if prefix in CONVENTIONAL_TYPES else suggest_type(rest)
+            scope = suggest_scope(rest)
+            if scope:
+                return f"{commit_type}({scope}): {rest}"
+
+    # Already has old-style "subsystem: description" format - convert it
+    colon_match = re.match(r'^([a-zA-Z][a-zA-Z0-9_/\-\. ]*): (.+)$', stripped)
+    if colon_match:
+        old_subsystem = colon_match.group(1).strip()
+        desc = colon_match.group(2).strip()
+        if len(desc) >= 5:
+            commit_type = suggest_type(desc)
+            # Use the old subsystem as scope (clean it up)
+            scope = old_subsystem.lower().replace(' ', '_')
+            return f"{commit_type}({scope}): {desc}"
+
+    # No format at all - try to guess both type and scope
+    commit_type = suggest_type(stripped)
+    scope = suggest_scope(stripped)
+    if scope:
+        desc = stripped[0].lower() + stripped[1:] if stripped else stripped
+        return f"{commit_type}({scope}): {desc}"
+
+    return None
+
+
+def check_title(title: str) -> bool:
+    title = title.strip()
+
+    if not title:
+        print("PR title is empty.", file=sys.stderr)
+        return False
+
+    for prefix in EXEMPT_PREFIXES:
+        if title.startswith(prefix):
+            return True
+
+    if parse_header(title):
+        return True
+
+    types_str = ', '.join(f'`{t}`' for t in CONVENTIONAL_TYPES.keys())
+    print(
+        f"PR title does not match conventional commits format.\n"
+        f"\n"
+        f"  Title: {title}\n"
+        f"\n"
+        f"Expected format:  type(scope): description\n"
+        f"\n"
+        f"Valid types: {types_str}\n"
+        f"\n"
+        f"Good examples:\n"
+        f"  feat(ekf2): add height fusion timeout\n"
+        f"  fix(mavlink): correct BATTERY_STATUS_V2 parsing\n"
+        f"  ci(workflows): migrate to reusable workflows\n"
+        f"  feat(boards/px4_fmu-v6x)!: remove deprecated driver API\n"
+        f"\n"
+        f"Bad examples:\n"
+        f"  fix stuff\n"
+        f"  Update file\n"
+        f"  ekf2: fix something   (missing type prefix)\n"
+        f"\n"
+        f"See the contributing guide for details:\n"
+        f"  https://github.com/PX4/PX4-Autopilot/blob/main/CONTRIBUTING.md#commit-message-convention\n",
+        file=sys.stderr,
+    )
+    return False
+
+
+def format_markdown(title: str) -> str:
+    """Format a markdown PR comment body for a bad title."""
+    lines = [
+        "## \u274c PR title needs conventional commit format",
+        "",
+        "Expected format: `type(scope): description` "
+        "([conventional commits](https://www.conventionalcommits.org/)).",
+        "",
+        "**Your title:**",
+        f"> {title}",
+        "",
+    ]
+
+    suggestion = suggest_title(title)
+    if suggestion:
+        lines.extend([
+            "**Suggested fix:**",
+            f"> {suggestion}",
+            "",
+        ])
+
+    lines.extend([
+        "**To fix this:** click the ✏️ next to the PR title at the top "
+        "of this page and update it.",
+        "",
+        "See [CONTRIBUTING.md](https://github.com/PX4/PX4-Autopilot/blob/main/CONTRIBUTING.md#commit-message-convention) "
+        "for details.",
+        "",
+        "---",
+        "*This comment will be automatically removed once the issue is resolved.*",
+    ])
+
+    return '\n'.join(lines)
+
+
+def main() -> None:
+    import argparse
+    parser = argparse.ArgumentParser(description='Check PR title format')
+    parser.add_argument('title', help='The PR title to validate')
+    parser.add_argument('--markdown', action='store_true',
+                        help='Output markdown to stdout on failure')
+    parser.add_argument('--markdown-file', metavar='FILE',
+                        help='Write markdown to FILE on failure')
+    args = parser.parse_args()
+
+    passed = check_title(args.title)
+
+    if not passed:
+        md = format_markdown(args.title)
+        if args.markdown:
+            print(md)
+        if args.markdown_file:
+            with open(args.markdown_file, 'w') as f:
+                f.write(md + '\n')
+    elif args.markdown_file:
+        with open(args.markdown_file, 'w') as f:
+            pass
+
+    sys.exit(0 if passed else 1)
+
+
+if __name__ == '__main__':
+    main()

Tools/ci/conventional_commits.py

@@ -0,0 +1,146 @@
+"""Shared constants and helpers for conventional commit validation.
+
+Format: type(scope): description
+Optional breaking change marker: type(scope)!: description
+"""
+
+import re
+
+CONVENTIONAL_TYPES = {
+    'feat':     'A new feature',
+    'fix':      'A bug fix',
+    'docs':     'Documentation only changes',
+    'style':    'Formatting, whitespace, no code change',
+    'refactor': 'Code change that neither fixes a bug nor adds a feature',
+    'perf':     'Performance improvement',
+    'test':     'Adding or correcting tests',
+    'build':    'Build system or external dependencies',
+    'ci':       'CI configuration files and scripts',
+    'chore':    'Other changes that don\'t modify src or test files',
+    'revert':   'Reverts a previous commit',
+}
+
+# type(scope)[!]: description
+# - type: one of CONVENTIONAL_TYPES keys
+# - scope: required, alphanumeric with _/-/.
+# - !: optional breaking change marker
+# - description: at least 5 chars
+HEADER_PATTERN = re.compile(
+    r'^(' + '|'.join(CONVENTIONAL_TYPES.keys()) + r')'
+    r'\(([a-zA-Z0-9_/\-\.]+)\)'
+    r'(!)?'
+    r': (.{5,})$'
+)
+
+EXEMPT_PREFIXES = ('Merge ',)
+
+# Common PX4 subsystem scopes for suggestions
+KNOWN_SCOPES = [
+    'ekf2', 'mavlink', 'commander', 'navigator', 'sensors',
+    'mc_att_control', 'mc_pos_control', 'mc_rate_control',
+    'fw_att_control', 'fw_pos_control', 'fw_rate_control',
+    'vtol', 'actuators', 'battery', 'param', 'logger',
+    'uorb', 'drivers', 'boards', 'simulation', 'sitl',
+    'gps', 'rc', 'safety', 'can', 'serial',
+    'ci', 'docs', 'build', 'cmake', 'tools',
+    'mixer', 'land_detector', 'airspeed', 'gyroscope',
+    'accelerometer', 'magnetometer', 'barometer',
+]
+
+# Keyword patterns to suggest scopes from description text
+KEYWORD_SCOPES = [
+    (r'\b(ekf|estimator|height|fusion|imu|baro)\b', 'ekf2'),
+    (r'\b(mavlink|MAVLink|MAVLINK|command_int|heartbeat)\b', 'mavlink'),
+    (r'\b(uorb|orb|pub|sub|topic)\b', 'uorb'),
+    (r'\b(board|fmu|nuttx|stm32)\b', 'boards'),
+    (r'\b(mixer|actuator|motor|servo|pwm|dshot)\b', 'actuators'),
+    (r'\b(battery|power)\b', 'battery'),
+    (r'\b(param|parameter)\b', 'param'),
+    (r'\b(log|logger|sdlog)\b', 'logger'),
+    (r'\b(sensor|accel|gyro)\b', 'sensors'),
+    (r'\b(land|takeoff|rtl|mission|navigator|geofence)\b', 'navigator'),
+    (r'\b(position|velocity|attitude|rate)\s*(control|ctrl)\b', 'mc_att_control'),
+    (r'\b(mc|multicopter|quad)\b', 'mc_att_control'),
+    (r'\b(fw|fixedwing|fixed.wing|plane)\b', 'fw_att_control'),
+    (r'\b(vtol|transition)\b', 'vtol'),
+    (r'\b(ci|workflow|github.action|pipeline)\b', 'ci'),
+    (r'\b(doc|docs|documentation|readme)\b', 'docs'),
+    (r'\b(cmake|make|toolchain|compiler)\b', 'build'),
+    (r'\b(sitl|simulation|gazebo|jmavsim|sih)\b', 'simulation'),
+    (r'\b(can|uavcan|cyphal|dronecan)\b', 'can'),
+    (r'\b(serial|uart|spi|i2c)\b', 'serial'),
+    (r'\b(safety|failsafe|arm|disarm|kill)\b', 'safety'),
+    (r'\b(rc|radio|sbus|crsf|elrs|dsm)\b', 'rc'),
+    (r'\b(gps|gnss|rtk|ubx)\b', 'gps'),
+    (r'\b(optical.flow|flow|rangefinder|lidar|distance)\b', 'sensors'),
+    (r'\b(orbit|follow|offboard)\b', 'commander'),
+    (r'\b(driver)\b', 'drivers'),
+]
+
+# Verb patterns to suggest conventional commit type
+VERB_TYPE_MAP = [
+    (r'^fix(e[ds])?[\s:]', 'fix'),
+    (r'^bug[\s:]', 'fix'),
+    (r'^add(s|ed|ing)?[\s:]', 'feat'),
+    (r'^implement', 'feat'),
+    (r'^introduce', 'feat'),
+    (r'^support', 'feat'),
+    (r'^enable', 'feat'),
+    (r'^update[ds]?[\s:]', 'feat'),
+    (r'^improv(e[ds]?|ing)', 'perf'),
+    (r'^optimi[zs](e[ds]?|ing)', 'perf'),
+    (r'^refactor', 'refactor'),
+    (r'^clean\s*up', 'refactor'),
+    (r'^restructure', 'refactor'),
+    (r'^simplif(y|ied)', 'refactor'),
+    (r'^remov(e[ds]?|ing)', 'refactor'),
+    (r'^delet(e[ds]?|ing)', 'refactor'),
+    (r'^deprecat', 'refactor'),
+    (r'^replac(e[ds]?|ing)', 'refactor'),
+    (r'^renam(e[ds]?|ing)', 'refactor'),
+    (r'^migrat', 'refactor'),
+    (r'^revert', 'revert'),
+    (r'^doc(s|ument)', 'docs'),
+    (r'^test', 'test'),
+    (r'^format', 'style'),
+    (r'^lint', 'style'),
+    (r'^whitespace', 'style'),
+    (r'^build', 'build'),
+    (r'^ci[\s:]', 'ci'),
+]
+
+
+def parse_header(text: str) -> dict | None:
+    """Parse a conventional commit header into components.
+
+    Returns dict with keys {type, scope, breaking, subject} or None if
+    the text doesn't match conventional commits format.
+    """
+    text = text.strip()
+    m = HEADER_PATTERN.match(text)
+    if not m:
+        return None
+    return {
+        'type': m.group(1),
+        'scope': m.group(2),
+        'breaking': m.group(3) == '!',
+        'subject': m.group(4),
+    }
+
+
+def suggest_type(text: str) -> str:
+    """Infer a conventional commit type from description text."""
+    lower = text.strip().lower()
+    for pattern, commit_type in VERB_TYPE_MAP:
+        if re.search(pattern, lower):
+            return commit_type
+    return 'feat'
+
+
+def suggest_scope(text: str) -> str | None:
+    """Infer a scope from keywords in the text."""
+    lower = text.strip().lower()
+    for pattern, scope in KEYWORD_SCOPES:
+        if re.search(pattern, lower, re.IGNORECASE):
+            return scope
+    return None

Tools/ci/package_build_artifacts.sh

@@ -6,32 +6,42 @@ cp **/**/*.elf artifacts/ 2>/dev/null || true
 for build_dir_path in build/*/ ; do
   build_dir_path=${build_dir_path::${#build_dir_path}-1}
   build_dir=${build_dir_path#*/}
-  mkdir artifacts/$build_dir
+  mkdir -p artifacts/$build_dir
   find artifacts/ -maxdepth 1 -type f -name "*$build_dir*"
-  # Airframe
-  cp $build_dir_path/airframes.xml artifacts/$build_dir/
+  # Airframe (NuttX: build root, SITL: docs/ subdirectory)
+  airframes_src=""
+  if [ -f "$build_dir_path/airframes.xml" ]; then
+    airframes_src="$build_dir_path/airframes.xml"
+  elif [ -f "$build_dir_path/docs/airframes.xml" ]; then
+    airframes_src="$build_dir_path/docs/airframes.xml"
+  fi
+  if [ -n "$airframes_src" ]; then
+    cp "$airframes_src" "artifacts/$build_dir/"
+  fi
   # Parameters
-  cp $build_dir_path/parameters.xml artifacts/$build_dir/
-  cp $build_dir_path/parameters.json artifacts/$build_dir/
-  cp $build_dir_path/parameters.json.xz artifacts/$build_dir/
+  cp $build_dir_path/parameters.xml artifacts/$build_dir/ 2>/dev/null || true
+  cp $build_dir_path/parameters.json artifacts/$build_dir/ 2>/dev/null || true
+  cp $build_dir_path/parameters.json.xz artifacts/$build_dir/ 2>/dev/null || true
   # Actuators
-  cp $build_dir_path/actuators.json artifacts/$build_dir/
-  cp $build_dir_path/actuators.json.xz artifacts/$build_dir/
+  cp $build_dir_path/actuators.json artifacts/$build_dir/ 2>/dev/null || true
+  cp $build_dir_path/actuators.json.xz artifacts/$build_dir/ 2>/dev/null || true
   # Events
-  cp $build_dir_path/events/all_events.json.xz artifacts/$build_dir/
-  # ROS 2 msgs
-  cp $build_dir_path/events/all_events.json.xz artifacts/$build_dir/
-  # Module Docs
+  mkdir -p artifacts/$build_dir/events/
+  cp $build_dir_path/events/all_events.json.xz artifacts/$build_dir/events/ 2>/dev/null || true
   ls -la artifacts/$build_dir
   echo "----------"
 done
 
 if [ -d artifacts/px4_sitl_default ]; then
-  # general metadata
-  mkdir artifacts/_general/
-  cp artifacts/px4_sitl_default/airframes.xml artifacts/_general/
+  # general metadata (used by Flight Review and other downstream consumers)
+  mkdir -p artifacts/_general/
   # Airframe
-  cp artifacts/px4_sitl_default/airframes.xml artifacts/_general/
+  if [ -f artifacts/px4_sitl_default/airframes.xml ]; then
+    cp artifacts/px4_sitl_default/airframes.xml artifacts/_general/
+  else
+    echo "Error: expected 'artifacts/px4_sitl_default/airframes.xml' not found." >&2
+    exit 1
+  fi
   # Parameters
   cp artifacts/px4_sitl_default/parameters.xml artifacts/_general/
   cp artifacts/px4_sitl_default/parameters.json artifacts/_general/
@@ -40,9 +50,11 @@ if [ -d artifacts/px4_sitl_default ]; then
   cp artifacts/px4_sitl_default/actuators.json artifacts/_general/
   cp artifacts/px4_sitl_default/actuators.json.xz artifacts/_general/
   # Events
-  cp artifacts/px4_sitl_default/events/all_events.json.xz artifacts/_general/
-  # ROS 2 msgs
-  cp artifacts/px4_sitl_default/events/all_events.json.xz artifacts/_general/
-  # Module Docs
+  if [ -f artifacts/px4_sitl_default/events/all_events.json.xz ]; then
+    cp artifacts/px4_sitl_default/events/all_events.json.xz artifacts/_general/
+  else
+    echo "Error: expected 'artifacts/px4_sitl_default/events/all_events.json.xz' not found." >&2
+    exit 1
+  fi
   ls -la artifacts/_general/
 fi

Tools/ci/pr_comment.py

@@ -0,0 +1,95 @@
+#!/usr/bin/env python3
+"""Post, update, or delete a PR comment with deduplication.
+
+Uses hidden HTML markers to find existing comments and avoid duplicates.
+Reads comment body from stdin when posting or updating.
+
+Usage:
+    echo "comment body" | python3 pr_comment.py --marker pr-title --pr 123 --result fail
+    python3 pr_comment.py --marker pr-title --pr 123 --result pass
+
+Results:
+    fail  - post/update comment with body from stdin
+    warn  - post/update comment with body from stdin
+    pass  - delete existing comment if any
+
+Requires GH_TOKEN and GITHUB_REPOSITORY environment variables.
+"""
+
+import argparse
+import json
+import os
+import subprocess
+import sys
+
+
+def gh_api(endpoint: str, method: str = 'GET', body: dict | None = None) -> str:
+    """Call the GitHub API via gh cli."""
+    cmd = ['gh', 'api', endpoint, '-X', method]
+    if body:
+        for key, value in body.items():
+            cmd.extend(['-f', f'{key}={value}'])
+    result = subprocess.run(cmd, capture_output=True, text=True)
+    if result.returncode != 0 and method != 'DELETE':
+        print(f"gh api error: {result.stderr}", file=sys.stderr)
+    return result.stdout
+
+
+def find_comment(repo: str, pr: int, marker: str) -> str | None:
+    """Find an existing comment by its hidden marker. Returns comment ID or None."""
+    response = gh_api(f"repos/{repo}/issues/{pr}/comments?per_page=100")
+    try:
+        comments = json.loads(response)
+    except json.JSONDecodeError:
+        return None
+
+    if not isinstance(comments, list):
+        return None
+
+    for comment in comments:
+        if isinstance(comment, dict) and comment.get('body', '').startswith(marker):
+            return str(comment['id'])
+    return None
+
+
+def main() -> None:
+    parser = argparse.ArgumentParser(description='Manage PR quality comments')
+    parser.add_argument('--marker', required=True,
+                        help='Marker name (e.g. pr-title, commit-msgs, pr-body)')
+    parser.add_argument('--pr', required=True, type=int,
+                        help='Pull request number')
+    parser.add_argument('--result', required=True, choices=['pass', 'fail', 'warn'],
+                        help='Check result: pass deletes comment, fail/warn posts it')
+    args = parser.parse_args()
+
+    repo = os.environ.get('GITHUB_REPOSITORY', '')
+    if not repo:
+        print("GITHUB_REPOSITORY not set", file=sys.stderr)
+        sys.exit(2)
+
+    marker = f"<!-- commit-quality-{args.marker} -->"
+    existing_id = find_comment(repo, args.pr, marker)
+
+    if args.result == 'pass':
+        if existing_id:
+            gh_api(f"repos/{repo}/issues/comments/{existing_id}", method='DELETE')
+        return
+
+    # Read comment body from stdin
+    body_content = sys.stdin.read().strip()
+    if not body_content:
+        print("No comment body provided on stdin", file=sys.stderr)
+        sys.exit(2)
+
+    full_body = f"{marker}\n{body_content}"
+
+    if existing_id:
+        gh_api(f"repos/{repo}/issues/comments/{existing_id}", method='PATCH',
+               body={'body': full_body})
+    else:
+        gh_api(f"repos/{repo}/issues/{args.pr}/comments", method='POST',
+               body={'body': full_body})
+
+
+if __name__ == '__main__':
+    main()

Tools/module_config/generate_actuators_metadata.py

@@ -234,14 +234,14 @@ def get_actuator_output(yaml_config, output_functions, timer_config_file, verbos
         param_prefix = process_param_prefix(group['param_prefix'])
         standard_params = group.get('standard_params', {})
         standard_params_array = [
-            ( 'function', 'Function', 'FUNC', False ),
-            ( 'disarmed', 'Disarmed', 'DIS', False ),
-            ( 'min', 'Minimum', 'MIN', False ),
-            ( 'max', 'Maximum', 'MAX', False ),
-            ( 'center', 'Center\n(for Servos)', 'CENT', False ),
-            ( 'failsafe', 'Failsafe', 'FAIL', True ),
+            ( 'function', 'Function', 'FUNC', False, True ),
+            ( 'disarmed', 'Disarmed', 'DIS', False, True ),
+            ( 'min', 'Minimum', 'MIN', False, True ),
+            ( 'center', 'Center\n(for Servos)', 'CENT', False, False ),
+            ( 'max', 'Maximum', 'MAX', False, True ),
+            ( 'failsafe', 'Failsafe', 'FAIL', True, True ),
             ]
-        for key, label, param_suffix, advanced in standard_params_array:
+        for key, label, param_suffix, advanced, has_function in standard_params_array:
             show_if = None
             if key in standard_params and 'show_if' in standard_params[key]:
                 show_if = standard_params[key]['show_if']
@@ -250,13 +250,12 @@ def get_actuator_output(yaml_config, output_functions, timer_config_file, verbos
                 param = {
                         'label': label,
                         'name': param_prefix+'_'+param_suffix+'${i}',
-                        'function': key,
                     }
+                if has_function: param['function'] = key
                 if advanced: param['advanced'] = True
                 if show_if: param['show-if'] = show_if
                 per_channel_params.append(param)
 
-
         param = {
                 'label': 'Rev Range\n(for Servos)',
                 'name': param_prefix+'_REV',

Tools/module_config/generate_params.py

@@ -312,11 +312,7 @@ When set to -1 (default), the value depends on the function (see {:}).
                 if standard_params[key]['max'] >= 1<<16:
                     raise Exception('maximum value for {:} expected <= {:} (got {:})'.format(key, 1<<16, standard_params[key]['max']))
 
-                if key == 'failsafe':
-                    standard_params[key]['default'] = -1
-                    standard_params[key]['min'] = -1
-
-                if key == 'center':
+                if key == 'failsafe' or key == 'center':
                     standard_params[key]['default'] = -1
                     standard_params[key]['min'] = -1
 

Tools/msg/generate_msg_docs.py

@@ -17,10 +17,10 @@ VALID_FIELDS = { #Note, also have to add the message types as those can be field
     'uint32'
 }
 
-ALLOWED_UNITS = set(["m", "m/s", "m/s^2", "(m/s)^2", "deg", "deg/s", "rad", "rad/s", "rad^2", "rpm" ,"V", "A", "mA", "mAh", "W", "dBm", "h", "s", "ms", "us", "Ohm", "MB", "Kb/s", "degC","Pa","%","-"])
+ALLOWED_UNITS = set(["m", "m/s", "m/s^2", "(m/s)^2", "deg", "deg/s", "rad", "rad/s", "rad^2", "rpm" ,"V", "A", "mA", "mAh", "W", "Wh", "dBm", "h", "minutes", "s", "ms", "us", "Ohm", "MB", "Kb/s", "degC","Pa", "%", "norm", "-"])
 invalid_units = set()
-ALLOWED_FRAMES = set(["NED","Body"])
-ALLOWED_INVALID_VALUES = set(["NaN", "0"])
+ALLOWED_FRAMES = set(["NED", "Body", "FRD", "ENU"])
+ALLOWED_INVALID_VALUES = set(["NaN", "0", "-1"])
 ALLOWED_CONSTANTS_NOT_IN_ENUM = set(["ORB_QUEUE_LENGTH","MESSAGE_VERSION"])
 
 class Error:
@@ -833,11 +833,9 @@ def generate_dds_yaml_doc(allMessageFiles, output_file = 'dds_topics.md'):
         for message in data["subscriptions"]:
             all_message_types.add(message['type'].split("::")[-1])
             all_topics.add(message['topic'].split('/')[-1])
-        if data["subscriptions_multi"]: # There is none now
-            dds_markdown += "None\n"
-            for message in data["subscriptions_multi"]:
-                all_message_types.add(message['type'].split("::")[-1])
-                all_topics.add(message['topic'].split('/')[-1])
+        for message in (data.get("subscriptions_multi") or []):
+            all_message_types.add(message['type'].split("::")[-1])
+            all_topics.add(message['topic'].split('/')[-1])
         for message in allMessageFiles:
             all_messages_in_source.add(message.split('/')[-1].split('.')[0])
         messagesNotExported = all_messages_in_source - all_message_types
@@ -874,13 +872,17 @@ Topic | Type| Rate Limit
 
         dds_markdown += "\n## Subscriptions Multi\n\n"
 
-        if not data["subscriptions_multi"]: # There is none now
+        subscriptions_multi = data.get("subscriptions_multi") or []
+        if not subscriptions_multi:
             dds_markdown += "None\n"
         else:
-            print("Warning - we now have subscription_multi data - check format")
-            dds_markdown += "Topic | Type\n--- | ---\n"
-            for message in data["subscriptions_multi"]:
-                dds_markdown += f"{message['topic']} | {message['type']}\n"
+            dds_markdown += "Topic | Type | Route Field | Max Instances\n--- | --- | --- | ---\n"
+            for message in subscriptions_multi:
+                type = message['type']
+                px4Type = type.split("::")[-1]
+                route_field = f"`{message['route_field']}`" if 'route_field' in message else "-"
+                max_instances = message.get('max_instances', '-')
+                dds_markdown += f"{message['topic']} | [{type}](../msg_docs/{px4Type}.md) | {route_field} | {max_instances}\n"
 
         if messagesNotExported:
             # Print the topics that are not exported to DDS

Tools/px_mkfw.py

@@ -42,6 +42,7 @@
 import argparse
 import json
 import base64
+import os
 import zlib
 import time
 import subprocess
@@ -99,14 +100,13 @@ if args.summary != None:
 if args.description != None:
 	desc['description']	= str(args.description)
 if args.git_identity != None:
-	cmd = "git --git-dir '{:}/.git' describe --exclude ext/* --always --tags".format(args.git_identity)
-	p = subprocess.Popen(cmd, shell=True, stdout=subprocess.PIPE).stdout
-	desc['git_identity']	= p.read().strip().decode('utf-8')
-	p.close()
-	cmd = "git --git-dir '{:}/.git' rev-parse --verify HEAD".format(args.git_identity)
-	p = subprocess.Popen(cmd, shell=True, stdout=subprocess.PIPE).stdout
-	desc['git_hash']	= p.read().strip().decode('utf-8')
-	p.close()
+	git_dir = os.path.join(args.git_identity, '.git')
+	p = subprocess.run(["git", "--git-dir", git_dir, "describe", "--exclude", "ext/*", "--always", "--tags"],
+		stdout=subprocess.PIPE, text=True)
+	desc['git_identity']	= p.stdout.strip()
+	p = subprocess.run(["git", "--git-dir", git_dir, "rev-parse", "--verify", "HEAD"],
+		stdout=subprocess.PIPE, text=True)
+	desc['git_hash']	= p.stdout.strip()
 if args.parameter_xml != None:
 	f = open(args.parameter_xml, "rb")
 	bytes = f.read()

Tools/setup/ubuntu.sh

@@ -196,6 +196,11 @@ if [[ $INSTALL_NUTTX == "true" ]]; then
 	fi
 fi
 
+if [[ "${UBUNTU_RELEASE}" == "25.10" ]]; then
+	echo "[ubuntu.sh] Gazebo binaries are not available for 25.10, skipping installation"
+	INSTALL_SIM="false"
+fi
+
 # Simulation tools
 if [[ $INSTALL_SIM == "true" ]]; then
 

Tools/simulation/collect_swarm_ulogs.py

@@ -0,0 +1,142 @@
+#!/usr/bin/env python3
+"""Collect ULog files from SIH swarm simulation instances.
+
+Gathers .ulg files from per-instance log directories and copies them
+into a single output folder with filenames that identify the source instance.
+"""
+
+import argparse
+import glob
+import os
+import shutil
+import sys
+from datetime import datetime
+from pathlib import Path
+
+
+def find_repo_root() -> Path:
+    """Walk up from script location to find the repository root."""
+    path = Path(__file__).resolve().parent
+    while path != path.parent:
+        if (path / "ROMFS").is_dir() and (path / "src").is_dir():
+            return path
+        path = path.parent
+    return Path(__file__).resolve().parent.parent.parent
+
+
+def parse_instance_number(filepath: str) -> int | None:
+    """Extract the instance number from a path containing instance_N."""
+    parts = Path(filepath).parts
+    for part in parts:
+        if part.startswith("instance_"):
+            try:
+                return int(part.split("_", 1)[1])
+            except (ValueError, IndexError):
+                continue
+    return None
+
+
+def collect_ulogs(build_dir: Path, output_dir: Path, max_instances: int | None) -> None:
+    """Find and copy ULog files from swarm instance directories."""
+    pattern = str(build_dir / "instance_*" / "log" / "**" / "*.ulg")
+    ulog_files = sorted(glob.glob(pattern, recursive=True))
+
+    if not ulog_files:
+        print(f"No ULog files found matching: {pattern}")
+        print()
+        print("Troubleshooting:")
+        print(f"  - Verify build directory exists: {build_dir}")
+        print(f"  - Check for instance_* subdirectories in {build_dir}")
+        print("  - Ensure the swarm simulation has run and produced logs")
+        sys.exit(1)
+
+    # Filter by instance count if specified.
+    if max_instances is not None:
+        ulog_files = [
+            f for f in ulog_files
+            if (n := parse_instance_number(f)) is not None and n < max_instances
+        ]
+
+    if not ulog_files:
+        assert max_instances is not None
+        print(f"No ULog files found for the requested instance range (0..{max_instances - 1}).")
+        sys.exit(1)
+
+    output_dir.mkdir(parents=True, exist_ok=True)
+
+    total_size = 0
+    instances_seen: set[int] = set()
+    copied = 0
+
+    for src in ulog_files:
+        instance = parse_instance_number(src)
+        if instance is None:
+            print(f"  skipping (could not determine instance): {src}")
+            continue
+
+        instances_seen.add(instance)
+        original_name = Path(src).name
+        dest_name = f"drone_{instance:02d}_{original_name}"
+        dest_path = output_dir / dest_name
+
+        shutil.copy2(src, dest_path)
+        file_size = os.path.getsize(src)
+        total_size += file_size
+        copied += 1
+        print(f"  [{instance:02d}] {original_name} ({file_size / 1024:.1f} KB)")
+
+    print()
+    print("Summary")
+    print("-" * 40)
+    print(f"  Files collected : {copied}")
+    print(f"  Instances       : {sorted(instances_seen)}")
+    print(f"  Total size      : {total_size / (1024 * 1024):.2f} MB")
+    print(f"  Output directory: {output_dir.resolve()}")
+
+
+def main() -> None:
+    repo_root = find_repo_root()
+    default_build = str(repo_root / "build" / "px4_sitl_sih")
+    timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
+
+    parser = argparse.ArgumentParser(
+        description="Collect ULog files from SIH swarm simulation instances.",
+    )
+    parser.add_argument(
+        "--build-dir",
+        default=default_build,
+        help=f"Path to the build directory (default: {default_build})",
+    )
+    parser.add_argument(
+        "--output-dir",
+        default=None,
+        help="Output directory (default: swarm_ulogs_YYYYMMDD_HHMMSS)",
+    )
+    parser.add_argument(
+        "--instances",
+        type=int,
+        default=None,
+        help="Number of instances to collect from (default: auto-detect)",
+    )
+
+    args = parser.parse_args()
+
+    build_dir = Path(args.build_dir)
+    if not build_dir.is_dir():
+        print(f"Error: build directory does not exist: {build_dir}")
+        sys.exit(1)
+
+    if args.output_dir is not None:
+        output_dir = Path(args.output_dir)
+    else:
+        output_dir = Path(f"swarm_ulogs_{timestamp}")
+
+    print(f"Collecting ULog files from: {build_dir}")
+    print(f"Output directory: {output_dir}")
+    print()
+
+    collect_ulogs(build_dir, output_dir, args.instances)
+
+
+if __name__ == "__main__":
+    main()

Tools/simulation/generate_swarm_ulogs.py

@@ -0,0 +1,487 @@
+#!/usr/bin/env python3
+"""Generate synthetic ULog files for testing mavsim-viewer swarm replay.
+
+Creates N drone ULog files with crafted trajectories in valid ULog binary
+format, without requiring any real PX4 simulation.
+"""
+
+import argparse
+import json
+import math
+import os
+import struct
+
+
+# ---------------------------------------------------------------------------
+# ULog binary writer
+# ---------------------------------------------------------------------------
+
+ULOG_MAGIC = b'\x55\x4c\x6f\x67\x01\x12\x35'
+ULOG_VERSION = 1
+
+
+class ULogWriter:
+    """Write a minimal but valid ULog binary file."""
+
+    def __init__(self, path: str, timestamp_us: int = 0):
+        self._path = path
+        self._fp = open(path, 'wb')
+        self._msg_id_counter = 0
+        self._subscriptions: dict[str, int] = {}  # topic_name -> msg_id
+        self._write_header(timestamp_us)
+        self._write_flag_bits()
+
+    # -- low-level helpers --------------------------------------------------
+
+    def _write_raw(self, data: bytes):
+        self._fp.write(data)
+
+    def _write_message(self, msg_type: int, payload: bytes):
+        """Write a ULog message: uint16 size + uint8 type + payload."""
+        self._write_raw(struct.pack('<HB', len(payload), msg_type))
+        self._write_raw(payload)
+
+    def _write_header(self, timestamp_us: int):
+        self._write_raw(ULOG_MAGIC)
+        self._write_raw(struct.pack('<BQ', ULOG_VERSION, timestamp_us))
+
+    def _write_flag_bits(self):
+        # 'B' message: 8 compat + 8 incompat + 3*uint64 appended offsets
+        payload = b'\x00' * 8 + b'\x00' * 8 + struct.pack('<QQQ', 0, 0, 0)
+        self._write_message(0x42, payload)
+
+    # -- definition messages ------------------------------------------------
+
+    def write_format(self, fmt_string: str):
+        """Write a Format ('F') definition message.
+
+        fmt_string example: "vehicle_local_position:uint64_t timestamp;float x;float y"
+        """
+        payload = fmt_string.encode('ascii')
+        self._write_message(0x46, payload)
+
+    def write_info(self, key: str, value: str):
+        """Write an Info ('I') message with a string value."""
+        key_with_type = f"char[{len(value)}] {key}"
+        key_bytes = key_with_type.encode('ascii')
+        value_bytes = value.encode('ascii')
+        payload = struct.pack('<B', len(key_bytes)) + key_bytes + value_bytes
+        self._write_message(0x49, payload)
+
+    def add_subscription(self, topic_name: str, multi_id: int = 0) -> int:
+        """Write an AddSubscription ('A') message. Returns the assigned msg_id."""
+        msg_id = self._msg_id_counter
+        self._msg_id_counter += 1
+        self._subscriptions[topic_name] = msg_id
+        name_bytes = topic_name.encode('ascii')
+        payload = struct.pack('<BH', multi_id, msg_id) + name_bytes
+        self._write_message(0x41, payload)
+        return msg_id
+
+    def write_data(self, topic_name: str, data_bytes: bytes):
+        """Write a Data ('D') message for a subscribed topic.
+
+        data_bytes must be the fully serialized payload (starting with
+        uint64 timestamp) matching the format definition.
+        """
+        msg_id = self._subscriptions[topic_name]
+        payload = struct.pack('<H', msg_id) + data_bytes
+        self._write_message(0x44, payload)
+
+    def close(self):
+        self._fp.close()
+
+
+# ---------------------------------------------------------------------------
+# Topic serialization helpers
+# ---------------------------------------------------------------------------
+
+def pack_vehicle_local_position(timestamp_us: int, x: float, y: float, z: float,
+                                 vx: float, vy: float, vz: float,
+                                 ref_lat: float, ref_lon: float, ref_alt: float) -> bytes:
+    return struct.pack('<Qffffffddf',
+                       timestamp_us, x, y, z, vx, vy, vz,
+                       ref_lat, ref_lon, ref_alt)
+
+
+def pack_vehicle_attitude(timestamp_us: int, q: tuple[float, ...]) -> bytes:
+    return struct.pack('<Qffff', timestamp_us, q[0], q[1], q[2], q[3])
+
+
+def pack_vehicle_global_position(timestamp_us: int, lat: float, lon: float, alt: float) -> bytes:
+    return struct.pack('<Qddf', timestamp_us, lat, lon, alt)
+
+
+def pack_vehicle_status(timestamp_us: int, vehicle_type: int, nav_state: int) -> bytes:
+    return struct.pack('<QBB', timestamp_us, vehicle_type, nav_state)
+
+
+# ---------------------------------------------------------------------------
+# Format definition strings (must match pack functions above)
+# ---------------------------------------------------------------------------
+
+FMT_LOCAL_POS = (
+    "vehicle_local_position:"
+    "uint64_t timestamp;"
+    "float x;float y;float z;"
+    "float vx;float vy;float vz;"
+    "double ref_lat;double ref_lon;float ref_alt"
+)
+
+FMT_ATTITUDE = (
+    "vehicle_attitude:"
+    "uint64_t timestamp;"
+    "float[4] q"
+)
+
+FMT_GLOBAL_POS = (
+    "vehicle_global_position:"
+    "uint64_t timestamp;"
+    "double lat;double lon;float alt"
+)
+
+FMT_STATUS = (
+    "vehicle_status:"
+    "uint64_t timestamp;"
+    "uint8_t vehicle_type;uint8_t nav_state"
+)
+
+# ---------------------------------------------------------------------------
+# Trajectory math
+# ---------------------------------------------------------------------------
+
+REF_LAT = 47.397742
+REF_LON = 8.545594
+REF_ALT = 488.0
+
+NAV_TAKEOFF = 2
+NAV_OFFBOARD = 14
+NAV_LAND = 18
+
+
+def quat_from_yaw(yaw: float) -> tuple[float, float, float, float]:
+    return (math.cos(yaw / 2.0), 0.0, 0.0, math.sin(yaw / 2.0))
+
+
+def smooth_interp(t: float) -> float:
+    """Cosine interpolation factor 0..1."""
+    return 0.5 - 0.5 * math.cos(math.pi * max(0.0, min(1.0, t)))
+
+
+def grid_position(drone_id: int) -> tuple[float, float]:
+    """4x4 grid, 3m spacing, centered on origin."""
+    row = drone_id // 4
+    col = drone_id % 4
+    x = (row - 1.5) * 3.0
+    y = (col - 1.5) * 3.0
+    return x, y
+
+
+def circle_position(drone_id: int, num_drones: int, radius: float = 30.0) -> tuple[float, float]:
+    angle = 2.0 * math.pi * drone_id / num_drones
+    return radius * math.cos(angle), radius * math.sin(angle)
+
+
+def local_to_global(x: float, y: float, z: float) -> tuple[float, float, float]:
+    lat = REF_LAT + (x / 111320.0)
+    lon = REF_LON + (y / (111320.0 * math.cos(math.radians(REF_LAT))))
+    alt = REF_ALT - z
+    return lat, lon, alt
+
+
+# ---------------------------------------------------------------------------
+# Phase computations
+# ---------------------------------------------------------------------------
+
+TARGET = (200.0, 0.0, 0.0)  # NED target for kamikaze
+
+
+def compute_state(drone_id: int, t: float, num_drones: int, _duration: float):
+    """Return (x, y, z, vx, vy, vz, yaw, nav_state) for a drone at time t.
+
+    Returns None if the drone hasn't started yet or has impacted.
+    """
+    start_time = drone_id * 0.5  # staggered start
+    if t < start_time:
+        return None
+
+    # Failure drone 12 -- crash at t=70, log ends
+    if drone_id == 12 and t > 70.0:
+        return None
+
+    # Default nav state
+    nav_state = NAV_OFFBOARD
+    yaw = 0.0
+    vx, vy, vz = 0.0, 0.0, 0.0
+
+    gx, gy = grid_position(drone_id)
+    cx, cy = circle_position(drone_id, num_drones)
+
+    if t < 10.0:
+        # Phase 1: staggered start, on ground rising slightly
+        frac = smooth_interp((t - start_time) / max(0.01, 10.0 - start_time))
+        x, y = gx * frac, gy * frac
+        z = 0.0
+        nav_state = NAV_TAKEOFF
+
+    elif t < 25.0:
+        # Phase 2: grid takeoff, ascend to -20 NED
+        frac = smooth_interp((t - 10.0) / 15.0)
+        x, y = gx, gy
+        z = -20.0 * frac
+        vz = -20.0 / 15.0 if frac < 1.0 else 0.0
+        nav_state = NAV_TAKEOFF
+
+    elif t < 40.0:
+        # Phase 3: grid -> ring morph
+        frac = smooth_interp((t - 25.0) / 15.0)
+        x = gx + (cx - gx) * frac
+        y = gy + (cy - gy) * frac
+        z = -20.0
+        vx = (cx - gx) / 15.0
+        vy = (cy - gy) / 15.0
+        nav_state = NAV_OFFBOARD
+
+    elif t < 55.0:
+        # Phase 4: ring cruise CW at 3 m/s
+        angular_speed = 3.0 / 30.0  # omega = v/r
+        base_angle = 2.0 * math.pi * drone_id / num_drones
+        angle = base_angle - angular_speed * (t - 40.0)  # CW -> negative
+        x = 30.0 * math.cos(angle)
+        y = 30.0 * math.sin(angle)
+        z = -20.0
+        vx = 30.0 * angular_speed * math.sin(angle)
+        vy = -30.0 * angular_speed * math.cos(angle)
+        yaw = math.atan2(-y, -x)  # face center
+        nav_state = NAV_OFFBOARD
+
+    else:
+        # Phase 5/6: Kamikaze dive toward target
+        # Starting position at t=55
+        base_angle = 2.0 * math.pi * drone_id / num_drones
+        angular_speed = 3.0 / 30.0
+        angle55 = base_angle - angular_speed * 15.0
+        start_x = 30.0 * math.cos(angle55)
+        start_y = 30.0 * math.sin(angle55)
+        start_z = -20.0
+
+        dx = TARGET[0] - start_x
+        dy = TARGET[1] - start_y
+        dz = TARGET[2] - start_z
+        dist = math.sqrt(dx * dx + dy * dy + dz * dz)
+        speed = 12.0  # m/s max
+        nx, ny, nz = dx / dist, dy / dist, dz / dist
+
+        dt = t - 55.0
+        x = start_x + nx * speed * dt
+        y = start_y + ny * speed * dt
+        z = start_z + nz * speed * dt
+        vx = nx * speed
+        vy = ny * speed
+        vz = nz * speed
+        yaw = math.atan2(ny, nx)
+        nav_state = NAV_LAND
+
+        # Impact: close to target or above ground
+        cur_dist = math.sqrt((TARGET[0] - x) ** 2 + (TARGET[1] - y) ** 2 + (TARGET[2] - z) ** 2)
+        if cur_dist < 2.0 or z > -1.0:
+            return None
+
+    # Drone 12 failure behavior (attitude wobble from t=60)
+    if drone_id == 12 and t >= 60.0:
+        wobble_t = t - 60.0
+        wobble_amp = min(wobble_t * 0.1, 0.8)  # growing oscillation
+        roll = wobble_amp * math.sin(wobble_t * 5.0)
+        pitch = wobble_amp * math.cos(wobble_t * 4.0)
+        # drift sideways
+        x += wobble_t * 2.0
+        y += wobble_t * 1.5
+        z += wobble_t * 0.5  # descending
+        # Build quaternion with roll/pitch/yaw
+        cr, sr = math.cos(roll / 2), math.sin(roll / 2)
+        cp, sp = math.cos(pitch / 2), math.sin(pitch / 2)
+        cy, sy = math.cos(yaw / 2), math.sin(yaw / 2)
+        q = (
+            cr * cp * cy + sr * sp * sy,
+            sr * cp * cy - cr * sp * sy,
+            cr * sp * cy + sr * cp * sy,
+            cr * cp * sy - sr * sp * cy,
+        )
+        return (x, y, z, vx, vy, vz, q, nav_state)
+
+    return (x, y, z, vx, vy, vz, quat_from_yaw(yaw), nav_state)
+
+
+# ---------------------------------------------------------------------------
+# Generate a single drone ULog
+# ---------------------------------------------------------------------------
+
+def generate_drone_ulog(drone_id: int, num_drones: int, duration: float,
+                        output_dir: str, scenario: str):
+    filename = os.path.join(output_dir, f"drone_{drone_id:02d}.ulg")
+    start_us = drone_id * 500_000  # stagger by 0.5s in timestamps
+
+    writer = ULogWriter(filename, timestamp_us=start_us)
+
+    # Write format definitions
+    writer.write_format(FMT_LOCAL_POS)
+    writer.write_format(FMT_ATTITUDE)
+    writer.write_format(FMT_GLOBAL_POS)
+    writer.write_format(FMT_STATUS)
+
+    # Write info messages
+    writer.write_info("sys_name", "PX4")
+    writer.write_info("ver_hw", "SITL")
+    writer.write_info("scenario", scenario)
+    writer.write_info("drone_id", str(drone_id))
+
+    # Add subscriptions
+    writer.add_subscription("vehicle_local_position")
+    writer.add_subscription("vehicle_attitude")
+    writer.add_subscription("vehicle_global_position")
+    writer.add_subscription("vehicle_status")
+
+    # Determine rates per drone (edge cases)
+    pos_dt = 0.02  # 50 Hz
+    att_dt = 0.01  # 100 Hz
+    if drone_id == 13:
+        pos_dt = 0.1  # 10 Hz low rate
+    if drone_id == 14:
+        att_dt = 0.005  # 200 Hz high rate
+
+    # Generate data
+    # We iterate at the finest resolution needed and emit when due
+    fine_dt = min(pos_dt, att_dt, 0.001)
+    # Use the finest rate needed for this drone
+    fine_dt = min(pos_dt, att_dt)
+
+    status_interval = 1.0  # 1 Hz
+    next_pos_t = 0.0
+    next_att_t = 0.0
+    next_status_t = 0.0
+    last_nav_state = -1
+
+    t = 0.0
+    samples_written = 0
+    while t <= duration:
+        state = compute_state(drone_id, t, num_drones, duration)
+
+        # Drone 15 data gap at t=50-52
+        in_gap = (drone_id == 15 and 50.0 <= t <= 52.0)
+
+        if state is not None and not in_gap:
+            x, y, z, vx, vy, vz, q, nav_state = state
+            ts = int(t * 1_000_000)
+
+            # Position data
+            if t >= next_pos_t:
+                writer.write_data("vehicle_local_position",
+                                  pack_vehicle_local_position(
+                                      ts, x, y, z, vx, vy, vz,
+                                      REF_LAT, REF_LON, REF_ALT))
+                lat, lon, alt = local_to_global(x, y, z)
+                writer.write_data("vehicle_global_position",
+                                  pack_vehicle_global_position(ts, lat, lon, alt))
+                next_pos_t = t + pos_dt
+                samples_written += 2
+
+            # Attitude data
+            if t >= next_att_t:
+                writer.write_data("vehicle_attitude",
+                                  pack_vehicle_attitude(ts, q))
+                next_att_t = t + att_dt
+                samples_written += 1
+
+            # Status data
+            if t >= next_status_t or nav_state != last_nav_state:
+                writer.write_data("vehicle_status",
+                                  pack_vehicle_status(ts, 2, nav_state))
+                next_status_t = t + status_interval
+                last_nav_state = nav_state
+                samples_written += 1
+
+        t += fine_dt
+
+    writer.close()
+    return samples_written
+
+
+# ---------------------------------------------------------------------------
+# Scenario metadata
+# ---------------------------------------------------------------------------
+
+def write_scenario_json(output_dir: str, scenario: str, num_drones: int, duration: float):
+    meta = {
+        "scenario": scenario,
+        "num_drones": num_drones,
+        "duration_s": duration,
+        "origin": {
+            "lat": REF_LAT,
+            "lon": REF_LON,
+            "alt": REF_ALT,
+        },
+        "phases": [
+            {"name": "staggered_start", "start_s": 0, "end_s": 10},
+            {"name": "grid_takeoff", "start_s": 10, "end_s": 25},
+            {"name": "grid_to_ring", "start_s": 25, "end_s": 40},
+            {"name": "ring_cruise", "start_s": 40, "end_s": 55},
+            {"name": "kamikaze_dive", "start_s": 55, "end_s": 75},
+        ],
+        "edge_cases": {
+            "drone_12": "failure at t=60, attitude oscillation, crash at t=70",
+            "drone_13": "low-rate position data (10Hz)",
+            "drone_14": "high-rate attitude data (200Hz)",
+            "drone_15": "2-second data gap at t=50-52",
+        },
+        "files": [f"drone_{i:02d}.ulg" for i in range(num_drones)],
+    }
+    path = os.path.join(output_dir, "scenario.json")
+    with open(path, 'w') as f:
+        json.dump(meta, f, indent=2)
+    print(f"  Wrote {path}")
+
+
+# ---------------------------------------------------------------------------
+# Main
+# ---------------------------------------------------------------------------
+
+def main():
+    parser = argparse.ArgumentParser(
+        description="Generate synthetic ULog files for swarm replay testing.")
+    parser.add_argument("--output-dir", default="synthetic_swarm_ulogs",
+                        help="Output directory (default: synthetic_swarm_ulogs)")
+    parser.add_argument("--num-drones", type=int, default=16,
+                        help="Number of drones (default: 16)")
+    parser.add_argument("--duration", type=float, default=90.0,
+                        help="Scenario duration in seconds (default: 90)")
+    parser.add_argument("--scenario", default="hawk_descent",
+                        help="Scenario name (default: hawk_descent)")
+    args = parser.parse_args()
+
+    os.makedirs(args.output_dir, exist_ok=True)
+    print(f"Generating {args.num_drones} drone ULog files for scenario '{args.scenario}'")
+    print(f"  Duration: {args.duration}s")
+    print(f"  Output:   {os.path.abspath(args.output_dir)}/")
+    print()
+
+    for i in range(args.num_drones):
+        samples = generate_drone_ulog(i, args.num_drones, args.duration,
+                                       args.output_dir, args.scenario)
+        tag = ""
+        if i == 12:
+            tag = " [failure drone]"
+        elif i == 13:
+            tag = " [low-rate pos]"
+        elif i == 14:
+            tag = " [high-rate att]"
+        elif i == 15:
+            tag = " [data gap]"
+        print(f"  drone_{i:02d}.ulg  ({samples} messages){tag}")
+
+    write_scenario_json(args.output_dir, args.scenario, args.num_drones, args.duration)
+    print()
+    print(f"Done. {args.num_drones} ULog files written to {os.path.abspath(args.output_dir)}/")
+
+
+if __name__ == "__main__":
+    main()

Tools/simulation/sih_swarm_run.sh

@@ -0,0 +1,141 @@
+#!/bin/bash
+# Launch N SIH quadrotor instances with XRCE-DDS agent.
+# Usage: ./sih_swarm_run.sh [CONFIG_YAML] [SPEED_FACTOR]
+#
+# Requires: build/px4_sitl_sih (make px4_sitl_sih sihsim_quadx_vision)
+
+set -euo pipefail
+
+ulimit -n 4096
+
+SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
+src_path="$SCRIPT_DIR/../../"
+build_path="${src_path}/build/px4_sitl_sih"
+
+CONFIG_YAML="${1:-${SCRIPT_DIR}/swarm_formation.yaml}"
+SPEED_FACTOR="${2:-1}"
+
+if [ ! -f "$CONFIG_YAML" ]; then
+    echo "ERROR: config not found: $CONFIG_YAML"
+    exit 1
+fi
+
+if [ ! -d "$build_path" ]; then
+    echo "ERROR: build directory not found: $build_path"
+    echo "Run: make px4_sitl_sih"
+    exit 1
+fi
+
+# --- Parse YAML config via Python ---
+eval "$(python3 -c "
+import yaml, sys
+with open('${CONFIG_YAML}') as f:
+    cfg = yaml.safe_load(f)
+origin = cfg['origin']
+print(f\"ORIGIN_LAT={origin['lat']}\")
+print(f\"ORIGIN_LON={origin['lon']}\")
+print(f\"ORIGIN_ALT={origin['alt']}\")
+target = cfg['target']
+print(f\"TARGET_X={target['x']}\")
+print(f\"TARGET_Y={target['y']}\")
+print(f\"TARGET_Z={target['z']}\")
+drones = cfg['formation']
+print(f\"DRONE_COUNT={len(drones)}\")
+for i, d in enumerate(drones):
+    print(f\"DRONE_{i}_ID={d['id']}\")
+    print(f\"DRONE_{i}_X={d['x']}\")
+    print(f\"DRONE_{i}_Y={d['y']}\")
+    print(f\"DRONE_{i}_Z={d['z']}\")
+")"
+
+echo "Swarm config: $DRONE_COUNT drones, origin ($ORIGIN_LAT, $ORIGIN_LON, ${ORIGIN_ALT}m)"
+echo "Target: ($TARGET_X, $TARGET_Y, $TARGET_Z)"
+echo "Speed factor: $SPEED_FACTOR"
+echo ""
+
+# --- Cleanup ---
+CHILD_PIDS=()
+AGENT_PID=""
+
+cleanup() {
+    echo ""
+    echo "Shutting down..."
+    for pid in "${CHILD_PIDS[@]}"; do
+        kill "$pid" 2>/dev/null || true
+    done
+    if [ -n "$AGENT_PID" ]; then
+        kill "$AGENT_PID" 2>/dev/null || true
+    fi
+    pkill -x px4 2>/dev/null || true
+    wait 2>/dev/null
+    echo "All processes stopped."
+}
+
+trap cleanup SIGINT SIGTERM
+
+# --- Kill existing instances ---
+echo "Killing running instances..."
+pkill -x px4 2>/dev/null || true
+sleep 1
+
+# --- Start XRCE-DDS agent (if available) ---
+if command -v MicroXRCEAgent &>/dev/null; then
+    echo "Starting MicroXRCEAgent on UDP port 8888..."
+    MicroXRCEAgent udp4 -p 8888 > /dev/null 2>&1 &
+    AGENT_PID=$!
+    echo "XRCE-DDS agent PID: $AGENT_PID"
+else
+    echo "WARNING: MicroXRCEAgent not found on host."
+    echo "  Start it separately (e.g. from Docker container):"
+    echo "  MicroXRCEAgent udp4 -p 8888"
+fi
+echo ""
+
+# --- Port map ---
+printf "%-10s %-12s %-14s %-14s\n" "Instance" "MAV_SYS_ID" "DDS Namespace" "Offboard Port"
+printf "%-10s %-12s %-14s %-14s\n" "--------" "----------" "-------------" "-------------"
+n=0
+while [ $n -lt "$DRONE_COUNT" ]; do
+    sysid=$((n + 1))
+    printf "%-10s %-12s %-14s %-14s\n" "$n" "$sysid" "px4_${n}" "$((14540 + n))"
+    n=$((n + 1))
+done
+echo ""
+
+# --- Launch instances ---
+export PX4_SIM_MODEL=sihsim_quadx_vision
+
+n=0
+while [ $n -lt "$DRONE_COUNT" ]; do
+    eval "FX=\$DRONE_${n}_X"
+    eval "FY=\$DRONE_${n}_Y"
+    eval "FZ=\$DRONE_${n}_Z"
+
+    working_dir="$build_path/instance_$n"
+    [ ! -d "$working_dir" ] && mkdir -p "$working_dir"
+
+    export PX4_HOME_LAT="$ORIGIN_LAT"
+    export PX4_HOME_LON="$ORIGIN_LON"
+    export PX4_HOME_ALT="$ORIGIN_ALT"
+    export PX4_FORMATION_X="$FX"
+    export PX4_FORMATION_Y="$FY"
+    export PX4_FORMATION_Z="$FZ"
+    export PX4_TARGET_X="$TARGET_X"
+    export PX4_TARGET_Y="$TARGET_Y"
+    export PX4_TARGET_Z="$TARGET_Z"
+    export PX4_SIM_SPEED_FACTOR="$SPEED_FACTOR"
+    # Force consistent DDS namespace for all instances (including 0)
+    export PX4_UXRCE_DDS_NS="px4_$n"
+
+    pushd "$working_dir" &>/dev/null
+    echo "Starting instance $n (formation: $FX, $FY, $FZ)"
+    "$build_path/bin/px4" -i "$n" -d "$build_path/etc" >out.log 2>err.log &
+    CHILD_PIDS+=($!)
+    popd &>/dev/null
+
+    n=$((n + 1))
+done
+
+echo ""
+echo "All $DRONE_COUNT instances launched. Press Ctrl-C to stop."
+wait

Tools/simulation/swarm_formation.yaml

@@ -0,0 +1,32 @@
+# Formation config for SIH swarm simulation
+# Used by sih_swarm_run.sh and swarm_ros2 coordinator
+
+origin:
+  lat: 47.397742
+  lon: 8.545594
+  alt: 488.0
+
+formation:
+  # NED offsets from origin (meters). 4x4 grid, 1m spacing, at 20m altitude
+  - {id: 0,  x: 0.0, y: 0.0, z: -20.0}
+  - {id: 1,  x: 0.0, y: 1.0, z: -20.0}
+  - {id: 2,  x: 0.0, y: 2.0, z: -20.0}
+  - {id: 3,  x: 0.0, y: 3.0, z: -20.0}
+  - {id: 4,  x: 1.0, y: 0.0, z: -20.0}
+  - {id: 5,  x: 1.0, y: 1.0, z: -20.0}
+  - {id: 6,  x: 1.0, y: 2.0, z: -20.0}
+  - {id: 7,  x: 1.0, y: 3.0, z: -20.0}
+  - {id: 8,  x: 2.0, y: 0.0, z: -20.0}
+  - {id: 9,  x: 2.0, y: 1.0, z: -20.0}
+  - {id: 10, x: 2.0, y: 2.0, z: -20.0}
+  - {id: 11, x: 2.0, y: 3.0, z: -20.0}
+  - {id: 12, x: 3.0, y: 0.0, z: -20.0}
+  - {id: 13, x: 3.0, y: 1.0, z: -20.0}
+  - {id: 14, x: 3.0, y: 2.0, z: -20.0}
+  - {id: 15, x: 3.0, y: 3.0, z: -20.0}
+
+target:
+  # Car position: 200m north of origin, ground level
+  x: 200.0
+  y: 0.0
+  z: 0.0

Tools/simulation/swarm_formation_designer.py

@@ -0,0 +1,485 @@
+#!/usr/bin/env python3
+"""
+Swarm Formation Designer - Create and preview formation YAML files for PX4 SIH swarm simulation.
+
+Generates swarm_formation.yaml files with preset formation patterns (grid, circle, line,
+v, diamond, random) for use with sih_swarm_run.sh and swarm_ros2 coordinator.
+
+Usage examples:
+    # 16-drone grid with 2m spacing, preview in terminal
+    python3 swarm_formation_designer.py --pattern grid --count 16 --spacing 2.0 --preview
+
+    # 8-drone circle, radius 10m, write to file
+    python3 swarm_formation_designer.py --pattern circle --count 8 --radius 10 -o formation.yaml
+
+    # V formation with custom origin and target
+    python3 swarm_formation_designer.py --pattern v --count 7 --spacing 3.0 \\
+        --origin-lat 37.4 --origin-lon -122.0 --target-north 150 --target-east 50
+"""
+
+import argparse
+import math
+import random
+import sys
+from typing import List, Tuple
+
+# Defaults matching PX4 SIH test site (Zurich area)
+DEFAULT_LAT = 47.397742
+DEFAULT_LON = 8.545594
+DEFAULT_ALT = 488.0
+DEFAULT_ALTITUDE = 20.0
+DEFAULT_SPACING = 1.0
+DEFAULT_RADIUS = 5.0
+DEFAULT_COUNT = 16
+DEFAULT_TARGET_NORTH = 200.0
+DEFAULT_TARGET_EAST = 0.0
+MIN_SEPARATION = 0.5  # meters, minimum allowed distance between any two drones
+
+
+def generate_grid(count: int, spacing: float, altitude: float) -> List[dict]:
+    """Arrange drones in a square grid pattern (ceil(sqrt(count)) columns)."""
+    cols = math.ceil(math.sqrt(count))
+    positions = []
+    for i in range(count):
+        row = i // cols
+        col = i % cols
+        positions.append({
+            "id": i,
+            "x": round(row * spacing, 2),
+            "y": round(col * spacing, 2),
+            "z": round(-altitude, 2),
+        })
+    return positions
+
+
+def generate_circle(count: int, radius: float, altitude: float) -> List[dict]:
+    """Arrange drones evenly spaced around a circle."""
+    positions = []
+    for i in range(count):
+        angle = 2.0 * math.pi * i / count
+        positions.append({
+            "id": i,
+            "x": round(radius * math.cos(angle), 2),
+            "y": round(radius * math.sin(angle), 2),
+            "z": round(-altitude, 2),
+        })
+    return positions
+
+
+def generate_line(count: int, spacing: float, altitude: float) -> List[dict]:
+    """Arrange drones in a line along the east (y) axis, centered at origin."""
+    positions = []
+    offset = (count - 1) * spacing / 2.0
+    for i in range(count):
+        positions.append({
+            "id": i,
+            "x": 0.0,
+            "y": round(i * spacing - offset, 2),
+            "z": round(-altitude, 2),
+        })
+    return positions
+
+
+def generate_v(count: int, spacing: float, altitude: float) -> List[dict]:
+    """Classic V formation with leader at front (northernmost position).
+
+    Leader is id 0 at the tip. Remaining drones alternate left/right,
+    each row stepping back (south) and outward (east/west).
+    """
+    positions = [{"id": 0, "x": 0.0, "y": 0.0, "z": round(-altitude, 2)}]
+    wing_index = 1
+    for i in range(1, count):
+        side = 1 if (i % 2 == 1) else -1
+        row = (i + 1) // 2
+        positions.append({
+            "id": wing_index,
+            "x": round(-row * spacing, 2),       # step back (south)
+            "y": round(side * row * spacing, 2),  # spread east/west
+            "z": round(-altitude, 2),
+        })
+        wing_index += 1
+    return positions
+
+
+def generate_diamond(count: int, spacing: float, altitude: float) -> List[dict]:
+    """Diamond/rhombus formation.
+
+    Builds concentric diamond rings outward from the center. Ring k has 4*k
+    positions (except the center which has 1). Drones fill from center outward.
+    """
+    # Generate diamond coordinates ring by ring
+    coords: List[Tuple[float, float]] = [(0.0, 0.0)]
+    ring = 1
+    while len(coords) < count:
+        # Each ring has 4 sides, each side has 'ring' segments
+        for side in range(4):
+            for step in range(ring):
+                if side == 0:   # top-right edge
+                    x = (ring - step) * spacing
+                    y = step * spacing
+                elif side == 1: # bottom-right edge
+                    x = -step * spacing
+                    y = (ring - step) * spacing
+                elif side == 2: # bottom-left edge
+                    x = -(ring - step) * spacing
+                    y = -step * spacing
+                else:           # top-left edge
+                    x = step * spacing
+                    y = -(ring - step) * spacing
+                coords.append((round(x, 2), round(y, 2)))
+                if len(coords) >= count:
+                    break
+            if len(coords) >= count:
+                break
+        ring += 1
+
+    positions = []
+    for i in range(count):
+        positions.append({
+            "id": i,
+            "x": coords[i][0],
+            "y": coords[i][1],
+            "z": round(-altitude, 2),
+        })
+    return positions
+
+
+def generate_random(count: int, spacing: float, altitude: float) -> List[dict]:
+    """Random positions within a bounding box, enforcing minimum separation.
+
+    The bounding box size scales with count and spacing. Uses rejection
+    sampling to guarantee minimum separation between all drones.
+    """
+    box_size = max(spacing * math.sqrt(count) * 2, spacing * 4)
+    positions = []
+    coords: List[Tuple[float, float]] = []
+    max_attempts = count * 1000
+
+    rng = random.Random(42)  # deterministic for reproducibility
+    attempts = 0
+    while len(coords) < count and attempts < max_attempts:
+        x = round(rng.uniform(-box_size / 2, box_size / 2), 2)
+        y = round(rng.uniform(-box_size / 2, box_size / 2), 2)
+        too_close = False
+        for cx, cy in coords:
+            if math.sqrt((x - cx) ** 2 + (y - cy) ** 2) < MIN_SEPARATION:
+                too_close = True
+                break
+        if not too_close:
+            coords.append((x, y))
+        attempts += 1
+
+    if len(coords) < count:
+        print(
+            f"Warning: could only place {len(coords)}/{count} drones "
+            f"with minimum separation {MIN_SEPARATION}m in bounding box {box_size:.1f}m",
+            file=sys.stderr,
+        )
+
+    for i, (x, y) in enumerate(coords):
+        positions.append({
+            "id": i,
+            "x": x,
+            "y": y,
+            "z": round(-altitude, 2),
+        })
+    return positions
+
+
+PATTERN_GENERATORS = {
+    "grid": generate_grid,
+    "circle": generate_circle,
+    "line": generate_line,
+    "v": generate_v,
+    "diamond": generate_diamond,
+    "random": generate_random,
+}
+
+
+def validate_no_overlap(positions: List[dict]) -> bool:
+    """Check that no two drones occupy the same horizontal position (within MIN_SEPARATION)."""
+    for i in range(len(positions)):
+        for j in range(i + 1, len(positions)):
+            dx = positions[i]["x"] - positions[j]["x"]
+            dy = positions[i]["y"] - positions[j]["y"]
+            dist = math.sqrt(dx * dx + dy * dy)
+            if dist < MIN_SEPARATION:
+                print(
+                    f"Error: drones {positions[i]['id']} and {positions[j]['id']} "
+                    f"are only {dist:.2f}m apart (minimum: {MIN_SEPARATION}m)",
+                    file=sys.stderr,
+                )
+                return False
+    return True
+
+
+def build_formation_dict(
+    positions: List[dict],
+    origin_lat: float,
+    origin_lon: float,
+    origin_alt: float,
+    target_north: float,
+    target_east: float,
+) -> dict:
+    """Build the formation dictionary matching swarm_formation.yaml structure."""
+    return {
+        "origin": {
+            "lat": origin_lat,
+            "lon": origin_lon,
+            "alt": origin_alt,
+        },
+        "formation": positions,
+        "target": {
+            "x": target_north,
+            "y": target_east,
+            "z": 0.0,
+        },
+    }
+
+
+def format_yaml(data: dict) -> str:
+    """Format the formation data as YAML with flow-style formation entries.
+
+    Produces output matching the established swarm_formation.yaml style with
+    inline dicts for formation entries and block style for origin/target.
+    """
+    lines = [
+        "# Formation config for SIH swarm simulation",
+        "# Generated by swarm_formation_designer.py",
+        "",
+        "origin:",
+        f"  lat: {data['origin']['lat']}",
+        f"  lon: {data['origin']['lon']}",
+        f"  alt: {data['origin']['alt']}",
+        "",
+        "formation:",
+    ]
+
+    # Determine padding width for aligned id field
+    max_id = max(p["id"] for p in data["formation"])
+    id_width = len(str(max_id))
+
+    for pos in data["formation"]:
+        id_str = str(pos["id"]).rjust(id_width)
+        lines.append(
+            f"  - {{id: {id_str}, "
+            f"x: {pos['x']}, "
+            f"y: {pos['y']}, "
+            f"z: {pos['z']}}}"
+        )
+
+    lines.extend([
+        "",
+        "target:",
+        f"  x: {data['target']['x']}",
+        f"  y: {data['target']['y']}",
+        f"  z: {data['target']['z']}",
+        "",
+    ])
+    return "\n".join(lines)
+
+
+def ascii_preview(positions: List[dict], target_north: float, target_east: float) -> str:
+    """Render an ASCII top-down map showing drone positions and target.
+
+    North is up, East is right. Axes are labeled. The target is shown as 'T'
+    and drones as their id number (or '*' if id >= 10).
+    """
+    width = 60
+    height = 30
+
+    # Collect all points (drones + target)
+    all_x = [p["x"] for p in positions] + [target_north]
+    all_y = [p["y"] for p in positions] + [target_east]
+
+    min_x, max_x = min(all_x), max(all_x)
+    min_y, max_y = min(all_y), max(all_y)
+
+    # Add margin
+    range_x = max_x - min_x if max_x != min_x else 1.0
+    range_y = max_y - min_y if max_y != min_y else 1.0
+    margin = 0.1
+    min_x -= range_x * margin
+    max_x += range_x * margin
+    min_y -= range_y * margin
+    max_y += range_y * margin
+    range_x = max_x - min_x
+    range_y = max_y - min_y
+
+    def to_grid(north: float, east: float) -> Tuple[int, int]:
+        col = int((east - min_y) / range_y * (width - 1))
+        row = int((max_x - north) / range_x * (height - 1))  # north is up
+        col = max(0, min(width - 1, col))
+        row = max(0, min(height - 1, row))
+        return row, col
+
+    # Initialize grid
+    grid = [[" " for _ in range(width)] for _ in range(height)]
+
+    # Place target
+    tr, tc = to_grid(target_north, target_east)
+    grid[tr][tc] = "T"
+
+    # Place drones (overwrite target if on same cell)
+    for pos in positions:
+        r, c = to_grid(pos["x"], pos["y"])
+        label = str(pos["id"]) if pos["id"] < 10 else "*"
+        grid[r][c] = label
+
+    # Build output
+    out = []
+    out.append(f"  Formation Preview ({len(positions)} drones)")
+    out.append(f"  North ^  (x range: {min_x:.1f} to {max_x:.1f} m)")
+    out.append("  " + "-" * (width + 2))
+    for row in grid:
+        out.append("  |" + "".join(row) + "|")
+    out.append("  " + "-" * (width + 2))
+    out.append(f"  East ->  (y range: {min_y:.1f} to {max_y:.1f} m)")
+    out.append("")
+    out.append("  Legend: 0-9 = drone id, * = drone id >= 10, T = target")
+    return "\n".join(out)
+
+
+def parse_args(argv: "List[str] | None" = None) -> argparse.Namespace:
+    parser = argparse.ArgumentParser(
+        description="Create and preview swarm formation YAML files for PX4 SIH simulation.",
+        formatter_class=argparse.RawDescriptionHelpFormatter,
+        epilog=(
+            "Examples:\n"
+            "  %(prog)s --pattern grid --count 16 --spacing 2 --preview\n"
+            "  %(prog)s --pattern circle --count 8 --radius 10 -o my_formation.yaml\n"
+            "  %(prog)s --pattern v --count 7 --spacing 3 --preview -o v_formation.yaml\n"
+        ),
+    )
+    parser.add_argument(
+        "--pattern",
+        choices=list(PATTERN_GENERATORS.keys()),
+        default="grid",
+        help="Formation pattern (default: grid)",
+    )
+    parser.add_argument(
+        "--count",
+        type=int,
+        default=DEFAULT_COUNT,
+        help=f"Number of drones (default: {DEFAULT_COUNT})",
+    )
+    parser.add_argument(
+        "--spacing",
+        type=float,
+        default=DEFAULT_SPACING,
+        help=f"Spacing between drones in meters (default: {DEFAULT_SPACING})",
+    )
+    parser.add_argument(
+        "--altitude",
+        type=float,
+        default=DEFAULT_ALTITUDE,
+        help=f"Flight altitude in meters AGL (default: {DEFAULT_ALTITUDE})",
+    )
+    parser.add_argument(
+        "--radius",
+        type=float,
+        default=DEFAULT_RADIUS,
+        help=f"Radius for circle pattern in meters (default: {DEFAULT_RADIUS})",
+    )
+    parser.add_argument(
+        "--origin-lat",
+        type=float,
+        default=DEFAULT_LAT,
+        help=f"Origin latitude (default: {DEFAULT_LAT})",
+    )
+    parser.add_argument(
+        "--origin-lon",
+        type=float,
+        default=DEFAULT_LON,
+        help=f"Origin longitude (default: {DEFAULT_LON})",
+    )
+    parser.add_argument(
+        "--origin-alt",
+        type=float,
+        default=DEFAULT_ALT,
+        help=f"Origin altitude AMSL in meters (default: {DEFAULT_ALT})",
+    )
+    parser.add_argument(
+        "--target-north",
+        type=float,
+        default=DEFAULT_TARGET_NORTH,
+        help=f"Target position north of origin in meters (default: {DEFAULT_TARGET_NORTH})",
+    )
+    parser.add_argument(
+        "--target-east",
+        type=float,
+        default=DEFAULT_TARGET_EAST,
+        help=f"Target position east of origin in meters (default: {DEFAULT_TARGET_EAST})",
+    )
+    parser.add_argument(
+        "--preview",
+        action="store_true",
+        help="Show ASCII preview of the formation",
+    )
+    parser.add_argument(
+        "-o", "--output",
+        type=str,
+        default=None,
+        help="Output YAML file path (default: print to stdout)",
+    )
+
+    args = parser.parse_args(argv)
+
+    if args.count < 1:
+        parser.error("--count must be at least 1")
+    if args.spacing <= 0:
+        parser.error("--spacing must be positive")
+    if args.altitude <= 0:
+        parser.error("--altitude must be positive")
+    if args.radius <= 0:
+        parser.error("--radius must be positive")
+
+    return args
+
+
+def main(argv: "List[str] | None" = None) -> int:
+    args = parse_args(argv)
+
+    # Select generator and dispatch with appropriate arguments
+    generator = PATTERN_GENERATORS[args.pattern]
+    if args.pattern == "circle":
+        positions = generator(args.count, args.radius, args.altitude)
+    else:
+        positions = generator(args.count, args.spacing, args.altitude)
+
+    # Validate
+    if not validate_no_overlap(positions):
+        return 1
+
+    # Build output
+    data = build_formation_dict(
+        positions,
+        args.origin_lat,
+        args.origin_lon,
+        args.origin_alt,
+        args.target_north,
+        args.target_east,
+    )
+    yaml_text = format_yaml(data)
+
+    # Preview
+    if args.preview:
+        print(ascii_preview(positions, args.target_north, args.target_east))
+        print()
+
+    # Output
+    if args.output:
+        with open(args.output, "w") as f:
+            f.write(yaml_text)
+        print(f"Wrote {len(positions)} drone formation to {args.output}")
+    else:
+        if not args.preview:
+            print(yaml_text, end="")
+        else:
+            print(yaml_text, end="")
+
+    return 0
+
+
+if __name__ == "__main__":
+    sys.exit(main())

Tools/simulation/swarm_ros2/.gitignore

@@ -0,0 +1,2 @@
+# Staging directory created by build_docker.sh (ephemeral)
+_px4_msgs_defs/

Tools/simulation/swarm_ros2/Dockerfile

@@ -0,0 +1,95 @@
+# ROS 2 Jazzy layer on top of PX4 dev environment
+# For swarm simulation with px4-ros2-interface-lib
+#
+# Build (from PX4-Autopilot root):
+#   Tools/simulation/swarm_ros2/build_docker.sh
+#
+# Run:
+#   docker run -it --rm -v /path/to/PX4-Autopilot:/px4 px4-swarm-ros2
+#
+FROM px4io/px4-dev:v1.17.0-rc1
+LABEL maintainer="Ramon Roche <mrpollo@gmail.com>"
+
+ENV DEBIAN_FRONTEND=noninteractive
+ENV ROS_DISTRO=jazzy
+
+# Add ROS 2 Jazzy repository (Ubuntu 24.04 = noble)
+RUN apt-get update && apt-get install -y --no-install-recommends \
+        curl \
+        gnupg \
+        lsb-release \
+        software-properties-common \
+    && curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \
+        -o /usr/share/keyrings/ros-archive-keyring.gpg \
+    && echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] \
+        http://packages.ros.org/ros2/ubuntu noble main" \
+        > /etc/apt/sources.list.d/ros2.list \
+    && apt-get update \
+    && apt-get install -y --no-install-recommends \
+        ros-jazzy-ros-base \
+        ros-jazzy-rmw-cyclonedds-cpp \
+        python3-colcon-common-extensions \
+        python3-rosdep \
+        python3-numpy \
+    && rm -rf /var/lib/apt/lists/*
+
+# Initialize rosdep
+RUN rosdep init 2>/dev/null || true \
+    && su user -c "rosdep update --rosdistro jazzy" 2>/dev/null || true
+
+# Set ROS 2 middleware to CycloneDDS (better multi-vehicle support)
+ENV RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
+
+# Build Micro XRCE-DDS Agent from source
+RUN git clone --depth 1 https://github.com/eProsima/Micro-XRCE-DDS-Agent.git /tmp/xrce-agent \
+    && cd /tmp/xrce-agent \
+    && mkdir build && cd build \
+    && cmake .. -DCMAKE_BUILD_TYPE=Release \
+    && make -j$(nproc) \
+    && make install \
+    && ldconfig \
+    && rm -rf /tmp/xrce-agent
+
+# Build px4_msgs from PX4 source tree msg definitions (ensures type hash match).
+# The build_docker.sh script copies msg/ and srv/ into _px4_msgs_defs/ before build.
+ENV ROS2_WS=/opt/ros2_ws
+COPY _px4_msgs_defs/ /tmp/px4_msg_defs/
+RUN mkdir -p ${ROS2_WS}/src \
+    && cd ${ROS2_WS}/src \
+    && git clone --depth 1 https://github.com/PX4/px4_msgs.git \
+    && rm -rf px4_msgs/msg/*.msg px4_msgs/srv/*.srv \
+    && cp /tmp/px4_msg_defs/msg/*.msg px4_msgs/msg/ \
+    && cp /tmp/px4_msg_defs/srv/*.srv px4_msgs/srv/ \
+    && rm -rf /tmp/px4_msg_defs \
+    && cd ${ROS2_WS} \
+    && . /opt/ros/jazzy/setup.sh \
+    && colcon build --packages-select px4_msgs --cmake-args -DCMAKE_BUILD_TYPE=Release \
+    && rm -rf log
+
+# Build px4-ros2-interface-lib (depends on px4_msgs built above)
+RUN cd ${ROS2_WS}/src \
+    && git clone --depth 1 --recursive https://github.com/Auterion/px4-ros2-interface-lib.git \
+    && cd ${ROS2_WS} \
+    && . /opt/ros/jazzy/setup.sh \
+    && . install/setup.sh \
+    && colcon build --packages-skip px4_msgs --cmake-args -DCMAKE_BUILD_TYPE=Release \
+    && rm -rf log src
+
+# Copy swarm_ros2 package and build it
+COPY . ${ROS2_WS}/src/swarm_ros2/
+RUN cd ${ROS2_WS} \
+    && . /opt/ros/jazzy/setup.sh \
+    && . install/setup.sh \
+    && colcon build --packages-select swarm_ros2 \
+    && rm -rf log
+
+# Source everything on shell entry
+RUN echo "source /opt/ros/jazzy/setup.bash" >> /etc/bash.bashrc \
+    && echo "source ${ROS2_WS}/install/setup.bash" >> /etc/bash.bashrc
+
+# XRCE-DDS UDP port
+EXPOSE 8888/udp
+
+WORKDIR ${ROS2_WS}
+
+CMD ["/bin/bash"]

Tools/simulation/swarm_ros2/build_docker.sh

@@ -0,0 +1,35 @@
+#!/bin/bash
+# Build the px4-swarm-ros2 Docker image with px4_msgs matching this PX4 commit.
+# Run from anywhere; the script locates PX4 root automatically.
+#
+# Usage: ./build_docker.sh
+
+set -eo pipefail
+
+SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
+PX4_ROOT="$( cd "${SCRIPT_DIR}/../../.." && pwd )"
+
+echo "PX4 root: ${PX4_ROOT}"
+echo "Staging px4_msgs definitions from this PX4 commit..."
+
+# Stage msg/srv files into the Docker build context
+STAGING_DIR="${SCRIPT_DIR}/_px4_msgs_defs"
+rm -rf "${STAGING_DIR}"
+mkdir -p "${STAGING_DIR}/msg" "${STAGING_DIR}/srv"
+
+cp "${PX4_ROOT}"/msg/*.msg "${STAGING_DIR}/msg/"
+cp "${PX4_ROOT}"/msg/versioned/*.msg "${STAGING_DIR}/msg/"
+cp "${PX4_ROOT}"/srv/*.srv "${STAGING_DIR}/srv/"
+
+echo "Staged $(ls "${STAGING_DIR}/msg/" | wc -l | tr -d ' ') .msg files and $(ls "${STAGING_DIR}/srv/" | wc -l | tr -d ' ') .srv files"
+echo ""
+echo "Building Docker image..."
+
+docker build -t px4-swarm-ros2 "${SCRIPT_DIR}"
+
+# Clean up staged files
+rm -rf "${STAGING_DIR}"
+
+echo ""
+echo "Done. Run with:"
+echo "  docker run -it --rm -v ${PX4_ROOT}:/px4 px4-swarm-ros2 bash /px4/Tools/simulation/swarm_ros2/run_swarm.sh 4"

Tools/simulation/swarm_ros2/launch/swarm_launch.py

@@ -0,0 +1,80 @@
+"""
+Launch file for the PX4 swarm coordination stack.
+
+Starts one vision_provider node per drone and a single swarm_controller node.
+
+Usage:
+    ros2 launch swarm_ros2 swarm_launch.py num_drones:=16 speed_factor:=1.0
+"""
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    # Declare launch arguments.
+    num_drones_arg = DeclareLaunchArgument(
+        'num_drones',
+        default_value='16',
+        description='Number of drones in the swarm',
+    )
+    speed_factor_arg = DeclareLaunchArgument(
+        'speed_factor',
+        default_value='1.0',
+        description='Simulation speed factor',
+    )
+    formation_config_arg = DeclareLaunchArgument(
+        'formation_config',
+        default_value='',
+        description='Path to YAML formation configuration file',
+    )
+
+    num_drones = LaunchConfiguration('num_drones')
+    speed_factor = LaunchConfiguration('speed_factor')
+
+    # We need a concrete integer to iterate, so we support a fixed max and
+    # conditionally launch.  ROS 2 launch does not natively support dynamic
+    # loops over LaunchConfiguration integers, so we generate nodes for a
+    # reasonable upper bound and rely on the controller's num_drones param
+    # to limit actual usage.  For simplicity, generate exactly 16 nodes
+    # (the default).  For other counts, users can call this launch file
+    # programmatically or adjust the constant below.
+    MAX_DRONES = 16
+
+    vision_nodes = []
+    for i in range(MAX_DRONES):
+        ns = f'px4_{i}'
+        vision_nodes.append(
+            Node(
+                package='swarm_ros2',
+                executable='vision_provider',
+                name=f'vision_provider_{i}',
+                parameters=[{
+                    'namespace': ns,
+                    'instance_id': i,
+                }],
+                output='screen',
+            )
+        )
+
+    # Single swarm controller.
+    controller_node = Node(
+        package='swarm_ros2',
+        executable='swarm_controller',
+        name='swarm_controller',
+        parameters=[{
+            'num_drones': num_drones,
+            'speed_factor': speed_factor,
+        }],
+        output='screen',
+    )
+
+    return LaunchDescription([
+        num_drones_arg,
+        speed_factor_arg,
+        formation_config_arg,
+        *vision_nodes,
+        controller_node,
+    ])

Tools/simulation/swarm_ros2/package.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>swarm_ros2</name>
+  <version>0.1.0</version>
+  <description>Swarm coordination for PX4 multi-vehicle simulation using px4-ros2-interface-lib concepts</description>
+  <maintainer email="mrpollo@gmail.com">Ramon Roche</maintainer>
+  <license>BSD-3-Clause</license>
+
+  <depend>rclpy</depend>
+  <depend>px4_msgs</depend>
+  <depend>px4_ros2_cpp</depend>
+  <depend>std_msgs</depend>
+  <depend>geometry_msgs</depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

Tools/simulation/swarm_ros2/run_swarm.sh

@@ -0,0 +1,99 @@
+#!/bin/bash
+# Run the full swarm test inside Docker: PX4 SIH + XRCE-DDS + ROS 2
+# Usage: ./run_swarm.sh [NUM_DRONES] [SPEED_FACTOR]
+#
+# Must be run inside the px4-swarm-ros2 container with PX4 source mounted at /px4:
+#   docker run -it --rm -v /path/to/PX4-Autopilot:/px4 px4-swarm-ros2 /opt/ros2_ws/src/swarm_ros2/run_swarm.sh 4
+
+set -eo pipefail
+
+NUM_DRONES="${1:-4}"
+SPEED_FACTOR="${2:-1}"
+PX4_SRC="${PX4_SRC:-/px4}"
+BUILD_DIR="${PX4_SRC}/build/px4_sitl_sih"
+
+echo "=== PX4 SIH Swarm Test ==="
+echo "  Drones: $NUM_DRONES"
+echo "  Speed factor: $SPEED_FACTOR"
+echo ""
+
+# Check PX4 build exists
+if [ ! -f "$BUILD_DIR/bin/px4" ]; then
+    echo "PX4 binary not found at $BUILD_DIR/bin/px4"
+    echo "Building PX4 (this may take a few minutes)..."
+    cd "$PX4_SRC"
+    make px4_sitl_sih
+fi
+
+# Source ROS 2
+source /opt/ros/jazzy/setup.bash
+source /opt/ros2_ws/install/setup.bash
+
+# Cleanup handler
+PIDS=()
+cleanup() {
+    echo ""
+    echo "Shutting down..."
+    for pid in "${PIDS[@]}"; do
+        kill "$pid" 2>/dev/null || true
+    done
+    pkill -x px4 2>/dev/null || true
+    pkill -x MicroXRCEAgent 2>/dev/null || true
+    wait 2>/dev/null
+    echo "Done."
+}
+trap cleanup EXIT SIGINT SIGTERM
+
+# Kill any existing px4
+pkill -x px4 2>/dev/null || true
+sleep 1
+
+# Start XRCE-DDS agent
+echo "Starting XRCE-DDS agent on port 8888..."
+MicroXRCEAgent udp4 -p 8888 > /dev/null 2>&1 &
+PIDS+=($!)
+sleep 1
+
+# Launch PX4 instances
+export PX4_SIM_MODEL=sihsim_quadx_vision
+export PX4_SIM_SPEED_FACTOR="$SPEED_FACTOR"
+
+# Clean stale instance data (old parameters.bson can override param set-default)
+echo "Cleaning stale instance data..."
+for n in $(seq 0 $((NUM_DRONES - 1))); do
+    rm -rf "$BUILD_DIR/instance_$n"
+done
+
+echo "Launching $NUM_DRONES PX4 SIH instances..."
+for n in $(seq 0 $((NUM_DRONES - 1))); do
+    working_dir="$BUILD_DIR/instance_$n"
+    mkdir -p "$working_dir"
+
+    pushd "$working_dir" &>/dev/null
+    # Force consistent DDS namespace for all instances (including 0)
+    # so ROS 2 nodes can use px4_0, px4_1, ... uniformly
+    PX4_UXRCE_DDS_NS="px4_$n" "$BUILD_DIR/bin/px4" -i "$n" -d "$BUILD_DIR/etc" >out.log 2>err.log &
+    PIDS+=($!)
+    popd &>/dev/null
+    echo "  Instance $n started (PID ${PIDS[-1]})"
+done
+
+# Wait for PX4 instances to initialize
+echo ""
+echo "Waiting 5s for PX4 instances to initialize..."
+sleep 5
+
+# Launch ROS 2 swarm (blocks until mission completes)
+echo ""
+echo "Launching ROS 2 swarm controller..."
+echo "=========================================="
+ros2 launch swarm_ros2 swarm_launch.py num_drones:="$NUM_DRONES"
+
+echo ""
+echo "=== Mission complete ==="
+
+# Collect ULogs
+echo "Collecting ULog files..."
+python3 "$PX4_SRC/Tools/simulation/collect_swarm_ulogs.py" \
+    --build-dir "$BUILD_DIR" \
+    --output-dir "$PX4_SRC/swarm_ulogs_$(date +%Y%m%d_%H%M%S)"

Tools/simulation/swarm_ros2/setup.cfg

@@ -0,0 +1,5 @@
+[develop]
+script_dir=$base/lib/swarm_ros2
+
+[install]
+install_scripts=$base/lib/swarm_ros2

Tools/simulation/swarm_ros2/setup.py

@@ -0,0 +1,29 @@
+from setuptools import setup
+import os
+from glob import glob
+
+package_name = 'swarm_ros2'
+
+setup(
+    name=package_name,
+    version='0.1.0',
+    packages=[package_name],
+    data_files=[
+        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        (os.path.join('share', package_name, 'launch'), glob('launch/*.py')),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='Ramon Roche',
+    maintainer_email='mrpollo@gmail.com',
+    description='Swarm coordination for PX4 multi-vehicle simulation',
+    license='BSD-3-Clause',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'vision_provider = swarm_ros2.vision_provider:main',
+            'swarm_controller = swarm_ros2.swarm_controller:main',
+        ],
+    },
+)

Tools/simulation/swarm_ros2/swarm_ros2/swarm_controller.py

@@ -0,0 +1,599 @@
+"""
+Swarm controller node for PX4 multi-vehicle coordination.
+
+Manages all drones through a phased state machine:
+  1. Wait for EV lock
+  2. Arm + takeoff to formation
+  3. Hold grid formation
+  4. Transition to circle via cosine interpolation
+  5. Hold circle and rotate
+  6. Kamikaze dive to target
+
+Uses PX4 offboard control via px4_msgs topics.
+"""
+
+import math
+import os
+from enum import IntEnum, auto
+
+import numpy as np
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
+
+from px4_msgs.msg import (
+    OffboardControlMode,
+    TrajectorySetpoint,
+    VehicleCommand,
+    VehicleLocalPosition,
+    VehicleStatus,
+)
+
+
+PX4_QOS = QoSProfile(
+    reliability=ReliabilityPolicy.BEST_EFFORT,
+    durability=DurabilityPolicy.TRANSIENT_LOCAL,
+    history=HistoryPolicy.KEEP_LAST,
+    depth=1,
+)
+
+# Volatile QoS for subscriptions to avoid stale data from before
+# namespace discovery is complete.
+PX4_SUB_QOS = QoSProfile(
+    reliability=ReliabilityPolicy.BEST_EFFORT,
+    durability=DurabilityPolicy.VOLATILE,
+    history=HistoryPolicy.KEEP_LAST,
+    depth=1,
+)
+
+
+class Phase(IntEnum):
+    """State machine phases."""
+    WAIT_EV_LOCK = auto()
+    ARM_TAKEOFF = auto()
+    HOLD_GRID = auto()
+    TRANSITION_CIRCLE = auto()
+    HOLD_CIRCLE = auto()
+    KAMIKAZE = auto()
+    DONE = auto()
+
+
+class DroneState:
+    """Per-drone bookkeeping."""
+
+    def __init__(self, drone_id: int, namespace: str):
+        self.drone_id = drone_id
+        self.namespace = namespace
+
+        # Telemetry
+        self.xy_valid = False
+        self.z_valid = False
+        self.position = np.zeros(3)  # NED [x, y, z]
+        self.has_initial_pos = False
+        self._init_samples: list = []  # Buffer for initial position consensus
+        self.armed = False
+        self.nav_state = 0
+
+        # Formation target (grid)
+        self.grid_pos = np.zeros(3)
+
+        # Circle target
+        self.circle_angle = 0.0
+
+        # Mission status
+        self.impacted = False
+
+
+class SwarmController(Node):
+    """Coordinates N drones through the swarm mission phases."""
+
+    # Phase durations in seconds
+    EV_LOCK_TIMEOUT = 15.0
+    TAKEOFF_DURATION = 20.0
+    HOLD_GRID_DURATION = 10.0
+    TRANSITION_DURATION = 15.0
+    HOLD_CIRCLE_DURATION = 10.0
+    KAMIKAZE_TIMEOUT = 120.0
+
+    # Circle parameters
+    CIRCLE_RADIUS = 4.0       # metres
+    CIRCLE_SPEED = 3.0        # m/s tangential
+
+    # Kamikaze parameters
+    KAMIKAZE_SPEED = 12.0     # m/s (MPC_XY_VEL_MAX)
+    IMPACT_DIST = 2.0         # metres
+    IMPACT_Z_THRESH = -1.0    # NED (z > -1 means within 1m of ground)
+
+    # PX4 command constants
+    VEHICLE_CMD_COMPONENT_ARM_DISARM = 400
+    VEHICLE_CMD_DO_SET_MODE = 176
+
+    CONTROL_HZ = 20.0
+
+    def __init__(self):
+        super().__init__('swarm_controller')
+
+        # Parameters
+        self.declare_parameter('num_drones', 16)
+        self.declare_parameter('speed_factor', 1.0)
+        self.declare_parameter('target_x', 200.0)
+        self.declare_parameter('target_y', 0.0)
+        self.declare_parameter('target_z', 0.0)
+        self.declare_parameter('takeoff_altitude', -5.0)  # NED, negative = up
+
+        self._num_drones = self.get_parameter('num_drones').value
+        self._speed_factor = self.get_parameter('speed_factor').value
+
+        # Kamikaze target (NED)
+        self._target = np.array([
+            float(os.environ.get('PX4_KAMIKAZE_X',
+                                 self.get_parameter('target_x').value)),
+            float(os.environ.get('PX4_KAMIKAZE_Y',
+                                 self.get_parameter('target_y').value)),
+            float(os.environ.get('PX4_KAMIKAZE_Z',
+                                 self.get_parameter('target_z').value)),
+        ])
+
+        takeoff_alt = self.get_parameter('takeoff_altitude').value
+
+        self.get_logger().info(
+            f'Swarm controller: {self._num_drones} drones, '
+            f'speed_factor={self._speed_factor}, '
+            f'target={self._target.tolist()}'
+        )
+
+        # Build grid formation (4x4 or NxM, 2m spacing, centered at origin).
+        cols = int(math.ceil(math.sqrt(self._num_drones)))
+        rows = int(math.ceil(self._num_drones / cols))
+        grid_center_x = (cols - 1) / 2.0
+        grid_center_y = (rows - 1) / 2.0
+
+        # ------------------------------------------------------------------ #
+        # Per-drone state and ROS interfaces
+        # ------------------------------------------------------------------ #
+        self._drones: list[DroneState] = []
+        self._pub_offboard: list[rclpy.publisher.Publisher] = []
+        self._pub_traj: list[rclpy.publisher.Publisher] = []
+        self._pub_cmd: list[rclpy.publisher.Publisher] = []
+
+        for i in range(self._num_drones):
+            ns = f'px4_{i}'
+            drone = DroneState(i, ns)
+
+            # Grid position: 2m spacing, centered, at takeoff altitude.
+            col = i % cols
+            row = i // cols
+            drone.grid_pos = np.array([
+                (col - grid_center_x) * 2.0,
+                (row - grid_center_y) * 2.0,
+                float(takeoff_alt),
+            ])
+
+            # Circle angle for this drone.
+            drone.circle_angle = 2.0 * math.pi * i / self._num_drones
+
+            self._drones.append(drone)
+
+            # Publishers
+            self._pub_offboard.append(
+                self.create_publisher(
+                    OffboardControlMode,
+                    f'/{ns}/fmu/in/offboard_control_mode',
+                    PX4_QOS,
+                )
+            )
+            self._pub_traj.append(
+                self.create_publisher(
+                    TrajectorySetpoint,
+                    f'/{ns}/fmu/in/trajectory_setpoint',
+                    PX4_QOS,
+                )
+            )
+            self._pub_cmd.append(
+                self.create_publisher(
+                    VehicleCommand,
+                    f'/{ns}/fmu/in/vehicle_command',
+                    PX4_QOS,
+                )
+            )
+
+            # Subscribers
+            # PX4 XRCE-DDS uses versioned topic names: _v<MESSAGE_VERSION>
+            # VehicleLocalPosition: MESSAGE_VERSION=1 -> _v1
+            # VehicleStatus: MESSAGE_VERSION=2 -> _v2
+            self.create_subscription(
+                VehicleLocalPosition,
+                f'/{ns}/fmu/out/vehicle_local_position_v1',
+                lambda msg, idx=i: self._local_pos_cb(idx, msg),
+                PX4_SUB_QOS,
+            )
+            self.create_subscription(
+                VehicleStatus,
+                f'/{ns}/fmu/out/vehicle_status_v2',
+                lambda msg, idx=i: self._vehicle_status_cb(idx, msg),
+                PX4_SUB_QOS,
+            )
+
+        # ------------------------------------------------------------------ #
+        # State machine
+        # ------------------------------------------------------------------ #
+        self._phase = Phase.WAIT_EV_LOCK
+        self._phase_start_time = self.get_clock().now()
+        self._mission_start_time = self.get_clock().now()
+        self._circle_center = np.array([0.0, 0.0, float(takeoff_alt)])
+        self._last_ev_print = 0
+
+        # Main control timer
+        period_s = 1.0 / self.CONTROL_HZ
+        self._timer = self.create_timer(period_s, self._control_loop)
+
+    # ===================================================================== #
+    # Subscriber callbacks
+    # ===================================================================== #
+    def _local_pos_cb(self, idx: int, msg: VehicleLocalPosition):
+        d = self._drones[idx]
+        d.xy_valid = msg.xy_valid
+        d.z_valid = msg.z_valid
+
+        new_pos = np.array([msg.x, msg.y, msg.z])
+
+        if not d.has_initial_pos:
+            if not (d.xy_valid and d.z_valid):
+                return
+            # All drones start near origin. Reject samples > 20m from origin
+            # during initialization (DDS cross-talk from other instances).
+            if np.linalg.norm(new_pos) > 20.0:
+                return
+            # Require 5 consistent samples within 5m to establish initial pos.
+            d._init_samples.append(new_pos)
+            if len(d._init_samples) >= 5:
+                ref = d._init_samples[-1]
+                consistent = all(
+                    np.linalg.norm(s - ref) < 5.0
+                    for s in d._init_samples[-5:]
+                )
+                if consistent:
+                    d.position = new_pos
+                    d.has_initial_pos = True
+                else:
+                    d._init_samples = d._init_samples[-1:]
+            return
+
+        # Guard against DDS cross-talk: reject position jumps > 50m/tick.
+        # At 50Hz and 12m/s max, real movement is < 0.3m per tick.
+        jump = np.linalg.norm(new_pos - d.position)
+        if jump > 50.0:
+            return  # Discard cross-talk sample
+
+        d.position = new_pos
+
+    def _vehicle_status_cb(self, idx: int, msg: VehicleStatus):
+        d = self._drones[idx]
+        d.armed = (msg.arming_state == VehicleStatus.ARMING_STATE_ARMED)
+        d.nav_state = msg.nav_state
+
+    # ===================================================================== #
+    # Publishing helpers
+    # ===================================================================== #
+    def _publish_offboard_mode(self, idx: int, *, position: bool = False,
+                               velocity: bool = False):
+        """Send OffboardControlMode for drone idx."""
+        msg = OffboardControlMode()
+        msg.timestamp = int(self.get_clock().now().nanoseconds / 1000)
+        msg.position = position
+        msg.velocity = velocity
+        msg.acceleration = False
+        msg.attitude = False
+        msg.body_rate = False
+        self._pub_offboard[idx].publish(msg)
+
+    def _publish_trajectory(self, idx: int, *,
+                            position: list[float] | None = None,
+                            velocity: list[float] | None = None,
+                            yaw: float = float('nan'),
+                            yawspeed: float = float('nan')):
+        """Send TrajectorySetpoint for drone idx."""
+        msg = TrajectorySetpoint()
+        msg.timestamp = int(self.get_clock().now().nanoseconds / 1000)
+
+        nan3 = [float('nan')] * 3
+        msg.position = list(position) if position is not None else nan3
+        msg.velocity = list(velocity) if velocity is not None else nan3
+        msg.yaw = yaw
+        msg.yawspeed = yawspeed
+        self._pub_traj[idx].publish(msg)
+
+    def _publish_vehicle_command(self, idx: int, command: int,
+                                 param1: float = 0.0, param2: float = 0.0,
+                                 param3: float = 0.0):
+        """Send VehicleCommand for drone idx."""
+        msg = VehicleCommand()
+        msg.timestamp = int(self.get_clock().now().nanoseconds / 1000)
+        msg.command = command
+        msg.param1 = param1
+        msg.param2 = param2
+        msg.param3 = param3
+        msg.target_system = idx + 1       # MAV_SYS_ID
+        msg.target_component = 1
+        msg.source_system = 255           # GCS
+        msg.source_component = 0
+        msg.from_external = True
+        self._pub_cmd[idx].publish(msg)
+
+    def _arm(self, idx: int):
+        self._publish_vehicle_command(
+            idx, self.VEHICLE_CMD_COMPONENT_ARM_DISARM, param1=1.0)
+
+    def _disarm_force(self, idx: int):
+        self._publish_vehicle_command(
+            idx, self.VEHICLE_CMD_COMPONENT_ARM_DISARM,
+            param1=0.0, param2=21196.0)
+
+    def _set_offboard_mode(self, idx: int):
+        self._publish_vehicle_command(
+            idx, self.VEHICLE_CMD_DO_SET_MODE,
+            param1=1.0, param2=6.0, param3=0.0)
+
+    # ===================================================================== #
+    # Phase elapsed time helper
+    # ===================================================================== #
+    def _phase_elapsed(self) -> float:
+        """Seconds since current phase started."""
+        return (self.get_clock().now() - self._phase_start_time).nanoseconds / 1e9
+
+    def _mission_elapsed(self) -> float:
+        """Seconds since mission started."""
+        return (self.get_clock().now() - self._mission_start_time).nanoseconds / 1e9
+
+    def _advance_phase(self, new_phase: Phase):
+        self.get_logger().info(f'Phase transition: {self._phase.name} -> {new_phase.name}')
+        self._phase = new_phase
+        self._phase_start_time = self.get_clock().now()
+
+    # ===================================================================== #
+    # Main control loop (20 Hz)
+    # ===================================================================== #
+    def _control_loop(self):
+        if self._phase == Phase.DONE:
+            return
+
+        if self._phase == Phase.WAIT_EV_LOCK:
+            self._phase_wait_ev_lock()
+        elif self._phase == Phase.ARM_TAKEOFF:
+            self._phase_arm_takeoff()
+        elif self._phase == Phase.HOLD_GRID:
+            self._phase_hold_grid()
+        elif self._phase == Phase.TRANSITION_CIRCLE:
+            self._phase_transition_circle()
+        elif self._phase == Phase.HOLD_CIRCLE:
+            self._phase_hold_circle()
+        elif self._phase == Phase.KAMIKAZE:
+            self._phase_kamikaze()
+
+    # ------------------------------------------------------------------ #
+    # Phase 1: Wait for EV lock
+    # ------------------------------------------------------------------ #
+    def _phase_wait_ev_lock(self):
+        locked = sum(1 for d in self._drones if d.xy_valid and d.z_valid)
+        elapsed = self._phase_elapsed()
+
+        # Print status every second.
+        elapsed_int = int(elapsed)
+        if elapsed_int > self._last_ev_print:
+            self._last_ev_print = elapsed_int
+            self.get_logger().info(
+                f'{locked}/{self._num_drones} drones have EV lock '
+                f'({elapsed:.0f}s elapsed)')
+
+        if locked == self._num_drones or elapsed > self.EV_LOCK_TIMEOUT:
+            if locked < self._num_drones:
+                self.get_logger().warn(
+                    f'EV lock timeout: only {locked}/{self._num_drones} locked')
+            self._advance_phase(Phase.ARM_TAKEOFF)
+
+    # ------------------------------------------------------------------ #
+    # Phase 2: Arm + takeoff
+    # ------------------------------------------------------------------ #
+    def _phase_arm_takeoff(self):
+        elapsed = self._phase_elapsed()
+
+        for i in range(self._num_drones):
+            d = self._drones[i]
+
+            # Keep sending offboard + setpoint before and after arming.
+            self._publish_offboard_mode(i, position=True)
+            self._publish_trajectory(
+                i,
+                position=d.grid_pos.tolist(),
+                yaw=0.0,
+            )
+
+            # Arm and set offboard mode repeatedly for the first few seconds.
+            if elapsed < 5.0:
+                self._set_offboard_mode(i)
+                self._arm(i)
+
+        # Log status every 5 seconds.
+        elapsed_int = int(elapsed)
+        if elapsed_int > 0 and elapsed_int % 5 == 0 and elapsed - elapsed_int < 0.1:
+            armed = sum(1 for d in self._drones if d.armed)
+            self.get_logger().info(
+                f'ARM_TAKEOFF {elapsed:.0f}s: {armed}/{self._num_drones} armed, '
+                f'positions: ' + ', '.join(
+                    f'D{i}=[{d.position[2]:.1f}m]'
+                    for i, d in enumerate(self._drones)
+                )
+            )
+
+        if elapsed > self.TAKEOFF_DURATION:
+            self._advance_phase(Phase.HOLD_GRID)
+
+    # ------------------------------------------------------------------ #
+    # Phase 3: Hold grid
+    # ------------------------------------------------------------------ #
+    def _phase_hold_grid(self):
+        for i in range(self._num_drones):
+            d = self._drones[i]
+            self._publish_offboard_mode(i, position=True)
+            self._publish_trajectory(
+                i,
+                position=d.grid_pos.tolist(),
+                yaw=0.0,
+            )
+
+        if self._phase_elapsed() > self.HOLD_GRID_DURATION:
+            # Compute circle center as the mean of grid positions.
+            mean_pos = np.mean([d.grid_pos for d in self._drones], axis=0)
+            self._circle_center = mean_pos.copy()
+            self._advance_phase(Phase.TRANSITION_CIRCLE)
+
+    # ------------------------------------------------------------------ #
+    # Phase 4: Transition to circle (cosine interpolation)
+    # ------------------------------------------------------------------ #
+    def _phase_transition_circle(self):
+        elapsed = self._phase_elapsed()
+        alpha = 0.5 - 0.5 * math.cos(math.pi * min(elapsed / self.TRANSITION_DURATION, 1.0))
+
+        for i in range(self._num_drones):
+            d = self._drones[i]
+
+            # Target circle position.
+            cx = self._circle_center[0] + self.CIRCLE_RADIUS * math.cos(d.circle_angle)
+            cy = self._circle_center[1] + self.CIRCLE_RADIUS * math.sin(d.circle_angle)
+            cz = self._circle_center[2]
+            circle_pos = np.array([cx, cy, cz])
+
+            # Interpolate between grid and circle.
+            target = (1.0 - alpha) * d.grid_pos + alpha * circle_pos
+
+            # Yaw facing center.
+            yaw = math.atan2(
+                self._circle_center[1] - target[1],
+                self._circle_center[0] - target[0],
+            )
+
+            self._publish_offboard_mode(i, position=True)
+            self._publish_trajectory(i, position=target.tolist(), yaw=yaw)
+
+        if elapsed > self.TRANSITION_DURATION:
+            self._advance_phase(Phase.HOLD_CIRCLE)
+
+    # ------------------------------------------------------------------ #
+    # Phase 5: Hold circle (rotate CW)
+    # ------------------------------------------------------------------ #
+    def _phase_hold_circle(self):
+        elapsed = self._phase_elapsed()
+        omega = self.CIRCLE_SPEED / self.CIRCLE_RADIUS  # rad/s
+
+        for i in range(self._num_drones):
+            d = self._drones[i]
+            # CW rotation: subtract omega*t from angle.
+            angle = d.circle_angle - omega * elapsed
+
+            px = self._circle_center[0] + self.CIRCLE_RADIUS * math.cos(angle)
+            py = self._circle_center[1] + self.CIRCLE_RADIUS * math.sin(angle)
+            pz = self._circle_center[2]
+
+            # Yaw facing center.
+            yaw = math.atan2(
+                self._circle_center[1] - py,
+                self._circle_center[0] - px,
+            )
+
+            self._publish_offboard_mode(i, position=True)
+            self._publish_trajectory(
+                i,
+                position=[px, py, pz],
+                yaw=yaw,
+            )
+
+        if elapsed > self.HOLD_CIRCLE_DURATION:
+            self._advance_phase(Phase.KAMIKAZE)
+
+    # ------------------------------------------------------------------ #
+    # Phase 6: Kamikaze dive
+    # ------------------------------------------------------------------ #
+    def _phase_kamikaze(self):
+        all_done = True
+        elapsed = self._phase_elapsed()
+
+        for i in range(self._num_drones):
+            d = self._drones[i]
+
+            if d.impacted:
+                continue
+            all_done = False
+
+            # Direction to target.
+            to_target = self._target - d.position
+            dist = np.linalg.norm(to_target)
+
+            # Impact detection.
+            if dist < self.IMPACT_DIST and d.position[2] > self.IMPACT_Z_THRESH:
+                self.get_logger().info(
+                    f'[Drone {i}] Impact! pos=[{d.position[0]:.1f}, '
+                    f'{d.position[1]:.1f}, {d.position[2]:.1f}]')
+                d.impacted = True
+                self._disarm_force(i)
+                continue
+
+            # Velocity toward target.
+            if dist > 0.01:
+                direction = to_target / dist
+            else:
+                direction = np.array([1.0, 0.0, 0.0])
+            vel = (direction * self.KAMIKAZE_SPEED).tolist()
+
+            # Yaw toward target.
+            yaw = math.atan2(to_target[1], to_target[0])
+
+            self._publish_offboard_mode(i, velocity=True)
+            self._publish_trajectory(i, velocity=vel, yaw=yaw)
+
+        # Log kamikaze progress every 10 seconds.
+        elapsed_int = int(elapsed)
+        if elapsed_int > 0 and elapsed_int % 10 == 0 and elapsed - elapsed_int < 0.1:
+            for i, d in enumerate(self._drones):
+                if not d.impacted:
+                    dist = np.linalg.norm(self._target - d.position)
+                    self.get_logger().info(
+                        f'KAMIKAZE {elapsed:.0f}s: D{i} pos=[{d.position[0]:.1f}, '
+                        f'{d.position[1]:.1f}, {d.position[2]:.1f}] '
+                        f'dist={dist:.1f}m armed={d.armed}')
+
+        if all_done or elapsed > self.KAMIKAZE_TIMEOUT:
+            self._print_summary()
+            self._advance_phase(Phase.DONE)
+            # Shutdown after a short delay to let final messages flush.
+            self.create_timer(1.0, lambda: self._shutdown())
+
+    def _print_summary(self):
+        impacted = sum(1 for d in self._drones if d.impacted)
+        total_time = self._mission_elapsed()
+        self.get_logger().info('=' * 50)
+        self.get_logger().info('SWARM MISSION SUMMARY')
+        self.get_logger().info(f'  Drones: {self._num_drones}')
+        self.get_logger().info(f'  Impacts: {impacted}/{self._num_drones}')
+        self.get_logger().info(f'  Total time: {total_time:.1f}s')
+        self.get_logger().info('=' * 50)
+
+    def _shutdown(self):
+        self.get_logger().info('Swarm controller shutting down.')
+        self._timer.cancel()
+        raise SystemExit(0)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = SwarmController()
+    try:
+        rclpy.spin(node)
+    except (KeyboardInterrupt, SystemExit):
+        node.get_logger().info('Swarm controller exiting.')
+    finally:
+        node.destroy_node()
+        rclpy.try_shutdown()
+
+
+if __name__ == '__main__':
+    main()

Tools/simulation/swarm_ros2/swarm_ros2/vision_provider.py

@@ -0,0 +1,156 @@
+"""
+Vision provider node for PX4 EKF2 external vision fusion.
+
+Subscribes to ground truth local position and publishes noisy visual odometry
+estimates, simulating an external vision system (e.g., motion capture, VIO).
+
+One instance runs per drone. The node decimates to 30 Hz and adds configurable
+Gaussian noise to position and velocity.
+"""
+
+import numpy as np
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
+
+from px4_msgs.msg import VehicleLocalPosition as VehicleLocalPositionGroundtruth
+from px4_msgs.msg import VehicleOdometry
+
+
+# QoS profile matching PX4 uORB-to-DDS bridge defaults.
+PX4_QOS = QoSProfile(
+    reliability=ReliabilityPolicy.BEST_EFFORT,
+    durability=DurabilityPolicy.TRANSIENT_LOCAL,
+    history=HistoryPolicy.KEEP_LAST,
+    depth=1,
+)
+
+
+class VisionProvider(Node):
+    """Publishes noisy VehicleOdometry derived from ground truth."""
+
+    # Noise standard deviations
+    POS_NOISE_STD = 0.5       # metres
+    ANG_VEL_NOISE_STD = 0.1   # rad/s (unused but kept for completeness)
+
+    # Output rate target
+    OUTPUT_HZ = 30.0
+    OUTPUT_PERIOD_US = int(1e6 / OUTPUT_HZ)
+
+    def __init__(self):
+        # Declare parameters before calling super().__init__ so they are
+        # available immediately.
+        super().__init__('vision_provider')
+
+        self.declare_parameter('namespace', 'px4_0')
+        self.declare_parameter('instance_id', 0)
+
+        self._namespace = self.get_parameter('namespace').value
+        self._instance_id = self.get_parameter('instance_id').value
+
+        self.get_logger().info(
+            f'Starting vision_provider_{self._instance_id} '
+            f'for namespace /{self._namespace}'
+        )
+        self.get_logger().warn(
+            'Note: vehicle_local_position_groundtruth is NOT published '
+            'over DDS by PX4. This node is a no-op when using '
+            'sensor_agp_sim (SENS_EN_AGPSIM=1) for position. '
+            'It will only produce output if EV fusion is enabled and '
+            'ground truth is published via a custom DDS bridge.'
+        )
+
+        self._last_pub_time_us: int = 0
+        self._msg_count: int = 0
+        self._rng = np.random.default_rng()
+
+        # -- Subscriber: ground truth local position --
+        # Note: PX4 SIH does not publish this topic over XRCE-DDS.
+        # With sensor_agp_sim, EKF2 gets position internally.
+        gt_topic = f'/{self._namespace}/fmu/out/vehicle_local_position_groundtruth'
+        self._sub_gt = self.create_subscription(
+            VehicleLocalPositionGroundtruth,
+            gt_topic,
+            self._gt_callback,
+            PX4_QOS,
+        )
+
+        # -- Publisher: visual odometry for EKF2 --
+        vo_topic = f'/{self._namespace}/fmu/in/vehicle_visual_odometry'
+        self._pub_vo = self.create_publisher(VehicleOdometry, vo_topic, PX4_QOS)
+
+    # --------------------------------------------------------------------- #
+    # Callback
+    # --------------------------------------------------------------------- #
+    def _gt_callback(self, msg: VehicleLocalPositionGroundtruth):
+        """Decimate ground truth and publish noisy visual odometry."""
+        now_us = int(self.get_clock().now().nanoseconds / 1000)
+
+        # Rate-limit to OUTPUT_HZ.
+        if (now_us - self._last_pub_time_us) < self.OUTPUT_PERIOD_US:
+            return
+        self._last_pub_time_us = now_us
+
+        # Build VehicleOdometry message.
+        vo = VehicleOdometry()
+        vo.timestamp = now_us
+        vo.timestamp_sample = msg.timestamp
+
+        # Frames: NED
+        vo.pose_frame = VehicleOdometry.POSE_FRAME_NED
+        vo.velocity_frame = VehicleOdometry.VELOCITY_FRAME_NED
+
+        # Position with Gaussian noise.
+        noise_pos = self._rng.normal(0.0, self.POS_NOISE_STD, size=3).astype(np.float32)
+        vo.position = [
+            msg.x + float(noise_pos[0]),
+            msg.y + float(noise_pos[1]),
+            msg.z + float(noise_pos[2]),
+        ]
+
+        # Quaternion (pass through, no noise on orientation).
+        vo.q = [msg.heading, 0.0, 0.0, 0.0]
+        # If ground truth provides a full quaternion, prefer that. The
+        # VehicleLocalPosition message only carries heading; construct a
+        # yaw-only quaternion.
+        half_yaw = msg.heading / 2.0
+        vo.q = [
+            float(np.cos(half_yaw)),
+            0.0,
+            0.0,
+            float(np.sin(half_yaw)),
+        ]
+
+        # Velocity with small noise.
+        noise_vel = self._rng.normal(0.0, 0.1, size=3).astype(np.float32)
+        vo.velocity = [
+            msg.vx + float(noise_vel[0]),
+            msg.vy + float(noise_vel[1]),
+            msg.vz + float(noise_vel[2]),
+        ]
+
+        # Angular velocity (not used, set to zero).
+        vo.angular_velocity = [0.0, 0.0, 0.0]
+
+        # Variances (squared standard deviations).
+        vo.position_variance = [0.25, 0.25, 0.25]       # 0.5^2
+        vo.orientation_variance = [0.01, 0.01, 0.01]
+        vo.velocity_variance = [0.1, 0.1, 0.1]
+
+        self._pub_vo.publish(vo)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = VisionProvider()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        node.get_logger().info('Shutting down vision provider.')
+    finally:
+        node.destroy_node()
+        rclpy.try_shutdown()
+
+
+if __name__ == '__main__':
+    main()

boards/accton-godwit/ga1/src/board_config.h

@@ -221,8 +221,10 @@
 /* HEATER
  * PWM in future
  */
-#define GPIO_HEATER_OUTPUT   /* PB10  T2CH3 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
-#define HEATER_OUTPUT_EN(on_true)	       px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true))
+#define GPIO_HEATER_OUTPUT
+#define HEATER_NUM 	1
+#define GPIO_HEATER1_OUTPUT  /* PB10  T2CH3 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
+#define HEATER1_OUTPUT_EN(on_true)	       px4_arch_gpiowrite(GPIO_HEATER1_OUTPUT, (on_true))
 
 /* PE6 is nARMED
  *  The GPIO will be set as input while not armed HW will have external HW Pull UP.
@@ -442,7 +444,7 @@
 		GPIO_CAN1_RX,                     \
 		GPIO_CAN2_TX,                     \
 		GPIO_CAN2_RX,                     \
-		GPIO_HEATER_OUTPUT,               \
+		GPIO_HEATER1_OUTPUT,              \
 		GPIO_nPOWER_IN_A,                 \
 		GPIO_nPOWER_IN_B,                 \
 		GPIO_nPOWER_IN_C,                 \

boards/ark/fmu-v6x/default.px4board

@@ -28,12 +28,8 @@ CONFIG_DRIVERS_IMU_INVENSENSE_IIM42653=y
 CONFIG_DRIVERS_IMU_MURATA_SCH16T=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_DRIVERS_MAGNETOMETER_BOSCH_BMM150=y
-CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y
-CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y
-CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8308=y
 CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y
 CONFIG_DRIVERS_MAGNETOMETER_LIS3MDL=y
-CONFIG_DRIVERS_MAGNETOMETER_LSM303AGR=y
 CONFIG_DRIVERS_MAGNETOMETER_RM3100=y
 CONFIG_DRIVERS_MAGNETOMETER_ST_IIS2MDC=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y

boards/ark/fmu-v6x/init/rc.board_defaults

@@ -17,21 +17,21 @@ param set-default MAV_2_UDP_PRT 14550
 param set-default SENS_EN_INA226 1
 param set-default SENS_EN_THERMAL 1
 param set-default SENS_IMU_MODE 1
-param set-default SENS_IMU_TEMP 10.0
-#param set-default SENS_IMU_TEMP_FF 0.0
-#param set-default SENS_IMU_TEMP_I 0.025
-#param set-default SENS_IMU_TEMP_P 1.0
+param set-default HEATER1_TEMP 10.0
+#param set-default HEATER1_TEMP_FF 0.0
+#param set-default HEATER1_TEMP_I 0.025
+#param set-default HEATER1_TEMP_P 1.0
 
 param set-default UAVCAN_ESC_IFACE 2
 
 if ver hwtypecmp ARKV6X000
 then
-	param set-default SENS_TEMP_ID 2818058
+	param set-default HEATER1_IMU_ID 2818058
 fi
 
 if ver hwtypecmp ARKV6X001
 then
-	param set-default SENS_TEMP_ID 3014666
+	param set-default HEATER1_IMU_ID 3014666
 fi
 
 safety_button start

boards/ark/fmu-v6x/init/rc.board_sensors

@@ -100,7 +100,7 @@ bmp388 -I start
 # Start an external PWM generator
 if param greater PCA9685_EN_BUS 0
 then
-	pca9685_pwm_out start
+	pca9685_pwm_out start -X
 fi
 
 unset HAVE_PM2

boards/ark/fmu-v6x/src/board_config.h

@@ -224,8 +224,10 @@
 /* HEATER
  * PWM in future
  */
-#define GPIO_HEATER_OUTPUT   /* PB10  T2CH3 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
-#define HEATER_OUTPUT_EN(on_true)	       px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true))
+#define GPIO_HEATER_OUTPUT
+#define HEATER_NUM 	1
+#define GPIO_HEATER1_OUTPUT  /* PB10  T2CH3 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
+#define HEATER1_OUTPUT_EN(on_true)	       px4_arch_gpiowrite(GPIO_HEATER1_OUTPUT, (on_true))
 
 /* PE6 is nARMED
  *  The GPIO will be set as input while not armed HW will have external HW Pull UP.
@@ -243,6 +245,15 @@
  */
 #define DIRECT_PWM_OUTPUT_CHANNELS   9
 
+#define GPIO_FMU_CH1                    /* PI0  */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTI|GPIO_PIN0)
+#define GPIO_FMU_CH2                    /* PH12 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTH|GPIO_PIN12)
+#define GPIO_FMU_CH3                    /* PH11 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTH|GPIO_PIN11)
+#define GPIO_FMU_CH4                    /* PH10 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTH|GPIO_PIN10)
+#define GPIO_FMU_CH5                    /* PD13 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTD|GPIO_PIN13)
+#define GPIO_FMU_CH6                    /* PD14 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTD|GPIO_PIN14)
+#define GPIO_FMU_CH7                    /* PH6  */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTH|GPIO_PIN6)
+#define GPIO_FMU_CH8                    /* PH9  */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTH|GPIO_PIN9)
+
 #define GPIO_FMU_CAP                    /* PE11 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTE|GPIO_PIN11)
 #define GPIO_SPIX_SYNC                  /* PE9  */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTE|GPIO_PIN9)
 
@@ -436,7 +447,7 @@
 		GPIO_CAN1_RX,                     \
 		GPIO_CAN2_TX,                     \
 		GPIO_CAN2_RX,                     \
-		GPIO_HEATER_OUTPUT,               \
+		GPIO_HEATER1_OUTPUT,              \
 		GPIO_nPOWER_IN_A,                 \
 		GPIO_nPOWER_IN_B,                 \
 		GPIO_nPOWER_IN_C,                 \
@@ -456,6 +467,14 @@
 		GPIO_SAFETY_SWITCH_IN,            \
 		GPIO_PG6,                         \
 		GPIO_nARMED_INIT,                 \
+		GPIO_FMU_CH1,     	          \
+		GPIO_FMU_CH2,     	          \
+		GPIO_FMU_CH3,     	          \
+		GPIO_FMU_CH4,     	          \
+		GPIO_FMU_CH5,     	          \
+		GPIO_FMU_CH6,     	          \
+		GPIO_FMU_CH7,     	          \
+		GPIO_FMU_CH8,     	          \
 		GPIO_FMU_CAP,     	          \
 		GPIO_SPIX_SYNC                    \
 	}

boards/ark/fpv/init/rc.board_defaults

@@ -15,14 +15,14 @@ fi
 
 # TODO: Tune the following parameters
 param set-default SENS_EN_THERMAL 1
-param set-default SENS_IMU_TEMP 10.0
-#param set-default SENS_IMU_TEMP_FF 0.0
-#param set-default SENS_IMU_TEMP_I 0.025
-#param set-default SENS_IMU_TEMP_P 1.0
+param set-default HEATER1_TEMP 10.0
+#param set-default HEATER1_TEMP_FF 0.0
+#param set-default HEATER1_TEMP_I 0.025
+#param set-default HEATER1_TEMP_P 1.0
 
 if ver hwtypecmp ARKFPV000
 then
-	param set-default SENS_TEMP_ID 3014666
+	param set-default HEATER1_IMU_ID 3014666
 fi
 
 param set-default BAT1_V_DIV 21.0

boards/ark/fpv/init/rc.board_sensors

@@ -20,5 +20,5 @@ bmp388 -I -b 2 start
 # Start an external PWM generator
 if param greater PCA9685_EN_BUS 0
 then
-	pca9685_pwm_out start
+	pca9685_pwm_out start -X
 fi

boards/ark/fpv/src/board_config.h

@@ -205,8 +205,10 @@
 /* HEATER
  * PWM in future
  */
-#define GPIO_HEATER_OUTPUT   /* PB10  T2CH3 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
-#define HEATER_OUTPUT_EN(on_true)	       px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true))
+#define GPIO_HEATER_OUTPUT
+#define HEATER_NUM 	1
+#define GPIO_HEATER1_OUTPUT  /* PB10  T2CH3 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
+#define HEATER1_OUTPUT_EN(on_true)	       px4_arch_gpiowrite(GPIO_HEATER1_OUTPUT, (on_true))
 
 /* PE6 is nARMED
  *  The GPIO will be set as input while not armed HW will have external HW Pull UP.
@@ -223,6 +225,16 @@
  */
 #define DIRECT_PWM_OUTPUT_CHANNELS   9
 
+#define GPIO_FMU_CH1                    /* PI0  */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTI|GPIO_PIN0)
+#define GPIO_FMU_CH2                    /* PH12 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTH|GPIO_PIN12)
+#define GPIO_FMU_CH3                    /* PH11 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTH|GPIO_PIN11)
+#define GPIO_FMU_CH4                    /* PH10 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTH|GPIO_PIN10)
+#define GPIO_FMU_CH5                    /* PI5  */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTI|GPIO_PIN5)
+#define GPIO_FMU_CH6                    /* PI6  */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTI|GPIO_PIN6)
+#define GPIO_FMU_CH7                    /* PI7  */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTI|GPIO_PIN7)
+#define GPIO_FMU_CH8                    /* PI2  */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTI|GPIO_PIN2)
+#define GPIO_FMU_CH9                    /* PD12 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTD|GPIO_PIN12)
+
 #define GPIO_SPIX_SYNC                  /* PE9  */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTE|GPIO_PIN9)
 
 /* Power supply control and monitoring GPIOs */
@@ -318,7 +330,7 @@
 		GPIO_HW_VER_REV_DRIVE,            \
 		GPIO_CAN1_TX,                     \
 		GPIO_CAN1_RX,                     \
-		GPIO_HEATER_OUTPUT,               \
+		GPIO_HEATER1_OUTPUT,              \
 		GPIO_VDD_5V_PGOOD,                \
 		GPIO_VDD_12V_PGOOD,               \
 		GPIO_VDD_12V_EN,                  \
@@ -326,6 +338,15 @@
 		GPIO_VDD_3V3_SD_CARD_EN,          \
 		GPIO_nARMED_INIT,                 \
 		SPI6_nRESET_EXTERNAL1,            \
+		GPIO_FMU_CH1,     	          \
+		GPIO_FMU_CH2,     	          \
+		GPIO_FMU_CH3,     	          \
+		GPIO_FMU_CH4,     	          \
+		GPIO_FMU_CH5,     	          \
+		GPIO_FMU_CH6,     	          \
+		GPIO_FMU_CH7,     	          \
+		GPIO_FMU_CH8,     	          \
+		GPIO_FMU_CH9,     	          \
 		GPIO_SPIX_SYNC                    \
 	}
 

boards/ark/pi6x/init/rc.board_defaults

@@ -21,17 +21,17 @@ param set-default SENS_EN_INA226 1
 
 # TODO: Tune the following parameters
 param set-default SENS_EN_THERMAL 1
-param set-default SENS_IMU_TEMP 10.0
-#param set-default SENS_IMU_TEMP_FF 0.0
-#param set-default SENS_IMU_TEMP_I 0.025
-#param set-default SENS_IMU_TEMP_P 1.0
+param set-default HEATER1_TEMP 10.0
+#param set-default HEATER1_TEMP_FF 0.0
+#param set-default HEATER1_TEMP_I 0.025
+#param set-default HEATER1_TEMP_P 1.0
 
 param set-default UAVCAN_ESC_IFACE 1
 
 if ver hwtypecmp ARKPI6X000
 then
 	# TODO: Add the correct sensor ID
-	param set-default SENS_TEMP_ID 2490378
+	param set-default HEATER1_IMU_ID 2490378
 fi
 
 param set-default EKF2_MULTI_IMU 0

boards/ark/pi6x/init/rc.board_sensors

@@ -38,5 +38,5 @@ afbrs50 start
 # Start an external PWM generator
 if param greater PCA9685_EN_BUS 0
 then
-	pca9685_pwm_out start
+	pca9685_pwm_out start -X
 fi

boards/ark/pi6x/src/board_config.h

@@ -188,8 +188,10 @@
 /* HEATER
  * PWM in future
  */
-#define GPIO_HEATER_OUTPUT   /* PB10  T2CH3 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
-#define HEATER_OUTPUT_EN(on_true)	       px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true))
+#define GPIO_HEATER_OUTPUT
+#define HEATER_NUM 	1
+#define GPIO_HEATER1_OUTPUT  /* PB10  T2CH3 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
+#define HEATER1_OUTPUT_EN(on_true)	       px4_arch_gpiowrite(GPIO_HEATER1_OUTPUT, (on_true))
 
 /* PE6 is nARMED
  *  The GPIO will be set as input while not armed HW will have external HW Pull UP.
@@ -206,6 +208,15 @@
  */
 #define DIRECT_PWM_OUTPUT_CHANNELS   8
 
+#define GPIO_FMU_CH1                    /* PI0  */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTI|GPIO_PIN0)
+#define GPIO_FMU_CH2                    /* PH12 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTH|GPIO_PIN12)
+#define GPIO_FMU_CH3                    /* PH11 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTH|GPIO_PIN11)
+#define GPIO_FMU_CH4                    /* PH10 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTH|GPIO_PIN10)
+#define GPIO_FMU_CH5                    /* PD13 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTD|GPIO_PIN13)
+#define GPIO_FMU_CH6                    /* PD14 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTD|GPIO_PIN14)
+#define GPIO_FMU_CH7                    /* PH6  */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTH|GPIO_PIN6)
+#define GPIO_FMU_CH8                    /* PH9  */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTH|GPIO_PIN9)
+
 #define GPIO_FMU_CAP                    /* PE11 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTE|GPIO_PIN11)
 #define GPIO_SPIX_SYNC                  /* PE9  */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTE|GPIO_PIN9)
 
@@ -324,7 +335,7 @@
 		GPIO_HW_VER_REV_DRIVE,            \
 		GPIO_CAN1_TX,                     \
 		GPIO_CAN1_RX,                     \
-		GPIO_HEATER_OUTPUT,               \
+		GPIO_HEATER1_OUTPUT,              \
 		GPIO_VDD_5V_HIPOWER_nEN,          \
 		GPIO_VDD_5V_HIPOWER_nOC,          \
 		GPIO_VDD_3V3_SD_CARD_EN,          \
@@ -332,6 +343,14 @@
 		GPIO_NFC_GPIO,                    \
 		GPIO_TONE_ALARM_IDLE,             \
 		GPIO_nARMED_INIT,                 \
+		GPIO_FMU_CH1,     	          \
+		GPIO_FMU_CH2,     	          \
+		GPIO_FMU_CH3,     	          \
+		GPIO_FMU_CH4,     	          \
+		GPIO_FMU_CH5,     	          \
+		GPIO_FMU_CH6,     	          \
+		GPIO_FMU_CH7,     	          \
+		GPIO_FMU_CH8,     	          \
 		GPIO_FMU_CAP,     	          \
 		GPIO_SPIX_SYNC                    \
 	}

boards/atl/mantis-edu/src/board_config.h

@@ -121,7 +121,9 @@
 #define BOARD_REAR_LED_MASK  (1 << 1) | (1 << 2)
 
 /* HEATER */
-#define GPIO_HEATER_OUTPUT   /* PA7  T14CH1 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7)
+#define GPIO_HEATER_OUTPUT
+#define HEATER_NUM 	1
+#define GPIO_HEATER1_OUTPUT  /* PA7  T14CH1 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7)
 
 #define BOARD_HAS_LED_PWM              1
 #define BOARD_LED_PWM_DRIVE_ACTIVE_LOW 1
@@ -182,7 +184,7 @@
 		PX4_ADC_GPIO,                     \
 		GPIO_HW_REV_DRIVE,                \
 		GPIO_HW_VER_DRIVE,                \
-		GPIO_HEATER_OUTPUT,               \
+		GPIO_HEATER1_OUTPUT,              \
 		GPIO_VDD_3V3_SD_CARD_EN,          \
 		GPIO_OTGFS_VBUS                   \
 	}

boards/auterion/fmu-v6s/init/rc.board_sensors

@@ -77,5 +77,5 @@ ist8310 -X -b 1 -R 10 start
 # Start an external PWM generator
 if param greater PCA9685_EN_BUS 0
 then
-	pca9685_pwm_out start
+	pca9685_pwm_out start -X
 fi

boards/auterion/fmu-v6x/src/board_config.h

@@ -224,8 +224,10 @@
 /* HEATER
  * PWM in future
  */
-#define GPIO_HEATER_OUTPUT   /* PB10  T2CH3 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
-#define HEATER_OUTPUT_EN(on_true)	       px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true))
+#define GPIO_HEATER_OUTPUT
+#define HEATER_NUM 	1
+#define GPIO_HEATER1_OUTPUT  /* PB10  T2CH3 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
+#define HEATER1_OUTPUT_EN(on_true)	       px4_arch_gpiowrite(GPIO_HEATER1_OUTPUT, (on_true))
 
 /* PE6 is nARMED
  *  The GPIO will be set as input while not armed HW will have external HW Pull UP.
@@ -443,7 +445,7 @@
 		GPIO_CAN1_RX,                     \
 		GPIO_CAN2_TX,                     \
 		GPIO_CAN2_RX,                     \
-		GPIO_HEATER_OUTPUT,               \
+		GPIO_HEATER1_OUTPUT,              \
 		GPIO_nPOWER_IN_A,                 \
 		GPIO_nPOWER_IN_B,                 \
 		GPIO_nPOWER_IN_C,                 \

boards/bluerobotics/navigator/init/rc.board_sensors

@@ -28,7 +28,7 @@ then
 	echo "ads1115 not found."
 fi
 
-if ! pca9685_pwm_out start
+if ! pca9685_pwm_out start -X
 then
 	echo "pca9685_pwm_out not found."
 fi

boards/cuav/fmu-v6x/src/board_config.h

@@ -230,8 +230,10 @@
 /* HEATER
  * PWM in future
  */
-#define GPIO_HEATER_OUTPUT   /* PB10  T2CH3 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
-#define HEATER_OUTPUT_EN(on_true)	       px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true))
+#define GPIO_HEATER_OUTPUT
+#define HEATER_NUM 	1
+#define GPIO_HEATER1_OUTPUT  /* PB10  T2CH3 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
+#define HEATER1_OUTPUT_EN(on_true)	       px4_arch_gpiowrite(GPIO_HEATER1_OUTPUT, (on_true))
 
 /* PE6 is nARMED
  *  The GPIO will be set as input while not armed HW will have external HW Pull UP.
@@ -453,7 +455,7 @@
 		GPIO_CAN1_RX,                     \
 		GPIO_CAN2_TX,                     \
 		GPIO_CAN2_RX,                     \
-		GPIO_HEATER_OUTPUT,               \
+		GPIO_HEATER1_OUTPUT,              \
 		GPIO_nPOWER_IN_A,                 \
 		GPIO_nPOWER_IN_B,                 \
 		GPIO_nPOWER_IN_C,                 \

boards/cuav/nora/src/board_config.h

@@ -104,8 +104,10 @@
 #define GPIO_CAN2_SILENT_S1  /* PH3  */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTH|GPIO_PIN3)
 
 /* HEATER */
-#define GPIO_HEATER_OUTPUT   /* PA8 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN8)
-#define HEATER_OUTPUT_EN(on_true)      px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true))
+#define GPIO_HEATER_OUTPUT
+#define HEATER_NUM 	1
+#define GPIO_HEATER1_OUTPUT  /* PA8 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN8)
+#define HEATER1_OUTPUT_EN(on_true)      px4_arch_gpiowrite(GPIO_HEATER1_OUTPUT, (on_true))
 
 /* PWM */
 #define DIRECT_PWM_OUTPUT_CHANNELS   14
@@ -212,7 +214,7 @@
 		GPIO_CAN2_RX,                     \
 		GPIO_CAN1_SILENT_S0,              \
 		GPIO_CAN2_SILENT_S1,              \
-		GPIO_HEATER_OUTPUT,               \
+		GPIO_HEATER1_OUTPUT,              \
 		GPIO_nPOWER_IN_CAN,               \
 		GPIO_nPOWER_IN_ADC,               \
 		GPIO_nPOWER_IN_C,                 \

boards/cuav/x25-evo/CMakeLists.txt

@@ -31,5 +31,4 @@
 #
 ############################################################################
 
-add_subdirectory(core_heater)
 add_subdirectory(pwm_voltage)

boards/cuav/x25-evo/core_heater/CMakeLists.txt

@@ -1,39 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2025 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-px4_add_module(
-	MODULE drivers__core_heater
-	MAIN core_heater
-	COMPILE_FLAGS
-	SRCS
-		core_heater.cpp
-	)

boards/cuav/x25-evo/core_heater/core_heater.cpp

@@ -1,263 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2025 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file core_heater.cpp
- *
- */
-
-#include "core_heater.h"
-
-#include <px4_platform_common/getopt.h>
-#include <px4_platform_common/log.h>
-#include <drivers/drv_hrt.h>
-#include <drivers/drv_io_heater.h>
-
-ModuleBase::Descriptor Core_Heater::desc{task_spawn, custom_command, print_usage};
-
-#  ifndef GPIO_CORE_HEATER_OUTPUT
-#  error "To use the heater driver, the board_config.h must define and initialize GPIO_CORE_HEATER_OUTPUT"
-#  endif
-
-Core_Heater::Core_Heater() :
-	ModuleParams(nullptr),
-	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
-{
-	_heater_status_pub.advertise();
-}
-
-Core_Heater::~Core_Heater()
-{
-	disable_core_heater();
-}
-
-int Core_Heater::custom_command(int argc, char *argv[])
-{
-	// Check if the driver is running.
-	if (!is_running(desc)) {
-		PX4_INFO("not running");
-		return PX4_ERROR;
-	}
-
-	return print_usage("Unrecognized command.");
-}
-
-void Core_Heater::disable_core_heater()
-{
-	// Reset heater to off state.
-	px4_arch_unconfiggpio(GPIO_CORE_HEATER_OUTPUT);
-}
-
-void Core_Heater::initialize_core_heater_io()
-{
-	// Initialize heater to off state.
-	px4_arch_configgpio(GPIO_CORE_HEATER_OUTPUT);
-}
-
-void Core_Heater::core_heater_off()
-{
-	CORE_HEATER_OUTPUT_EN(false);
-}
-
-void Core_Heater::core_heater_on()
-{
-	CORE_HEATER_OUTPUT_EN(true);
-}
-
-bool Core_Heater::initialize_topics()
-{
-	for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
-		uORB::SubscriptionData<sensor_accel_s> sensor_accel_sub{ORB_ID(sensor_accel), i};
-
-		if (sensor_accel_sub.get().timestamp != 0 &&
-		    sensor_accel_sub.get().device_id != 0 &&
-		    PX4_ISFINITE(sensor_accel_sub.get().temperature)) {
-
-			// If the correct ID is found, exit the for-loop with _sensor_accel_sub pointing to the correct instance.
-			if (sensor_accel_sub.get().device_id == (uint32_t)_param_core_temp_id.get()) {
-				_sensor_accel_sub.ChangeInstance(i);
-				_sensor_device_id = sensor_accel_sub.get().device_id;
-				initialize_core_heater_io();
-				return true;
-			}
-		}
-	}
-
-	return false;
-}
-
-void Core_Heater::Run()
-{
-	if (should_exit()) {
-		exit_and_cleanup(desc);
-		return;
-	}
-
-	update_params();
-
-	if (_sensor_device_id == 0) {
-		if (!initialize_topics()) {
-			// if sensor still not found try again in 1 second
-			ScheduleDelayed(1_s);
-			return;
-		}
-	}
-
-	sensor_accel_s sensor_accel;
-	float temperature_delta {0.f};
-
-	if (_core_heater_on) {
-		// Turn the heater off.
-		_core_heater_on = false;
-		core_heater_off();
-		ScheduleDelayed(_controller_period_usec - _controller_time_on_usec);
-
-	} else if (_sensor_accel_sub.update(&sensor_accel)) {
-		// Update the current IMU sensor temperature if valid.
-		if (PX4_ISFINITE(sensor_accel.temperature)) {
-			temperature_delta = _param_core_imu_temp.get() - sensor_accel.temperature;
-			_temperature_last = sensor_accel.temperature;
-		}
-
-		_proportional_value = temperature_delta * _param_core_imu_temp_p.get();
-		_integrator_value += temperature_delta * _param_core_imu_temp_i.get();
-
-		_integrator_value = math::constrain(_integrator_value, -0.25f, 0.25f);
-
-		_controller_time_on_usec = static_cast<int>((_param_core_imu_temp_ff.get() + _proportional_value +
-					   _integrator_value) * static_cast<float>(_controller_period_usec));
-
-		_controller_time_on_usec = math::constrain(_controller_time_on_usec, 0, _controller_period_usec);
-
-		if (fabsf(temperature_delta) < TEMPERATURE_TARGET_THRESHOLD) {
-			_temperature_target_met = true;
-
-		} else {
-
-			_temperature_target_met = false;
-		}
-
-		_core_heater_on = true;
-		core_heater_on();
-		ScheduleDelayed(_controller_time_on_usec);
-	}
-
-	publish_status();
-}
-
-void Core_Heater::publish_status()
-{
-	heater_status_s status{};
-	status.device_id               = _sensor_device_id;
-	status.heater_on               = _core_heater_on;
-	status.temperature_sensor      = _temperature_last;
-	status.temperature_target      = _param_core_imu_temp.get();
-	status.temperature_target_met  = _temperature_target_met;
-	status.controller_period_usec  = _controller_period_usec;
-	status.controller_time_on_usec = _controller_time_on_usec;
-	status.proportional_value      = _proportional_value;
-	status.integrator_value        = _integrator_value;
-	status.feed_forward_value      = _param_core_imu_temp_ff.get();
-
-	status.mode = heater_status_s::MODE_GPIO;
-
-	status.timestamp = hrt_absolute_time();
-	_heater_status_pub.publish(status);
-}
-
-int Core_Heater::start()
-{
-	// Exit the driver if the sensor ID does not match the desired sensor.
-	if (_param_core_temp_id.get() == 0) {
-		PX4_ERR("Valid CORE_TEMP_ID required");
-		request_stop();
-		return PX4_ERROR;
-	}
-
-	update_params(true);
-	ScheduleNow();
-	return PX4_OK;
-}
-
-int Core_Heater::task_spawn(int argc, char *argv[])
-{
-	Core_Heater *core_heater = new Core_Heater();
-
-	if (!core_heater) {
-		PX4_ERR("driver allocation failed");
-		return PX4_ERROR;
-	}
-
-	desc.object.store(core_heater);
-	desc.task_id = task_id_is_work_queue;
-
-	core_heater->start();
-	return 0;
-}
-
-void Core_Heater::update_params(const bool force)
-{
-	if (_parameter_update_sub.updated() || force) {
-		// clear update
-		parameter_update_s param_update;
-		_parameter_update_sub.copy(&param_update);
-
-		// update parameters from storage
-		ModuleParams::updateParams();
-	}
-}
-
-int Core_Heater::print_usage(const char *reason)
-{
-	if (reason) {
-		printf("%s\n\n", reason);
-	}
-
-	PRINT_MODULE_DESCRIPTION(
-		R"DESCR_STR(
-### Description
-Background process running periodically on the LP work queue to regulate IMU temperature at a setpoint.
-
-)DESCR_STR");
-
-	PRINT_MODULE_USAGE_NAME("core_heater", "system");
-	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
-
-	return 0;
-}
-
-extern "C" __EXPORT int core_heater_main(int argc, char *argv[])
-{
-	return ModuleBase::main(Core_Heater::desc, argc, argv);
-}

boards/cuav/x25-evo/core_heater/core_heater.h

@@ -1,161 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2025 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file core_heater.h
- *
- */
-
-#pragma once
-
-#include <px4_platform_common/px4_config.h>
-#include <px4_platform_common/getopt.h>
-#include <px4_platform_common/module.h>
-#include <px4_platform_common/module_params.h>
-#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
-#include <uORB/Publication.hpp>
-#include <uORB/SubscriptionInterval.hpp>
-#include <uORB/topics/heater_status.h>
-#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/sensor_accel.h>
-
-#include <mathlib/mathlib.h>
-
-using namespace time_literals;
-
-#define CONTROLLER_PERIOD_DEFAULT    10000
-#define TEMPERATURE_TARGET_THRESHOLD 2.5f
-
-class Core_Heater : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem
-{
-public:
-	static Descriptor desc;
-
-	Core_Heater();
-
-	virtual ~Core_Heater();
-
-	/**
-	 * @see ModuleBase::custom_command().
-	 * @brief main Main entry point to the module that should be
-	 *        called directly from the module's main method.
-	 * @param argc The input argument count.
-	 * @param argv Pointer to the input argument array.
-	 * @return Returns 0 iff successful, -1 otherwise.
-	 */
-	static int custom_command(int argc, char *argv[]);
-
-	/**
-	 * @see ModuleBase::print_usage().
-	 * @brief Prints the module usage to the nuttshell console.
-	 * @param reason The requested reason for printing to console.
-	 */
-	static int print_usage(const char *reason = nullptr);
-
-	/**
-	 * @see ModuleBase::task_spawn().
-	 * @brief Initializes the class in the same context as the work queue
-	 *        and starts the background listener.
-	 * @param argv Pointer to the input argument array.
-	 * @return Returns 0 iff successful, -1 otherwise.
-	 */
-	static int task_spawn(int argc, char *argv[]);
-
-	/**
-	 * @brief Initiates the heater driver work queue, starts a new background task,
-	 *        and fails if it is already running.
-	 * @return Returns 1 iff start was successful.
-	 */
-	int start();
-
-private:
-
-	/** Disables the heater (either by GPIO). */
-	void disable_core_heater();
-
-	/** Turns the heater on (either by GPIO). */
-	void core_heater_on();
-
-	/** Turns the heater off (either by GPIO). */
-	void core_heater_off();
-
-	void initialize();
-
-	/** Enables / configures the heater (either by GPIO). */
-	void initialize_core_heater_io();
-
-	/** @brief Called once to initialize uORB topics. */
-	bool initialize_topics();
-
-	void publish_status();
-
-	/** @brief Calculates the heater element on/off time and schedules the next cycle. */
-	void Run() override;
-
-	/**
-	 * @brief Updates and checks for updated uORB parameters.
-	 * @param force Boolean to determine if an update check should be forced.
-	 */
-	void update_params(const bool force = false);
-
-	/** Work queue struct for the scheduler. */
-	static struct work_s _work;
-
-	bool _core_heater_initialized     = false;
-	bool _core_heater_on              = false;
-	bool _temperature_target_met = false;
-
-	int _controller_period_usec = CONTROLLER_PERIOD_DEFAULT;
-	int _controller_time_on_usec = 0;
-
-	float _integrator_value   = 0.0f;
-	float _proportional_value = 0.0f;
-
-	uORB::Publication<heater_status_s> _heater_status_pub{ORB_ID(heater_status)};
-
-	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
-
-	uORB::Subscription _sensor_accel_sub{ORB_ID(sensor_accel)};
-
-	uint32_t _sensor_device_id{0};
-
-	float _temperature_last{NAN};
-
-	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::CORE_IMU_TEMP_FF>) _param_core_imu_temp_ff,
-		(ParamFloat<px4::params::CORE_IMU_TEMP_I>)  _param_core_imu_temp_i,
-		(ParamFloat<px4::params::CORE_IMU_TEMP_P>)  _param_core_imu_temp_p,
-		(ParamFloat<px4::params::CORE_IMU_TEMP>)    _param_core_imu_temp,
-		(ParamInt<px4::params::CORE_TEMP_ID>)       _param_core_temp_id
-	)
-};

boards/cuav/x25-evo/init/rc.board_defaults

@@ -20,15 +20,14 @@ param set-default USB_MAV_MODE 5
 param set-default UAVCAN_SUB_GPS 1
 param set-default UAVCAN_SUB_BAT 1
 
-# Enable IMU thermal control
+# IMU thermal control (multi-instance heater)
+# HEATER1_IMU_ID: 2818058(IIM42652 SPI1)
+# HEATER2_IMU_ID: 3014698(IIM42653 SPI5)
 param set-default SENS_EN_THERMAL 1
-param set-default SENS_IMU_TEMP 45
-param set-default SENS_TEMP_ID 2818058
-
-# CUAV core board IMU thermal control
-param set-default CORE_IMU_TEMP 45
-param set-default CORE_TEMP_ID 3014698
-core_heater start
+param set-default HEATER1_IMU_ID 2818058
+param set-default HEATER1_TEMP 45
+param set-default HEATER2_IMU_ID 3014698
+param set-default HEATER2_TEMP 45
 
 # CUAV pwm voltage 3.3V/5V switch
 pwm_voltage_apply start

boards/cuav/x25-evo/src/board_config.h

@@ -175,15 +175,14 @@
 #define GPIO_HW_REV_SENSE      /* PH4 */  GPIO_ADC3_INP15
 #define GPIO_HW_VER_SENSE      /* PH3 */  GPIO_ADC3_INP14
 
-/* HEATER
- * PWM in future
- */
-// IMU BOARD HEATER
-#define GPIO_HEATER_OUTPUT        /* PB10  T2CH3 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
-#define HEATER_OUTPUT_EN(on_true)	       px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true))
-// CORE BOARD HEATER
-#define GPIO_CORE_HEATER_OUTPUT    /* PE6  T15CH2 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN6)
-#define CORE_HEATER_OUTPUT_EN(on_true)	       px4_arch_gpiowrite(GPIO_CORE_HEATER_OUTPUT, (on_true))
+
+/* HEATER */
+#define GPIO_HEATER_OUTPUT
+#define HEATER_NUM	2
+#define GPIO_HEATER1_OUTPUT       /* PB10 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
+#define HEATER1_OUTPUT_EN(on_true)             px4_arch_gpiowrite(GPIO_HEATER1_OUTPUT, (on_true))
+#define GPIO_HEATER2_OUTPUT       /* PE6  */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN6)
+#define HEATER2_OUTPUT_EN(on_true)             px4_arch_gpiowrite(GPIO_HEATER2_OUTPUT, (on_true))
 
 /* PE7 is nARMED
  *  The GPIO will be set as input while not armed HW will have external HW Pull UP.
@@ -403,8 +402,8 @@
 		GPIO_CAN2_RX,                     \
 		GPIO_CAN1_SILENT_S0,		  \
 		GPIO_CAN2_SILENT_S1,              \
-		GPIO_HEATER_OUTPUT,               \
-		GPIO_CORE_HEATER_OUTPUT,	  \
+		GPIO_HEATER1_OUTPUT,              \
+		GPIO_HEATER2_OUTPUT,              \
 		GPIO_nPOWER_IN_A,                 \
 		GPIO_nPOWER_IN_B,                 \
 		GPIO_nPOWER_IN_C,                 \

boards/cuav/x25-super/CMakeLists.txt

@@ -31,5 +31,4 @@
 #
 ############################################################################
 
-add_subdirectory(core_heater)
 add_subdirectory(pwm_voltage)

boards/cuav/x25-super/core_heater/core_heater.cpp

@@ -1,261 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2026 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file core_heater.cpp
- *
- */
-
-#include "core_heater.h"
-
-#include <px4_platform_common/getopt.h>
-#include <px4_platform_common/log.h>
-#include <drivers/drv_hrt.h>
-#include <drivers/drv_io_heater.h>
-
-#  ifndef GPIO_CORE_HEATER_OUTPUT
-#  error "To use the heater driver, the board_config.h must define and initialize GPIO_CORE_HEATER_OUTPUT"
-#  endif
-
-Core_Heater::Core_Heater() :
-	ModuleParams(nullptr),
-	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
-{
-	_heater_status_pub.advertise();
-}
-
-Core_Heater::~Core_Heater()
-{
-	disable_core_heater();
-}
-
-int Core_Heater::custom_command(int argc, char *argv[])
-{
-	// Check if the driver is running.
-	if (!is_running()) {
-		PX4_INFO("not running");
-		return PX4_ERROR;
-	}
-
-	return print_usage("Unrecognized command.");
-}
-
-void Core_Heater::disable_core_heater()
-{
-	// Reset heater to off state.
-	px4_arch_unconfiggpio(GPIO_CORE_HEATER_OUTPUT);
-}
-
-void Core_Heater::initialize_core_heater_io()
-{
-	// Initialize heater to off state.
-	px4_arch_configgpio(GPIO_CORE_HEATER_OUTPUT);
-}
-
-void Core_Heater::core_heater_off()
-{
-	CORE_HEATER_OUTPUT_EN(false);
-}
-
-void Core_Heater::core_heater_on()
-{
-	CORE_HEATER_OUTPUT_EN(true);
-}
-
-bool Core_Heater::initialize_topics()
-{
-	for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
-		uORB::SubscriptionData<sensor_accel_s> sensor_accel_sub{ORB_ID(sensor_accel), i};
-
-		if (sensor_accel_sub.get().timestamp != 0 &&
-		    sensor_accel_sub.get().device_id != 0 &&
-		    PX4_ISFINITE(sensor_accel_sub.get().temperature)) {
-
-			// If the correct ID is found, exit the for-loop with _sensor_accel_sub pointing to the correct instance.
-			if (sensor_accel_sub.get().device_id == (uint32_t)_param_core_temp_id.get()) {
-				_sensor_accel_sub.ChangeInstance(i);
-				_sensor_device_id = sensor_accel_sub.get().device_id;
-				initialize_core_heater_io();
-				return true;
-			}
-		}
-	}
-
-	return false;
-}
-
-void Core_Heater::Run()
-{
-	if (should_exit()) {
-		exit_and_cleanup();
-		return;
-	}
-
-	update_params();
-
-	if (_sensor_device_id == 0) {
-		if (!initialize_topics()) {
-			// if sensor still not found try again in 1 second
-			ScheduleDelayed(1_s);
-			return;
-		}
-	}
-
-	sensor_accel_s sensor_accel;
-	float temperature_delta {0.f};
-
-	if (_core_heater_on) {
-		// Turn the heater off.
-		_core_heater_on = false;
-		core_heater_off();
-		ScheduleDelayed(_controller_period_usec - _controller_time_on_usec);
-
-	} else if (_sensor_accel_sub.update(&sensor_accel)) {
-		// Update the current IMU sensor temperature if valid.
-		if (PX4_ISFINITE(sensor_accel.temperature)) {
-			temperature_delta = _param_core_imu_temp.get() - sensor_accel.temperature;
-			_temperature_last = sensor_accel.temperature;
-		}
-
-		_proportional_value = temperature_delta * _param_core_imu_temp_p.get();
-		_integrator_value += temperature_delta * _param_core_imu_temp_i.get();
-
-		_integrator_value = math::constrain(_integrator_value, -0.25f, 0.25f);
-
-		_controller_time_on_usec = static_cast<int>((_param_core_imu_temp_ff.get() + _proportional_value +
-					   _integrator_value) * static_cast<float>(_controller_period_usec));
-
-		_controller_time_on_usec = math::constrain(_controller_time_on_usec, 0, _controller_period_usec);
-
-		if (fabsf(temperature_delta) < TEMPERATURE_TARGET_THRESHOLD) {
-			_temperature_target_met = true;
-
-		} else {
-
-			_temperature_target_met = false;
-		}
-
-		_core_heater_on = true;
-		core_heater_on();
-		ScheduleDelayed(_controller_time_on_usec);
-	}
-
-	publish_status();
-}
-
-void Core_Heater::publish_status()
-{
-	heater_status_s status{};
-	status.device_id               = _sensor_device_id;
-	status.heater_on               = _core_heater_on;
-	status.temperature_sensor      = _temperature_last;
-	status.temperature_target      = _param_core_imu_temp.get();
-	status.temperature_target_met  = _temperature_target_met;
-	status.controller_period_usec  = _controller_period_usec;
-	status.controller_time_on_usec = _controller_time_on_usec;
-	status.proportional_value      = _proportional_value;
-	status.integrator_value        = _integrator_value;
-	status.feed_forward_value      = _param_core_imu_temp_ff.get();
-
-	status.mode = heater_status_s::MODE_GPIO;
-
-	status.timestamp = hrt_absolute_time();
-	_heater_status_pub.publish(status);
-}
-
-int Core_Heater::start()
-{
-	// Exit the driver if the sensor ID does not match the desired sensor.
-	if (_param_core_temp_id.get() == 0) {
-		PX4_ERR("Valid CORE_TEMP_ID required");
-		request_stop();
-		return PX4_ERROR;
-	}
-
-	update_params(true);
-	ScheduleNow();
-	return PX4_OK;
-}
-
-int Core_Heater::task_spawn(int argc, char *argv[])
-{
-	Core_Heater *core_heater = new Core_Heater();
-
-	if (!core_heater) {
-		PX4_ERR("driver allocation failed");
-		return PX4_ERROR;
-	}
-
-	_object.store(core_heater);
-	_task_id = task_id_is_work_queue;
-
-	core_heater->start();
-	return 0;
-}
-
-void Core_Heater::update_params(const bool force)
-{
-	if (_parameter_update_sub.updated() || force) {
-		// clear update
-		parameter_update_s param_update;
-		_parameter_update_sub.copy(&param_update);
-
-		// update parameters from storage
-		ModuleParams::updateParams();
-	}
-}
-
-int Core_Heater::print_usage(const char *reason)
-{
-	if (reason) {
-		printf("%s\n\n", reason);
-	}
-
-	PRINT_MODULE_DESCRIPTION(
-		R"DESCR_STR(
-### Description
-Background process running periodically on the LP work queue to regulate IMU temperature at a setpoint.
-
-)DESCR_STR");
-
-	PRINT_MODULE_USAGE_NAME("core_heater", "system");
-	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
-
-	return 0;
-}
-
-extern "C" __EXPORT int core_heater_main(int argc, char *argv[])
-{
-	return Core_Heater::main(argc, argv);
-}

boards/cuav/x25-super/core_heater/core_heater.h

@@ -1,159 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2026 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file core_heater.h
- *
- */
-
-#pragma once
-
-#include <px4_platform_common/px4_config.h>
-#include <px4_platform_common/getopt.h>
-#include <px4_platform_common/module.h>
-#include <px4_platform_common/module_params.h>
-#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
-#include <uORB/Publication.hpp>
-#include <uORB/SubscriptionInterval.hpp>
-#include <uORB/topics/heater_status.h>
-#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/sensor_accel.h>
-
-#include <mathlib/mathlib.h>
-
-using namespace time_literals;
-
-#define CONTROLLER_PERIOD_DEFAULT    10000
-#define TEMPERATURE_TARGET_THRESHOLD 2.5f
-
-class Core_Heater : public ModuleBase<Core_Heater>, public ModuleParams, public px4::ScheduledWorkItem
-{
-public:
-	Core_Heater();
-
-	virtual ~Core_Heater();
-
-	/**
-	 * @see ModuleBase::custom_command().
-	 * @brief main Main entry point to the module that should be
-	 *        called directly from the module's main method.
-	 * @param argc The input argument count.
-	 * @param argv Pointer to the input argument array.
-	 * @return Returns 0 iff successful, -1 otherwise.
-	 */
-	static int custom_command(int argc, char *argv[]);
-
-	/**
-	 * @see ModuleBase::print_usage().
-	 * @brief Prints the module usage to the nuttshell console.
-	 * @param reason The requested reason for printing to console.
-	 */
-	static int print_usage(const char *reason = nullptr);
-
-	/**
-	 * @see ModuleBase::task_spawn().
-	 * @brief Initializes the class in the same context as the work queue
-	 *        and starts the background listener.
-	 * @param argv Pointer to the input argument array.
-	 * @return Returns 0 iff successful, -1 otherwise.
-	 */
-	static int task_spawn(int argc, char *argv[]);
-
-	/**
-	 * @brief Initiates the heater driver work queue, starts a new background task,
-	 *        and fails if it is already running.
-	 * @return Returns 1 iff start was successful.
-	 */
-	int start();
-
-private:
-
-	/** Disables the heater (either by GPIO). */
-	void disable_core_heater();
-
-	/** Turns the heater on (either by GPIO). */
-	void core_heater_on();
-
-	/** Turns the heater off (either by GPIO). */
-	void core_heater_off();
-
-	void initialize();
-
-	/** Enables / configures the heater (either by GPIO). */
-	void initialize_core_heater_io();
-
-	/** @brief Called once to initialize uORB topics. */
-	bool initialize_topics();
-
-	void publish_status();
-
-	/** @brief Calculates the heater element on/off time and schedules the next cycle. */
-	void Run() override;
-
-	/**
-	 * @brief Updates and checks for updated uORB parameters.
-	 * @param force Boolean to determine if an update check should be forced.
-	 */
-	void update_params(const bool force = false);
-
-	/** Work queue struct for the scheduler. */
-	static struct work_s _work;
-
-	bool _core_heater_initialized     = false;
-	bool _core_heater_on              = false;
-	bool _temperature_target_met = false;
-
-	int _controller_period_usec = CONTROLLER_PERIOD_DEFAULT;
-	int _controller_time_on_usec = 0;
-
-	float _integrator_value   = 0.0f;
-	float _proportional_value = 0.0f;
-
-	uORB::Publication<heater_status_s> _heater_status_pub{ORB_ID(heater_status)};
-
-	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
-
-	uORB::Subscription _sensor_accel_sub{ORB_ID(sensor_accel)};
-
-	uint32_t _sensor_device_id{0};
-
-	float _temperature_last{NAN};
-
-	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::CORE_IMU_TEMP_FF>) _param_core_imu_temp_ff,
-		(ParamFloat<px4::params::CORE_IMU_TEMP_I>)  _param_core_imu_temp_i,
-		(ParamFloat<px4::params::CORE_IMU_TEMP_P>)  _param_core_imu_temp_p,
-		(ParamFloat<px4::params::CORE_IMU_TEMP>)    _param_core_imu_temp,
-		(ParamInt<px4::params::CORE_TEMP_ID>)       _param_core_temp_id
-	)
-};

boards/cuav/x25-super/init/rc.board_defaults

@@ -21,15 +21,12 @@ param set-default USB_MAV_MODE 5
 param set-default UAVCAN_SUB_GPS 1
 param set-default UAVCAN_SUB_BAT 1
 
-# Enable IMU thermal control
+# IMU thermal control (multi-instance heater)
 param set-default SENS_EN_THERMAL 1
-param set-default SENS_IMU_TEMP 45
-param set-default SENS_TEMP_ID 5963786
-
-# CUAV core board IMU thermal control
-param set-default CORE_IMU_TEMP 45
-param set-default CORE_TEMP_ID 3014698
-core_heater start
+param set-default HEATER1_IMU_ID 2818066
+param set-default HEATER1_TEMP 45
+param set-default HEATER2_IMU_ID 3014698
+param set-default HEATER2_TEMP 45
 
 # CUAV pwm voltage 3.3V/5V switch
 pwm_voltage_apply start

boards/cuav/x25-super/src/board_config.h

@@ -170,16 +170,13 @@
 #define GPIO_HW_REV_SENSE      /* PH4 */  GPIO_ADC3_INP15
 #define GPIO_HW_VER_SENSE      /* PH3 */  GPIO_ADC3_INP14
 
-/* HEATER
- * PWM in future
- */
-// IMU BOARD HEATER
-#define GPIO_HEATER_OUTPUT        /* PB10 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
-#define HEATER_OUTPUT_EN(on_true)	       px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true))
-
-// CORE BOARD HEATER
-#define GPIO_CORE_HEATER_OUTPUT    /* PE6 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN6)
-#define CORE_HEATER_OUTPUT_EN(on_true)	       px4_arch_gpiowrite(GPIO_CORE_HEATER_OUTPUT, (on_true))
+/* HEATER */
+#define GPIO_HEATER_OUTPUT
+#define HEATER_NUM	2
+#define GPIO_HEATER1_OUTPUT       /* PB10 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
+#define HEATER1_OUTPUT_EN(on_true)             px4_arch_gpiowrite(GPIO_HEATER1_OUTPUT, (on_true))
+#define GPIO_HEATER2_OUTPUT       /* PE6  */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN6)
+#define HEATER2_OUTPUT_EN(on_true)             px4_arch_gpiowrite(GPIO_HEATER2_OUTPUT, (on_true))
 
 /* PE7 is nARMED
  *  The GPIO will be set as input while not armed HW will have external HW Pull UP.
@@ -404,8 +401,8 @@
 		GPIO_CAN2_RX,                   \
 		GPIO_CAN1_SILENT_S0,		\
 		GPIO_CAN2_SILENT_S1,            \
-		GPIO_HEATER_OUTPUT,             \
-		GPIO_CORE_HEATER_OUTPUT,	\
+		GPIO_HEATER1_OUTPUT,            \
+		GPIO_HEATER2_OUTPUT,            \
 		GPIO_nPOWER_IN_A,               \
 		GPIO_nPOWER_IN_B,               \
 		GPIO_nPOWER_IN_C,               \

boards/cuav/x7pro/init/rc.board_defaults

@@ -12,7 +12,7 @@ param set-default BAT2_A_PER_V 24
 # Enable IMU thermal control
 param set-default SENS_EN_THERMAL 1
 
-param set-default SENS_TEMP_ID 6946850
+param set-default HEATER1_IMU_ID 6946850
 
 rgbled_pwm start
 safety_button start

boards/cuav/x7pro/src/board_config.h

@@ -104,8 +104,10 @@
 #define GPIO_CAN2_SILENT_S1  /* PH3  */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTH|GPIO_PIN3)
 
 /* HEATER */
-#define GPIO_HEATER_OUTPUT   /* PA8 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN8)
-#define HEATER_OUTPUT_EN(on_true)      px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true))
+#define GPIO_HEATER_OUTPUT
+#define HEATER_NUM 	1
+#define GPIO_HEATER1_OUTPUT  /* PA8 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN8)
+#define HEATER1_OUTPUT_EN(on_true)      px4_arch_gpiowrite(GPIO_HEATER1_OUTPUT, (on_true))
 
 /* PWM */
 #define DIRECT_PWM_OUTPUT_CHANNELS   14
@@ -212,7 +214,7 @@
 		GPIO_CAN2_RX,                     \
 		GPIO_CAN1_SILENT_S0,              \
 		GPIO_CAN2_SILENT_S1,              \
-		GPIO_HEATER_OUTPUT,               \
+		GPIO_HEATER1_OUTPUT,              \
 		GPIO_nPOWER_IN_CAN,               \
 		GPIO_nPOWER_IN_ADC,               \
 		GPIO_nPOWER_IN_C,                 \

boards/cubepilot/cubeorange/init/rc.board_defaults

@@ -12,6 +12,6 @@ param set-default BAT2_A_PER_V 17
 # Disable IMU thermal control
 param set-default SENS_EN_THERMAL 0
 
-param set-default -s SENS_TEMP_ID 2621474
+param set-default -s HEATER1_IMU_ID 2621474
 
 set IOFW "/etc/extras/cubepilot_io-v2_default.bin"

boards/cubepilot/cubeorange/src/board_config.h

@@ -59,6 +59,7 @@
 #define PX4IO_SERIAL_BITRATE           1500000               /* 1.5Mbps -> max rate for IO */
 
 #define PX4IO_HEATER_ENABLED
+#define HEATER_NUM 1
 
 /* LEDs */
 #define GPIO_nLED_AMBER        /* PE12 */  (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN12)

boards/cubepilot/cubeorangeplus/init/rc.board_sensors

@@ -18,7 +18,7 @@ ms5611 -s -b 4 start
 if icm42688p -s -b 4 -R 10 -q start -c 15
 then
 	# We need to use the temperature of the first isolated IMU for heater control.
-	param set-default SENS_TEMP_ID 2490402
+	param set-default HEATER1_IMU_ID 2490402
 
 	if ! icm20948 -s -b 4 -R 10 -M -q start
 	then
@@ -28,7 +28,7 @@ else
 	icm45686 -s -b 4 -R 10 start -c 15
 	icm45686 -s -b 4 -R 6 start -c 13
 
-	param set-default SENS_TEMP_ID 3407906
+	param set-default HEATER1_IMU_ID 3407906
 fi
 
 # SPI1, body-fixed

boards/cubepilot/cubeorangeplus/src/board_config.h

@@ -69,6 +69,7 @@
 #define PX4IO_SERIAL_BITRATE           1500000               /* 1.5Mbps -> max rate for IO */
 
 #define PX4IO_HEATER_ENABLED
+#define HEATER_NUM 1
 
 /* LEDs */
 #define GPIO_nLED_AMBER        /* PE12 */  (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN12)

boards/cubepilot/io-v2/src/board_config.h

@@ -80,8 +80,10 @@
 
 
 /* HEATER */
-#define GPIO_HEATER_OUTPUT   /* PB14 */ (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14)
-#define HEATER_OUTPUT_EN(on_true)       stm32_gpiowrite(GPIO_HEATER_OUTPUT, (on_true))
+#define GPIO_HEATER_OUTPUT
+#define HEATER_NUM 	1
+#define GPIO_HEATER1_OUTPUT  /* PB14 */ (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14)
+#define HEATER1_OUTPUT_EN(on_true)       stm32_gpiowrite(GPIO_HEATER1_OUTPUT, (on_true))
 
 
 #define GPIO_USART1_RX_SPEKTRUM		(GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN10)

boards/cubepilot/io-v2/src/init.c

@@ -86,7 +86,7 @@ __EXPORT void stm32_boardinitialize(void)
 {
 	/* configure GPIOs */
 
-	stm32_configgpio(GPIO_HEATER_OUTPUT);
+	stm32_configgpio(GPIO_HEATER1_OUTPUT);
 
 	/* LEDS - default to off */
 	stm32_configgpio(GPIO_LED_AMBER);
@@ -141,5 +141,5 @@ __EXPORT void stm32_boardinitialize(void)
 	stm32_configgpio(GPIO_PWM8);
 
 	/* disable heater */
-	HEATER_OUTPUT_EN(false);
+	HEATER1_OUTPUT_EN(false);
 }

boards/hkust/nxt-dual/src/timer_config.cpp

@@ -46,10 +46,10 @@ constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
 	initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel2}, {GPIO::PortE, GPIO::Pin11}),
 	initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel3}, {GPIO::PortE, GPIO::Pin13}),
 	initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel4}, {GPIO::PortE, GPIO::Pin14}),
+	initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel4}, {GPIO::PortB, GPIO::Pin1}),
+	initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel3}, {GPIO::PortB, GPIO::Pin0}),
 	initIOTimerChannel(io_timers, {Timer::Timer2, Timer::Channel3}, {GPIO::PortB, GPIO::Pin10}),
 	initIOTimerChannel(io_timers, {Timer::Timer2, Timer::Channel4}, {GPIO::PortB, GPIO::Pin11}),
-	initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel3}, {GPIO::PortB, GPIO::Pin0}),
-	initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel4}, {GPIO::PortB, GPIO::Pin1}),
 };
 
 constexpr io_timers_channel_mapping_t io_timers_channel_mapping =

boards/holybro/durandal-v1/src/board_config.h

@@ -170,8 +170,10 @@
 /* HEATER
  * PWM in future
  */
-#define GPIO_HEATER_OUTPUT   /* PA7  T14CH1 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7)
-#define HEATER_OUTPUT_EN(on_true)              px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true))
+#define GPIO_HEATER_OUTPUT
+#define HEATER_NUM 	1
+#define GPIO_HEATER1_OUTPUT  /* PA7  T14CH1 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7)
+#define HEATER1_OUTPUT_EN(on_true)              px4_arch_gpiowrite(GPIO_HEATER1_OUTPUT, (on_true))
 
 /* PWM
  */
@@ -299,7 +301,7 @@
 		GPIO_CAN2_RX,                     \
 		GPIO_CAN1_SILENT_S0,              \
 		GPIO_CAN2_SILENT_S1,              \
-		GPIO_HEATER_OUTPUT,               \
+		GPIO_HEATER1_OUTPUT,              \
 		GPIO_nPOWER_IN_A,                 \
 		GPIO_nPOWER_IN_B,                 \
 		GPIO_nPOWER_IN_C,                 \

boards/holybro/pix32v5/src/board_config.h

@@ -204,8 +204,10 @@
 /* HEATER
  * PWM in future
  */
-#define GPIO_HEATER_OUTPUT   /* PA7  T14CH1 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7)
-#define HEATER_OUTPUT_EN(on_true)              px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true))
+#define GPIO_HEATER_OUTPUT
+#define HEATER_NUM 	1
+#define GPIO_HEATER1_OUTPUT  /* PA7  T14CH1 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7)
+#define HEATER1_OUTPUT_EN(on_true)              px4_arch_gpiowrite(GPIO_HEATER1_OUTPUT, (on_true))
 
 /* PI0 is nARMED
  *  The GPIO will be set as input while not armed HW will have external HW Pull UP.
@@ -409,7 +411,7 @@
 		GPIO_CAN1_SILENT_S0,              \
 		GPIO_CAN2_SILENT_S1,              \
 		GPIO_CAN3_SILENT_S2,              \
-		GPIO_HEATER_OUTPUT,               \
+		GPIO_HEATER1_OUTPUT,              \
 		GPIO_nPOWER_IN_A,                 \
 		GPIO_nPOWER_IN_B,                 \
 		GPIO_nPOWER_IN_C,                 \

boards/modalai/voxl2/scripts/build-deps.sh

@@ -1,9 +0,0 @@
-#!/bin/bash
-
-cd boards/modalai/voxl2/libfc-sensor-api
-rm -fR build
-mkdir build
-cd build
-CC=/home/4.1.0.4/tools/linaro64/bin/aarch64-linux-gnu-gcc cmake ..
-make
-cd ../../../../..

boards/modalai/voxl2/scripts/install-voxl.sh

@@ -65,7 +65,6 @@ adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-logger"
 adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-manual_control"
 adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-mavlink"
 adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-mavlink_bridge"
-adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-mavlink_tests"
 adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-mb12xx"
 adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-mc_att_control"
 adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-mc_pos_control"

boards/modalai/voxl2/target/voxl-px4

@@ -29,10 +29,12 @@ if /bin/ls /usr/lib/rfsa/adsp/testsig-*.so &> /dev/null; then
     /bin/echo "Found DSP signature file"
 else
     /bin/echo "[WARNING] Could not find DSP signature file"
-    # Look for the DSP signature generation script
-    if [ -f /share/modalai/qrb5165-slpi-test-sig/generate-test-sig.sh ]; then
-        /bin/echo "[INFO] Attempting to generate the DSP signature file"
-        # Automatically generate the test signature so that px4 can run on SLPI DSP
+    # Look for the DSP signature generation script (platform-specific)
+    if [ -f /share/modalai/qcs6490-slpi-test-sig/generate-test-sig.sh ]; then
+        /bin/echo "[INFO] Attempting to generate the DSP signature file (qcs6490)"
+        /share/modalai/qcs6490-slpi-test-sig/generate-test-sig.sh
+    elif [ -f /share/modalai/qrb5165-slpi-test-sig/generate-test-sig.sh ]; then
+        /bin/echo "[INFO] Attempting to generate the DSP signature file (qrb5165)"
         /share/modalai/qrb5165-slpi-test-sig/generate-test-sig.sh
     else
         /bin/echo "[ERROR] Could not find the DSP signature file generation script"

boards/modalai/voxl2/target/voxl-px4-hitl

@@ -6,10 +6,12 @@ if /bin/ls /usr/lib/rfsa/adsp/testsig-*.so &> /dev/null; then
     /bin/echo "Found DSP signature file"
 else
     /bin/echo "[WARNING] Could not find DSP signature file"
-    # Look for the DSP signature generation script
-    if [ -f /share/modalai/qrb5165-slpi-test-sig/generate-test-sig.sh ]; then
-        /bin/echo "[INFO] Attempting to generate the DSP signature file"
-        # Automatically generate the test signature so that px4 can run on SLPI DSP
+    # Look for the DSP signature generation script (platform-specific)
+    if [ -f /share/modalai/qcs6490-slpi-test-sig/generate-test-sig.sh ]; then
+        /bin/echo "[INFO] Attempting to generate the DSP signature file (qcs6490)"
+        /share/modalai/qcs6490-slpi-test-sig/generate-test-sig.sh
+    elif [ -f /share/modalai/qrb5165-slpi-test-sig/generate-test-sig.sh ]; then
+        /bin/echo "[INFO] Attempting to generate the DSP signature file (qrb5165)"
         /share/modalai/qrb5165-slpi-test-sig/generate-test-sig.sh
     else
         /bin/echo "[ERROR] Could not find the DSP signature file generation script"

boards/modalai/voxl2/target/voxl-px4-start

@@ -302,4 +302,28 @@ done
 # marked as optional will only be logged if they have been advertised when
 # this is started. By starting it last it makes sure to see those
 # advertisements as the other modules are starting before it.
-logger start
+#
+# Set logger mode based on SDLOG_MODE parameter:
+#   0: log when armed until disarm (default)
+#   1: log from boot until disarm
+#   2: log from boot until shutdown
+#   3: log based on AUX1 RC channel
+#   4: log from first armed until shutdown
+LOGGER_ARGS=""
+if param compare SDLOG_MODE 1
+then
+	LOGGER_ARGS="-e"
+fi
+if param compare SDLOG_MODE 2
+then
+	LOGGER_ARGS="-f"
+fi
+if param compare SDLOG_MODE 3
+then
+	LOGGER_ARGS="-x"
+fi
+if param compare SDLOG_MODE 4
+then
+	LOGGER_ARGS="-a"
+fi
+logger start $LOGGER_ARGS

boards/narinfc/h7/init/rc.board_defaults

@@ -12,7 +12,7 @@ param set-default BAT2_A_PER_V 24
 # Enable IMU thermal control
 param set-default SENS_EN_THERMAL 1
 
-param set-default SENS_TEMP_ID 6946850
+param set-default HEATER1_IMU_ID 6946850
 
 rgbled_pwm start
 safety_button start

boards/narinfc/h7/src/board_config.h

@@ -104,8 +104,10 @@
 #define GPIO_CAN2_SILENT_S1  /* PH3  */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTH|GPIO_PIN3)
 
 /* HEATER */
-#define GPIO_HEATER_OUTPUT   /* PA8 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN8)
-#define HEATER_OUTPUT_EN(on_true)      px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true))
+#define GPIO_HEATER_OUTPUT
+#define HEATER_NUM 	1
+#define GPIO_HEATER1_OUTPUT  /* PA8 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN8)
+#define HEATER1_OUTPUT_EN(on_true)      px4_arch_gpiowrite(GPIO_HEATER1_OUTPUT, (on_true))
 
 /* PWM */
 #define DIRECT_PWM_OUTPUT_CHANNELS   14
@@ -212,7 +214,7 @@
 		GPIO_CAN2_RX,                     \
 		GPIO_CAN1_SILENT_S0,              \
 		GPIO_CAN2_SILENT_S1,              \
-		GPIO_HEATER_OUTPUT,               \
+		GPIO_HEATER1_OUTPUT,              \
 		GPIO_nPOWER_IN_CAN,               \
 		GPIO_nPOWER_IN_ADC,               \
 		GPIO_nPOWER_IN_C,                 \

boards/nxp/mr-tropic/nuttx-config/scripts/itcm_functions_includes.ld

@@ -202,7 +202,7 @@
 *(.text._ZN21MavlinkStreamAltitude8get_sizeEv)
 *(.text._ZN7Mavlink29check_requested_subscriptionsEv)
 *(.text.imxrt_lpspi_setmode)
-*(.text._ZN3Ekf34controlZeroInnovationHeadingUpdateEv)
+*(.text._ZN3Ekf36uncorrelateAndLimitHeadingCovarianceEv)
 *(.text.perf_begin)
 *(.text.imxrt_lpspi_setfrequency)
 *(.text._ZN17FlightModeManager9_initTaskE15FlightTaskIndex)

boards/nxp/tropic-community/nuttx-config/scripts/itcm_functions_includes.ld

@@ -202,7 +202,7 @@
 *(.text._ZN21MavlinkStreamAltitude8get_sizeEv)
 *(.text._ZN7Mavlink29check_requested_subscriptionsEv)
 *(.text.imxrt_lpspi_setmode)
-*(.text._ZN3Ekf34controlZeroInnovationHeadingUpdateEv)
+*(.text._ZN3Ekf36uncorrelateAndLimitHeadingCovarianceEv)
 *(.text.perf_begin)
 *(.text.imxrt_lpspi_setfrequency)
 *(.text._ZN17FlightModeManager9_initTaskE15FlightTaskIndex)

boards/px4/fmu-v3/src/board_config.h

@@ -164,6 +164,7 @@
  * Connected to the IO MCU; tell compiler to enable support
  */
 #define PX4IO_HEATER_ENABLED
+#define HEATER_NUM 1
 
 __BEGIN_DECLS
 

boards/px4/fmu-v4/src/board_config.h

@@ -140,8 +140,10 @@
 
 /* Heater pins */
 #define GPIO_HEATER_INPUT            (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTC|GPIO_PIN6)
-#define GPIO_HEATER_OUTPUT           (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN6)
-#define HEATER_OUTPUT_EN(on_true)    px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true))
+#define GPIO_HEATER_OUTPUT
+#define HEATER_NUM 	1
+#define GPIO_HEATER1_OUTPUT          (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN6)
+#define HEATER1_OUTPUT_EN(on_true)    px4_arch_gpiowrite(GPIO_HEATER1_OUTPUT, (on_true))
 
 /* Power switch controls */
 

boards/px4/fmu-v4/src/init.c

@@ -222,7 +222,7 @@ stm32_boardinitialize(void)
 
 	// Configure heater GPIO.
 	stm32_configgpio(GPIO_HEATER_INPUT);
-	stm32_configgpio(GPIO_HEATER_OUTPUT);
+	stm32_configgpio(GPIO_HEATER1_OUTPUT);
 }
 
 /****************************************************************************
@@ -283,7 +283,7 @@ __EXPORT int board_app_initialize(uintptr_t arg)
 	}
 
 	// Power down the heater.
-	stm32_gpiowrite(GPIO_HEATER_OUTPUT, 0);
+	stm32_gpiowrite(GPIO_HEATER1_OUTPUT, 0);
 
 	// Configure SPI-based devices.
 	spi1 = stm32_spibus_initialize(1);

boards/px4/fmu-v5/src/board_config.h

@@ -209,8 +209,10 @@
 /* HEATER
  * PWM in future
  */
-#define GPIO_HEATER_OUTPUT   /* PA7  T14CH1 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7)
-#define HEATER_OUTPUT_EN(on_true)              px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true))
+#define GPIO_HEATER_OUTPUT
+#define HEATER_NUM 	1
+#define GPIO_HEATER1_OUTPUT  /* PA7  T14CH1 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7)
+#define HEATER1_OUTPUT_EN(on_true)              px4_arch_gpiowrite(GPIO_HEATER1_OUTPUT, (on_true))
 
 
 /* PI0 is nARMED
@@ -426,7 +428,7 @@
 		GPIO_CAN1_SILENT_S0,              \
 		GPIO_CAN2_SILENT_S1,              \
 		GPIO_CAN3_SILENT_S2,              \
-		GPIO_HEATER_OUTPUT,               \
+		GPIO_HEATER1_OUTPUT,              \
 		GPIO_nPOWER_IN_A,                 \
 		GPIO_nPOWER_IN_B,                 \
 		GPIO_nPOWER_IN_C,                 \

boards/px4/fmu-v5x/src/board_config.h

@@ -196,8 +196,10 @@
 /* HEATER
  * PWM in future
  */
-#define GPIO_HEATER_OUTPUT   /* PB10  T2CH3 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
-#define HEATER_OUTPUT_EN(on_true)	       px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true))
+#define GPIO_HEATER_OUTPUT
+#define HEATER_NUM 	1
+#define GPIO_HEATER1_OUTPUT  /* PB10  T2CH3 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
+#define HEATER1_OUTPUT_EN(on_true)	       px4_arch_gpiowrite(GPIO_HEATER1_OUTPUT, (on_true))
 
 /* PC12 is nARMED
  *  The GPIO will be set as input while not armed HW will have external HW Pull UP.
@@ -399,7 +401,7 @@
 		GPIO_CAN1_RX,                     \
 		GPIO_CAN2_TX,                     \
 		GPIO_CAN2_RX,                     \
-		GPIO_HEATER_OUTPUT,               \
+		GPIO_HEATER1_OUTPUT,              \
 		GPIO_nPOWER_IN_A,                 \
 		GPIO_nPOWER_IN_B,                 \
 		GPIO_nPOWER_IN_C,                 \

boards/px4/fmu-v6c/src/board_config.h

@@ -154,8 +154,10 @@
 /* HEATER
  * PWM in future
  */
-#define GPIO_HEATER_OUTPUT   /* PB9  T17CH1 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN9)
-#define HEATER_OUTPUT_EN(on_true)	       px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true))
+#define GPIO_HEATER_OUTPUT
+#define HEATER_NUM 	1
+#define GPIO_HEATER1_OUTPUT  /* PB9  T17CH1 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN9)
+#define HEATER1_OUTPUT_EN(on_true)	       px4_arch_gpiowrite(GPIO_HEATER1_OUTPUT, (on_true))
 
 /* PWM
  */
@@ -257,7 +259,7 @@
 		GPIO_CAN1_RX,                     \
 		GPIO_CAN2_TX,                     \
 		GPIO_CAN2_RX,                     \
-		GPIO_HEATER_OUTPUT,               \
+		GPIO_HEATER1_OUTPUT,              \
 		GPIO_nPOWER_IN_A,                 \
 		GPIO_nPOWER_IN_B,                 \
 		GPIO_nPOWER_IN_C,                 \

boards/px4/fmu-v6u/src/board_config.h

@@ -179,8 +179,10 @@
 /* HEATER
  * PWM in future
  */
-#define GPIO_HEATER_OUTPUT   /* PA2  T2CH3 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN2)
-#define HEATER_OUTPUT_EN(on_true)	      px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true))
+#define GPIO_HEATER_OUTPUT
+#define HEATER_NUM 	1
+#define GPIO_HEATER1_OUTPUT  /* PA2  T2CH3 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN2)
+#define HEATER1_OUTPUT_EN(on_true)	      px4_arch_gpiowrite(GPIO_HEATER1_OUTPUT, (on_true))
 
 /* PE6 is nARMED
  *  The GPIO will be set as input while not armed HW will have external HW Pull UP.
@@ -368,7 +370,7 @@
 		GPIO_HW_VER_REV_DRIVE,            \
 		GPIO_CAN1_TX,                     \
 		GPIO_CAN1_RX,                     \
-		GPIO_HEATER_OUTPUT,               \
+		GPIO_HEATER1_OUTPUT,              \
 		GPIO_nPOWER_IN_A,                 \
 		GPIO_nPOWER_IN_B,                 \
 		GPIO_nPOWER_IN_C,                 \

boards/px4/fmu-v6x/src/board_config.h

@@ -230,8 +230,10 @@
 /* HEATER
  * PWM in future
  */
-#define GPIO_HEATER_OUTPUT   /* PB10  T2CH3 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
-#define HEATER_OUTPUT_EN(on_true)	       px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true))
+#define GPIO_HEATER_OUTPUT
+#define HEATER_NUM 	1
+#define GPIO_HEATER1_OUTPUT  /* PB10  T2CH3 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
+#define HEATER1_OUTPUT_EN(on_true)	       px4_arch_gpiowrite(GPIO_HEATER1_OUTPUT, (on_true))
 
 /* PE6 is nARMED
  *  The GPIO will be set as input while not armed HW will have external HW Pull UP.
@@ -444,7 +446,7 @@
 		GPIO_CAN1_RX,                     \
 		GPIO_CAN2_TX,                     \
 		GPIO_CAN2_RX,                     \
-		GPIO_HEATER_OUTPUT,               \
+		GPIO_HEATER1_OUTPUT,              \
 		GPIO_nPOWER_IN_A,                 \
 		GPIO_nPOWER_IN_B,                 \
 		GPIO_nPOWER_IN_C,                 \

boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld

@@ -205,7 +205,7 @@
 *(.text._ZN21MavlinkStreamAltitude8get_sizeEv)
 *(.text._ZN7Mavlink29check_requested_subscriptionsEv)
 *(.text.imxrt_lpspi_setmode)
-*(.text._ZN3Ekf34controlZeroInnovationHeadingUpdateEv)
+*(.text._ZN3Ekf36uncorrelateAndLimitHeadingCovarianceEv)
 *(.text.perf_begin)
 *(.text.imxrt_lpspi_setfrequency)
 *(.text._ZN17FlightModeManager9_initTaskE15FlightTaskIndex)