fix: remove iostream

fix: format
feat: spacecraft tooling for commander and VehicleStatus
2026-05-25 01:07:34 +08:00 · 2025-04-13 15:14:09 +02:00 · 2025-04-13 15:02:09 +02:00 · 2025-04-13 14:53:16 +02:00 · 2025-04-13 14:51:38 +02:00 · 2025-04-11 23:48:40 +02:00
871 changed files with 16316 additions and 11205 deletions
@@ -19,7 +19,7 @@ jobs:
      fail-fast: false
      max-parallel: 1 # Should be 1 to avoid parallel builds
      matrix:
-        lc: [ko, uk, zh] # Target languages https://developer.crowdin.com/language-codes/
+        lc: [ko, uk, zh-CN] # Target languages https://developer.crowdin.com/language-codes/
    steps:
      - name: Checkout
        uses: actions/checkout@v4
@@ -41,6 +41,8 @@ jobs:
          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_labels: 'Documentation 📑'
+          pull_request_reviewers: hamishwillee
          download_language: ${{ matrix.lc }}
        env:
          # A classic GitHub Personal Access Token with the 'repo' scope selected (the user should have write access to the repository).
@@ -8,18 +8,18 @@ on:
      - main
    paths:
      - 'docs/en/**'
-  pull_request:
-    types:
-      - closed
-    branches:
-      - main
-    paths:
-      - 'docs/en/**'
+  #pull_request:
+  #  types:
+  #    - closed
+  #  branches:
+  #    - main
+  #  paths:
+  #    - 'docs/en/**'
  workflow_dispatch:

 jobs:
  upload-to-crowdin:
-    if: github.event.pull_request.merged == true || github.event_name == 'push'
+    #if: github.event.pull_request.merged == true || github.event_name == 'push'  || github.event_name == 'workflow_dispatch'
    runs-on: ubuntu-latest

    steps:
@@ -28,7 +28,7 @@ jobs:
      - name: crowdin push
        uses: crowdin/github-action@v2
        with:
-          config: 'docs/crowdin.yml'
+          config: 'docs/crowdin_docs.yml'
          upload_sources: true
          upload_translations: false
          download_translations: false
@@ -5,8 +5,6 @@ on:
    branches:
      - 'main'
      - 'release/**'
-    tags:
-      - 'v*'
    paths:
      - 'docs/en/**'
  pull_request:
@@ -70,7 +68,7 @@ jobs:
  deploy:
    if: ${{ github.event_name == 'push' || (github.event_name == 'pull_request' && github.event.pull_request.merged) || github.event_name == 'workflow_dispatch' }}
    needs: build
-    runs-on: [runs-on,runner=2cpu-linux-x64,image=ubuntu24-full-x64,"run-id=${{ github.run_id }}",spot=false]
+    runs-on: [runs-on,runner=8cpu-linux-x64,image=ubuntu24-full-x64,"run-id=${{ github.run_id }}",spot=false]

    steps:
      - name: Download Artifact
@@ -21,13 +21,13 @@ jobs:
      - name: Install Node.js
        uses: actions/setup-node@v3
        with:
-          node-version: '16'
+          node-version: '18'

      - name: Create logs directory
        run: |
          mkdir logs

-      - name: Get changed english files
+      - name: Get changed english doc files
        id: get_changed_markdown_english
        uses: tj-actions/changed-files@v35.9.2
        with:
@@ -48,10 +48,10 @@ jobs:
      - name: Run link checker
        id: link-check
        run: |
-          npm -g install markdown_link_checker_sc@0.0.134
-          markdown_link_checker_sc -r ${{ github.workspace }} -d en -f ./docs/logs/prFiles.json -i assets -u docs.px4.io/main/ > ./docs/logs/errorsFilteredByPrPages.md
+          npm -g install markdown_link_checker_sc@0.0.138
+          markdown_link_checker_sc -r ${{ github.workspace }} -d docs -e en -f ./logs/prFiles.json -i assets -u docs.px4.io/main/ > ./logs/errorsFilteredByPrPages.md
          mkdir -p ./pr
-          cp ./docs/logs/errorsFilteredByPrPages.md ./pr/errorsFilteredByPrPages.md
+          cp ./logs/errorsFilteredByPrPages.md ./pr/errorsFilteredByPrPages.md

      - name: Read errorsFilteredByPrPages.md file
        id: read-errors-by-page
@@ -95,62 +95,61 @@ jobs:
          echo "$EOF" >> $GITHUB_OUTPUT

  # TODO:
-  # This part of the workflow is causing errors, we should find a way to fix this and enable this test again
+  # This part of the workflow is causing errors for forks. We should find a way to fix this and enable it again for forks.
  # Track this issue https://github.com/PX4/PX4-Autopilot/issues/24408
-  #
-  #post_pr_comment:
-    #name: Publish Results
-    #runs-on: [runs-on,runner=1cpu-linux-x64,image=ubuntu24-full-x64,"run-id=${{ github.run_id }}",spot=false]
-    #needs: [analyze_flash]
-    #env:
-      #V5X-SUMMARY-MAP-ABS: ${{ fromJSON(fromJSON(needs.analyze_flash.outputs.px4_fmu-v5x-bloaty-summary-map).vm-absolute) }}
-      #V5X-SUMMARY-MAP-PERC: ${{ fromJSON(fromJSON(needs.analyze_flash.outputs.px4_fmu-v5x-bloaty-summary-map).vm-percentage) }}
-      #V6X-SUMMARY-MAP-ABS: ${{ fromJSON(fromJSON(needs.analyze_flash.outputs.px4_fmu-v6x-bloaty-summary-map).vm-absolute) }}
-      #V6X-SUMMARY-MAP-PERC: ${{ fromJSON(fromJSON(needs.analyze_flash.outputs.px4_fmu-v6x-bloaty-summary-map).vm-percentage) }}
-    #if: ${{ github.event.pull_request }}
-    #steps:
-      #- name: Find Comment
-        #uses: peter-evans/find-comment@v3
-        #id: fc
-        #with:
-          #issue-number: ${{ github.event.pull_request.number }}
-          #comment-author: 'github-actions[bot]'
-          #body-includes: FLASH Analysis
+  post_pr_comment:
+    name: Publish Results
+    runs-on: [runs-on,runner=1cpu-linux-x64,image=ubuntu24-full-x64,"run-id=${{ github.run_id }}",spot=false]
+    needs: [analyze_flash]
+    env:
+      V5X-SUMMARY-MAP-ABS: ${{ fromJSON(fromJSON(needs.analyze_flash.outputs.px4_fmu-v5x-bloaty-summary-map).vm-absolute) }}
+      V5X-SUMMARY-MAP-PERC: ${{ fromJSON(fromJSON(needs.analyze_flash.outputs.px4_fmu-v5x-bloaty-summary-map).vm-percentage) }}
+      V6X-SUMMARY-MAP-ABS: ${{ fromJSON(fromJSON(needs.analyze_flash.outputs.px4_fmu-v6x-bloaty-summary-map).vm-absolute) }}
+      V6X-SUMMARY-MAP-PERC: ${{ fromJSON(fromJSON(needs.analyze_flash.outputs.px4_fmu-v6x-bloaty-summary-map).vm-percentage) }}
+    if: github.event.pull_request && github.event.pull_request.head.repo.full_name == github.repository
+    steps:
+      - name: Find Comment
+        uses: peter-evans/find-comment@v3
+        id: fc
+        with:
+          issue-number: ${{ github.event.pull_request.number }}
+          comment-author: 'github-actions[bot]'
+          body-includes: FLASH Analysis

-      #- name: Set Build Time
-        #id: bt
-        #run: |
-          #echo "timestamp=$(date +'%Y-%m-%dT%H:%M:%S')" >> $GITHUB_OUTPUT
+      - name: Set Build Time
+        id: bt
+        run: |
+          echo "timestamp=$(date +'%Y-%m-%dT%H:%M:%S')" >> $GITHUB_OUTPUT

-      #- name: Create or update comment
-        ## This can't be moved to the job-level conditions, as GH actions don't allow a job-level if condition to access the env.
-        #if: |
-          #steps.fc.outputs.comment-id != '' ||
-          #env.V5X-SUMMARY-MAP-ABS >= fromJSON(env.MIN_FLASH_POS_DIFF_FOR_COMMENT) ||
-          #env.V5X-SUMMARY-MAP-ABS <= fromJSON(env.MIN_FLASH_NEG_DIFF_FOR_COMMENT) ||
-          #env.V6X-SUMMARY-MAP-ABS >= fromJSON(env.MIN_FLASH_POS_DIFF_FOR_COMMENT) ||
-          #env.V6X-SUMMARY-MAP-ABS <= fromJSON(env.MIN_FLASH_NEG_DIFF_FOR_COMMENT)
-        #uses: peter-evans/create-or-update-comment@v4
-        #with:
-          #comment-id: ${{ steps.fc.outputs.comment-id }}
-          #issue-number: ${{ github.event.pull_request.number }}
-          #body: |
-            ### 🔎 FLASH Analysis
-            #<details>
-              #<summary>px4_fmu-v5x [Total VM Diff: ${{ env.V5X-SUMMARY-MAP-ABS }} byte (${{ env.V5X-SUMMARY-MAP-PERC}} %)]</summary>
+      - name: Create or update comment
+        # This can't be moved to the job-level conditions, as GH actions don't allow a job-level if condition to access the env.
+        if: |
+          steps.fc.outputs.comment-id != '' ||
+          env.V5X-SUMMARY-MAP-ABS >= fromJSON(env.MIN_FLASH_POS_DIFF_FOR_COMMENT) ||
+          env.V5X-SUMMARY-MAP-ABS <= fromJSON(env.MIN_FLASH_NEG_DIFF_FOR_COMMENT) ||
+          env.V6X-SUMMARY-MAP-ABS >= fromJSON(env.MIN_FLASH_POS_DIFF_FOR_COMMENT) ||
+          env.V6X-SUMMARY-MAP-ABS <= fromJSON(env.MIN_FLASH_NEG_DIFF_FOR_COMMENT)
+        uses: peter-evans/create-or-update-comment@v4
+        with:
+          comment-id: ${{ steps.fc.outputs.comment-id }}
+          issue-number: ${{ github.event.pull_request.number }}
+          body: |
+            ## 🔎 FLASH Analysis
+            <details>
+              <summary>px4_fmu-v5x [Total VM Diff: ${{ env.V5X-SUMMARY-MAP-ABS }} byte (${{ env.V5X-SUMMARY-MAP-PERC}} %)]</summary>

-              #```
-              #${{ needs.analyze_flash.outputs.px4_fmu-v5x-bloaty-output }}
-              #```
-            #</details>
+              ```
+              ${{ needs.analyze_flash.outputs.px4_fmu-v5x-bloaty-output }}
+              ```
+            </details>

-            #<details>
-              #<summary>px4_fmu-v6x [Total VM Diff: ${{ env.V6X-SUMMARY-MAP-ABS }} byte (${{ env.V6X-SUMMARY-MAP-PERC }} %)]</summary>
+            <details>
+              <summary>px4_fmu-v6x [Total VM Diff: ${{ env.V6X-SUMMARY-MAP-ABS }} byte (${{ env.V6X-SUMMARY-MAP-PERC }} %)]</summary>

-              #```
-              #${{ needs.analyze_flash.outputs.px4_fmu-v6x-bloaty-output }}
-              #```
-            #</details>
+              ```
+              ${{ needs.analyze_flash.outputs.px4_fmu-v6x-bloaty-output }}
+              ```
+            </details>

-            #**Updated: _${{ steps.bt.outputs.timestamp }}_**
-          #edit-mode: replace
+            **Updated: _${{ steps.bt.outputs.timestamp }}_**
+          edit-mode: replace
@@ -0,0 +1,123 @@
+# NOTE: this workflow is now running on Dronecode / PX4 AWS account.
+# - If you want to keep the tests running in GitHub Actions you need to uncomment the "runs-on: ubuntu-latest" lines
+#   and comment the "runs-on: [runs-on,runner=..." lines.
+# - If you would like to duplicate this setup try setting up "RunsOn" on your own AWS account try https://runs-on.com
+
+name: ROS Integration Tests
+
+on:
+  push:
+    branches:
+    - 'main'
+    paths-ignore:
+      - 'docs/**'
+      - '.github/**'
+  pull_request:
+    branches:
+    - '*'
+    paths-ignore:
+      - 'docs/**'
+      - '.github/**'
+
+jobs:
+  build:
+    runs-on: [runs-on,runner=16cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}",spot=false]
+    container:
+      image: px4io/px4-dev-ros2-galactic:2021-09-08
+      options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
+    steps:
+    - uses: actions/checkout@v4
+      with:
+        fetch-depth: 0
+
+    - name: Git Ownership Workaround
+      run: git config --system --add safe.directory '*'
+
+    - name: Install gazebo
+      run: |
+        apt update && apt install -y gazebo11 libgazebo11-dev gstreamer1.0-plugins-bad gstreamer1.0-plugins-base gstreamer1.0-plugins-good gstreamer1.0-plugins-ugly libgstreamer-plugins-base1.0-dev
+
+    - name: Prepare ccache timestamp
+      id: ccache_cache_timestamp
+      shell: cmake -P {0}
+      run: |
+        string(TIMESTAMP current_date "%Y-%m-%d-%H;%M;%S" UTC)
+        message("::set-output name=timestamp::${current_date}")
+    - name: ccache cache files
+      uses: actions/cache@v4
+      with:
+        path: ~/.ccache
+        key: ros_integration_tests-${{matrix.config.build_type}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}}
+        restore-keys: ros_integration_tests-${{matrix.config.build_type}}-ccache-
+    - name: setup ccache
+      run: |
+        mkdir -p ~/.ccache
+        echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf
+        echo "compression = true" >> ~/.ccache/ccache.conf
+        echo "compression_level = 6" >> ~/.ccache/ccache.conf
+        echo "max_size = 300M" >> ~/.ccache/ccache.conf
+        echo "hash_dir = false" >> ~/.ccache/ccache.conf
+        ccache -s
+        ccache -z
+
+    - name: Get and build micro-xrce-dds-agent
+      run: |
+        cd /opt
+        git clone --recursive https://github.com/eProsima/Micro-XRCE-DDS-Agent.git
+        cd Micro-XRCE-DDS-Agent
+        git checkout v2.2.1 # recent versions require cmake 3.22, but px4-dev-ros2-galactic:2021-09-08 is on 3.16
+        sed -i 's/_fastdds_tag 2.8.x/_fastdds_tag 2.8.2/g' CMakeLists.txt
+        mkdir build
+        cd build
+        cmake ..
+        make -j2
+    - name: ccache post-run micro-xrce-dds-agent
+      run: ccache -s
+
+    - name: Get and build the ros2 interface library
+      shell: bash
+      run: |
+        PX4_DIR="$(pwd)"
+        . /opt/ros/galactic/setup.bash
+        mkdir -p /opt/px4_ws/src
+        cd /opt/px4_ws/src
+        git clone --recursive https://github.com/Auterion/px4-ros2-interface-lib.git
+        cd ..
+        # Copy messages to ROS workspace
+        "${PX4_DIR}/Tools/copy_to_ros_ws.sh" "$(pwd)"
+        rm -rf src/translation_node src/px4_msgs_old
+        colcon build --symlink-install
+    - name: ccache post-run ros workspace
+      run: ccache -s
+
+    - name: Build PX4
+      run: make px4_sitl_default
+    - name: ccache post-run px4/firmware
+      run: ccache -s
+    - name: Build SITL Gazebo
+      run: make px4_sitl_default sitl_gazebo-classic
+    - name: ccache post-run sitl_gazebo-classic
+      run: ccache -s
+
+    - name: Core dump settings
+      run: |
+        ulimit -c unlimited
+        echo "`pwd`/%e.core" > /proc/sys/kernel/core_pattern
+
+    - name: Run tests
+      shell: bash
+      run: |
+        . /opt/px4_ws/install/setup.bash
+        /opt/Micro-XRCE-DDS-Agent/build/MicroXRCEAgent udp4 localhost -p 8888 -v 0 &
+        test/ros_test_runner.py --verbose --model iris --upload --force-color
+      timeout-minutes: 45
+
+    - name: Upload failed logs
+      if: failure()
+      uses: actions/upload-artifact@v4
+      with:
+        name: failed-logs.zip
+        path: |
+          logs/**/**/**/*.log
+          logs/**/**/**/*.ulg
+          build/px4_sitl_default/tmp_ros_tests/rootfs/log/**/*.ulg
@@ -116,7 +116,7 @@ jobs:
          PX4_HOME_LON: ${{matrix.config.longitude}}
          PX4_HOME_ALT: ${{matrix.config.altitude}}
          PX4_CMAKE_BUILD_TYPE: ${{matrix.config.build_type}}
-        run: test/mavsdk_tests/mavsdk_test_runner.py --speed-factor 10 --abort-early --model ${{matrix.config.model}} --upload test/mavsdk_tests/configs/sitl.json --verbose
+        run: test/mavsdk_tests/mavsdk_test_runner.py --speed-factor 10 --abort-early --model ${{matrix.config.model}} --upload test/mavsdk_tests/configs/sitl.json --verbose --force-color
        timeout-minutes: 45

      - name: Upload failed logs
@@ -127,7 +127,7 @@ jobs:
          path: |
            logs/**/**/**/*.log
            logs/**/**/**/*.ulg
-            build/px4_sitl_default/tmp_mavsdk_tests/rootfs/*.ulg
+            build/px4_sitl_default/tmp_mavsdk_tests/rootfs/log/**/*.ulg

      - name: Look at Core files
        if: failure() && ${{ hashFiles('px4.core') != '' }}
@@ -108,3 +108,4 @@ src/systemcmds/topic_listener/listener_generated.cpp

 # colcon
 log/
+keys/
@@ -231,6 +231,11 @@ CONFIG:
      buildType: MinSizeRel
      settings:
        CONFIG: bitcraze_crazyflie_default
+    bluerobotics_navigator_default:
+      short: bluerobotics_navigator
+      buildType: MinSizeRel
+      settings:
+        CONFIG: bluerobotics_navigator_default
    cuav_can-gps-v1_default:
      short: cuav_can-gps-v1_default
      buildType: MinSizeRel
@@ -35,8 +35,11 @@ add_subdirectory(airframes)

 px4_add_romfs_files(
 	px4-rc.mavlink
-	px4-rc.params
 	px4-rc.simulator
+	px4-rc.gzsim
+	px4-rc.jmavsim
+	px4-rc.mavlinksim
+	px4-rc.sihsim
 	rc.replay
 	rcS
 )
@@ -20,7 +20,7 @@ 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?

 param set-default CA_AIRFRAME 14
-param set-default MAV_TYPE 99
+param set-default MAV_TYPE 7  # Using Airship

 param set-default CA_THRUSTER_CNT 8
 param set-default CA_R_REV 0
@@ -0,0 +1,180 @@
+#!/bin/sh
+# shellcheck disable=SC2154
+
+echo "INFO  [init] Gazebo simulator"
+
+# Enforce minimum gz version as Harmonic (gz-sim8)
+MIN_GZ_VERSION="8.0.0"
+GZ_SIM_VERSION=$(gz sim --versions 2>/dev/null | head -n 1 | tr -d ' ')
+
+if [ -z "$GZ_SIM_VERSION" ]; then
+    echo "ERROR [init] Gazebo gz sim not found. Please install gz-harmonic"
+    exit 1
+fi
+
+# Use sort compare, check that MIN_GZ_VERSION is ordered last
+if [ "$(printf '%s\n' "$GZ_SIM_VERSION" "$MIN_GZ_VERSION" | sort -V | head -n1)" = "$MIN_GZ_VERSION" ]; then
+	gz_command="gz"
+	gz_sub_command="sim"
+
+	# Specify render engine if `GZ_SIM_RENDER_ENGINE` is set
+	# (for example, if you want to use Ogre 1.x instead of Ogre 2.x):
+	if [ -n "${PX4_GZ_SIM_RENDER_ENGINE}" ]; then
+		echo "INFO  [init] Setting Gazebo render engine to '${PX4_GZ_SIM_RENDER_ENGINE}'!"
+		gz_sub_command="${gz_sub_command} --render-engine ${PX4_GZ_SIM_RENDER_ENGINE}"
+	fi
+else
+    echo "ERROR [init] Gazebo gz sim version is too old ($GZ_SIM_VERSION). Minimum required version is $MIN_GZ_VERSION"
+    exit 1
+fi
+
+# If not standalone
+if [ -z "${PX4_GZ_STANDALONE}" ]; then
+
+	# Look for an already running world
+	gz_world=$( ${gz_command} topic -l | grep -m 1 -e "^/world/.*/clock" | sed 's/\/world\///g; s/\/clock//g' )
+
+	# shellcheck disable=SC2153
+	if [ -z "${gz_world}" ] && [ -n "${PX4_GZ_WORLD}" ]; then
+
+		# Setup gz environment variables
+		if [ -f ./gz_env.sh ]; then
+			. ./gz_env.sh
+
+		elif [ -f ../gz_env.sh ]; then
+			. ../gz_env.sh
+		fi
+
+		echo "INFO  [init] Starting gazebo with world: ${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf"
+
+		${gz_command} ${gz_sub_command} --verbose=${GZ_VERBOSE:=1} -r -s "${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf" &
+
+		if [ -z "${HEADLESS}" ]; then
+			echo "INFO  [init] Starting gz gui"
+			${gz_command} ${gz_sub_command} -g > /dev/null 2>&1 &
+		fi
+
+	else
+		# Gazebo is already running
+		echo "INFO  [init] gazebo already running world: ${gz_world}"
+		PX4_GZ_WORLD=${gz_world}
+	fi
+
+else
+	echo "INFO  [init] Standalone PX4 launch, waiting for Gazebo"
+fi
+
+# Wait for Gazebo world to be ready before proceeding
+check_scene_info() {
+	SERVICE_INFO=$(${gz_command} service -i --service "/world/${PX4_GZ_WORLD}/scene/info" 2>&1)
+	if echo "$SERVICE_INFO" | grep -q "Service providers"; then
+		return 0
+	else
+		return 1
+	fi
+}
+
+ATTEMPTS=30
+while [ $ATTEMPTS -gt 0 ]; do
+	if check_scene_info; then
+		echo "INFO  [init] Gazebo world is ready"
+		break
+	fi
+	ATTEMPTS=$((ATTEMPTS-1))
+	if [ $ATTEMPTS -eq 0 ]; then
+		echo "ERROR [init] Timed out waiting for Gazebo world"
+		exit 1
+	fi
+	echo "INFO  [init] Waiting for Gazebo world..."
+	sleep 1
+done
+
+# Start gz_bridge - either spawn a model or connect to existing one
+if [ -n "${PX4_SIM_MODEL#*gz_}" ] && [ -z "${PX4_GZ_MODEL_NAME}" ]; then
+	# Spawn a model
+	MODEL_NAME="${PX4_SIM_MODEL#*gz_}"
+	MODEL_NAME_INSTANCE="${MODEL_NAME}_${px4_instance}"
+
+	POSE_ARG=""
+	if [ -n "${PX4_GZ_MODEL_POSE}" ]; then
+		pos_x=$(echo "${PX4_GZ_MODEL_POSE}" | awk -F',' '{print $1}')
+		pos_y=$(echo "${PX4_GZ_MODEL_POSE}" | awk -F',' '{print $2}')
+		pos_z=$(echo "${PX4_GZ_MODEL_POSE}" | awk -F',' '{print $3}')
+		pos_x=${pos_x:-0}
+		pos_y=${pos_y:-0}
+		pos_z=${pos_z:-0}
+
+		POSE_ARG=", pose: { position: { x: ${pos_x}, y: ${pos_y}, z: ${pos_z} } }"
+		echo "INFO  [init] Spawning model at position: ${pos_x} ${pos_y} ${pos_z}"
+	fi
+
+	echo "INFO  [init] Spawning model"
+
+	# Spawn model
+	${gz_command} service -s "/world/${PX4_GZ_WORLD}/create" --reqtype gz.msgs.EntityFactory \
+			--reptype gz.msgs.Boolean --timeout 5000 \
+			--req "sdf_filename: \"${PX4_GZ_MODELS}/${MODEL_NAME}/model.sdf\", name: \"${MODEL_NAME_INSTANCE}\", allow_renaming: false${POSE_ARG}" > /dev/null 2>&1
+
+	# Wait for model to spawn
+	sleep 1
+
+	# Start gz_bridge
+	if ! gz_bridge start -w "${PX4_GZ_WORLD}" -n "${MODEL_NAME_INSTANCE}"; then
+		echo "ERROR [init] gz_bridge failed to start and spawn model"
+		exit 1
+	fi
+
+	# Set physics parameters for faster-than-realtime simulation if needed
+	if [ -n "${PX4_SIM_SPEED_FACTOR}" ]; then
+		echo "INFO  [init] Setting simulation speed factor: ${PX4_SIM_SPEED_FACTOR}"
+		${gz_command} service -s "/world/${PX4_GZ_WORLD}/set_physics" --reqtype gz.msgs.Physics \
+					--reptype gz.msgs.Boolean --timeout 5000 \
+					--req "real_time_factor: ${PX4_SIM_SPEED_FACTOR}" > /dev/null 2>&1
+	fi
+
+	# Set up camera to follow the model if requested
+	if [ -n "${PX4_GZ_FOLLOW}" ]; then
+
+		echo "INFO  [init] Setting camera to follow ${MODEL_NAME_INSTANCE}"
+
+		# Set camera to follow the model
+		${gz_command} service -s "/gui/follow" --reqtype gz.msgs.StringMsg \
+				--reptype gz.msgs.Boolean --timeout 5000 \
+				--req "data: \"${MODEL_NAME_INSTANCE}\"" > /dev/null 2>&1
+
+		# Set default camera offset if not specified
+		follow_x=${PX4_GZ_FOLLOW_OFFSET_X:--2.0}
+		follow_y=${PX4_GZ_FOLLOW_OFFSET_Y:--2.0}
+		follow_z=${PX4_GZ_FOLLOW_OFFSET_Z:-2.0}
+
+		# Set camera offset
+		${gz_command} service -s "/gui/follow/offset" --reqtype gz.msgs.Vector3d \
+				--reptype gz.msgs.Boolean --timeout 5000 \
+				--req "x: ${follow_x}, y: ${follow_y}, z: ${follow_z}" > /dev/null 2>&1
+
+		echo "INFO  [init] Camera follow offset set to ${follow_x}, ${follow_y}, ${follow_z}"
+	fi
+
+elif [ -n "${PX4_GZ_MODEL_NAME}" ]; then
+	# Connect to existing model
+	echo "INFO  [init] PX4_GZ_MODEL_NAME set, PX4 will attach to existing model"
+	if ! gz_bridge start -w "${PX4_GZ_WORLD}" -n "${PX4_GZ_MODEL_NAME}"; then
+		echo "ERROR [init] gz_bridge failed to start and attach to existing model"
+		exit 1
+	fi
+else
+	echo "ERROR [init] failed to pass either PX4_GZ_MODEL_NAME or PX4_SIM_MODEL"
+	exit 1
+fi
+
+# NOTE: Only for rover_mecanum and spacecraft_2d. All other models have
+# the magnetometer sensor in the model.sdf.
+if param compare -s SENS_EN_MAGSIM 1
+then
+	sensor_mag_sim start
+fi
+# NOTE: new gz has airspeed sensor, remove once added
+if param compare -s SENS_EN_ARSPDSIM 1
+then
+	sensor_airspeed_sim start
+fi
@@ -0,0 +1,14 @@
+#!/bin/sh
+# shellcheck disable=SC2154
+
+echo "INFO  [init] jMAVSim simulator"
+
+if jps | grep -i jmavsim; then
+	kill "$(jps | grep -i jmavsim | awk '{print $1}')" || true
+	sleep 1
+fi
+
+param set IMU_INTEG_RATE 250
+./jmavsim_run.sh -l -r 250 &
+
+simulator_mavlink start -h localhost $((4560+px4_instance))
@@ -0,0 +1,27 @@
+#!/bin/sh
+# shellcheck disable=SC2154
+
+# EKF2 specifics
+param set-default EKF2_GPS_DELAY 10
+param set-default EKF2_MULTI_IMU 3
+param set-default SENS_IMU_MODE 0
+
+simulator_tcp_port=$((4560+px4_instance))
+
+# Check if PX4_SIM_HOSTNAME environment variable is empty
+# If empty check if PX4_SIM_HOST_ADDR environment variable is empty
+# If both are empty use localhost for simulator
+if [ -z "${PX4_SIM_HOSTNAME}" ]; then
+
+	if [ -z "${PX4_SIM_HOST_ADDR}" ]; then
+		echo "INFO  [init] PX4_SIM_HOSTNAME: localhost"
+		simulator_mavlink start -c $simulator_tcp_port
+	else
+		echo "INFO  [init] PX4_SIM_HOSTNAME: ${PX4_SIM_HOST_ADDR}"
+		simulator_mavlink start -t "${PX4_SIM_HOST_ADDR}" "${simulator_tcp_port}"
+	fi
+
+else
+	echo "INFO  [init] PX4_SIM_HOSTNAME: ${PX4_SIM_HOSTNAME}"
+	simulator_mavlink start -h "${PX4_SIM_HOSTNAME}" "${simulator_tcp_port}"
+fi
@@ -1,5 +0,0 @@
-#!/bin/sh
-# shellcheck disable=SC2154
-
-#param set-default MAV_SYS_ID $((px4_instance+1))
-#param set-default IMU_INTEG_RATE 250
@@ -0,0 +1,38 @@
+#!/bin/sh
+
+echo "INFO  [init] SIH simulator"
+
+if [ -n "${PX4_HOME_LAT}" ]; then
+	param set SIH_LOC_LAT0 ${PX4_HOME_LAT}
+fi
+
+if [ -n "${PX4_HOME_LON}" ]; then
+	param set SIH_LOC_LON0 ${PX4_HOME_LON}
+fi
+if [ -n "${PX4_HOME_ALT}" ]; then
+	param set SIH_LOC_H0 ${PX4_HOME_ALT}
+fi
+
+if simulator_sih start; then
+
+	if param compare -s SENS_EN_BAROSIM 1
+	then
+		sensor_baro_sim start
+	fi
+	if param compare -s SENS_EN_GPSSIM 1
+	then
+		sensor_gps_sim start
+	fi
+	if param compare -s SENS_EN_MAGSIM 1
+	then
+		sensor_mag_sim start
+	fi
+	if param compare -s SENS_EN_AGPSIM 1
+	then
+		sensor_agp_sim start
+	fi
+
+else
+	echo "ERROR [init] simulator_sih failed to start"
+	exit 1
+fi
@@ -8,268 +8,18 @@ param set-default IMU_INTEG_RATE 250
 param set-default COM_MODE_ARM_CHK 1

 if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0" ]; then
-
-	echo "INFO  [init] SIH simulator"
-
-	if [ -n "${PX4_HOME_LAT}" ]; then
-		param set SIH_LOC_LAT0 ${PX4_HOME_LAT}
-	fi
-
-	if [ -n "${PX4_HOME_LON}" ]; then
-		param set SIH_LOC_LON0 ${PX4_HOME_LON}
-	fi
-	if [ -n "${PX4_HOME_ALT}" ]; then
-		param set SIH_LOC_H0 ${PX4_HOME_ALT}
-	fi
-
-	if simulator_sih start; then
-
-		if param compare -s SENS_EN_BAROSIM 1
-		then
-			sensor_baro_sim start
-		fi
-		if param compare -s SENS_EN_GPSSIM 1
-		then
-			sensor_gps_sim start
-		fi
-		if param compare -s SENS_EN_MAGSIM 1
-		then
-			sensor_mag_sim start
-		fi
-		if param compare -s SENS_EN_AGPSIM 1
-		then
-			sensor_agp_sim start
-		fi
-
-	else
-		echo "ERROR [init] simulator_sih failed to start"
-		exit 1
-	fi
+    # Run SIH
+    . px4-rc.sihsim

 elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then
-	# Use Gazebo
-
-	echo "INFO  [init] Gazebo simulator"
-
-	# Only start up Gazebo if PX4_GZ_STANDALONE is not set.
-	if [ -z "${PX4_GZ_STANDALONE}" ]; then
-
-		# "gz sim" only available in Garden and later
-		GZ_SIM_VERSIONS=$(gz sim --versions 2>&1)
-		if [ $? -eq 0 ] && [ "${GZ_SIM_VERSIONS}" != "" ]
-		then
-			# "gz sim" from Garden on
-			gz_command="gz"
-			gz_sub_command="sim"
-
-			# Specify render engine if `GZ_SIM_RENDER_ENGINE` is set
-			# (for example, if you want to use Ogre 1.x instead of Ogre 2.x):
-			if [ -n "${PX4_GZ_SIM_RENDER_ENGINE}" ]; then
-				echo "INFO  [init] Setting Gazebo render engine to '${PX4_GZ_SIM_RENDER_ENGINE}'!"
-				gz_sub_command="${gz_sub_command} --render-engine ${PX4_GZ_SIM_RENDER_ENGINE}"
-			fi
-		else
-			echo "ERROR [init] Gazebo gz please install gz-garden"
-			exit 1
-		fi
-
-		# Look for an already running world
-		gz_world=$( ${gz_command} topic -l | grep -m 1 -e "^/world/.*/clock" | sed 's/\/world\///g; s/\/clock//g' )
-
-		# shellcheck disable=SC2153
-		if [ -z "${gz_world}" ] && [ -n "${PX4_GZ_WORLD}" ]; then
-
-			# Setup gz environment variables
-			if [ -f ./gz_env.sh ]; then
-				. ./gz_env.sh
-
-			elif [ -f ../gz_env.sh ]; then
-				. ../gz_env.sh
-			fi
-
-			echo "INFO  [init] Starting gazebo with world: ${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf"
-
-			${gz_command} ${gz_sub_command} --verbose=${GZ_VERBOSE:=1} -r -s "${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf" &
-
-			if [ -z "${HEADLESS}" ]; then
-				echo "INFO  [init] Starting gz gui"
-				${gz_command} ${gz_sub_command} -g > /dev/null 2>&1 &
-			fi
-
-			# Wait for Gazebo world to be ready before proceeding
-			check_scene_info() {
-				SERVICE_INFO=$(${gz_command} service -i --service "/world/${PX4_GZ_WORLD}/scene/info" 2>&1)
-				if echo "$SERVICE_INFO" | grep -q "Service providers"; then
-					return 0
-				else
-					return 1
-				fi
-			}
-
-			ATTEMPTS=30
-			while [ $ATTEMPTS -gt 0 ]; do
-				if check_scene_info; then
-					echo "INFO  [init] Gazebo world is ready"
-					break
-				fi
-				ATTEMPTS=$((ATTEMPTS-1))
-				if [ $ATTEMPTS -eq 0 ]; then
-					echo "ERROR [init] Timed out waiting for Gazebo world"
-					exit 1
-				fi
-				echo "INFO  [init] Waiting for Gazebo world..."
-				sleep 1
-			done
-
-		else
-			# Gazebo is already running
-			echo "INFO  [init] gazebo already running world: ${gz_world}"
-			PX4_GZ_WORLD=${gz_world}
-		fi
-
-	else
-		echo "INFO  [init] Standalone PX4 launch, waiting for Gazebo"
-	fi
-
-	# Start gz_bridge - either spawn a model or connect to existing one
-	if [ -n "${PX4_SIM_MODEL#*gz_}" ] && [ -z "${PX4_GZ_MODEL_NAME}" ]; then
-		# Spawn a model
-		MODEL_NAME="${PX4_SIM_MODEL#*gz_}"
-		MODEL_NAME_INSTANCE="${MODEL_NAME}_${px4_instance}"
-
-		POSE_ARG=""
-		if [ -n "${PX4_GZ_MODEL_POSE}" ]; then
-			pos_x=$(echo "${PX4_GZ_MODEL_POSE}" | awk -F',' '{print $1}')
-			pos_y=$(echo "${PX4_GZ_MODEL_POSE}" | awk -F',' '{print $2}')
-			pos_z=$(echo "${PX4_GZ_MODEL_POSE}" | awk -F',' '{print $3}')
-			pos_x=${pos_x:-0}
-			pos_y=${pos_y:-0}
-			pos_z=${pos_z:-0}
-
-			POSE_ARG=", pose: { position: { x: ${pos_x}, y: ${pos_y}, z: ${pos_z} } }"
-			echo "INFO  [init] Spawning model at position: ${pos_x} ${pos_y} ${pos_z}"
-		fi
-
-		echo "INFO  [init] Spawning model"
-
-		# Spawn model
-		${gz_command} service -s "/world/${PX4_GZ_WORLD}/create" --reqtype gz.msgs.EntityFactory \
-				--reptype gz.msgs.Boolean --timeout 5000 \
-				--req "sdf_filename: \"${PX4_GZ_MODELS}/${MODEL_NAME}/model.sdf\", name: \"${MODEL_NAME_INSTANCE}\", allow_renaming: false${POSE_ARG}" > /dev/null 2>&1
-
-		# Wait for model to spawn
-		sleep 1
-
-		# Start gz_bridge
-		if ! gz_bridge start -w "${PX4_GZ_WORLD}" -n "${MODEL_NAME_INSTANCE}"; then
-			echo "ERROR [init] gz_bridge failed to start and spawn model"
-			exit 1
-		fi
-
-		# Set physics parameters for faster-than-realtime simulation if needed
-		if [ -n "${PX4_SIM_SPEED_FACTOR}" ]; then
-			echo "INFO  [init] Setting simulation speed factor: ${PX4_SIM_SPEED_FACTOR}"
-			${gz_command} service -s "/world/${PX4_GZ_WORLD}/set_physics" --reqtype gz.msgs.Physics \
-						--reptype gz.msgs.Boolean --timeout 5000 \
-						--req "real_time_factor: ${PX4_SIM_SPEED_FACTOR}" > /dev/null 2>&1
-		fi
-
-		# Set up camera to follow the model if requested
-		if [ -n "${PX4_GZ_FOLLOW}" ]; then
-
-			echo "INFO  [init] Setting camera to follow ${MODEL_NAME_INSTANCE}"
-
-			# Set camera to follow the model
-			${gz_command} service -s "/gui/follow" --reqtype gz.msgs.StringMsg \
-					--reptype gz.msgs.Boolean --timeout 5000 \
-					--req "data: \"${MODEL_NAME_INSTANCE}\"" > /dev/null 2>&1
-
-			# Set default camera offset if not specified
-			follow_x=${PX4_GZ_FOLLOW_OFFSET_X:--2.0}
-			follow_y=${PX4_GZ_FOLLOW_OFFSET_Y:--2.0}
-			follow_z=${PX4_GZ_FOLLOW_OFFSET_Z:-2.0}
-
-			# Set camera offset
-			${gz_command} service -s "/gui/follow/offset" --reqtype gz.msgs.Vector3d \
-					--reptype gz.msgs.Boolean --timeout 5000 \
-					--req "x: ${follow_x}, y: ${follow_y}, z: ${follow_z}" > /dev/null 2>&1
-
-			echo "INFO  [init] Camera follow offset set to ${follow_x}, ${follow_y}, ${follow_z}"
-		fi
-
-	elif [ -n "${PX4_GZ_MODEL_NAME}" ]; then
-		# Connect to existing model
-		echo "INFO  [init] PX4_GZ_MODEL_NAME set, PX4 will attach to existing model"
-		if ! gz_bridge start -w "${PX4_GZ_WORLD}" -n "${PX4_GZ_MODEL_NAME}"; then
-			echo "ERROR [init] gz_bridge failed to start and attach to existing model"
-			exit 1
-		fi
-	else
-		echo "ERROR [init] failed to pass either PX4_GZ_MODEL_NAME or PX4_SIM_MODEL"
-		exit 1
-	fi
-	# Start the sensor simulator modules
-	if param compare -s SENS_EN_BAROSIM 1
-	then
-		sensor_baro_sim start
-	fi
-	if param compare -s SENS_EN_GPSSIM 1
-	then
-		sensor_gps_sim start
-	fi
-	if param compare -s SENS_EN_MAGSIM 1
-	then
-		sensor_mag_sim start
-	fi
-	if param compare -s SENS_EN_ARSPDSIM 1
-	then
-		sensor_airspeed_sim start
-	fi
-	if param compare -s SENS_EN_AGPSIM 1
-	then
-		sensor_agp_sim start
-	fi
+    # Run Gazebo (gz)
+    . px4-rc.gzsim

 elif [ "$PX4_SIM_MODEL" = "jmavsim_iris" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "10017" ]; then
-
-	echo "INFO  [init] jMAVSim simulator"
-
-	if jps | grep -i jmavsim; then
-		kill "$(jps | grep -i jmavsim | awk '{print $1}')" || true
-		sleep 1
-	fi
-
-	param set IMU_INTEG_RATE 250
-	./jmavsim_run.sh -l -r 250 &
-
-	simulator_mavlink start -h localhost $((4560+px4_instance))
+    # Run jMAVSim
+    . px4-rc.jmavsim

 else
-	# otherwise start simulator (mavlink) module
-
-	# EKF2 specifics
-	param set-default EKF2_GPS_DELAY 10
-	param set-default EKF2_MULTI_IMU 3
-	param set-default SENS_IMU_MODE 0
-
-	simulator_tcp_port=$((4560+px4_instance))
-
-	# Check if PX4_SIM_HOSTNAME environment variable is empty
-	# If empty check if PX4_SIM_HOST_ADDR environment variable is empty
-	# If both are empty use localhost for simulator
-	if [ -z "${PX4_SIM_HOSTNAME}" ]; then
-
-		if [ -z "${PX4_SIM_HOST_ADDR}" ]; then
-			echo "INFO  [init] PX4_SIM_HOSTNAME: localhost"
-			simulator_mavlink start -c $simulator_tcp_port
-		else
-			echo "INFO  [init] PX4_SIM_HOSTNAME: ${PX4_SIM_HOST_ADDR}"
-			simulator_mavlink start -t "${PX4_SIM_HOST_ADDR}" "${simulator_tcp_port}"
-		fi
-
-	else
-		echo "INFO  [init] PX4_SIM_HOSTNAME: ${PX4_SIM_HOSTNAME}"
-		simulator_mavlink start -h "${PX4_SIM_HOSTNAME}" "${simulator_tcp_port}"
-	fi
-
+    # Run simulator_mavlink module for interface with gazebo-classic
+    . px4-rc.mavlinksim
 fi
@@ -126,6 +126,15 @@ then
 	set AUTOCNF yes
 fi

+# Allow overriding parameters via env variables: export PX4_PARAM_{name}={value}
+env | while IFS='=' read -r line; do
+  value=${line#*=}
+  name=${line%%=*}
+  case $name in
+    "PX4_PARAM_"*) param set "${name#PX4_PARAM_}" "$value" ;;
+  esac
+done
+
 # multi-instance setup
 # shellcheck disable=SC2154
 param set MAV_SYS_ID $((px4_instance+1))
@@ -227,9 +236,6 @@ then
 	exit 1
 fi

-#user defined params for instances can be in PATH
-. px4-rc.params
-
 dataman start

 # only start the simulator if not in replay mode, as both control the lockstep time
@@ -30,12 +30,12 @@ param set-default RO_JERK_LIM 20
 param set-default RO_MAX_THR_SPEED 2.8

 # Rover Rate Control Parameters
-param set-default RO_YAW_RATE_I 0
-param set-default RO_YAW_RATE_P 0
-param set-default RO_YAW_RATE_LIM 0
+param set-default RO_YAW_RATE_I 0.1
+param set-default RO_YAW_RATE_P 0.1
+param set-default RO_YAW_RATE_LIM 120

 # Rover Attitude Control Parameters
-param set-default RO_YAW_P 0
+param set-default RO_YAW_P 2.5

 # Rover Position Control Parameters
 param set-default RO_SPEED_LIM 2.5
@@ -11,7 +11,7 @@
 . ${R}etc/init.d/rc.sc_defaults

 param set-default CA_AIRFRAME 14
-param set-default MAV_TYPE 99
+param set-default MAV_TYPE 7

 param set-default CA_THRUSTER_CNT 8
 param set-default CA_R_REV 0
@@ -5,8 +5,8 @@

 set VEHICLE_TYPE sc

-# MAV_TYPE_QUADROTOR 2
-#param set-default MAV_TYPE 12
+# MAV_TYPE_SPACECRAFT
+param set-default MAV_TYPE 7

 # Set micro-dds-client to use ethernet and IP-address 192.168.0.1
 param set-default UXRCE_DDS_AG_IP -1062731775
@@ -71,12 +71,6 @@ then
 fi


-# Heater driver for temperature regulated IMUs.
-if param compare -s SENS_EN_THERMAL 1
-then
-	heater start
-fi
-
 # Teraranger one tof sensor
 if param greater -s SENS_EN_TRANGER 0
 then
@@ -232,10 +226,7 @@ then

 	# compasses
 	hmc5883 -T -X -q start
-	if ! iis2mdc -X -q start
-	then
-		lis2mdl -X -q start
-	fi
+	iis2mdc -X -q start
 	ist8308 -X -q start
 	ist8310 -X -q start
 	if ! lis3mdl -X -q start
@@ -32,6 +32,15 @@ then
 	. ${R}etc/init.d/rc.rover_apps
 fi

+#
+# Spapcecraft setup.
+#
+if [ $VEHICLE_TYPE = sc ]
+then
+	# Start standard multicopter apps.
+	. ${R}etc/init.d/rc.sc_apps
+fi
+
 #
 # Differential Rover setup.
 #
@@ -404,6 +404,14 @@ else
 		fi
 	fi

+	# Heater driver for temperature regulated IMUs.
+	# The heater needs to start after px4io.
+	if param compare -s SENS_EN_THERMAL 1
+	then
+		heater start
+	fi
+
+
 	#
 	# RC update (map raw RC input to calibrate manual control)
 	#  start before commander
@@ -1,172 +0,0 @@
-#!/usr/bin/env python3
-
-import nacl.encoding
-import nacl.signing
-import nacl.hash
-import struct
-import binascii
-import json
-import time
-import argparse
-from pathlib import Path
-import sys
-
-def make_public_key_h_file(signing_key,key_name):
-    """
-    This file generate the public key header file
-    to be included into the bootloader build.
-    """
-    public_key_c='\n'
-    for i,c in enumerate(signing_key.verify_key.encode(encoder=nacl.encoding.RawEncoder)):
-        public_key_c+= hex(c)
-        public_key_c+= ', '
-        if((i+1)%8==0):
-            public_key_c+= '\n'
-    with open(key_name+'.pub' ,mode='w') as f:
-        f.write("//Public key to verify signed binaries")
-        f.write(public_key_c)
-
-def make_key_file(signing_key, key_name):
-    """
-    Writes the key.json file.
-    Attention do not override your existing key files.
-    Do not publish your private key!!
-    """
-
-    key_file = Path(key_name+'.json')
-    if key_file.is_file():
-        print("ATTENTION: key.json already exists, are you sure you want to overwrite it?")
-        print("Remove file and run script again.")
-        print("Script aborted!")
-        sys.exit(1)
-
-    keys={}
-    keys["date"] = time.asctime()
-    keys["public"] = (signing_key.verify_key.encode(encoder=nacl.encoding.HexEncoder)).decode()
-    keys["private"] = binascii.hexlify(signing_key._seed).decode()
-    #print (keys)
-    with open(key_name+'.json', "w") as write_file:
-        json.dump(keys, write_file)
-    return keys
-
-def ed25519_sign(private_key, signee_bin):
-    """
-    This function creates the signature. It takes the private key and the binary file
-    and returns the tuple (signature, public key)
-    """
-
-    signing_key = nacl.signing.SigningKey(private_key, encoder=nacl.encoding.HexEncoder)
-
-    # Sign a message with the signing key
-    signed = signing_key.sign(signee_bin,encoder=nacl.encoding.RawEncoder)
-
-    # Obtain the verify key for a given signing key
-    verify_key = signing_key.verify_key
-
-    # Serialize the verify key to send it to a third party
-    verify_key_hex = verify_key.encode(encoder=nacl.encoding.HexEncoder)
-
-    return signed.signature, verify_key_hex
-
-
-def sign(bin_file_path, key_file_path=None, generated_key_file=None):
-    """
-    reads the binary file and the key file.
-    If the key file does not exist, it generates a
-    new key file.
-    """
-
-    with open(bin_file_path,mode='rb') as f:
-        signee_bin = f.read()
-        # Align to 4 bytes. Signature always starts at
-        # 4 byte aligned address, but the signee size
-        # might not be aligned
-        if len(signee_bin)%4 != 0:
-            signee_bin += bytearray(b'\xff')*(4-len(signee_bin)%4)
-
-    try:
-        with open(key_file_path,mode='r') as f:
-            keys = json.load(f)
-        #print(keys)
-    except:
-        print('ERROR: Key file',key_file_path,'not  found')
-        sys.exit(1)
-
-    signature, public_key = ed25519_sign(keys["private"], signee_bin)
-
-    # Do a sanity check. This type of signature is always 64 bytes long
-    assert len(signature) == 64
-
-    # Print out the signing information
-    print("Binary \"%s\" signed."%bin_file_path)
-    print("Signature:",binascii.hexlify(signature))
-    print("Public key:",binascii.hexlify(public_key))
-
-    return signee_bin + signature, public_key
-
-def generate_key(key_file):
-    """
-    Generate two files:
-      "key_file.pub" containing the public key in C-format to be included in the bootloader build
-      "key_file.json, containt both private and public key.
-    Do not leak or loose the key file. This is mandatory for signing
-    all future binaries you want to deploy!
-    """
-
-    # Generate a new random signing key
-    signing_key = nacl.signing.SigningKey.generate()
-     # Serialize the verify key to send it to a third party
-    verify_key_hex = signing_key.verify_key.encode(encoder=nacl.encoding.HexEncoder)
-    print("public key :",verify_key_hex)
-
-    private_key_hex=binascii.hexlify(signing_key._seed)
-    print("private key :",private_key_hex)
-
-    keys = make_key_file(signing_key,key_file)
-    make_public_key_h_file(signing_key,key_file)
-    return keys
-
-if(__name__ == "__main__"):
-
-    parser = argparse.ArgumentParser(description="""CLI tool to calculate and add signature to px4. bin files\n
-                                                  if given it takes an existing key file, else it generate new keys""",
-                                    epilog="Output: SignedBin.bin and a key.json file")
-    parser.add_argument("signee", help=".bin file to add signature", nargs='?', default=None)
-    parser.add_argument("signed", help="signed output .bin", nargs='?', default=None)
-
-    parser.add_argument("--key", help="key.json file", default="Tools/test_keys/test_keys.json")
-    parser.add_argument("--rdct", help="binary R&D certificate file", default=None)
-    parser.add_argument("--genkey", help="new generated key", default=None)
-    args = parser.parse_args()
-
-    # Only generate a key pair, don't sign
-    if args.genkey:
-        # Only create a key file, don't sign
-        generate_key(args.genkey)
-        print('New key file generated:',args.genkey)
-        sys.exit(0);
-
-    # Check that both signee and signed exist
-    if not args.signee or not args.signed:
-        print("ERROR: Must either provide file names for both signee and signed")
-        print("       or --genkey [key] to generate a new key pair")
-        sys.exit(1)
-
-    # Issue a warning when signing with testing key
-    if args.key=='Tools/test_keys/test_keys.json':
-        print("WARNING: Signing with PX4 test key")
-
-    # Sign the binary
-    signed, public_key = sign(args.signee, args.key, args.genkey)
-
-    with open(args.signed, mode='wb') as fs:
-        # Write signed binary
-        fs.write(signed)
-
-    # Append rdcert if given
-    try:
-        with open(args.rdct ,mode='rb') as f:
-            with open(args.signed, mode='ab') as fs:
-                fs.write(f.read())
-    except:
-        pass
@@ -1,91 +0,0 @@
-#!/usr/bin/env python3
-
-import sys
-
-try:
-    from Crypto.Cipher import ChaCha20
-except ImportError as e:
-    print("Failed to import crypto: " + str(e))
-    print("")
-    print("You may need to install it using:")
-    print("    pip3 install --user pycryptodome")
-    print("")
-    sys.exit(1)
-
-from Crypto.PublicKey import RSA
-from Crypto.Cipher import PKCS1_OAEP
-from Crypto.Hash import SHA256
-from pathlib import Path
-import argparse
-
-
-if __name__ == "__main__":
-
-    parser = argparse.ArgumentParser(description="""CLI tool to decrypt an ulog file\n""")
-    parser.add_argument("ulog_file", help=".ulge/.ulgc, encrypted log file", nargs='?', default=None)
-    parser.add_argument("ulog_key", help=".ulgk, legacy encrypted key (give empty string '' to ignore for .ulge)", nargs='?', default=None)
-    parser.add_argument("rsa_key", help=".pem format key for decrypting the ulog key", nargs='?', default=None)
-
-    args = parser.parse_args()
-
-    # Check all arguments are given
-    if not args.rsa_key:
-        print('Need all arguments, the encrypted ulog file, key file (or empty string if not needed) and the key decryption key (.pem)')
-        sys.exit(1)
-
-    # Read the private RSA key to decrypt the cahcha key
-    with open(args.rsa_key, 'rb') as f:
-        r = RSA.importKey(f.read(), passphrase='')
-
-    if args.ulog_key == "":
-        key_data_filename = args.ulog_file
-        magic = "ULogEnc"
-    else:
-        key_data_filename = args.ulog_key
-        magic = "ULogKey"
-
-    with open(key_data_filename, 'rb') as f:
-        # Read the encrypted xchacha key and the nonce
-        ulog_key_header = f.read(22)
-
-        # Parse the header
-        try:
-            # magic
-            if not ulog_key_header.startswith(bytearray(magic.encode())):
-                print("Incorrect header magic")
-                raise Exception()
-            # version
-            if ulog_key_header[7] != 1:
-                print("Unsupported header version")
-                raise Exception()
-            # expected key exchange algorithm (RSA_OAEP)
-            if ulog_key_header[16] != 4:
-                print("Unsupported key algorithm")
-                raise Exception()
-            key_size = ulog_key_header[19] << 8 | ulog_key_header[18]
-            nonce_size = ulog_key_header[21] << 8 | ulog_key_header[20]
-            ulog_key_cipher = f.read(key_size)
-            nonce = f.read(nonce_size)
-        except:
-            print("Keydata format error")
-            sys.exit(1)
-
-    if magic == "ULogEnc":
-        data_offset = 22 + key_size + nonce_size
-    else:
-        data_offset = 0
-
-    # Decrypt the xchacha key
-    cipher_rsa = PKCS1_OAEP.new(r,SHA256)
-    ulog_key = cipher_rsa.decrypt(ulog_key_cipher)
-    #print(binascii.hexlify(ulog_key))
-
-    # Read and decrypt the ulog data
-    cipher = ChaCha20.new(key=ulog_key, nonce=nonce)
-
-    outfilename = Path(args.ulog_file).stem + ".ulog"
-    with open(args.ulog_file, 'rb') as f:
-        if data_offset > 0:
-            f.seek(data_offset)
-        with open(outfilename, 'wb') as out:
-            out.write(cipher.decrypt(f.read()))
@@ -5,8 +5,8 @@ if [ -z ${PX4_DOCKER_REPO+x} ]; then
 	if [[ $@ =~ .*px4_fmu.* ]]; then
 		# nuttx-px4fmu-v{1,2,3,4,5}
 		PX4_DOCKER_REPO="px4io/px4-dev-nuttx-focal:2022-08-12"
-	elif [[ $@ =~ .*navio2.* ]] || [[ $@ =~ .*raspberry.* ]] || [[ $@ =~ .*beaglebone.* ]] || [[ $@ =~ .*pilotpi.default ]]; then
-		# beaglebone_blue_default, emlid_navio2_default, px4_raspberrypi_default, scumaker_pilotpi_default
+	elif [[ $@ =~ .*navio2.* ]] || [[ $@ =~ .*raspberry.* ]] || [[ $@ =~ .*beaglebone.* ]] || [[ $@ =~ .*pilotpi.default ]] || [[ $@ =~ .*navigator.* ]]; then
+		# beaglebone_blue_default, emlid_navio2_default, px4_raspberrypi_default, scumaker_pilotpi_default, bluerobotics_navigator_default
 		PX4_DOCKER_REPO="px4io/px4-dev-armhf:2023-06-26"
 	elif [[ $@ =~ .*pilotpi.arm64 ]]; then
 		# scumaker_pilotpi_arm64
@@ -0,0 +1,86 @@
+# PX4 Log Encryption Tools
+
+Tools for generating encryption keys, building PX4 firmware with encrypted logs, downloading logs, and decrypting them.
+
+For more information see: https://docs.px4.io/main/en/dev_log/log_encryption.html
+
+## Usage
+
+1. **Get the board file**:
+   In order to use these tools you need to create an `encrypted_logs` target in your target board directory. For example:
+   ```bash
+   encrypted_logs.px4board
+   ```
+   Using `make menuconfig` you should enable these settings: `Blake2s hash algorithm`, `entropy pool` and `strong random number generator` and select `use interrupts` to feed timing randomness to the entropy pool.
+   Once you have generated the keys make sure you add them to the boardconfig.
+
+   ```bash
+   make <your_board_name>_encrypted_logs menuconfig
+   ```
+
+2. **Generate Keys**:
+   ```bash
+   cd PX4-Autopilot/Tools/log_encryption
+   python3 generate_keys.py
+   ```
+
+   Make sure you have the right key in your board file
+   ```CONFIG_PUBLIC_KEY1="../../../keys/public/public_key.pub"```
+
+3. **Build Firmware**:
+   ```bash
+   cd PX4-Autopilot
+
+   AND
+
+   make <your_board_name>_encrypted_logs
+
+   FOR INSTANCE
+   make_ark_fpv_encrypted_logs
+
+   Upload the custom firmware on your flight controller and record some logs
+   ```
+
+4. **Download Logs**:
+   ```bash
+   cd PX4-Autopilot/Tools/log_encryption
+
+   python3 download_logs.py /dev/ttyACM0 --baudrate 57600
+
+   OR
+
+   python3 download_logs.py udp:0.0.0.0:14550
+   ```
+
+   Addresses might need to be adjusted
+
+5. **Decrypt Logs**:
+   The easiest way to run this is to have your private key and encrypted logs in the following folders respectively:
+   ```bash
+   PX4-Autopilot/keys/private
+   PX4-Autopilot/logs/encrypted
+   ```
+   Then run:
+   ```bash
+   cd PX4-Autopilot/Tools/log_encryption
+
+   AND
+   # Uses default key + default folder
+   python3 decrypt_logs.py
+
+   OR
+   # Use --help to get all the options
+   python3 decrypt_logs.py --help
+   ```
+
+   Your decrypted logs can be found in:
+   ```bash
+   PX4-Autopilot/logs/decrypted
+   ```
+   Otherwise
+
+## Directory Structure
+
+- **`keys/`**: Encryption keys.
+- **`logs/encrypted/`**: Downloaded encrypted logs.
+- **`logs/decrypted/`**: Decrypted logs.
@@ -0,0 +1,135 @@
+#!/usr/bin/env python3
+
+import os
+import sys
+import argparse
+from pathlib import Path
+
+try:
+    from Crypto.Cipher import ChaCha20
+    from Crypto.PublicKey import RSA
+    from Crypto.Cipher import PKCS1_OAEP
+    from Crypto.Hash import SHA256
+except ImportError as e:
+    print("Failed to import crypto: " + str(e))
+    print("You may need to install it using:")
+    print("    pip3 install --user pycryptodome")
+    sys.exit(1)
+
+PX4_MAIN_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), "../.."))
+ENCRYPTED_LOGS_DIR = os.path.join(PX4_MAIN_DIR, "logs/encrypted")
+DECRYPTED_LOGS_DIR = os.path.join(PX4_MAIN_DIR, "logs/decrypted")
+DEFAULT_PRIVATE_KEY = os.path.join(PX4_MAIN_DIR, "keys/private/private_key.pem")
+
+def decrypt_log_file(ulog_file, private_key, output_folder):
+    """Decrypts a single encrypted log file (.ulge) and saves it as .ulg in the output folder."""
+
+    try:
+        # Read the private RSA key
+        with open(private_key, 'rb') as f:
+            key = RSA.import_key(f.read())
+
+        magic = "ULogEnc"
+        header_size = 22
+
+        with open(ulog_file, 'rb') as f:
+            # Encrypted .ulge file contains following sections:
+            # -------------------------
+            # | Header                |
+            # -------------------------
+            # | Wrapped symmetric key |
+            # -------------------------
+            # | Encrypted ulog data   |
+            # -------------------------
+            header = f.read(header_size)
+
+            # Parse the header
+            if not header.startswith(bytearray(magic.encode())):
+                print(f"Skipping {ulog_file}: Incorrect header magic")
+                return
+            if header[7] != 1:
+                print(f"Skipping {ulog_file}: Unsupported header version")
+                return
+            if header[16] != 4:
+                print(f"Skipping {ulog_file}: Unsupported key algorithm")
+                return
+
+            key_size = header[19] << 8 | header[18]
+            nonce_size = header[21] << 8 | header[20]
+            cipher = f.read(key_size)
+            nonce = f.read(nonce_size)
+
+        data_offset = header_size + key_size + nonce_size
+
+        # Try to decrypt the ChaCha key
+        cipher_rsa = PKCS1_OAEP.new(key, SHA256)
+        try:
+            ulog_key = cipher_rsa.decrypt(cipher)
+        except ValueError:
+            print(f"Skipping {ulog_file}: Incorrect decryption (wrong key)")
+            return
+
+        # Read and decrypt the log data
+        cipher = ChaCha20.new(key=ulog_key, nonce=nonce)
+
+        # Save decrypted log with .ulg extension
+        output_path = os.path.join(output_folder, Path(ulog_file).stem + ".ulg")
+        with open(ulog_file, 'rb') as f:
+            if data_offset > 0:
+                f.seek(data_offset)
+            with open(output_path, 'wb') as out:
+                out.write(cipher.decrypt(f.read()))
+
+        print(f"{output_path}")
+
+    except Exception as e:
+        print(f"Skipping {ulog_file}: Error occurred - {e}")
+
+
+def decrypt_all_logs(private_key_path, log_source_path=None):
+    """Decrypts all logs in the given folder or a single file."""
+
+    if log_source_path and os.path.isfile(log_source_path):
+        logs = [log_source_path]
+    else:
+        # Use default encrypted logs directory if not provided
+        folder = log_source_path if log_source_path else ENCRYPTED_LOGS_DIR
+        logs = [os.path.join(folder, f) for f in os.listdir(folder) if f.endswith(".ulge")]
+
+    if not logs:
+        print("No encrypted logs found.")
+        return
+
+    print(f"Found {len(logs)} encrypted log(s). Decrypting...")
+
+    os.makedirs(DECRYPTED_LOGS_DIR, exist_ok=True)
+
+    for log_path in logs:
+        decrypt_log_file(log_path, private_key_path, DECRYPTED_LOGS_DIR)
+
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser(
+    description="Decrypt PX4 encrypted log files (.ulge) using a ChaCha20+RSA scheme.\n\n"
+                "Usage examples:\n"
+                "  python3 decrypt_logs.py /path/to/private_key.pem /path/to/custom_log.ulge\n"
+                "  python3 decrypt_logs.py /path/to/private_key.pem /path/to/folder_with_ulge_files\n"
+                "  python3 decrypt_logs.py                  # Uses default key + default log folder\n",
+    formatter_class=argparse.RawTextHelpFormatter
+    )
+
+    parser.add_argument("private_key", nargs="?", default=None,
+                        help="Path to the private RSA key (.pem). If omitted, uses default key.")
+    parser.add_argument("log_file_or_folder", nargs="?", default=None,
+                        help="Path to a single .ulge file or folder containing them. If omitted, uses default encrypted log folder.")
+
+
+    args = parser.parse_args()
+
+    private_key_path = args.private_key if args.private_key else DEFAULT_PRIVATE_KEY
+
+    if not os.path.exists(private_key_path):
+        print(f"Error: Private key file not found at {private_key_path}")
+        sys.exit(1)
+
+    decrypt_all_logs(private_key_path, args.log_file_or_folder)
@@ -0,0 +1,190 @@
+#!/usr/bin/env python3
+
+import os
+import sys
+import time
+import shutil
+import threading
+from pymavlink import mavutil
+from argparse import ArgumentParser
+
+class MavlinkLogDownloader:
+    def __init__(self, connection_url, output_dir, baudrate=57600, source_system=254):
+        self.connection_url = connection_url
+        self.output_dir = output_dir
+        self.encrypted_dir = os.path.join(output_dir, "encrypted")
+        self.running = True
+
+        # Ensure directories exist
+        os.makedirs(self.output_dir, exist_ok=True)
+        os.makedirs(self.encrypted_dir, exist_ok=True)
+
+        # # Handle serial or UDP connections
+        if os.path.exists(connection_url):  # likely a serial device
+            self.mav = mavutil.mavlink_connection(connection_url, baud=baudrate, source_system=source_system)
+        else:
+            self.mav = mavutil.mavlink_connection(connection_url, source_system=source_system)
+
+
+        self.mav.WIRE_PROTOCOL_VERSION = "2.0"
+
+        # Start heartbeat thread
+        self.heartbeat_thread = threading.Thread(target=self.send_heartbeat_thread)
+        self.heartbeat_thread.daemon = True
+        self.heartbeat_thread.start()
+
+        self.mav.wait_heartbeat()
+        print(f"Heartbeat received from system {self.mav.target_system}, component {self.mav.target_component}")
+
+        # Waking up the autopilot, it is needed to ensure we get answer for log request
+        self.mav.mav.command_long_send(
+            self.mav.target_system,
+            self.mav.target_component,
+            mavutil.mavlink.MAV_CMD_REQUEST_MESSAGE,  # Command ID 512
+            0,  # Confirmation
+            mavutil.mavlink.MAVLINK_MSG_ID_AUTOPILOT_VERSION,  # param1: Message ID 148
+            0, 0, 0, 0, 0, 0  # params 2–7 are not used for this message
+        )
+
+        # Allow heartbeats to establish connection
+        time.sleep(3)
+
+
+    def send_heartbeat_thread(self):
+        while self.running:
+            self.mav.mav.heartbeat_send(
+                mavutil.mavlink.MAV_TYPE_GCS,
+                mavutil.mavlink.MAV_AUTOPILOT_GENERIC,
+                0, 0, 0
+            )
+            time.sleep(1)
+
+
+    def download_logs(self):
+        """Downloads logs to the output_dir."""
+        print("Request logs...")
+        self.mav.mav.log_request_list_send(self.mav.target_system, self.mav.target_component, 0, 0xFFFF)
+
+        log_entries = {}
+        total_logs = None
+        start_time = time.time()
+        last_entry_time = None
+
+        while True:
+            current_time = time.time()
+
+            # Case 1: If we haven't received any entries yet and we've waited more than 5 seconds
+            if not log_entries and current_time > start_time + 5:
+                print("Timed out waiting for first log entry (5s)")
+                break
+
+            # Case 2: If we have received at least one entry and it's been more than 3 seconds since the last one
+            if last_entry_time and current_time - last_entry_time > 3:
+                print(f"No new log entries received for 3 seconds. Moving on.")
+                break
+
+            # Case 3: If we've received all expected logs
+            if total_logs is not None and len(log_entries) >= total_logs:
+                print(f"Received all {total_logs} log entries.")
+                break
+
+            msg = self.mav.recv_match(type='LOG_ENTRY', blocking=True, timeout=1)
+
+            if msg and msg.id not in log_entries:
+                last_entry_time = time.time()
+                log_entries[msg.id] = msg
+
+                if total_logs is None:
+                    total_logs = msg.num_logs
+
+                print(f"Log ID: {msg.id}, Size: {msg.size} bytes, Date: {msg.time_utc} ({len(log_entries)}/{total_logs})")
+
+        if not log_entries:
+            print("No log entries found.")
+            return
+
+        for entry in log_entries.values():
+            self.download_log_file(entry)
+
+        self.classify_logs()
+
+
+    def download_log_file(self, log_entry):
+        """Downloads a log file to the output_dir."""
+        log_id = log_entry.id
+        log_size = log_entry.size
+        log_date = time.strftime("%Y-%m-%d_%H-%M-%S", time.gmtime(log_entry.time_utc))
+        output_filename = os.path.join(self.output_dir, f"log-{log_date}_{log_id}.ulg")
+
+        print(f"Downloading log {log_id} ({log_size} bytes) to {output_filename}...")
+
+        with open(output_filename, 'wb') as f:
+            self.mav.mav.log_request_data_send(self.mav.target_system, self.mav.target_component, log_id, 0, 0xFFFFFFFF)
+
+            bytes_received = 0
+            while bytes_received < log_size:
+                msg = self.mav.recv_match(type='LOG_DATA', blocking=True, timeout=5)
+                if msg:
+                    data_bytes = bytes(msg.data[:msg.count])
+                    f.write(data_bytes)
+                    bytes_received += msg.count
+                else:
+                    print("Timeout waiting for log data.")
+                    break
+
+        print(f"Finished downloading log {log_id}.")
+
+    def classify_logs(self):
+        """Classifies logs as encrypted (.ulge) based on file content."""
+        for log_file in os.listdir(self.output_dir):
+            log_path = os.path.join(self.output_dir, log_file)
+
+            if not os.path.isfile(log_path):
+                continue
+
+            # Read first 10 bytes to check for "ULogEnc"
+            with open(log_path, 'rb') as f:
+                first_bytes = f.read(10)
+
+            if b'ULogEnc' in first_bytes:
+                new_filename = log_file.replace(".ulg", ".ulge")
+                new_path = os.path.join(self.encrypted_dir, new_filename)
+                print(f"Found encrypted log: {new_path}")
+                shutil.move(log_path, new_path)
+
+
+    def cleanup(self):
+        """Stop the heartbeat thread and clean up resources."""
+        self.running = False
+        if self.heartbeat_thread.is_alive():
+            self.heartbeat_thread.join(timeout=2.0)
+
+
+def main():
+    parser = ArgumentParser(description="Download PX4 log files over MAVLink.")
+    parser.add_argument('connection_url', help="MAVLink connection URL (e.g., udp:0.0.0.0:14550, /dev/ttyACM0 --baudrate 57600)")
+    parser.add_argument('--output', '-o', default=os.path.join(os.path.dirname(__file__), "../..", "logs"), help="Output directory for log files (default: ../../logs)")
+    parser.add_argument('--baudrate', type=int, default=57600, help="Baudrate for serial connection (default: 57600)")
+    parser.add_argument('--source-system', type=int, default=254, help="MAVLink source system ID (default: 254)")
+
+
+    args = parser.parse_args()
+
+    output_dir = os.path.abspath(args.output)
+
+    print(f"Connecting to {args.connection_url}...")
+    log_downloader = MavlinkLogDownloader(
+        args.connection_url,
+        output_dir,
+        baudrate=args.baudrate,
+        source_system=args.source_system
+    )
+
+
+    try:
+        log_downloader.download_logs()
+    finally:
+        log_downloader.cleanup()
+
+if __name__ == '__main__':
+    main()
@@ -0,0 +1,59 @@
+import os
+import subprocess
+
+# Define the main PX4 directory (one level up from Tools)
+PX4_MAIN_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), "../.."))
+
+# Define the key folder paths
+KEY_FOLDER = os.path.join(PX4_MAIN_DIR, "keys")
+PUBLIC_KEY_FOLDER = os.path.join(KEY_FOLDER, "public")
+PRIVATE_KEY_FOLDER = os.path.join(KEY_FOLDER, "private")
+
+# Define the key file paths
+PRIVATE_KEY_PATH = os.path.join(PRIVATE_KEY_FOLDER, "private_key.pem")
+PUBLIC_KEY_DER_PATH = os.path.join(PUBLIC_KEY_FOLDER, "public_key.der")
+PUBLIC_KEY_PUB_PATH = os.path.join(PUBLIC_KEY_FOLDER, "public_key.pub")
+
+def create_key_folders():
+    """Creates key, public, and private folders if they do not exist."""
+    for folder in [KEY_FOLDER, PUBLIC_KEY_FOLDER, PRIVATE_KEY_FOLDER]:
+        if not os.path.exists(folder):
+            os.makedirs(folder)
+            print(f"Created '{folder}' directory.")
+        else:
+            print(f"'{folder}' directory already exists.")
+
+def generate_private_key():
+    """Generates a private key if it does not exist."""
+    if not os.path.exists(PRIVATE_KEY_PATH):
+        print("Generating private key...")
+        subprocess.run(["openssl", "genpkey", "-algorithm", "RSA", "-out", PRIVATE_KEY_PATH, "-pkeyopt", "rsa_keygen_bits:2048"])
+        print(f"Private key generated at: {PRIVATE_KEY_PATH}")
+    else:
+        print("Private key already exists.")
+
+def generate_public_key():
+    """Generates a public key in DER and PUB formats if they do not exist."""
+    if not os.path.exists(PUBLIC_KEY_DER_PATH):
+        print("Generating public key in DER format...")
+        subprocess.run(["openssl", "rsa", "-pubout", "-in", PRIVATE_KEY_PATH, "-outform", "DER", "-out", PUBLIC_KEY_DER_PATH])
+        print(f"Public key (DER) generated at: {PUBLIC_KEY_DER_PATH}")
+    else:
+        print("Public key (DER) already exists.")
+
+    if not os.path.exists(PUBLIC_KEY_PUB_PATH):
+        print("Generating public key in hex format...")
+        with open(PUBLIC_KEY_PUB_PATH, "w") as pub_file:
+            process = subprocess.Popen(["xxd", "-p", PUBLIC_KEY_DER_PATH], stdout=subprocess.PIPE)
+            output, _ = process.communicate()
+            hex_string = output.decode().strip().replace("\n", "")
+            formatted_hex = ", ".join(f"0x{hex_string[i:i+2]}" for i in range(0, len(hex_string), 2))
+            pub_file.write(formatted_hex)
+        print(f"Public key (hex) generated at: {PUBLIC_KEY_PUB_PATH}")
+    else:
+        print("Public key (hex) already exists.")
+
+if __name__ == "__main__":
+    create_key_folders()
+    generate_private_key()
+    generate_public_key()
@@ -30,12 +30,12 @@ def find_matching_brackets(brackets, s, verbose):

 def extract_timer(line):
    # Try format: initIOTimer(Timer::Timer5, DMA{DMA::Index1, DMA::Stream0, DMA::Channel6}),
-    search = re.search('Timer::([0-9a-zA-Z_]+)[,\)]', line, re.IGNORECASE)
+    search = re.search('Timer::([0-9a-zA-Z_]+)[,)]', line, re.IGNORECASE)
    if search:
        return search.group(1), 'generic'

    # NXP FlexPWM format format: initIOPWM(PWM::FlexPWM2),
-    search = re.search('PWM::Flex([0-9a-zA-Z_]+)..PWM::Submodule([0-9])[,\)]', line, re.IGNORECASE)
+    search = re.search('PWM::Flex([0-9a-zA-Z_]+)..PWM::Submodule([0-9])[,)]', line, re.IGNORECASE)
    if search:
        return (search.group(1) + '_' +  search.group(2)), 'imxrt'

@@ -59,7 +59,7 @@ from sys import platform as _platform

 def get_version():
    """
-        Get PX4 Firmware latest Git tag.
+        Get PX4-Autopilot Latest Git Tag.
    """
    px4_dir = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))

@@ -74,7 +74,7 @@ def get_version():
            exit(1)
    else:
        # Else, get it from remote repo tags (requires network access)
-        cmd = "git ls-remote --tags git://github.com/PX4/Firmware.git | cut -d/ -f3- | sort -n -t. -k1,1 -k2,2 -k3,3 | awk '/^v[^{]*$/{version=$1}END{print version}'"
+        cmd = "git ls-remote --tags git@github.com:PX4/PX4-Autopilot.git | cut -d/ -f3- | sort -n -t. -k1,1 -k2,2 -k3,3 | awk '/^v[^{]*$/{version=$1}END{print version}'"

        try:
            version = subprocess.check_output(
@@ -94,7 +94,7 @@ else:


 def main():
-    print("PX4 release", get_version())
+    print("PX4 Release", get_version())


 if __name__ == '__main__':
@@ -26,3 +26,4 @@ setuptools>=39.2.0
 six>=1.12.0
 toml>=0.9
 sympy>=1.10.1
+pycryptodome
@@ -70,7 +70,7 @@ export JSBSIM_AIRCRAFT_MODEL="$MODEL_NAME"
 if [[ -n "$HEADLESS" ]]; then
 	echo "not running flightgear gui"
 else
-	export FG_AIRCRAFT="${SRC_DIR}/Tools/simulation/jsbsim/jsbsim_bridge/models"
+	export FG_AIRCRAFT="${src_path}/Tools/simulation/jsbsim/jsbsim_bridge/models"

 	$FG_BINARY --fdm=null \
 		--native-fdm=socket,in,60,,5550,udp \
@@ -129,7 +129,7 @@ __EXPORT void board_on_reset(int status)
 *
 * Description:
 *   All STM32 architectures must provide the following entry point.  This entry point
- *   is called early in the intitialization -- after all memory has been configured
+ *   is called early in the initialization -- after all memory has been configured
 *   and mapped but before any devices have been initialized.
 *
 ************************************************************************************/
@@ -67,7 +67,7 @@ void rgb_led(int r, int g, int b, int freqs)
 	if (!once) {
 		once = 1;

-		/* Enabel Clock to Block */
+		/* Enable Clock to Block */
 		modifyreg32(STM32_RCC_APB2ENR, 0, RCC_APB2ENR_TIM1EN);

 		/* Reload */
@@ -15,6 +15,7 @@ CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=y
 CONFIG_UAVCANNODE_FLOW_MEASUREMENT=y
 CONFIG_UAVCANNODE_RANGE_SENSOR_MEASUREMENT=y
+CONFIG_UAVCANNODE_RAW_IMU=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_SENSORS=y
 # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
@@ -20,6 +20,7 @@ CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH=y
 CONFIG_UAVCANNODE_SAFETY_BUTTON=y
 CONFIG_UAVCANNODE_STATIC_PRESSURE=y
 CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y
+CONFIG_UAVCANNODE_RAW_IMU=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_SENSORS=y
@@ -67,7 +67,7 @@ void rgb_led(int r, int g, int b, int freqs)
 	if (!once) {
 		once = 1;

-		/* Enabel Clock to Block */
+		/* Enable Clock to Block */
 		modifyreg32(STM32_RCC_APB2ENR, 0, RCC_APB2ENR_TIM1EN);

 		/* Reload */
@@ -21,6 +21,7 @@ CONFIG_UAVCANNODE_RTK_DATA=y
 CONFIG_UAVCANNODE_SAFETY_BUTTON=y
 CONFIG_UAVCANNODE_STATIC_PRESSURE=y
 CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y
+CONFIG_UAVCANNODE_RAW_IMU=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_SENSORS=y
@@ -67,7 +67,7 @@ void rgb_led(int r, int g, int b, int freqs)
 	if (!once) {
 		once = 1;

-		/* Enabel Clock to Block */
+		/* Enable Clock to Block */
 		modifyreg32(STM32_RCC_APB2ENR, 0, RCC_APB2ENR_TIM1EN);

 		/* Reload */
@@ -36,6 +36,7 @@ CONFIG_UAVCANNODE_LIGHTS_COMMAND=y
 CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH=y
 CONFIG_UAVCANNODE_RANGE_SENSOR_MEASUREMENT=y
 CONFIG_UAVCANNODE_RAW_AIR_DATA=y
+CONFIG_UAVCANNODE_RAW_IMU=y
 CONFIG_UAVCANNODE_RTK_DATA=y
 CONFIG_UAVCANNODE_SERVO_ARRAY_COMMAND=y
 CONFIG_UAVCANNODE_STATIC_PRESSURE=y
@@ -107,7 +107,7 @@ icm20948_i2c_passthrough -X -q start
 hmc5883 -T -X -q start
 ist8308 -X -q start
 ist8310 -X -q start
-lis2mdl -X -q start
+iis2mdc -X -q start
 lis3mdl -X -q start
 qmc5883l -X -q start
 rm3100 -X -q start
@@ -67,7 +67,7 @@ void rgb_led(int r, int g, int b, int freqs)
 	if (!once) {
 		once = 1;

-		/* Enabel Clock to Block */
+		/* Enable Clock to Block */
 		modifyreg32(STM32_RCC_APB2ENR, 0, RCC_APB2ENR_TIM1EN);

 		/* Reload */
@@ -25,7 +25,16 @@ CONFIG_DRIVERS_IMU_INVENSENSE_IIM42652=y
 CONFIG_DRIVERS_IMU_INVENSENSE_IIM42653=y
 CONFIG_DRIVERS_IMU_MURATA_SCH16T=y
 CONFIG_COMMON_LIGHT=y
-CONFIG_COMMON_MAGNETOMETER=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_MEMSIC_MMC5983MA=y
+CONFIG_DRIVERS_MAGNETOMETER_ST_IIS2MDC=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_POWER_MONITOR_INA228=y
 CONFIG_DRIVERS_POWER_MONITOR_INA238=y
@@ -66,6 +75,7 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
 CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
+CONFIG_NUM_MISSION_ITMES_SUPPORTED=1000
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
@@ -0,0 +1,8 @@
+CONFIG_DRIVERS_BATT_SMBUS=n
+CONFIG_DRIVERS_PX4IO=n
+CONFIG_BOARD_CRYPTO=y
+CONFIG_DRIVERS_STUB_KEYSTORE=y
+CONFIG_DRIVERS_SW_CRYPTO=y
+# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set
+CONFIG_PUBLIC_KEY0="../../../Tools/test_keys/key0.pub"
+CONFIG_PUBLIC_KEY1="../../../Tools/test_keys/rsa2048.pub"
@@ -84,6 +84,8 @@ CONFIG_CDCACM_RXBUFSIZE=600
 CONFIG_CDCACM_TXBUFSIZE=12000
 CONFIG_CDCACM_VENDORID=0x3185
 CONFIG_CDCACM_VENDORSTR="ARK"
+CONFIG_CRYPTO=y
+CONFIG_CRYPTO_RANDOM_POOL=y
 CONFIG_DEBUG_FULLOPT=y
 CONFIG_DEBUG_HARDFAULT_ALERT=y
 CONFIG_DEBUG_MEMFAULT=y
@@ -294,6 +296,7 @@ CONFIG_UART4_RXBUFSIZE=600
 CONFIG_UART4_TXBUFSIZE=1500
 CONFIG_UART5_IFLOWCONTROL=y
 CONFIG_UART5_OFLOWCONTROL=y
+CONFIG_UART5_RXBUFSIZE=800
 CONFIG_UART5_RXDMA=y
 CONFIG_UART5_TXBUFSIZE=10000
 CONFIG_UART5_TXDMA=y
@@ -8,6 +8,8 @@ CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS3"
 CONFIG_BOARD_SERIAL_RC="/dev/ttyS5"
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_BMP388=y
+CONFIG_DRIVERS_CAMERA_CAPTURE=y
+CONFIG_DRIVERS_CAMERA_TRIGGER=y
 CONFIG_DRIVERS_CDCACM_AUTOSTART=y
 CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
 CONFIG_COMMON_DISTANCE_SENSOR=y
@@ -25,6 +27,7 @@ CONFIG_DRIVERS_UAVCAN=y
 CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
 CONFIG_MODULES_BATTERY_STATUS=y
+CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
@@ -0,0 +1,7 @@
+
+CONFIG_BOARD_CRYPTO=y
+CONFIG_DRIVERS_STUB_KEYSTORE=y
+CONFIG_DRIVERS_SW_CRYPTO=y
+# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set
+CONFIG_PUBLIC_KEY0="../../../Tools/test_keys/key0.pub"
+CONFIG_PUBLIC_KEY1="../../../Tools/test_keys/rsa2048.pub"
@@ -82,6 +82,8 @@ CONFIG_CDCACM_RXBUFSIZE=600
 CONFIG_CDCACM_TXBUFSIZE=12000
 CONFIG_CDCACM_VENDORID=0x3185
 CONFIG_CDCACM_VENDORSTR="ARK"
+CONFIG_CRYPTO=y
+CONFIG_CRYPTO_RANDOM_POOL=y
 CONFIG_DEBUG_FULLOPT=y
 CONFIG_DEBUG_HARDFAULT_ALERT=y
 CONFIG_DEBUG_MEMFAULT=y
@@ -0,0 +1,6 @@
+CONFIG_BOARD_CRYPTO=y
+CONFIG_DRIVERS_STUB_KEYSTORE=y
+CONFIG_DRIVERS_SW_CRYPTO=y
+# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set
+CONFIG_PUBLIC_KEY0="../../../Tools/test_keys/key0.pub"
+CONFIG_PUBLIC_KEY1="../../../Tools/test_keys/rsa2048.pub"
@@ -82,6 +82,8 @@ CONFIG_CDCACM_RXBUFSIZE=600
 CONFIG_CDCACM_TXBUFSIZE=12000
 CONFIG_CDCACM_VENDORID=0x3185
 CONFIG_CDCACM_VENDORSTR="ARK"
+CONFIG_CRYPTO=y
+CONFIG_CRYPTO_RANDOM_POOL=y
 CONFIG_DEBUG_FULLOPT=y
 CONFIG_DEBUG_HARDFAULT_ALERT=y
 CONFIG_DEBUG_MEMFAULT=y
@@ -17,6 +17,7 @@ CONFIG_UAVCANNODE_GNSS_FIX=y
 CONFIG_UAVCANNODE_LIGHTS_COMMAND=y
 CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH=y
 CONFIG_UAVCANNODE_RTK_DATA=y
+CONFIG_UAVCANNODE_RAW_IMU=y
 CONFIG_UAVCANNODE_SAFETY_BUTTON=y
 CONFIG_UAVCANNODE_STATIC_PRESSURE=y
 CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y
@@ -67,7 +67,7 @@ void rgb_led(int r, int g, int b, int freqs)
 	if (!once) {
 		once = 1;

-		/* Enabel Clock to Block */
+		/* Enable Clock to Block */
 		modifyreg32(STM32_RCC_APB2ENR, 0, RCC_APB2ENR_TIM1EN);

 		/* Reload */
@@ -67,7 +67,7 @@ void rgb_led(int r, int g, int b, int freqs)
 	if (!once) {
 		once = 1;

-		/* Enabel Clock to Block */
+		/* Enable Clock to Block */
 		modifyreg32(STM32_RCC_APB2ENR, 0, RCC_APB2ENR_TIM1EN);

 		/* Reload */
@@ -59,7 +59,6 @@
 #define BOARD_ARMED_STATE_LED  LED_BLUE

 #define FLASH_BASED_PARAMS
-#define RAM_BASED_MISSIONS

 // Hacks for MAVLink RC button input
 #define ATL_MANTIS_RC_INPUT_HACKS
@@ -100,7 +100,7 @@ __END_DECLS
 *
 * Description:
 *   All STM32 architectures must provide the following entry point.  This entry point
- *   is called early in the intitialization -- after all memory has been configured
+ *   is called early in the initialization -- after all memory has been configured
 *   and mapped but before any devices have been initialized.
 *
 ************************************************************************************/
@@ -100,7 +100,7 @@ __END_DECLS
 *
 * Description:
 *   All STM32 architectures must provide the following entry point.  This entry point
- *   is called early in the intitialization -- after all memory has been configured
+ *   is called early in the initialization -- after all memory has been configured
 *   and mapped but before any devices have been initialized.
 *
 ************************************************************************************/
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2024 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
@@ -30,12 +30,5 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 ############################################################################
-px4_add_module(
-	MODULE drivers__lis2mdl
-	MAIN lis2mdl
-	SRCS
-		lis2mdl_i2c.cpp
-		lis2mdl_spi.cpp
-		lis2mdl_main.cpp
-		lis2mdl.cpp
-	)
+
+add_subdirectory(neopixel)
@@ -0,0 +1,53 @@
+############################################################################
+#
+#   Copyright (c) 2020 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.
+#
+############################################################################
+
+if(DEFINED ENV{AUTOPILOT_HOST})
+	set(AUTOPILOT_HOST $ENV{AUTOPILOT_HOST})
+else()
+	set(AUTOPILOT_HOST "raspberrypi")
+endif()
+
+if(DEFINED ENV{AUTOPILOT_USER})
+	set(AUTOPILOT_USER $ENV{AUTOPILOT_USER})
+else()
+	set(AUTOPILOT_USER "pi")
+endif()
+
+add_custom_target(upload
+	COMMAND rsync -arh --progress
+			${CMAKE_RUNTIME_OUTPUT_DIRECTORY} ${PX4_SOURCE_DIR}/posix-configs/rpi/navigator/*.config ${PX4_BINARY_DIR}/etc # source
+			"${AUTOPILOT_USER}@${AUTOPILOT_HOST}:/home/${AUTOPILOT_USER}/px4" # destination
+	DEPENDS px4
+	COMMENT "uploading px4"
+	USES_TERMINAL
+)
@@ -0,0 +1,78 @@
+CONFIG_PLATFORM_POSIX=y
+CONFIG_BOARD_LINUX_TARGET=y
+CONFIG_BOARD_TESTING=y
+CONFIG_BOARD_TOOLCHAIN="arm-linux-gnueabihf"
+CONFIG_BOARD_ARCHITECTURE="cortex-a53"
+CONFIG_DRIVERS_ADC_ADS1115=y
+CONFIG_DRIVERS_BATT_SMBUS=y
+CONFIG_DRIVERS_CAMERA_TRIGGER=y
+CONFIG_DRIVERS_LIGHTS_NEOPIXEL=y
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
+CONFIG_DRIVERS_BAROMETER_BMP280=y
+CONFIG_DRIVERS_GPS=y
+CONFIG_DRIVERS_MAGNETOMETER_MEMSIC_MMC5983MA=y
+CONFIG_DRIVERS_MAGNETOMETER_AKM_AK09916=y
+CONFIG_DRIVERS_PCA9685_PWM_OUT=y
+CONFIG_PCA9685_USE_EXTERNAL_CRYSTAL=y
+CONFIG_DRIVERS_RC_INPUT=y
+CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
+CONFIG_COMMON_RC=y
+CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
+CONFIG_COMMON_DISTANCE_SENSOR=y
+CONFIG_MODULES_AIRSPEED_SELECTOR=y
+CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
+CONFIG_MODULES_BATTERY_STATUS=y
+CONFIG_MODULES_CAMERA_FEEDBACK=y
+CONFIG_MODULES_COMMANDER=y
+CONFIG_MODULES_CONTROL_ALLOCATOR=y
+CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_EKF2=y
+CONFIG_MODULES_EVENTS=y
+CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
+CONFIG_MODULES_GIMBAL=y
+CONFIG_MODULES_GYRO_CALIBRATION=y
+CONFIG_MODULES_GYRO_FFT=y
+CONFIG_MODULES_LAND_DETECTOR=y
+CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
+CONFIG_MODULES_LOAD_MON=y
+CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
+CONFIG_MODULES_LOGGER=y
+CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
+CONFIG_MODULES_MAVLINK=y
+CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
+CONFIG_MODULES_NAVIGATOR=y
+CONFIG_MODULES_RC_UPDATE=y
+CONFIG_MODULES_SENSORS=y
+CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
+CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
+CONFIG_MODULES_UUV_ATT_CONTROL=y
+CONFIG_MODULES_UUV_POS_CONTROL=y
+CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
+CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_RATE_CONTROL=y
+CONFIG_MODULES_MANUAL_CONTROL=y
+CONFIG_MODULES_MC_ATT_CONTROL=y
+CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y
+CONFIG_MODULES_MC_POS_CONTROL=y
+CONFIG_MODULES_MC_RATE_CONTROL=y
+CONFIG_MODULES_ROVER_POS_CONTROL=y
+CONFIG_MODULES_VTOL_ATT_CONTROL=y
+CONFIG_SYSTEMCMDS_BSONDUMP=y
+CONFIG_SYSTEMCMDS_DYN=y
+CONFIG_SYSTEMCMDS_LED_CONTROL=y
+CONFIG_SYSTEMCMDS_PARAM=y
+CONFIG_SYSTEMCMDS_PERF=y
+CONFIG_SYSTEMCMDS_SD_BENCH=y
+CONFIG_SYSTEMCMDS_SHUTDOWN=y
+CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
+CONFIG_SYSTEMCMDS_TUNE_CONTROL=y
+CONFIG_SYSTEMCMDS_UORB=y
+CONFIG_SYSTEMCMDS_VER=y
+CONFIG_SYSTEMCMDS_WORK_QUEUE=y
+CONFIG_EXAMPLES_DYN_HELLO=y
+CONFIG_EXAMPLES_FAKE_GPS=y
+CONFIG_EXAMPLES_HELLO=y
+CONFIG_EXAMPLES_PX4_MAVLINK_DEBUG=y
+CONFIG_EXAMPLES_PX4_SIMPLE_APP=y
+CONFIG_EXAMPLES_WORK_ITEM=y
@@ -0,0 +1,13 @@
+#!/bin/sh
+#
+# board specific defaults
+#------------------------------------------------------------------------------
+
+# system_power not implemented
+param set CBRK_SUPPLY_CHK 894281
+
+# 1K + 4.7K
+param set BAT1_V_DIV 5.7
+
+# Always keep current config
+param set SYS_AUTOCONFIG 0
@@ -0,0 +1,7 @@
+#!/bin/sh
+#
+# board specific extras init
+#------------------------------------------------------------------------------
+
+rc_input start -d /dev/ttyAMA1
+rc_update start
@@ -0,0 +1,39 @@
+#!/bin/sh
+#
+# Navigator specific board sensors init
+#------------------------------------------------------------------------------
+
+if ! mmc5983ma start -s -m 0 -R 4
+then
+	echo "mmc5983ma not found."
+fi
+
+if ! ak09916 start -I -R 6
+then
+	echo "AK09916 not found."
+fi
+
+if ! icm20602 start -s -m 0 -R 14
+then
+	echo "ICM20602 not found."
+fi
+
+if ! bmp280 start -I
+then
+	echo "bmp280 not found."
+fi
+
+if ! ads1115 start -I
+then
+	echo "ads1115 not found."
+fi
+
+if ! pca9685_pwm_out start -a 0x40 -b 4
+then
+	echo "pca9685_pwm_out not found."
+fi
+
+if ! neopixel start
+then
+	echo "neopixel not found."
+fi
@@ -0,0 +1,36 @@
+############################################################################
+#
+#   Copyright (c) 2024 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_library(arch_srgbled_dma
+	srgbled_impl.cpp
+)
@@ -0,0 +1,157 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2024 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 srgbled_impl.cpp
+ *
+ * Implementation of functions to control NeoPixel LEDs on the BlueRobotics Navigator for the neopixel driver.
+ *
+ * Definitions:
+ * - BOARD_HAS_N_S_RGB_LED: Number of NeoPixel LEDs on the Navigator.
+ * - BOARD_RGB_SPI_BUS: SPI bus connected to the NeoPixel data line.
+ * - BOARD_RGB_SPI_FREQ: SPI bus frequency for the NeoPixel connection.
+ */
+
+#include <board_config.h>
+#include <drivers/drv_neopixel.h>
+#include <px4_platform_common/i2c_spi_buses.h>
+#include <px4_platform_common/log.h>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/sem.h>
+
+#if defined(BOARD_HAS_N_S_RGB_LED)
+
+#if !defined(BOARD_RGB_SPI_BUS)
+#  error  BOARD_RGB_SPI_BUS must be defined to use the NeoPixel driver implementation for the BlueRobotics Navigator
+#endif
+#if !defined(BOARD_RGB_SPI_FREQ)
+#  error  BOARD_RGB_SPI_FREQ must be defined to use the NeoPixel driver implementation for the BlueRobotics Navigator
+#endif
+
+#define LED_T0 0b11000000
+#define LED_T1 0b11110000
+
+class NavigatorLED : public device::SPI
+{
+public:
+	NavigatorLED();
+	~NavigatorLED() override = default;
+
+	int init();
+	int write(uint8_t red, uint8_t green, uint8_t blue);
+	int deinit();
+
+protected:
+	void _setup_data(uint8_t red, uint8_t green, uint8_t blue);
+
+private:
+	uint8_t _data[24];
+	px4_sem_t _sem;
+};
+
+NavigatorLED::NavigatorLED() :
+	// By default we don't use the CS line and MODE must be 0 for compatibility with Raspberry Pi
+	SPI(DRV_DEVTYPE_UNUSED, "navigator-neopixel", BOARD_RGB_SPI_BUS, 0, SPIDEV_MODE0, BOARD_RGB_SPI_FREQ)
+{
+	px4_sem_init(&_sem, 0, 1);
+}
+
+int NavigatorLED::init()
+{
+	int ret = SPI::init();
+
+	if (ret != PX4_OK) {
+		PX4_ERR("Neopixel SPI init failed");
+		return ret;
+	}
+
+	return PX4_OK;
+}
+
+void NavigatorLED::_setup_data(uint8_t red, uint8_t green, uint8_t blue)
+{
+	for (uint8_t i = 0; i < 8; i++) {
+		_data[i] = (green & (1 << (7 - i))) ? LED_T1 : LED_T0;
+	}
+
+	for (uint8_t i = 0; i < 8; i++) {
+		_data[8 + i] = (red & (1 << (7 - i))) ? LED_T1 : LED_T0;
+	}
+
+	for (uint8_t i = 0; i < 8; i++) {
+		_data[16 + i] = (blue & (1 << (7 - i))) ? LED_T1 : LED_T0;
+	}
+}
+
+int NavigatorLED::write(uint8_t red, uint8_t green, uint8_t blue)
+{
+	_setup_data(red, green, blue);
+
+	px4_sem_wait(&_sem);
+
+	int ret = transfer(_data, nullptr, sizeof(_data));
+
+	px4_sem_post(&_sem);
+
+	if (ret != PX4_OK) {
+		PX4_ERR("Failed to write data to NeoPixel %d", ret);
+	}
+
+	return ret;
+}
+
+int NavigatorLED::deinit()
+{
+	PX4_INFO("Neopixel deinitialized");
+	return PX4_OK;
+}
+
+// Neopixel driver impl
+
+NavigatorLED navigator_led;
+
+int neopixel_init(neopixel::NeoLEDData *led_data, int number_of_packages)
+{
+	return navigator_led.init();
+}
+
+int neopixel_write(neopixel::NeoLEDData *led_data, int number_of_packages)
+{
+	return navigator_led.write(led_data->R(), led_data->G(), led_data->B());
+}
+
+int neopixel_deinit()
+{
+	return navigator_led.deinit();
+}
+#endif // defined(BOARD_HAS_N_S_RGB_LED)
@@ -0,0 +1,37 @@
+############################################################################
+#
+#   Copyright (c) 2024 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.
+#
+############################################################################
+
+add_library(drivers_board
+	i2c.cpp
+	spi.cpp
+)
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2024 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
@@ -32,92 +32,35 @@
 ****************************************************************************/

 /**
- * @file lis2mdl_i2c.cpp
+ * @file board_config.h
 *
- * I2C interface for LIS2MDL
+ * BlueRobotics Navigator board configuration.
 */

-#include <px4_platform_common/px4_config.h>
+#pragma once

-#include <assert.h>
-#include <errno.h>
-#include <stdint.h>
-#include <stdbool.h>
-#include <string.h>
-#include <sys/types.h>
-#include <unistd.h>
+#define BOARD_OVERRIDE_UUID "BRNAVID000000000" // must be of length 16
+#define PX4_SOC_ARCH_ID     PX4_SOC_ARCH_ID_RPI

-#include <drivers/device/i2c.h>
+#define BOARD_MAX_LEDS 1

-#include "board_config.h"
-#include "lis2mdl.h"
+// Neopixel config
+#define BOARD_HAS_N_S_RGB_LED 1
+#define BOARD_RGB_SPI_BUS 0
+#define BOARD_RGB_SPI_FREQ 8 * 1000 * 1000 // Emulated 800Kbps since we use 1 byte as 1 bit

-class LIS2MDL_I2C : public device::I2C
-{
-public:
-	LIS2MDL_I2C(const I2CSPIDriverConfig &config);
-	virtual ~LIS2MDL_I2C() = default;
+// I2C
+#define CONFIG_I2C 1
+#define PX4_NUMBER_I2C_BUSES    3

-	virtual int     read(unsigned address, void *data, unsigned count);
-	virtual int     write(unsigned address, void *data, unsigned count);
+// SPI
+#define CONFIG_SPI 1

-protected:
-	virtual int     probe();
+#define ADC_BATTERY_VOLTAGE_CHANNEL  3
+#define ADC_BATTERY_CURRENT_CHANNEL  2
+#define ADC_5V_RAIL_SENSE            0

-};
+#define ADC_DP_V_DIV 1.0f

-device::Device *
-LIS2MDL_I2C_interface(const I2CSPIDriverConfig &config);
-
-device::Device *
-LIS2MDL_I2C_interface(const I2CSPIDriverConfig &config)
-{
-	return new LIS2MDL_I2C(config);
-}
-
-LIS2MDL_I2C::LIS2MDL_I2C(const I2CSPIDriverConfig &config) :
-	I2C(config)
-{
-}
-
-int
-LIS2MDL_I2C::probe()
-{
-	uint8_t data = 0;
-
-	_retries = 1;
-
-	if (read(ADDR_WHO_AM_I, &data, 1)) {
-		DEVICE_DEBUG("read_reg fail");
-		return -EIO;
-	}
-
-	if (data != ID_WHO_AM_I) {
-		DEVICE_DEBUG("LIS2MDL bad ID: %02x", data);
-		return -EIO;
-	}
-
-	return OK;
-}
-
-int
-LIS2MDL_I2C::read(unsigned address, void *data, unsigned count)
-{
-	uint8_t cmd = address;
-	return transfer(&cmd, 1, (uint8_t *)data, count);
-}
-
-int
-LIS2MDL_I2C::write(unsigned address, void *data, unsigned count)
-{
-	uint8_t buf[32];
-
-	if (sizeof(buf) < (count + 1)) {
-		return -EIO;
-	}
-
-	buf[0] = address;
-	memcpy(&buf[1], data, count);
-
-	return transfer(&buf[0], count + 1, nullptr, 0);
-}
+#include <system_config.h>
+#include <px4_platform_common/board_common.h>
@@ -0,0 +1,40 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2024 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.
+ *
+ ****************************************************************************/
+
+#include <px4_arch/i2c_hw_description.h>
+
+constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = {
+	initI2CBusInternal(1),
+	initI2CBusInternal(4),
+	initI2CBusExternal(6),
+};
@@ -0,0 +1,45 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2024 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.
+ *
+ ****************************************************************************/
+
+#include <px4_arch/spi_hw_description.h>
+#include <drivers/drv_sensor.h>
+
+constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
+	initSPIBus(1, {
+		initSPIDevice(DRV_MAG_DEVTYPE_MMC5983MA, 1),
+		initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, 2),
+	}),
+	initSPIBus(0, {
+		initSPIDevice(DRV_DEVTYPE_UNUSED, 0),
+	}),
+};
@@ -31,6 +31,7 @@ CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_INPUT=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_PX4IO=y
+CONFIG_DRIVERS_HEATER=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
@@ -65,6 +66,7 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
 CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
+CONFIG_NUM_MISSION_ITMES_SUPPORTED=1000
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
@@ -58,6 +58,8 @@
 #define PX4IO_SERIAL_CLOCK             STM32_PCLK2_FREQUENCY
 #define PX4IO_SERIAL_BITRATE           1500000               /* 1.5Mbps -> max rate for IO */

+#define PX4IO_HEATER_ENABLED
+
 /* LEDs */
 #define GPIO_nLED_AMBER        /* PE12 */  (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN12)

@@ -32,6 +32,7 @@ CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_INPUT=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_PX4IO=y
+CONFIG_DRIVERS_HEATER=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
@@ -66,6 +67,7 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
 CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
+CONFIG_NUM_MISSION_ITMES_SUPPORTED=1000
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
@@ -12,6 +12,4 @@ 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
-
 set IOFW "/etc/extras/cubepilot_io-v2_default.bin"
@@ -17,6 +17,9 @@ 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
+
 	if ! icm20948 -s -b 4 -R 10 -M -q start
 	then
 		icm42688p -s -b 4 -R 6 start -c 13
@@ -24,6 +27,8 @@ then
 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
 fi

 # SPI1, body-fixed
@@ -33,7 +38,6 @@ then
 	ak09916 start -I -R 13
 else
 	icm20649 -s -b 1 start
-
 fi

 ms5611 -s -b 1 start
@@ -68,6 +68,8 @@
 #define PX4IO_SERIAL_CLOCK             STM32_PCLK2_FREQUENCY
 #define PX4IO_SERIAL_BITRATE           1500000               /* 1.5Mbps -> max rate for IO */

+#define PX4IO_HEATER_ENABLED
+
 /* LEDs */
 #define GPIO_nLED_AMBER        /* PE12 */  (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN12)

@@ -77,7 +77,7 @@
 *
 * Description:
 *   All STM32 architectures must provide the following entry point.  This entry point
- *   is called early in the intitialization -- after all memory has been configured
+ *   is called early in the initialization -- after all memory has been configured
 *   and mapped but before any devices have been initialized.
 *
 ************************************************************************************/
@@ -59,7 +59,7 @@
 *   2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0].
 *      ST programmed value: System bootloader at 0x1FF0:0000
 *
- * The Durandal has a Swtich on board, the BOOT0 pin is at ground so by default,
+ * The Durandal has a Switch on board, the BOOT0 pin is at ground so by default,
 * the STM32 will boot to address 0x0800:0000 in FLASH unless the swiutch is
 * drepresed, then the boot will be from 0x1FF0:0000
 *
@@ -62,7 +62,7 @@
 typedef struct {
 	uint32_t                hw_ver_rev; /* the version and revision */
 	const px4_hw_mft_item_t *mft;       /* The first entry */
-	uint32_t                entries;    /* the lenght of the list */
+	uint32_t                entries;    /* the length of the list */
 } px4_hw_mft_list_entry_t;

 typedef px4_hw_mft_list_entry_t *px4_hw_mft_list_entry;
@@ -59,7 +59,7 @@
 *   2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0].
 *      ST programmed value: System bootloader at 0x1FF0:0000
 *
- * The Durandal has a Swtich on board, the BOOT0 pin is at ground so by default,
+ * The Durandal has a Switch on board, the BOOT0 pin is at ground so by default,
 * the STM32 will boot to address 0x0800:0000 in FLASH unless the swiutch is
 * drepresed, then the boot will be from 0x1FF0:0000
 *
@@ -62,7 +62,7 @@
 typedef struct {
 	uint32_t                hw_ver_rev; /* the version and revision */
 	const px4_hw_mft_item_t *mft;       /* The first entry */
-	uint32_t                entries;    /* the lenght of the list */
+	uint32_t                entries;    /* the length of the list */
 } px4_hw_mft_list_entry_t;

 typedef px4_hw_mft_list_entry_t *px4_hw_mft_list_entry;
@@ -59,7 +59,7 @@
 *   2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0].
 *      ST programmed value: System bootloader at 0x1FF0:0000
 *
- * The Durandal has a Swtich on board, the BOOT0 pin is at ground so by default,
+ * The Durandal has a Switch on board, the BOOT0 pin is at ground so by default,
 * the STM32 will boot to address 0x0800:0000 in FLASH unless the swiutch is
 * drepresed, then the boot will be from 0x1FF0:0000
 *
@@ -59,7 +59,7 @@
 *   2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0].
 *      ST programmed value: System bootloader at 0x1FF0:0000
 *
- * The Durandal has a Swtich on board, the BOOT0 pin is at ground so by default,
+ * The Durandal has a Switch on board, the BOOT0 pin is at ground so by default,
 * the STM32 will boot to address 0x0800:0000 in FLASH unless the swiutch is
 * drepresed, then the boot will be from 0x1FF0:0000
 *
@@ -62,7 +62,7 @@
 typedef struct {
 	uint32_t                hw_ver_rev; /* the version and revision */
 	const px4_hw_mft_item_t *mft;       /* The first entry */
-	uint32_t                entries;    /* the lenght of the list */
+	uint32_t                entries;    /* the length of the list */
 } px4_hw_mft_list_entry_t;

 typedef px4_hw_mft_list_entry_t *px4_hw_mft_list_entry;
@@ -63,6 +63,7 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
 CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
+CONFIG_NUM_MISSION_ITMES_SUPPORTED=1000
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
@@ -63,6 +63,7 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
 CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
+CONFIG_NUM_MISSION_ITMES_SUPPORTED=1000
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
@@ -63,6 +63,7 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
 CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
+CONFIG_NUM_MISSION_ITMES_SUPPORTED=1000
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
@@ -62,7 +62,7 @@
 typedef struct {
 	uint32_t                hw_ver_rev; /* the version and revision */
 	const px4_hw_mft_item_t *mft;       /* The first entry */
-	uint32_t                entries;    /* the lenght of the list */
+	uint32_t                entries;    /* the length of the list */
 } px4_hw_mft_list_entry_t;

 typedef px4_hw_mft_list_entry_t *px4_hw_mft_list_entry;
@@ -71,7 +71,7 @@ void rgb_led(int r, int g, int b, int freqs)
 	if (!once) {
 		once = 1;

-		/* Enabel Clock to Block */
+		/* Enable Clock to Block */
 		modifyreg32(STM32_RCC_APB2ENR, 0, RCC_APB2ENR_TIM1EN);

 		/* Reload */
@@ -59,7 +59,7 @@
 *   2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0].
 *      ST programmed value: System bootloader at 0x1FF0:0000
 *
- * The Durandal has a Swtich on board, the BOOT0 pin is at ground so by default,
+ * The Durandal has a Switch on board, the BOOT0 pin is at ground so by default,
 * the STM32 will boot to address 0x0800:0000 in FLASH unless the swiutch is
 * drepresed, then the boot will be from 0x1FF0:0000
 *
@@ -59,7 +59,7 @@
 *   2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0].
 *      ST programmed value: System bootloader at 0x1FF0:0000
 *
- * The Durandal has a Swtich on board, the BOOT0 pin is at ground so by default,
+ * The Durandal has a Switch on board, the BOOT0 pin is at ground so by default,
 * the STM32 will boot to address 0x0800:0000 in FLASH unless the swiutch is
 * drepresed, then the boot will be from 0x1FF0:0000
 *
@@ -59,7 +59,7 @@
 *   2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0].
 *      ST programmed value: System bootloader at 0x1FF0:0000
 *
- * The Durandal has a Swtich on board, the BOOT0 pin is at ground so by default,
+ * The Durandal has a Switch on board, the BOOT0 pin is at ground so by default,
 * the STM32 will boot to address 0x0800:0000 in FLASH unless the swiutch is
 * drepresed, then the boot will be from 0x1FF0:0000
 *
@@ -59,7 +59,7 @@
 *   2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0].
 *      ST programmed value: System bootloader at 0x1FF0:0000
 *
- * The Durandal has a Swtich on board, the BOOT0 pin is at ground so by default,
+ * The Durandal has a Switch on board, the BOOT0 pin is at ground so by default,
 * the STM32 will boot to address 0x0800:0000 in FLASH unless the swiutch is
 * drepresed, then the boot will be from 0x1FF0:0000
 *
@@ -59,7 +59,7 @@
 *   2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0].
 *      ST programmed value: System bootloader at 0x1FF0:0000
 *
- * The Durandal has a Swtich on board, the BOOT0 pin is at ground so by default,
+ * The Durandal has a Switch on board, the BOOT0 pin is at ground so by default,
 * the STM32 will boot to address 0x0800:0000 in FLASH unless the swiutch is
 * drepresed, then the boot will be from 0x1FF0:0000
 *
@@ -59,7 +59,7 @@
 *   2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0].
 *      ST programmed value: System bootloader at 0x1FF0:0000
 *
- * The Durandal has a Swtich on board, the BOOT0 pin is at ground so by default,
+ * The Durandal has a Switch on board, the BOOT0 pin is at ground so by default,
 * the STM32 will boot to address 0x0800:0000 in FLASH unless the swiutch is
 * drepresed, then the boot will be from 0x1FF0:0000
 *
@@ -62,7 +62,7 @@
 typedef struct {
 	uint32_t                hw_ver_rev; /* the version and revision */
 	const px4_hw_mft_item_t *mft;       /* The first entry */
-	uint32_t                entries;    /* the lenght of the list */
+	uint32_t                entries;    /* the length of the list */
 } px4_hw_mft_list_entry_t;

 typedef px4_hw_mft_list_entry_t *px4_hw_mft_list_entry;
@@ -62,7 +62,7 @@
 typedef struct {
 	uint32_t                hw_ver_rev; /* the version and revision */
 	const px4_hw_mft_item_t *mft;       /* The first entry */
-	uint32_t                entries;    /* the lenght of the list */
+	uint32_t                entries;    /* the length of the list */
 } px4_hw_mft_list_entry_t;

 typedef px4_hw_mft_list_entry_t *px4_hw_mft_list_entry;
@@ -77,7 +77,7 @@
 *
 * Description:
 *   All STM32 architectures must provide the following entry point.  This entry point
- *   is called early in the intitialization -- after all memory has been configured
+ *   is called early in the initialization -- after all memory has been configured
 *   and mapped but before any devices have been initialized.
 *
 ************************************************************************************/
--- a/Show More
+++ b/Show More