ci: github actions: reenable some tests to increase coverage

- Updated container images to newer versions with Ubuntu Jammy as per 2024-05-18
- Migrated ROS container from Melodic to Noetic, since Melodic is EOL

.github/workflows/checks.yml

@@ -28,12 +28,14 @@ jobs:
           "parameters_metadata",
         ]
     container:
-      image: px4io/px4-dev-nuttx-focal:2022-08-12
+      image: px4io/px4-dev-nuttx-jammy:2024-05-18
       options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
     steps:
     - uses: actions/checkout@v1
       with:
         token: ${{ secrets.ACCESS_TOKEN }}
+    - name: ownership workaround
+      run: git config --global --add safe.directory '*'
 
     - name: check environment
       run: |

.github/workflows/clang-tidy.yml

@@ -11,11 +11,13 @@ on:
 jobs:
   build:
     runs-on: ubuntu-latest
-    container: px4io/px4-dev-clang:2021-09-08
+    container: px4io/px4-dev-clang:2024-05-18
     steps:
     - uses: actions/checkout@v1
       with:
         token: ${{secrets.ACCESS_TOKEN}}
+    - name: ownership workaround
+      run: git config --global --add safe.directory '*'
 
     - name: make clang-tidy-quiet
       run: make clang-tidy-quiet

.github/workflows/compile_linux.yml

@@ -14,7 +14,7 @@ on:
 jobs:
   build:
     runs-on: ubuntu-latest
-    container: px4io/px4-dev-armhf:2023-06-26
+    container: px4io/px4-dev-armhf:2024-05-18
     strategy:
       matrix:
         config: [
@@ -28,7 +28,7 @@ jobs:
       with:
         token: ${{secrets.ACCESS_TOKEN}}
     - name: ownership workaround
-      run: git config --system --add safe.directory '*'
+      run: git config --global --add safe.directory '*'
     - name: Prepare ccache timestamp
       id: ccache_cache_timestamp
       shell: cmake -P {0}

.github/workflows/compile_linux_arm64.yml

@@ -14,7 +14,7 @@ on:
 jobs:
   build:
     runs-on: ubuntu-latest
-    container: px4io/px4-dev-aarch64:2022-08-12
+    container: px4io/px4-dev-aarch64:2024-05-18
     strategy:
       matrix:
         config: [
@@ -24,6 +24,8 @@ jobs:
     - uses: actions/checkout@v1
       with:
         token: ${{secrets.ACCESS_TOKEN}}
+    - name: ownership workaround
+      run: git config --global --add safe.directory '*'
 
     - name: Prepare ccache timestamp
       id: ccache_cache_timestamp

.github/workflows/compile_nuttx.yml

@@ -14,7 +14,7 @@ on:
 jobs:
   build:
     runs-on: ubuntu-latest
-    container: px4io/px4-dev-nuttx-focal:2022-08-12
+    container: px4io/px4-dev-nuttx-jammy:2024-05-18
     strategy:
       fail-fast: false
       matrix:
@@ -83,6 +83,8 @@ jobs:
     - uses: actions/checkout@v1
       with:
         token: ${{secrets.ACCESS_TOKEN}}
+    - name: ownership workaround
+      run: git config --global --add safe.directory '*'
 
     - name: Prepare ccache timestamp
       id: ccache_cache_timestamp

.github/workflows/deploy_all.yml

@@ -10,13 +10,15 @@ on:
 jobs:
   enumerate_targets:
     runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-09-08
+    container: px4io/px4-dev-base-jammy:2024-05-18
     outputs:
       matrix: ${{ steps.set-matrix.outputs.matrix }}
     steps:
     - uses: actions/checkout@v1
       with:
         token: ${{secrets.ACCESS_TOKEN}}
+    - name: ownership workaround
+      run: git config --global --add safe.directory '*'
     - id: set-matrix
       run: echo "::set-output name=matrix::$(./Tools/generate_board_targets_json.py)"
   build:
@@ -29,9 +31,11 @@ jobs:
     - uses: actions/checkout@v1
       with:
         token: ${{secrets.ACCESS_TOKEN}}
+    - name: ownership workaround
+      run: git config --global --add safe.directory '*'
 
     - name: ownership workaround
-      run: git config --system --add safe.directory '*'
+      run: git config --global --add safe.directory '*'
 
     - name: make ${{matrix.target}}
       run: make ${{matrix.target}}

.github/workflows/ekf_functional_change_indicator.yml

@@ -5,11 +5,13 @@ on: pull_request
 jobs:
   unit_tests:
     runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-09-08
+    container: px4io/px4-dev-base-jammy:2024-05-18
     steps:
-    - uses: actions/checkout@v2.3.1
+    - uses: actions/checkout@v1
       with:
-        fetch-depth: 0
+        token: ${{ secrets.ACCESS_TOKEN }}
+    - name: ownership workaround
+      run: git config --global --add safe.directory '*'
     - name: checkout newest version of branch
       run: |
         git fetch origin pull/${{github.event.pull_request.number}}/head:${{github.head_ref}}

.github/workflows/ekf_update_change_indicator.yml

@@ -5,14 +5,16 @@ on: push
 jobs:
   unit_tests:
     runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-09-08
+    container: px4io/px4-dev-base-jammy:2024-05-18
     env:
       GIT_COMMITTER_EMAIL: bot@px4.io
       GIT_COMMITTER_NAME: PX4BuildBot
     steps:
-    - uses: actions/checkout@v2.3.1
+    - uses: actions/checkout@v1
       with:
-        fetch-depth: 0
+        token: ${{ secrets.ACCESS_TOKEN }}
+    - name: ownership workaround
+      run: git config --global --add safe.directory '*'
     - name: main test updates change indication files
       run: make tests TESTFILTER=EKF
     - name: Check if there exists diff and save result in variable

.github/workflows/failsafe_sim.yml

@@ -21,12 +21,14 @@ jobs:
           "failsafe_web",
         ]
     container:
-      image: px4io/px4-dev-nuttx-focal:2022-08-12
+      image: px4io/px4-dev-nuttx-jammy:2024-05-18
       options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
     steps:
     - uses: actions/checkout@v1
       with:
         token: ${{ secrets.ACCESS_TOKEN }}
+    - name: ownership workaround
+      run: git config --global --add safe.directory '*'
 
     - name: check environment
       run: |

.github/workflows/mavros_mission_tests.yml

@@ -17,20 +17,22 @@ jobs:
         config:
           - {vehicle: "iris",          mission: "MC_mission_box",  build_type: "RelWithDebInfo"}
           - {vehicle: "rover",         mission: "rover_mission_1", build_type: "RelWithDebInfo"}
-          #- {vehicle: "plane",         mission: "FW_mission_1",    build_type: "RelWithDebInfo"}
+          - {vehicle: "plane",         mission: "FW_mission_1",    build_type: "RelWithDebInfo"}
           #- {vehicle: "plane_catapult",mission: "FW_mission_1",    build_type: "RelWithDebInfo"}
           #- {vehicle: "standard_vtol", mission: "VTOL_mission_1",  build_type: "Coverage"}
           #- {vehicle: "standard_vtol", mission: "VTOL_mission_1",  build_type: "AddressSanitizer"}
-          #- {vehicle: "tailsitter",    mission: "VTOL_mission_1",  build_type: "RelWithDebInfo"}
-          #- {vehicle: "tiltrotor",     mission: "VTOL_mission_1",  build_type: "RelWithDebInfo"}
+          - {vehicle: "tailsitter",    mission: "VTOL_mission_1",  build_type: "RelWithDebInfo"}
+          - {vehicle: "tiltrotor",     mission: "VTOL_mission_1",  build_type: "RelWithDebInfo"}
 
     container:
-      image: px4io/px4-dev-ros-melodic:2021-09-08
+      image: px4io/px4-dev-ros-noetic:2024-05-18
       options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
     steps:
     - uses: actions/checkout@v1
       with:
         token: ${{ secrets.ACCESS_TOKEN }}
+    - name: ownership workaround
+      run: git config --global --add safe.directory '*'
 
     - name: Prepare ccache timestamp
       id: ccache_cache_timestamp

.github/workflows/mavros_offboard_tests.yml

@@ -20,12 +20,14 @@ jobs:
           #- {test_file: "mavros_posix_tests_offboard_rpyrt_ctl.test", vehicle: "iris", build_type: "RelWithDebInfo"}
 
     container:
-      image: px4io/px4-dev-ros-melodic:2021-09-08
+      image: px4io/px4-dev-ros-noetic:2024-05-18
       options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
     steps:
     - uses: actions/checkout@v1
       with:
         token: ${{ secrets.ACCESS_TOKEN }}
+    - name: ownership workaround
+      run: git config --global --add safe.directory '*'
 
     - name: Prepare ccache timestamp
       id: ccache_cache_timestamp

.github/workflows/metadata.yml

@@ -11,11 +11,13 @@ jobs:
 
   airframe:
     runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-09-08
+    container: px4io/px4-dev-base-focal:2024-05-18
     steps:
     - uses: actions/checkout@v1
       with:
         token: ${{ secrets.ACCESS_TOKEN }}
+    - name: ownership workaround
+      run: git config --global --add safe.directory '*'
 
     - name: airframe metadata
       run: |
@@ -37,11 +39,13 @@ jobs:
 
   module:
     runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-09-08
+    container: px4io/px4-dev-base-jammy:2024-05-18
     steps:
     - uses: actions/checkout@v1
       with:
         token: ${{ secrets.ACCESS_TOKEN }}
+    - name: ownership workaround
+      run: git config --global --add safe.directory '*'
 
     - name: module documentation
       run: |
@@ -52,11 +56,13 @@ jobs:
 
   parameter:
     runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-09-08
+    container: px4io/px4-dev-base-jammy:2024-05-18
     steps:
     - uses: actions/checkout@v1
       with:
         token: ${{ secrets.ACCESS_TOKEN }}
+    - name: ownership workaround
+      run: git config --global --add safe.directory '*'
 
     - name: parameter metadata
       run: |
@@ -76,11 +82,13 @@ jobs:
 
   events:
     runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-09-08
+    container: px4io/px4-dev-base-jammy:2024-05-18
     steps:
     - uses: actions/checkout@v1
       with:
         token: ${{ secrets.ACCESS_TOKEN }}
+    - name: ownership workaround
+      run: git config --global --add safe.directory '*'
 
     - name: events metadata
       run: |
@@ -103,11 +111,13 @@ jobs:
 
   uorb_graph:
     runs-on: ubuntu-latest
-    container: px4io/px4-dev-nuttx-focal:2022-08-12
+    container: px4io/px4-dev-nuttx-jammy:2024-05-18
     steps:
     - uses: actions/checkout@v1
       with:
         token: ${{ secrets.ACCESS_TOKEN }}
+    - name: ownership workaround
+      run: git config --global --add safe.directory '*'
 
     - name: uORB graph
       run: |
@@ -118,11 +128,13 @@ jobs:
 
   ROS2_msgs:
     runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-09-08
+    container: px4io/px4-dev-base-jammy:2024-05-18
     steps:
     - uses: actions/checkout@v1
       with:
         token: ${{ secrets.ACCESS_TOKEN }}
+    - name: ownership workaround
+      run: git config --global --add safe.directory '*'
 
     - name: PX4 ROS2 msgs
       run: |

.github/workflows/nuttx_env_config.yml

@@ -11,7 +11,7 @@ on:
 jobs:
   build:
     runs-on: ubuntu-latest
-    container: px4io/px4-dev-nuttx-focal:2022-08-12
+    container: px4io/px4-dev-nuttx-jammy:2024-05-18
     strategy:
       matrix:
         config: [
@@ -21,6 +21,8 @@ jobs:
     - uses: actions/checkout@v1
       with:
         token: ${{secrets.ACCESS_TOKEN}}
+    - name: ownership workaround
+      run: git config --global --add safe.directory '*'
 
     - name: make ${{matrix.config}}
       env:

.github/workflows/sitl_tests.yml

@@ -21,12 +21,14 @@ jobs:
           - {model: "standard_vtol", latitude:  "47.397742", longitude:    "8.545594", altitude: "488", build_type: "Coverage" } # Zurich
 
     container:
-      image: px4io/px4-dev-simulation-focal:2021-09-08
+      image: px4io/px4-dev-simulation-focal:2024-05-18
       options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
     steps:
     - uses: actions/checkout@v1
       with:
         token: ${{ secrets.ACCESS_TOKEN }}
+    - name: ownership workaround
+      run: git config --global --add safe.directory '*'
 
     - name: Download MAVSDK
       run: wget "https://github.com/mavlink/MAVSDK/releases/download/v$(cat test/mavsdk_tests/MAVSDK_VERSION)/libmavsdk-dev_$(cat test/mavsdk_tests/MAVSDK_VERSION)_ubuntu20.04_amd64.deb"

ROMFS/px4fmu_common/init.d/CMakeLists.txt

@@ -72,6 +72,8 @@ if(CONFIG_MODULES_ROVER_POS_CONTROL)
 	px4_add_romfs_files(
 		rc.rover_apps
 		rc.rover_defaults
+
+		rc.boat_defaults # hack
 	)
 endif()
 
@@ -82,13 +84,6 @@ if(CONFIG_MODULES_DIFFERENTIAL_DRIVE)
 	)
 endif()
 
-if(CONFIG_MODULES_BOAT)
-	px4_add_romfs_files(
-		rc.boat_apps
-		rc.boat_defaults
-	)
-endif()
-
 if(CONFIG_MODULES_UUV_ATT_CONTROL)
 	px4_add_romfs_files(
 		rc.uuv_apps

ROMFS/px4fmu_common/init.d/airframes/70001_boat

@@ -1,59 +0,0 @@
-#!/bin/sh
-#
-# @name RC Boat
-#
-# @type Boat
-# @class Boat
-#
-# @output Servo1 Steering
-# @output Motor1 Throttle
-
-. ${R}etc/init.d/rc.boat_defaults
-
-# Failsafe
-param set-default NAV_RCL_ACT 6 # Disarm on stick input loss
-param set-default NAV_DLL_ACT 6 # Disarm on ground station loss
-param set-default COM_DL_LOSS_T 5 # timeout for ground station connection
-param set-default COM_RC_LOSS_T 10 # timeout for stick input
-param set-default COM_RCL_EXCEPT 1 # Continue mission without stick input
-
-param set-default COM_ARM_WO_GPS 1 # Disable GPS prearm check
-param set-default COM_DISARM_LAND -1 # Don't disarm automatically
-param set-default COM_DISARM_PRFLT -1
-
-# Steering PWM output
-param set-default PWM_AUX_FUNC1 201
-param set-default PWM_AUX_DIS1 1500
-param set-default PWM_AUX_MIN1 1000
-param set-default PWM_AUX_MAX1 2000
-# Engine throttle PWM output
-param set-default PWM_AUX_FUNC2 101
-param set-default PWM_AUX_DIS2 900
-param set-default PWM_AUX_MIN2 940 # Engine doesn't rev below that
-param set-default PWM_AUX_MAX2 2000 # Maximum trhottle at which it's not so easy to flip the boat
-
-param set-default CA_AIRFRAME 5
-
-# Estimation
-param set-default EKF2_MULTI_IMU 0 # Disable multi-IMU
-param set-default SENS_IMU_MODE 1
-param set-default CAL_MAG0_PRIO 0 # Disable internal Magnetometer
-param set-default MBE_ENABLE 0 # Disable online mag calibration when disarmed
-param set-default EKF2_GPS_CTRL 15 # Enable GPS heading fusion
-param set-default SENS_GPS_MASK 0 # Disable multi-GPS blending
-
-# Estimation workarounds, should be double-checked if necessary
-param set-default EKF2_GBIAS_INIT 0.01
-param set-default EKF2_SYNT_MAG_Z 1 # Why? Who said magnetometer Z is not usable?
-
-param set-default EKF2_GPS_V_NOISE 0.751 # Lower certainty for GPS
-param set-default EKF2_GPS_POS_X 2.5 # GPS position, accurate?
-param set-default EKF2_REQ_EPV 10 # Accept GPS easier
-param set-default EKF2_REQ_HDRIFT 1
-param set-default EKF2_REQ_SACC 2
-param set-default EKF2_REQ_VDRIFT 20
-
-param set-default EKF2_MAG_CHK_STR 0.5 # Accept mag easier
-param set-default COM_ARM_MAG_STR 0 # Disable magnetic field strength prearm check
-param set-default COM_ARM_MAG_ANG 360 # Disable internal/external magnetometer consistency check
-param set-default EKF2_ABL_LIM 1 # Higher accelerometer bias limit, was sometimes above the threshold, GPS reporting 0 altitude issue?

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

@@ -157,9 +157,3 @@ if(CONFIG_MODULES_UUV_ATT_CONTROL)
 		60002_uuv_bluerov2_heavy
 	)
 endif()
-
-if(CONFIG_MODULES_BOAT)
-	px4_add_romfs_files(
-		70001_boat
-	)
-endif()

ROMFS/px4fmu_common/init.d/rc.boat_apps

@@ -1,11 +0,0 @@
-#!/bin/sh
-# Standard apps for a differential drive rover.
-
-# Start the attitude and position estimator.
-ekf2 start &
-
-# Start rover differential drive controller.
-boat start
-
-# Start Land Detector.
-land_detector start rover

ROMFS/px4fmu_common/init.d/rc.boat_defaults

@@ -5,7 +5,7 @@
 # NOTE: Script variables are declared/initialized/unset in the rcS script.
 #
 
-set VEHICLE_TYPE boat
+set VEHICLE_TYPE rover
 
 # MAV_TYPE_SURFACE_BOAT 11
 param set-default MAV_TYPE 11

ROMFS/px4fmu_common/init.d/rc.vehicle_setup

@@ -32,15 +32,6 @@ then
 	. ${R}etc/init.d/rc.rover_apps
 fi
 
-#
-# Boat setup.
-#
-if [ $VEHICLE_TYPE = boat ]
-then
-	# Start standard Boat apps.
-	. ${R}etc/init.d/rc.boat_apps
-fi
-
 #
 # Differential Rover setup.
 #

Tools/serial/rc.serial_port.jinja

@@ -5,7 +5,7 @@
 set SERIAL_DEV none
 {% for serial_device in serial_devices -%}
 if param compare "$PRT" {{ serial_device.index }}; then
-	if [ "x$PRT_{{ serial_device.tag }}_" = "x" ]; then
+	if [ "$PRT_{{ serial_device.tag }}_" = "" ]; then
 		set SERIAL_DEV {{ serial_device.device }}
 		set BAUD_PARAM SER_{{ serial_device.tag }}_BAUD
 		set PRT_{{ serial_device.tag }}_ 1

boards/ark/cannode/default.px4board

@@ -14,7 +14,6 @@ CONFIG_DRIVERS_DSHOT=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_COMMON_HYGROMETERS=y
 CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y
-CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16507=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
 CONFIG_DRIVERS_IRLOCK=y

boards/mro/ctrl-zero-classic/init/rc.board_defaults

@@ -6,5 +6,4 @@
 param set-default BAT1_V_DIV 10.1
 param set-default BAT1_A_PER_V 17
 
-param set-default GPS_2_CONFIG 202
 param set-default TEL_FRSKY_CONFIG 103

boards/px4/fmu-v5x/boat.px4board

@@ -1,17 +0,0 @@
-CONFIG_DRIVERS_IRLOCK=n
-CONFIG_MODULES_AIRSPEED_SELECTOR=n
-CONFIG_MODULES_BOAT=y
-CONFIG_MODULES_FLIGHT_MODE_MANAGER=n
-CONFIG_MODULES_FW_ATT_CONTROL=n
-CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
-CONFIG_MODULES_FW_POS_CONTROL=n
-CONFIG_MODULES_FW_RATE_CONTROL=n
-CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n
-CONFIG_MODULES_MC_ATT_CONTROL=n
-CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n
-CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n
-CONFIG_MODULES_MC_POS_CONTROL=n
-CONFIG_MODULES_MC_RATE_CONTROL=n
-CONFIG_MODULES_VTOL_ATT_CONTROL=n
-CONFIG_EKF2_AUX_GLOBAL_POSITION=y
-# CONFIG_EKF2_WIND is not set

boards/px4/fmu-v6x/boat.px4board

@@ -1,17 +0,0 @@
-CONFIG_DRIVERS_IRLOCK=n
-CONFIG_MODULES_AIRSPEED_SELECTOR=n
-CONFIG_MODULES_BOAT=y
-CONFIG_MODULES_FLIGHT_MODE_MANAGER=n
-CONFIG_MODULES_FW_ATT_CONTROL=n
-CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
-CONFIG_MODULES_FW_POS_CONTROL=n
-CONFIG_MODULES_FW_RATE_CONTROL=n
-CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n
-CONFIG_MODULES_MC_ATT_CONTROL=n
-CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n
-CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n
-CONFIG_MODULES_MC_POS_CONTROL=n
-CONFIG_MODULES_MC_RATE_CONTROL=n
-CONFIG_MODULES_VTOL_ATT_CONTROL=n
-CONFIG_EKF2_AUX_GLOBAL_POSITION=y
-# CONFIG_EKF2_WIND is not set

boards/px4/fmu-v6x/default.px4board

@@ -66,7 +66,6 @@ CONFIG_MODULES_LOGGER=y
 CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
-CONFIG_MAVLINK_DIALECT="development"
 CONFIG_MODULES_MC_ATT_CONTROL=y
 CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y

integrationtests/python_src/px4_it/mavros/mavros_offboard_attctl_test.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python2
+#!/usr/bin/env python
 #***************************************************************************
 #
 #   Copyright (c) 2015 PX4 Development Team. All rights reserved.
@@ -35,8 +35,7 @@
 #
 # @author Andreas Antener <andreas@uaventure.com>
 #
-# The shebang of this file is currently Python2 because some
-# dependencies such as pymavlink don't play well with Python3 yet.
+
 from __future__ import division
 
 PKG = 'px4'

integrationtests/python_src/px4_it/mavros/mavros_offboard_posctl_test.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python2
+#!/usr/bin/env python
 #***************************************************************************
 #
 #   Copyright (c) 2015 PX4 Development Team. All rights reserved.
@@ -35,8 +35,7 @@
 #
 # @author Andreas Antener <andreas@uaventure.com>
 #
-# The shebang of this file is currently Python2 because some
-# dependencies such as pymavlink don't play well with Python3 yet.
+
 from __future__ import division
 
 PKG = 'px4'

integrationtests/python_src/px4_it/mavros/mavros_offboard_yawrate_test.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python2
+#!/usr/bin/env python
 #***************************************************************************
 #
 #   Copyright (c) 2020 PX4 Development Team. All rights reserved.

integrationtests/python_src/px4_it/mavros/mavros_test_common.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python2
+#!/usr/bin/env python
 from __future__ import division
 
 import unittest

integrationtests/python_src/px4_it/mavros/mission_test.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python2
+#!/usr/bin/env python
 #***************************************************************************
 #
 #   Copyright (c) 2015-2016 PX4 Development Team. All rights reserved.
@@ -36,8 +36,6 @@
 # @author Andreas Antener <andreas@uaventure.com>
 #
 
-# The shebang of this file is currently Python2 because some
-# dependencies such as pymavlink don't play well with Python3 yet.
 from __future__ import division
 
 PKG = 'px4'
@@ -308,7 +306,7 @@ class MavrosMissionTest(MavrosTestCommon):
         self.assertTrue(res['pitch_error_std'] < 5.0, str(res))
 
         # TODO: fix by excluding initial heading init and reset preflight
-        self.assertTrue(res['yaw_error_std'] < 13.0, str(res))
+        self.assertTrue(res['yaw_error_std'] < 10.0, str(res))
 
 
 if __name__ == '__main__':

msg/BoatSetpoint.msg

@@ -1,10 +0,0 @@
-uint64 timestamp # time since system start (microseconds)
-
-float32 speed # [m/s] collective roll-off speed in body x-axis
-bool closed_loop_speed_control # true if speed is controlled using estimator feedback, false if direct feed-forward
-float32 yaw_rate # [rad] yaw_rate
-bool closed_loop_yaw_rate_control # true if rudder angle is controlled using feedback, false if direct feed-forward
-float32 heading_setpoint # [rad] desired heading
-bool closed_loop_heading_control # true if heading is controlled using feedback, false if direct feed-forward
-
-# TOPICS boat_setpoint boat_control_output

msg/CMakeLists.txt

@@ -53,7 +53,6 @@ set(msg_files
 	ArmingCheckRequest.msg
 	AutotuneAttitudeControlStatus.msg
 	BatteryStatus.msg
-	BoatSetpoint.msg
 	Buffer128.msg
 	ButtonEvent.msg
 	CameraCapture.msg

src/lib/wind_estimator/python/wind_estimator_replay.py

@@ -54,7 +54,7 @@ def getData(log, topic_name, variable_name, instance=0):
 def us2s(time_ms):
     return time_ms * 1e-6
 
-def run(logfile, use_gnss, scale_init):
+def run(logfile, use_gnss):
     log = ULog(logfile)
 
     if use_gnss:
@@ -75,10 +75,7 @@ def run(logfile, use_gnss, scale_init):
     dist_bottom = getData(log, 'vehicle_local_position', 'dist_bottom')
     t_dist_bottom = us2s(getData(log, 'vehicle_local_position', 'timestamp'))
 
-    if scale_init is None:
-        scale_init = 1.0
-
-    state = np.array([0.0, 0.0, scale_init])
+    state = np.array([0.0, 0.0, 1.0])
     P = np.diag([1.0, 1.0, 1e-4])
     wind_nsd = 1e-2
     scale_nsd = 1e-4
@@ -106,12 +103,12 @@ def run(logfile, use_gnss, scale_init):
 
             P += Q * dt
 
-            if i_airspeed < len(t_true_airspeed) and t_true_airspeed[i_airspeed] < t_now:
+            if t_true_airspeed[i_airspeed] < t_now:
                 while i_airspeed < len(t_true_airspeed) and t_true_airspeed[i_airspeed] < t_now:
                     i_airspeed += 1
                 i_airspeed -= 1
 
-                (H, K, innov_var, innov) = fuse_airspeed(np.asarray(v_local[:,i]), state, P, true_airspeed[i_airspeed], R, epsilon)
+                (H, K, innov_var, innov) = fuse_airspeed(np.asarray(v_local[:,i]), state, P.flatten(), true_airspeed[i_airspeed], R, epsilon)
                 state += np.array(K) * innov
                 P -= K * H * P
                 i_airspeed += 1
@@ -148,9 +145,8 @@ if __name__ == '__main__':
     parser.add_argument('logfile', help='Full ulog file path, name and extension', type=str)
     parser.add_argument('--gnss', help='Use GNSS velocity instead of local velocity estimate',
                         action='store_true')
-    parser.add_argument('--scale_init', help='Initial airsped scale factor (1.0 if not specified)', type=float)
     args = parser.parse_args()
 
     logfile = os.path.abspath(args.logfile) # Convert to absolute path
 
-    run(logfile, args.gnss, args.scale_init)
+    run(logfile, args.gnss)

src/modules/boat/Boat.cpp

@@ -1,189 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2023-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 "Boat.hpp"
-
-Boat::Boat() :
-	ModuleParams(nullptr),
-	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
-{
-	updateParams();
-}
-
-bool Boat::init()
-{
-	ScheduleOnInterval(10_ms); // 100 Hz
-	return true;
-}
-
-void Boat::updateParams()
-{
-	ModuleParams::updateParams();
-
-	_max_speed = _param_bt_spd_max.get();
-	_boat_guidance.setMaxSpeed(_max_speed);
-	_boat_kinematics.setMaxSpeed(_max_speed);
-
-	_max_angular_velocity = _param_bt_ang_max.get();
-	_boat_guidance.setMaxAngularVelocity(_max_angular_velocity);
-	_boat_kinematics.setMaxAngularVelocity(_max_angular_velocity);
-}
-
-void Boat::Run()
-{
-	if (should_exit()) {
-		ScheduleClear();
-		exit_and_cleanup();
-	}
-
-	hrt_abstime now = hrt_absolute_time();
-	const float dt = math::min((now - _time_stamp_last), 5000_ms) / 1e6f;
-	_time_stamp_last = now;
-
-	if (_parameter_update_sub.updated()) {
-		parameter_update_s parameter_update;
-		_parameter_update_sub.copy(&parameter_update);
-		updateParams();
-	}
-
-	if (_vehicle_control_mode_sub.updated()) {
-		vehicle_control_mode_s vehicle_control_mode{};
-
-		if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) {
-			_manual_driving = vehicle_control_mode.flag_control_manual_enabled;
-			_mission_driving = vehicle_control_mode.flag_control_auto_enabled;
-		}
-	}
-
-	if (_vehicle_status_sub.updated()) {
-		vehicle_status_s vehicle_status{};
-
-		if (_vehicle_status_sub.copy(&vehicle_status)) {
-			const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
-			const bool spooled_up = armed && (hrt_elapsed_time(&vehicle_status.armed_time) > _param_com_spoolup_time.get() * 1_s);
-			_boat_kinematics.setArmed(spooled_up);
-			_acro_driving = (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ACRO);
-		}
-	}
-
-	if (_manual_driving) {
-		// Manual mode
-		// Directly produce setpoints from the manual control setpoint (joystick)
-		if (_manual_control_setpoint_sub.updated()) {
-			manual_control_setpoint_s manual_control_setpoint{};
-
-			if (_manual_control_setpoint_sub.copy(&manual_control_setpoint)) {
-				boat_setpoint_s setpoint{};
-				setpoint.speed = ((manual_control_setpoint.throttle + 1.f) * 0.5f) * math::max(0.f, _param_bt_spd_scale.get());
-				setpoint.yaw_rate = manual_control_setpoint.roll * _param_bt_ang_vel_scale.get();
-
-				// if acro mode, we activate the yaw rate control
-				if (_acro_driving) {
-					setpoint.closed_loop_speed_control = false;
-					setpoint.closed_loop_yaw_rate_control = true;
-
-				} else {
-					setpoint.closed_loop_speed_control = false;
-					setpoint.closed_loop_yaw_rate_control = false;
-				}
-
-
-				setpoint.timestamp = now;
-				_boat_setpoint_pub.publish(setpoint);
-			}
-		}
-
-	} else if (_mission_driving) {
-		// Mission mode
-		// Directly receive setpoints from the guidance library
-		_boat_guidance.computeGuidance(
-			_boat_control.getVehicleYaw(),
-			_boat_control.getLocalPosition(),
-			dt
-		);
-	}
-
-	_boat_control.control(dt);
-	_boat_kinematics.allocate();
-}
-
-int Boat::task_spawn(int argc, char *argv[])
-{
-	Boat *instance = new Boat();
-
-	if (instance) {
-		_object.store(instance);
-		_task_id = task_id_is_work_queue;
-
-		if (instance->init()) {
-			return PX4_OK;
-		}
-
-	} else {
-		PX4_ERR("alloc failed");
-	}
-
-	delete instance;
-	_object.store(nullptr);
-	_task_id = -1;
-
-	return PX4_ERROR;
-}
-
-int Boat::custom_command(int argc, char *argv[])
-{
-	return print_usage("unknown command");
-}
-
-int Boat::print_usage(const char *reason)
-{
-	if (reason) {
-		PX4_ERR("%s\n", reason);
-	}
-
-	PRINT_MODULE_DESCRIPTION(
-		R"DESCR_STR(
-### Description
-Boat Drive controller.
-)DESCR_STR");
-
-	PRINT_MODULE_USAGE_NAME("boat", "controller");
-	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
-	return 0;
-}
-
-extern "C" __EXPORT int boat_main(int argc, char *argv[])
-{
-	return Boat::main(argc, argv);
-}

src/modules/boat/Boat.hpp

@@ -1,109 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2023-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.
- *
- ****************************************************************************/
-
-#pragma once
-
-#include <px4_platform_common/px4_config.h>
-#include <px4_platform_common/defines.h>
-#include <px4_platform_common/module.h>
-#include <px4_platform_common/module_params.h>
-#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
-#include <uORB/Publication.hpp>
-#include <uORB/Subscription.hpp>
-#include <uORB/PublicationMulti.hpp>
-#include <uORB/topics/actuator_motors.h>
-#include <uORB/topics/actuator_servos.h>
-#include <uORB/topics/boat_setpoint.h>
-#include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/vehicle_control_mode.h>
-#include <uORB/topics/vehicle_status.h>
-
-#include "BoatControl/BoatControl.hpp"
-#include "BoatGuidance/BoatGuidance.hpp"
-#include "BoatKinematics/BoatKinematics.hpp"
-
-using namespace time_literals;
-
-class Boat : public ModuleBase<Boat>, public ModuleParams,
-	public px4::ScheduledWorkItem
-{
-public:
-	Boat();
-	~Boat() override = default;
-
-	/** @see ModuleBase */
-	static int task_spawn(int argc, char *argv[]);
-
-	/** @see ModuleBase */
-	static int custom_command(int argc, char *argv[]);
-
-	/** @see ModuleBase */
-	static int print_usage(const char *reason = nullptr);
-
-	bool init();
-
-protected:
-	void updateParams() override;
-
-private:
-	void Run() override;
-	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
-	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
-	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
-	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
-
-	uORB::Publication<boat_setpoint_s> _boat_setpoint_pub{ORB_ID(boat_setpoint)};
-
-	bool _manual_driving = false;
-	bool _mission_driving = false;
-	bool _acro_driving = false;
-
-
-	BoatGuidance _boat_guidance{this};
-	BoatControl _boat_control{this};
-	BoatKinematics _boat_kinematics{this};
-
-	float _max_speed{0.f};
-	float _max_angular_velocity{0.f};
-
-	hrt_abstime _time_stamp_last{0};
-
-	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::COM_SPOOLUP_TIME>) _param_com_spoolup_time,
-		(ParamFloat<px4::params::BT_ANG_VEL_SCALE>) _param_bt_ang_vel_scale,
-		(ParamFloat<px4::params::BT_SPD_SCALE>) _param_bt_spd_scale,
-		(ParamFloat<px4::params::BT_ANG_MAX>) _param_bt_ang_max,
-		(ParamFloat<px4::params::BT_SPD_MAX>) _param_bt_spd_max
-	)
-};

src/modules/boat/BoatControl/BoatControl.cpp

@@ -1,110 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2023-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 "BoatControl.hpp"
-
-using namespace matrix;
-
-BoatControl::BoatControl(ModuleParams *parent) : ModuleParams(parent)
-{
-	pid_init(&_pid_angular_velocity, PID_MODE_DERIVATIV_NONE, 0.001f);
-	pid_init(&_pid_speed, PID_MODE_DERIVATIV_NONE, 0.001f);
-}
-
-void BoatControl::updateParams()
-{
-	ModuleParams::updateParams();
-
-	pid_set_parameters(&_pid_angular_velocity,
-			   _param_bt_ang_p.get(),
-			   _param_bt_ang_i.get(),
-			   0,
-			   _param_bt_ang_imax.get(),
-			   _param_bt_ang_outlim.get());
-
-	pid_set_parameters(&_pid_speed,
-			   _param_bt_spd_p.get(),
-			   _param_bt_spd_i.get(),
-			   0,
-			   _param_bt_spd_imax.get(),
-			   _param_bt_spd_outlim.get());
-}
-
-void BoatControl::control(float dt)
-{
-	if (_vehicle_angular_velocity_sub.updated()) {
-		vehicle_angular_velocity_s vehicle_angular_velocity{};
-
-		if (_vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity)) {
-			_vehicle_body_yaw_rate = vehicle_angular_velocity.xyz[2];
-		}
-	}
-
-	if (_vehicle_attitude_sub.updated()) {
-		vehicle_attitude_s vehicle_attitude{};
-
-		if (_vehicle_attitude_sub.copy(&vehicle_attitude)) {
-			_vehicle_attitude_quaternion = Quatf(vehicle_attitude.q);
-			_vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi();
-		}
-	}
-
-	if (_vehicle_local_position_sub.updated()) {
-		vehicle_local_position_s vehicle_local_position{};
-
-		if (_vehicle_local_position_sub.copy(&vehicle_local_position)) {
-			_vehicle_local_position = vehicle_local_position;
-			Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
-			Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame);
-			_vehicle_forward_speed = velocity_in_body_frame(0);
-		}
-	}
-
-	_boat_setpoint_sub.update(&_boat_setpoint);
-
-	// PID to reach setpoint using control_output
-	boat_setpoint_s boat_control_output = _boat_setpoint;
-
-	if (_boat_setpoint.closed_loop_speed_control) {
-		boat_control_output.speed +=
-			pid_calculate(&_pid_speed, _boat_setpoint.speed, _vehicle_forward_speed, 0, dt);
-	}
-
-	if (_boat_setpoint.closed_loop_yaw_rate_control) {
-		boat_control_output.yaw_rate +=
-			pid_calculate(&_pid_angular_velocity, _boat_setpoint.yaw_rate, _vehicle_body_yaw_rate, 0, dt);
-	}
-
-	boat_control_output.timestamp = hrt_absolute_time();
-	_boat_control_output_pub.publish(boat_control_output);
-}

src/modules/boat/BoatControl/BoatControl.hpp

@@ -1,101 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2023-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 BoatControl.hpp
- *
- * Controller for heading rate and forward speed.
- */
-
-#pragma once
-
-#include <lib/pid/pid.h>
-#include <matrix/matrix/math.hpp>
-#include <px4_platform_common/module_params.h>
-#include <uORB/Publication.hpp>
-#include <uORB/Subscription.hpp>
-#include <uORB/topics/boat_setpoint.h>
-#include <uORB/topics/vehicle_angular_velocity.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_local_position.h>
-
-using namespace matrix;
-
-class BoatControl : public ModuleParams
-{
-public:
-	BoatControl(ModuleParams *parent);
-	~BoatControl() = default;
-
-	void control(float dt);
-	float getVehicleBodyYawRate() const { return _vehicle_body_yaw_rate; }
-	float getVehicleYaw() const { return _vehicle_yaw; }
-	vehicle_local_position_s getLocalPosition() const { return _vehicle_local_position; }
-
-protected:
-	void updateParams() override;
-
-private:
-	uORB::Subscription _boat_setpoint_sub{ORB_ID(boat_setpoint)};
-	uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
-	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
-	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
-
-	uORB::Publication<boat_setpoint_s> _boat_control_output_pub{ORB_ID(boat_control_output)};
-
-	boat_setpoint_s _boat_setpoint{};
-
-	matrix::Quatf _vehicle_attitude_quaternion{};
-	float _vehicle_yaw{0.f};
-
-
-	// States
-	float _vehicle_body_yaw_rate{0.f};
-	float _vehicle_forward_speed{0.f};
-	Vector2f _vehicle_speed{0.f, 0.f};
-	vehicle_local_position_s _vehicle_local_position{};
-
-	PID_t _pid_angular_velocity;
-	PID_t _pid_speed;
-
-	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::BT_SPD_P>) _param_bt_spd_p,
-		(ParamFloat<px4::params::BT_SPD_I>) _param_bt_spd_i,
-		(ParamFloat<px4::params::BT_SPD_IMAX>) _param_bt_spd_imax,
-		(ParamFloat<px4::params::BT_SPD_OUTLIM>) _param_bt_spd_outlim,
-		(ParamFloat<px4::params::BT_ANG_P>) _param_bt_ang_p,
-		(ParamFloat<px4::params::BT_ANG_I>) _param_bt_ang_i,
-		(ParamFloat<px4::params::BT_ANG_IMAX>) _param_bt_ang_imax,
-		(ParamFloat<px4::params::BT_ANG_OUTLIM>) _param_bt_ang_outlim
-	)
-};

src/modules/boat/BoatControl/CMakeLists.txt

@@ -1,39 +0,0 @@
-############################################################################
-#
-#   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(BoatControl
-	BoatControl.cpp
-)
-
-target_link_libraries(BoatControl PUBLIC pid)
-target_include_directories(BoatControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/modules/boat/BoatGuidance/BoatGuidance.cpp

@@ -1,264 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2023-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 "BoatGuidance.hpp"
-
-#include <mathlib/math/Limits.hpp>
-
-using namespace matrix;
-
-BoatGuidance::BoatGuidance(ModuleParams *parent) : ModuleParams(parent)
-{
-	updateParams();
-
-	_currentState = GuidanceState::kDriving;
-}
-
-void BoatGuidance::computeGuidance(float yaw, vehicle_local_position_s vehicle_local_position,
-				   float dt)
-{
-	if (_position_setpoint_triplet_sub.updated()) {
-		_position_setpoint_triplet_sub.copy(&_position_setpoint_triplet);
-	}
-
-	if (_vehicle_global_position_sub.updated()) {
-		_vehicle_global_position_sub.copy(&_vehicle_global_position);
-	}
-
-	const matrix::Vector2d global_position(_vehicle_global_position.lat, _vehicle_global_position.lon);
-	const matrix::Vector2d current_waypoint(_position_setpoint_triplet.current.lat, _position_setpoint_triplet.current.lon);
-	const matrix::Vector2d next_waypoint(_position_setpoint_triplet.next.lat, _position_setpoint_triplet.next.lon);
-	const matrix::Vector2d previous_waypoint(_position_setpoint_triplet.previous.lat,
-			_position_setpoint_triplet.previous.lon);
-
-	if (!_global_local_proj_ref.isInitialized()
-	    || (_global_local_proj_ref.getProjectionReferenceTimestamp() != vehicle_local_position.timestamp)) {
-		_global_local_proj_ref.initReference(vehicle_local_position.ref_lat, vehicle_local_position.ref_lon,
-						     vehicle_local_position.timestamp);
-	}
-
-	const Vector2f current_waypoint_local_frame = _global_local_proj_ref.project(
-				current_waypoint(0),
-				current_waypoint(1));
-	const Vector2f previous_waypoint_local_frame = _global_local_proj_ref.project(
-				previous_waypoint(0),
-				previous_waypoint(1));
-	const Vector2f local_frame_position = Vector2f(vehicle_local_position.x, vehicle_local_position.y);
-
-	const float distance_to_next_wp = get_distance_to_next_waypoint(
-			global_position(0),
-			global_position(1),
-			current_waypoint(0),
-			current_waypoint(1));
-
-	float heading_error = 0.f;
-	float speed_interpolation = 0.f;
-	float heading_to_next_waypoint = 0.f;
-	float heading_error_to_next_waypoint = 0.f;
-	float desired_speed = _param_bt_spd_cruise.get();
-
-	// Go back to driving, when the waypoint has been reached
-	if (_current_waypoint != current_waypoint) {
-		_currentState = GuidanceState::kDriving;
-	}
-
-	// Make boat stop when it arrives at the last waypoint
-	if ((current_waypoint == next_waypoint) && distance_to_next_wp <= _param_nav_acc_rad.get()) {
-		_currentState = GuidanceState::kGoalReached;
-	}
-
-	switch (_currentState) {
-	case GuidanceState::kDriving: {
-			if (PX4_ISFINITE(previous_waypoint(0)) && PX4_ISFINITE(previous_waypoint(1))) {
-
-				float look_ahead_distance = getLookAheadDistance(
-								    current_waypoint_local_frame,
-								    previous_waypoint_local_frame,
-								    local_frame_position
-							    );
-
-				float desired_heading = calcDesiredHeading(
-								current_waypoint_local_frame,
-								previous_waypoint_local_frame,
-								local_frame_position,
-								look_ahead_distance
-							);
-
-				heading_error = matrix::wrap_pi(desired_heading - yaw);
-				_desired_angular_velocity = heading_error;
-
-				heading_to_next_waypoint = get_bearing_to_next_waypoint(
-								   previous_waypoint(0),
-								   previous_waypoint(1),
-								   current_waypoint(0),
-								   current_waypoint(1)
-							   );
-
-			} else {
-				_previous_local_position = local_frame_position;
-				_previous_position = global_position;
-				_currentState = GuidanceState::kDrivingToAPoint;
-			}
-
-			break;
-		}
-
-	// Control logic if there is no previous waypoint
-	case GuidanceState::kDrivingToAPoint: {
-			float look_ahead_distance = getLookAheadDistance(
-							    current_waypoint_local_frame,
-							    _previous_local_position,
-							    local_frame_position
-						    );
-
-			float desired_heading = calcDesiredHeading(
-							current_waypoint_local_frame,
-							_previous_local_position,
-							local_frame_position,
-							look_ahead_distance
-						);
-
-			heading_error = matrix::wrap_pi(desired_heading - yaw);
-			_desired_angular_velocity = heading_error;
-
-			heading_to_next_waypoint = get_bearing_to_next_waypoint(
-							   _previous_position(0),
-							   _previous_position(1),
-							   current_waypoint(0),
-							   current_waypoint(1)
-						   );
-			break;
-		}
-
-	case GuidanceState::kGoalReached:
-		desired_speed = 0.f;
-		heading_error = 0.f;
-		_desired_angular_velocity = 0.f;
-		break;
-	}
-
-	heading_error_to_next_waypoint = matrix::wrap_pi(heading_to_next_waypoint - yaw);
-
-	// Interpolate the speed based on the heading error
-	if (PX4_ISFINITE(heading_error_to_next_waypoint) && desired_speed > 0.1f) {
-
-		speed_interpolation = math::interpolate<float>(abs(heading_error_to_next_waypoint),
-				      _param_bt_min_heading_error.get() * M_PI_F / 180.f,
-				      _param_bt_max_heading_error.get() * M_PI_F / 180.f,
-				      _param_bt_spd_cruise.get(),
-				      _param_bt_spd_min.get());
-		desired_speed = math::constrain(speed_interpolation, _param_bt_spd_min.get(), _max_speed);
-
-	}
-
-	boat_setpoint_s output{};
-	output.speed = math::constrain(desired_speed, -_max_speed, _max_speed);
-	output.yaw_rate = math::constrain(_desired_angular_velocity, -_max_angular_velocity, _max_angular_velocity);
-	output.closed_loop_speed_control = true;
-	output.closed_loop_yaw_rate_control = true;
-	output.timestamp = hrt_absolute_time();
-
-	_boat_setpoint_pub.publish(output);
-
-	_current_waypoint = current_waypoint;
-}
-
-float BoatGuidance::getLookAheadDistance(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local,
-		const Vector2f &curr_pos_local)
-{
-	// Calculate crosstrack error
-	const Vector2f prev_wp_to_curr_wp_local = curr_wp_local - prev_wp_local;
-
-	if (prev_wp_to_curr_wp_local.norm() < FLT_EPSILON) { // Avoid division by 0 (this case should not happen)
-		return 0.f;
-	}
-
-	const Vector2f prev_wp_to_curr_pos_local = curr_pos_local - prev_wp_local;
-	const Vector2f distance_on_line_segment = ((prev_wp_to_curr_pos_local * prev_wp_to_curr_wp_local) /
-			prev_wp_to_curr_wp_local.norm()) * prev_wp_to_curr_wp_local.normalized();
-	const Vector2f crosstrack_error = (prev_wp_local + distance_on_line_segment) - curr_pos_local;
-
-	if (crosstrack_error.length() < _param_look_ahead_distance.get()) {
-		return _param_look_ahead_distance.get();
-
-	} else {
-		return crosstrack_error.length();
-	}
-}
-
-float BoatGuidance::calcDesiredHeading(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local,
-				       Vector2f const &curr_pos_local, float const &lookahead_distance)
-{
-	// Setup variables
-	const float line_segment_slope = (curr_wp_local(1) - prev_wp_local(1)) / (curr_wp_local(0) - prev_wp_local(0));
-	const float line_segment_rover_offset = prev_wp_local(1) - curr_pos_local(1) + line_segment_slope * (curr_pos_local(
-			0) - prev_wp_local(0));
-	const float a = -line_segment_slope;
-	const float c = -line_segment_rover_offset;
-	const float r = lookahead_distance;
-	const float x0 = -a * c / (a * a + 1.0f);
-	const float y0 = -c / (a * a + 1.0f);
-
-	// Calculate intersection points
-	if (c * c > r * r * (a * a + 1.0f) + FLT_EPSILON) { // No intersection points exist
-		return 0.f;
-
-	} else if (abs(c * c - r * r * (a * a + 1.0f)) < FLT_EPSILON) { // One intersection point exists
-		return atan2f(y0, x0);
-
-	} else { // Two intersetion points exist
-		const float d = r * r - c * c / (a * a + 1.0f);
-		const float mult = sqrt(d / (a * a + 1.0f));
-		const float ax = x0 + mult;
-		const float bx = x0 - mult;
-		const float ay = y0 - a * mult;
-		const float by = y0 + a * mult;
-		const Vector2f point1(ax, ay);
-		const Vector2f point2(bx, by);
-		const Vector2f distance1 = (curr_wp_local - curr_pos_local) - point1;
-		const Vector2f distance2 = (curr_wp_local - curr_pos_local) - point2;
-
-		// Return intersection point closer to current waypoint
-		if (distance1.norm_squared() < distance2.norm_squared()) {
-			return atan2f(ay, ax);
-
-		} else {
-			return atan2f(by, bx);
-		}
-	}
-}
-
-void BoatGuidance::updateParams()
-{
-	ModuleParams::updateParams();
-}

src/modules/boat/BoatGuidance/BoatGuidance.hpp

@@ -1,170 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2023-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.
- *
- ****************************************************************************/
-
-#pragma once
-
-#include <px4_platform_common/module_params.h>
-
-#include <matrix/matrix/math.hpp>
-#include <matrix/math.hpp>
-#include <mathlib/mathlib.h>
-#include <lib/geo/geo.h>
-#include <math.h>
-
-#include <lib/motion_planning/PositionSmoothing.hpp>
-#include <lib/motion_planning/VelocitySmoothing.hpp>
-
-#include <uORB/Publication.hpp>
-#include <uORB/Subscription.hpp>
-#include <uORB/topics/boat_setpoint.h>
-#include <uORB/topics/position_setpoint_triplet.h>
-#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_local_position.h>
-
-#include <lib/pid/pid.h>
-#include <lib/l1/ECL_L1_Pos_Controller.hpp>
-
-using namespace matrix;
-/**
- * @brief Enum class for the different states of guidance.
- */
-enum class GuidanceState {
-	kDriving, ///< The vehicle is currently driving straight.
-	kDrivingToAPoint, ///< The vehicle is currently driving to the next waypoint.
-	kGoalReached ///< The vehicle has reached its goal.
-};
-
-/**
- * @brief Class for boat drive guidance.
- */
-class BoatGuidance : public ModuleParams
-{
-public:
-	/**
-	 * @brief Constructor for BoatGuidance.
-	 * @param parent The parent ModuleParams object.
-	 */
-	BoatGuidance(ModuleParams *parent);
-	~BoatGuidance() = default;
-
-	/**
-	 * @brief Compute guidance for the vehicle.
-	 * @param global_pos The global position of the vehicle in degrees.
-	 * @param current_waypoint The current waypoint the vehicle is heading towards in degrees.
-	 * @param next_waypoint The next waypoint the vehicle will head towards after reaching the current waypoint in degrees.
-	 * @param vehicle_yaw The yaw orientation of the vehicle in radians.
-	 * @param body_velocity The velocity of the vehicle in m/s.
-	 * @param angular_velocity The angular velocity of the vehicle in rad/s.
-	 * @param dt The time step in seconds.
-	 */
-	void computeGuidance(float yaw, vehicle_local_position_s vehicle_local_position, float dt);
-
-	/**
-	 * @brief Calculate the lookahead distance based on the current and previous waypoints and the current position.
-	 * @param curr_wp_local The current waypoint in local coordinates.
-	 * @param prev_wp_local The previous waypoint in local coordinates.
-	 * @param curr_pos_local The current position in local coordinates.
-	 * @return The calculated lookahead distance.
-	 */
-	float getLookAheadDistance(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local,
-				   const Vector2f &curr_pos_local);
-
-	/**
-	 * @brief Calculate the desired heading based on the current and previous waypoints, current position, and lookahead distance.
-	 * @param curr_wp_local The current waypoint in local coordinates.
-	 * @param prev_wp_local The previous waypoint in local coordinates.
-	 * @param curr_pos_local The current position in local coordinates.
-	 * @param lookahead_distance The lookahead distance.
-	 * @return The calculated desired heading.
-	 */
-	float calcDesiredHeading(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local, const Vector2f &curr_pos_local,
-				 const float &lookahead_distance);
-
-	/**
-	 * @brief Set the maximum speed for the vehicle.
-	 * @param max_speed The maximum speed in m/s.
-	 * @return The set maximum speed in m/s.
-	 */
-	float setMaxSpeed(float max_speed) { return _max_speed = max_speed; }
-
-
-	/**
-	 * @brief Set the maximum angular velocity for the boat.
-	 * @param max_angular_velocity The maximum angular velocity in rad/s.
-	 * @return The set maximum angular velocity in rad/s.
-	 */
-	float setMaxAngularVelocity(float max_angular_velocity) { return _max_angular_velocity = max_angular_velocity; }
-
-
-
-protected:
-	/**
-	 * @brief Update the parameters of the module.
-	 */
-	void updateParams() override;
-
-private:
-	uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)};
-	uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
-
-	uORB::Publication<boat_setpoint_s> _boat_setpoint_pub{ORB_ID(boat_setpoint)};
-
-	position_setpoint_triplet_s _position_setpoint_triplet{};
-	vehicle_global_position_s _vehicle_global_position{};
-
-	GuidanceState _currentState;
-
-	float _desired_angular_velocity{};
-	float _max_angular_velocity{};
-	float _look_ahead_distance{};
-	float _max_speed{};
-
-	VelocitySmoothing _forwards_velocity_smoothing{};
-	PositionSmoothing _position_smoothing{};
-	MapProjection _global_local_proj_ref{};
-	matrix::Vector2d _current_waypoint{};
-	ECL_L1_Pos_Controller _l1_guidance{};
-	Vector2f _previous_local_position{};
-	Vector2d _previous_position{};
-
-	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::BT_MAX_HERR>) _param_bt_max_heading_error,
-		(ParamFloat<px4::params::BT_MIN_HERR>) _param_bt_min_heading_error,
-		(ParamFloat<px4::params::BT_LOOKAHEAD>) _param_look_ahead_distance,
-		(ParamFloat<px4::params::NAV_LOITER_RAD>) _param_nav_loiter_rad,
-		(ParamFloat<px4::params::BT_SPD_CRUISE>) _param_bt_spd_cruise,
-		(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad,
-		(ParamFloat<px4::params::BT_SPD_MAX>) _param_bt_spd_max,
-		(ParamFloat<px4::params::BT_SPD_MIN>) _param_bt_spd_min
-	)
-};

src/modules/boat/BoatGuidance/CMakeLists.txt

@@ -1,39 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2023-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(BoatGuidance
-	BoatGuidance.cpp
-	BoatGuidance.hpp
-)
-
-target_include_directories(BoatGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/modules/boat/BoatKinematics/BoatKinematics.cpp

@@ -1,91 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2023-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 "BoatKinematics.hpp"
-
-#include <mathlib/mathlib.h>
-
-using namespace matrix;
-using namespace time_literals;
-
-BoatKinematics::BoatKinematics(ModuleParams *parent) : ModuleParams(parent)
-{}
-
-void BoatKinematics::allocate()
-{
-	hrt_abstime now = hrt_absolute_time();
-
-	if (_boat_control_output_sub.updated()) {
-		_boat_control_output_sub.copy(&_boat_control_output);
-	}
-
-	const bool setpoint_timeout = (_boat_control_output.timestamp + 100_ms) < now;
-
-	Vector2f boat_output =
-		computeInverseKinematics(_boat_control_output.speed, _boat_control_output.yaw_rate);
-
-	if (!_armed || setpoint_timeout) {
-		boat_output = {}; // stop
-	}
-
-	boat_output = matrix::constrain(boat_output, -1.f, 1.f);
-
-	actuator_motors_s actuator_motors{};
-	actuator_motors.control[0] = boat_output(0);
-	actuator_motors.control[1] = boat_output(0);
-	actuator_motors.timestamp = now;
-	_actuator_motors_pub.publish(actuator_motors);
-
-	actuator_servos_s actuator_servos{};
-	actuator_servos.control[0] = boat_output(1);
-	actuator_servos.control[1] = boat_output(1);
-	actuator_servos.timestamp = now;
-	_actuator_servos_pub.publish(actuator_servos);
-}
-
-matrix::Vector2f BoatKinematics::computeInverseKinematics(float linear_velocity_x, float yaw_rate) const
-{
-	if (_max_speed < FLT_EPSILON) {
-		return Vector2f();
-	}
-
-	// Room for more advanced dynamics, if required
-
-	linear_velocity_x = math::constrain(linear_velocity_x, -_max_speed, _max_speed);
-	yaw_rate = math::constrain(yaw_rate, -_max_angular_velocity, _max_angular_velocity);
-
-	float throttle = linear_velocity_x / _max_speed;
-	float rudder_angle = yaw_rate / _max_angular_velocity;
-
-	return Vector2f(throttle, rudder_angle);
-}

src/modules/boat/BoatKinematics/BoatKinematics.hpp

@@ -1,107 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2023-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.
- *
- ****************************************************************************/
-
-#pragma once
-
-#include <matrix/matrix/math.hpp>
-#include <px4_platform_common/module_params.h>
-#include <uORB/PublicationMulti.hpp>
-#include <uORB/Subscription.hpp>
-#include <uORB/topics/actuator_motors.h>
-#include <uORB/topics/actuator_servos.h>
-#include <uORB/topics/boat_setpoint.h>
-
-
-/**
- * @brief Boat Drive Kinematics class for computing the kinematics of a boat drive robot.
- *
- * This class provides functions to set the wheel base and radius, and to compute the inverse kinematics
- * given linear velocity and yaw rate.
- */
-class BoatKinematics : public ModuleParams
-{
-public:
-	BoatKinematics(ModuleParams *parent);
-	~BoatKinematics() = default;
-
-	/**
-	 * @brief Sets the wheel base of the robot.
-	 *
-	 * @param wheel_base The distance between the centers of the wheels.
-	 */
-	void setWheelBase(const float wheel_base) { _wheel_base = wheel_base; };
-
-	/**
-	 * @brief Sets the maximum speed of the robot.
-	 *
-	 * @param max_speed The maximum speed of the robot.
-	 */
-	void setMaxSpeed(const float max_speed) { _max_speed = max_speed; };
-
-	/**
-	 * @brief Sets the maximum angular speed of the robot.
-	 *
-	 * @param max_angular_speed The maximum angular speed of the robot.
-	 */
-	void setMaxAngularVelocity(const float max_angular_velocity) { _max_angular_velocity = max_angular_velocity; };
-
-	void setArmed(const bool armed) { _armed = armed; };
-
-	void allocate();
-
-	/**
-	 * @brief Computes the inverse kinematics for boat drive.
-	 *
-	 * @param linear_velocity_x Linear velocity along the x-axis.
-	 * @param yaw_rate Yaw rate of the robot.
-	 * @return matrix::Vector2f Motor velocities for the right and left motors.
-	 */
-	matrix::Vector2f computeInverseKinematics(float linear_velocity_x, float yaw_rate) const;
-
-private:
-	uORB::Subscription _boat_control_output_sub{ORB_ID(boat_control_output)};
-	uORB::PublicationMulti<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
-	uORB::PublicationMulti<actuator_servos_s> _actuator_servos_pub{ORB_ID(actuator_servos)};
-
-	boat_setpoint_s _boat_control_output{};
-
-	bool _armed = false;
-
-	float _wheel_base{0.f};
-	float _max_speed{0.f};
-	float _max_angular_velocity{0.f};
-
-	DEFINE_PARAMETERS(
-		(ParamInt<px4::params::CA_R_REV>) _param_r_rev
-	)
-};

src/modules/boat/BoatKinematics/CMakeLists.txt

@@ -1,40 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2023-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(BoatKinematics
-	BoatKinematics.cpp
-)
-
-target_include_directories(BoatKinematics PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
-
-px4_add_functional_gtest(SRC BoatKinematicsTest.cpp LINKLIBS BoatKinematics)

src/modules/boat/CMakeLists.txt

@@ -1,53 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2023-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_subdirectory(BoatControl)
-add_subdirectory(BoatGuidance)
-add_subdirectory(BoatKinematics)
-
-px4_add_module(
-	MODULE modules__boat
-	MAIN boat
-	SRCS
-		Boat.cpp
-		Boat.hpp
-	DEPENDS
-		BoatControl
-		BoatGuidance
-		BoatKinematics
-		px4_work_queue
-		l1
-		# modules__control_allocator # for parameter CA_R_REV
-	MODULE_CONFIG
-		module.yaml
-)

src/modules/boat/Kconfig

@@ -1,6 +0,0 @@
-menuconfig MODULES_BOAT
-	bool "boat"
-	default n
-	depends on MODULES_CONTROL_ALLOCATOR
-	---help---
-		Enable support for control of boats

src/modules/boat/module.yaml

@@ -1,149 +0,0 @@
-module_name: Boat Drive
-
-parameters:
-  - group: Boat Drive
-    definitions:
-      BT_SPD_SCALE:
-        description:
-          short: Manual speed scale
-        type: float
-        min: 0
-        max: 1
-        increment: 0.01
-        decimal: 2
-        default: 1
-      BT_ANG_VEL_SCALE:
-        description:
-          short: Manual angular velocity scale
-        type: float
-        min: 0
-        max: 1
-        increment: 0.01
-        decimal: 2
-        default: 1
-      BT_SPD_CRUISE:
-        description:
-          short: Default cruise speed
-        type: float
-        unit: m/s
-        min: 0.0
-        max: 50.0
-        decimal: 1
-        default: 7.0
-      BT_SPD_MAX:
-        description:
-          short: Maximum speed
-        type: float
-        unit: m/s
-        min: 1.0
-        max: 50.0
-        decimal: 1
-        default: 17.0
-      BT_SPD_MIN:
-        description:
-          short: Minimum speed
-        type: float
-        unit: m/s
-        min: 0.0
-        max: 50.0
-        decimal: 1
-        default: 2.0
-      BT_SPD_P:
-        description:
-          short: Speed controller proportional gain
-        type: float
-        min: 0.0
-        max: 2.0
-        decimal: 2
-        default: 2.0
-      BT_SPD_I:
-        description:
-          short: Speed controller integral gain
-        type: float
-        min: 0.0
-        max: 20.0
-        decimal: 2
-        default: 1.0
-      BT_SPD_IMAX:
-        description:
-          short: Speed integral maximum value
-        type: float
-        unit: m/s
-        min: 0.0
-        max: 20.0
-        decimal: 1
-        default: 20.0
-      BT_SPD_OUTLIM:
-        description:
-          short: Speed PI controller output limit
-        type: float
-        unit: m/s
-        min: 0.0
-        max: 200.0
-        decimal: 1
-        default: 50.0
-      BT_ANG_MAX:
-        description:
-          short: Maximum angular velocity
-        type: float
-        unit: rad/s
-        min: 0.0
-        max: 20.0
-        decimal: 1
-        default: 1.0
-      BT_ANG_P:
-        description:
-          short: Angular velocity controller proportional gain
-        type: float
-        min: 0.0
-        max: 2.0
-        decimal: 2
-        default: 0.2
-      BT_ANG_I:
-        description:
-          short: Angular velocity controller integral gain
-        type: float
-        min: 0.0
-        max: 20.0
-        decimal: 2
-        default: 0.0
-      BT_ANG_IMAX:
-        description:
-          short: Angular velocity controller integral maximum value
-        type: float
-        unit: m/s
-        min: 0.0
-        max: 20.0
-        decimal: 1
-        default: 20.0
-      BT_ANG_OUTLIM:
-        description:
-          short: Angular velocity controller PI controller output limit
-        type: float
-        unit: m/s
-        min: 0.0
-        max: 200.0
-        decimal: 1
-        default: 50.0
-      BT_MAX_HERR:
-        description:
-          short: Max heading error for slowdown
-        type: float
-        min: 0.0
-        decimal: 2
-        default: 90.0
-      BT_MIN_HERR:
-        description:
-          short: Min heading error for full speed
-        type: float
-        min: 0.0
-        decimal: 2
-        default: 30.0
-      BT_LOOKAHEAD:
-        description:
-          short: Lookahead distance for heading error
-        type: float
-        unit: m
-        min: 0.0
-        decimal: 2
-        default: 10.0

src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.cpp

@@ -72,7 +72,7 @@ void DifferentialDriveGuidance::computeGuidance(float yaw, float angular_velocit
 
 	// Make rover stop when it arrives at the last waypoint instead of loitering and driving around weirdly.
 	if ((current_waypoint == next_waypoint) && distance_to_next_wp <= _param_nav_acc_rad.get()) {
-		_currentState = GuidanceState::kGoalReached;
+		_currentState = GuidanceState::GOAL_REACHED;
 	}
 
 	float desired_speed = 0.f;
@@ -82,12 +82,12 @@ void DifferentialDriveGuidance::computeGuidance(float yaw, float angular_velocit
 		desired_speed = 0.f;
 
 		if (fabsf(heading_error) < 0.05f) {
-			_currentState = GuidanceState::kDriving;
+			_currentState = GuidanceState::DRIVING;
 		}
 
 		break;
 
-	case GuidanceState::kDriving: {
+	case GuidanceState::DRIVING: {
 			const float max_velocity = math::trajectory::computeMaxSpeedFromDistance(_param_rdd_max_jerk.get(),
 						   _param_rdd_max_accel.get(), distance_to_next_wp, 0.0f);
 			_forwards_velocity_smoothing.updateDurations(max_velocity);
@@ -97,7 +97,7 @@ void DifferentialDriveGuidance::computeGuidance(float yaw, float angular_velocit
 			break;
 		}
 
-	case GuidanceState::kGoalReached:
+	case GuidanceState::GOAL_REACHED:
 		// temporary till I find a better way to stop the vehicle
 		desired_speed = 0.f;
 		heading_error = 0.f;

src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp

@@ -58,8 +58,8 @@
  */
 enum class GuidanceState {
 	TURNING, ///< The vehicle is currently turning.
-	kDriving, ///< The vehicle is currently driving straight.
-	kGoalReached ///< The vehicle has reached its goal.
+	DRIVING, ///< The vehicle is currently driving straight.
+	GOAL_REACHED ///< The vehicle has reached its goal.
 };
 
 /**

src/modules/ekf2/CMakeLists.txt

@@ -187,6 +187,7 @@ endif()
 
 if(CONFIG_EKF2_MAGNETOMETER)
 	list(APPEND EKF_SRCS
+		EKF/aid_sources/magnetometer/mag_3d_control.cpp
 		EKF/aid_sources/magnetometer/mag_control.cpp
 		EKF/aid_sources/magnetometer/mag_fusion.cpp
 	)

src/modules/ekf2/EKF/CMakeLists.txt

@@ -109,6 +109,7 @@ endif()
 
 if(CONFIG_EKF2_MAGNETOMETER)
 	list(APPEND EKF_SRCS
+		aid_sources/magnetometer/mag_3d_control.cpp
 		aid_sources/magnetometer/mag_control.cpp
 		aid_sources/magnetometer/mag_fusion.cpp
 	)

src/modules/ekf2/EKF/aid_sources/magnetometer/mag_3d_control.cpp

@@ -0,0 +1,225 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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 mag_3d_control.cpp
+ * Control functions for ekf mag 3D fusion
+ */
+
+#include "ekf.h"
+
+void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_starting_conditions_passing,
+			     estimator_aid_source3d_s &aid_src)
+{
+	static constexpr const char *AID_SRC_NAME = "mag";
+
+	resetEstimatorAidStatus(aid_src);
+
+	const bool wmm_updated = (_wmm_gps_time_last_set >= aid_src.time_last_fuse); // WMM update can occur on the last epoch, just after mag fusion
+
+	// determine if we should use mag fusion
+	bool continuing_conditions_passing = (_params.mag_fusion_type != MagFuseType::NONE)
+					     && _control_status.flags.tilt_align
+					     && (_control_status.flags.yaw_align || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align))
+					     && mag_sample.mag.longerThan(0.f)
+					     && mag_sample.mag.isAllFinite();
+
+	const bool starting_conditions_passing = common_starting_conditions_passing
+			&& continuing_conditions_passing;
+
+	// For the first few seconds after in-flight alignment we allow the magnetic field state estimates to stabilise
+	// before they are used to constrain heading drift
+	_control_status.flags.mag_3D = (_params.mag_fusion_type == MagFuseType::AUTO)
+				       && _control_status.flags.mag
+				       && _control_status.flags.mag_aligned_in_flight
+				       && (_control_status.flags.mag_heading_consistent || !_control_status.flags.gps)
+				       && !_control_status.flags.mag_fault
+				       && !_control_status.flags.ev_yaw
+				       && !_control_status.flags.gps_yaw;
+
+	const bool mag_consistent_or_no_gnss = _control_status.flags.mag_heading_consistent || !_control_status.flags.gps;
+
+	_control_status.flags.mag_hdg = ((_params.mag_fusion_type == MagFuseType::HEADING)
+					      || (_params.mag_fusion_type == MagFuseType::AUTO && !_control_status.flags.mag_3D))
+					     && _control_status.flags.tilt_align
+					     && ((_control_status.flags.yaw_align && mag_consistent_or_no_gnss)
+					         || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align))
+					     && !_control_status.flags.mag_fault
+					     && !_control_status.flags.mag_field_disturbed
+					     && !_control_status.flags.ev_yaw
+					     && !_control_status.flags.gps_yaw;
+
+	// TODO: allow clearing mag_fault if mag_3d is good?
+
+	if (_control_status.flags.mag_3D && !_control_status_prev.flags.mag_3D) {
+		ECL_INFO("starting mag 3D fusion");
+
+	} else if (!_control_status.flags.mag_3D && _control_status_prev.flags.mag_3D) {
+		ECL_INFO("stopping mag 3D fusion");
+	}
+
+	// if we are using 3-axis magnetometer fusion, but without external NE aiding,
+	// then the declination must be fused as an observation to prevent long term heading drift
+	// fusing declination when gps aiding is available is optional.
+	const bool not_using_ne_aiding = !_control_status.flags.gps && !_control_status.flags.aux_gpos;
+	_control_status.flags.mag_dec = (_control_status.flags.mag && (not_using_ne_aiding || !_control_status.flags.mag_aligned_in_flight));
+
+	if (_control_status.flags.mag) {
+		aid_src.timestamp_sample = mag_sample.time_us;
+
+		if (continuing_conditions_passing && _control_status.flags.yaw_align) {
+
+			if (mag_sample.reset || checkHaglYawResetReq() || wmm_updated) {
+				ECL_INFO("reset to %s", AID_SRC_NAME);
+				resetMagStates(_mag_lpf.getState(), _control_status.flags.mag_hdg || _control_status.flags.mag_3D);
+				aid_src.time_last_fuse = _time_delayed_us;
+
+			} else {
+				if (!_mag_decl_cov_reset) {
+					// After any magnetic field covariance reset event the earth field state
+					// covariances need to be corrected to incorporate knowledge of the declination
+					// before fusing magnetometer data to prevent rapid rotation of the earth field
+					// states for the first few observations.
+					fuseDeclination(0.02f);
+					_mag_decl_cov_reset = true;
+					fuseMag(mag_sample.mag, aid_src, false);
+
+				} else {
+					// The normal sequence is to fuse the magnetometer data first before fusing
+					// declination angle at a higher uncertainty to allow some learning of
+					// declination angle over time.
+					const bool update_all_states = _control_status.flags.mag_3D || _control_status.flags.mag_hdg;
+					const bool update_tilt = _control_status.flags.mag_3D;
+					fuseMag(mag_sample.mag, aid_src, update_all_states, update_tilt);
+
+					if (_control_status.flags.mag_dec) {
+						fuseDeclination(0.5f);
+					}
+				}
+			}
+
+			const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.reset_timeout_max);
+
+			if (is_fusion_failing) {
+				if (_nb_mag_3d_reset_available > 0) {
+					// Data seems good, attempt a reset (mag states only unless mag_3D currently active)
+					ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME);
+					resetMagStates(_mag_lpf.getState(), _control_status.flags.mag_hdg || _control_status.flags.mag_3D);
+					aid_src.time_last_fuse = _time_delayed_us;
+
+					if (_control_status.flags.in_air) {
+						_nb_mag_3d_reset_available--;
+					}
+
+				} else if (starting_conditions_passing) {
+					// Data seems good, but previous reset did not fix the issue
+					// something else must be wrong, declare the sensor faulty and stop the fusion
+					//_control_status.flags.mag_fault = true;
+					ECL_WARN("stopping %s fusion, starting conditions failing", AID_SRC_NAME);
+					stopMagFusion();
+
+				} else {
+					// A reset did not fix the issue but all the starting checks are not passing
+					// This could be a temporary issue, stop the fusion without declaring the sensor faulty
+					ECL_WARN("stopping %s, fusion failing", AID_SRC_NAME);
+					stopMagFusion();
+				}
+			}
+
+		} else {
+			// Stop fusion but do not declare it faulty
+			ECL_DEBUG("stopping %s fusion, continuing conditions no longer passing", AID_SRC_NAME);
+			stopMagFusion();
+		}
+
+	} else {
+		if (starting_conditions_passing) {
+
+			_control_status.flags.mag = true;
+
+			// activate fusion, reset mag states and initialize variance if first init or in flight reset
+			if (!_control_status.flags.yaw_align
+			    || wmm_updated
+			    || !_mag_decl_cov_reset
+			    || !_state.mag_I.longerThan(0.f)
+			    || (getStateVariance<State::mag_I>().min() < kMagVarianceMin)
+			    || (getStateVariance<State::mag_B>().min() < kMagVarianceMin)
+			   ) {
+				ECL_INFO("starting %s fusion, resetting states", AID_SRC_NAME);
+
+				bool reset_heading = !_control_status.flags.yaw_align;
+
+				resetMagStates(_mag_lpf.getState(), reset_heading);
+
+				if (reset_heading) {
+					_control_status.flags.yaw_align = true;
+				}
+
+			} else {
+				ECL_INFO("starting %s fusion", AID_SRC_NAME);
+				fuseMag(mag_sample.mag, aid_src, false);
+			}
+
+			aid_src.time_last_fuse = _time_delayed_us;
+
+			_nb_mag_3d_reset_available = 2;
+		}
+	}
+}
+
+void Ekf::stopMagFusion()
+{
+	if (_control_status.flags.mag) {
+		ECL_INFO("stopping mag fusion");
+
+		_control_status.flags.mag = false;
+		_control_status.flags.mag_dec = false;
+
+		if (_control_status.flags.mag_3D) {
+			ECL_INFO("stopping mag 3D fusion");
+			_control_status.flags.mag_3D = false;
+		}
+
+		if (_control_status.flags.mag_hdg) {
+			ECL_INFO("stopping mag heading fusion");
+			_control_status.flags.mag_hdg = false;
+			_fault_status.flags.bad_hdg = false;
+		}
+
+		_fault_status.flags.bad_mag_x = false;
+		_fault_status.flags.bad_mag_y = false;
+		_fault_status.flags.bad_mag_z = false;
+
+		_fault_status.flags.bad_mag_decl = false;
+	}
+}

src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp

@@ -39,13 +39,8 @@
 #include "ekf.h"
 #include <mathlib/mathlib.h>
 
-#include <ekf_derivation/generated/compute_mag_innov_innov_var_and_hx.h>
-
 void Ekf::controlMagFusion()
 {
-	static constexpr const char *AID_SRC_NAME = "mag";
-	estimator_aid_source3d_s &aid_src = _aid_src_mag;
-
 	// reset the flight alignment flag so that the mag fields will be
 	//  re-initialised next time we achieve flight altitude
 	if (!_control_status_prev.flags.in_air && _control_status.flags.in_air) {
@@ -71,7 +66,7 @@ void Ekf::controlMagFusion()
 	if (_mag_buffer && _mag_buffer->pop_first_older_than(_time_delayed_us, &mag_sample)) {
 
 		if (mag_sample.reset || (_mag_counter == 0)) {
-			// sensor or calibration has changed, reset low pass filter
+			// sensor or calibration has changed, reset low pass filter (reset handled by controlMag3DFusion/controlMagHeadingFusion)
 			_control_status.flags.mag_fault = false;
 
 			_state.mag_B.zero();
@@ -85,6 +80,13 @@ void Ekf::controlMagFusion()
 			_mag_counter++;
 		}
 
+		const bool starting_conditions_passing = (_params.mag_fusion_type != MagFuseType::NONE)
+				&& checkMagField(mag_sample.mag)
+				&& (_mag_counter > 5) // wait until we have more than a few samples through the filter
+				&& (_control_status.flags.yaw_align == _control_status_prev.flags.yaw_align) // no yaw alignment change this frame
+				&& (_state_reset_status.reset_count.quat == _state_reset_count_prev.quat) // don't allow starting on same frame as yaw reset
+				&& isNewestSampleRecent(_time_last_mag_buffer_push, MAG_MAX_INTERVAL);
+
 		// if enabled, use knowledge of theoretical magnetic field vector to calculate a synthetic magnetomter Z component value.
 		// this is useful if there is a lot of interference on the sensor measurement.
 		if (_params.synthesize_mag_z && (_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL)
@@ -101,202 +103,8 @@ void Ekf::controlMagFusion()
 			_control_status.flags.synthetic_mag_z = false;
 		}
 
-		// reset flags
-		_fault_status.flags.bad_mag_x = false;
-		_fault_status.flags.bad_mag_y = false;
-		_fault_status.flags.bad_mag_z = false;
-
-
-		resetEstimatorAidStatus(aid_src);
-		aid_src.timestamp_sample = mag_sample.time_us;
-
-		// XYZ Measurement uncertainty. Need to consider timing errors for fast rotations
-		const float R_MAG = math::max(sq(_params.mag_noise), sq(0.01f));
-
-		// calculate intermediate variables used for X axis innovation variance, observation Jacobians and Kalman gains
-		Vector3f mag_innov;
-		Vector3f innov_var;
-
-		// Observation jacobian and Kalman gain vectors
-		VectorState H;
-		sym::ComputeMagInnovInnovVarAndHx(_state.vector(), P, mag_sample.mag, R_MAG, FLT_EPSILON, &mag_innov, &innov_var, &H);
-
-		for (int i = 0; i < 3; i++) {
-			aid_src.observation[i] = mag_sample.mag(i);
-			aid_src.observation_variance[i] = R_MAG;
-			aid_src.innovation[i] = mag_innov(i);
-			aid_src.innovation_variance[i] = innov_var(i);
-		}
-
-		const float innov_gate = math::max(_params.mag_innov_gate, 1.f);
-		setEstimatorAidStatusTestRatio(aid_src, innov_gate);
-
-		// Perform an innovation consistency check and report the result
-		_innov_check_fail_status.flags.reject_mag_x = (aid_src.test_ratio[0] > 1.f);
-		_innov_check_fail_status.flags.reject_mag_y = (aid_src.test_ratio[1] > 1.f);
-		_innov_check_fail_status.flags.reject_mag_z = (aid_src.test_ratio[2] > 1.f);
-
-		// determine if we should use mag fusion
-		bool continuing_conditions_passing = (_params.mag_fusion_type != MagFuseType::NONE)
-						     && _control_status.flags.tilt_align
-						     && (_control_status.flags.yaw_align || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align))
-						     && mag_sample.mag.longerThan(0.f)
-						     && mag_sample.mag.isAllFinite();
-
-		const bool starting_conditions_passing = continuing_conditions_passing
-				&& checkMagField(mag_sample.mag)
-				&& (_mag_counter > 3) // wait until we have more than a few samples through the filter
-				&& (_control_status.flags.yaw_align == _control_status_prev.flags.yaw_align) // no yaw alignment change this frame
-				&& (_state_reset_status.reset_count.quat == _state_reset_count_prev.quat) // don't allow starting on same frame as yaw reset
-				&& isNewestSampleRecent(_time_last_mag_buffer_push, MAG_MAX_INTERVAL);
-
 		checkMagHeadingConsistency(mag_sample);
-
-		// WMM update can occur on the last epoch, just after mag fusion
-		const bool wmm_updated = (_wmm_gps_time_last_set >= aid_src.time_last_fuse);
-
-		{
-		const bool mag_consistent_or_no_gnss = _control_status.flags.mag_heading_consistent || !_control_status.flags.gps;
-		const bool common_conditions_passing = _control_status.flags.mag
-						       && ((_control_status.flags.yaw_align && mag_consistent_or_no_gnss)
-						           || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align))
-						       && !_control_status.flags.mag_fault
-						       && !_control_status.flags.mag_field_disturbed
-						       && !_control_status.flags.ev_yaw
-						       && !_control_status.flags.gps_yaw;
-
-		_control_status.flags.mag_3D = common_conditions_passing
-					       && (_params.mag_fusion_type == MagFuseType::AUTO)
-					       && _control_status.flags.mag_aligned_in_flight;
-
-		_control_status.flags.mag_hdg = common_conditions_passing
-						&& ((_params.mag_fusion_type == MagFuseType::HEADING)
-						    || (_params.mag_fusion_type == MagFuseType::AUTO && !_control_status.flags.mag_3D));
-		}
-
-		// TODO: allow clearing mag_fault if mag_3d is good?
-
-		if (_control_status.flags.mag_3D && !_control_status_prev.flags.mag_3D) {
-			ECL_INFO("starting mag 3D fusion");
-
-		} else if (!_control_status.flags.mag_3D && _control_status_prev.flags.mag_3D) {
-			ECL_INFO("stopping mag 3D fusion");
-		}
-
-		// if we are using 3-axis magnetometer fusion, but without external NE aiding,
-		// then the declination must be fused as an observation to prevent long term heading drift
-		// fusing declination when gps aiding is available is optional.
-		const bool not_using_ne_aiding = !_control_status.flags.gps && !_control_status.flags.aux_gpos;
-		_control_status.flags.mag_dec = _control_status.flags.mag
-						&& (not_using_ne_aiding || !_control_status.flags.mag_aligned_in_flight);
-
-		if (_control_status.flags.mag) {
-
-			if (continuing_conditions_passing && _control_status.flags.yaw_align) {
-
-				if (mag_sample.reset || checkHaglYawResetReq() || wmm_updated) {
-					ECL_INFO("reset to %s", AID_SRC_NAME);
-					resetMagStates(_mag_lpf.getState(), _control_status.flags.mag_hdg || _control_status.flags.mag_3D);
-					aid_src.time_last_fuse = _time_delayed_us;
-
-				} else {
-					if (!_mag_decl_cov_reset) {
-						// After any magnetic field covariance reset event the earth field state
-						// covariances need to be corrected to incorporate knowledge of the declination
-						// before fusing magnetometer data to prevent rapid rotation of the earth field
-						// states for the first few observations.
-						fuseDeclination(0.02f);
-						_mag_decl_cov_reset = true;
-						fuseMag(mag_sample.mag, R_MAG, H, aid_src);
-
-					} else {
-						// The normal sequence is to fuse the magnetometer data first before fusing
-						// declination angle at a higher uncertainty to allow some learning of
-						// declination angle over time.
-						const bool update_all_states = _control_status.flags.mag_3D || _control_status.flags.mag_hdg;
-						const bool update_tilt = _control_status.flags.mag_3D;
-						fuseMag(mag_sample.mag, R_MAG, H, aid_src, update_all_states, update_tilt);
-
-						// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
-						if (update_all_states && update_tilt) {
-							_fault_status.flags.bad_mag_x = (aid_src.innovation_variance[0] < aid_src.observation_variance[0]);
-							_fault_status.flags.bad_mag_y = (aid_src.innovation_variance[1] < aid_src.observation_variance[1]);
-							_fault_status.flags.bad_mag_z = (aid_src.innovation_variance[2] < aid_src.observation_variance[2]);
-						}
-
-						if (_control_status.flags.mag_dec) {
-							fuseDeclination(0.5f);
-						}
-					}
-				}
-
-				const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.reset_timeout_max);
-
-				if (is_fusion_failing) {
-					if (_nb_mag_3d_reset_available > 0) {
-						// Data seems good, attempt a reset (mag states only unless mag_3D currently active)
-						ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME);
-						resetMagStates(_mag_lpf.getState(), _control_status.flags.mag_hdg || _control_status.flags.mag_3D);
-						aid_src.time_last_fuse = _time_delayed_us;
-
-						if (_control_status.flags.in_air) {
-							_nb_mag_3d_reset_available--;
-						}
-
-					} else if (starting_conditions_passing) {
-						// Data seems good, but previous reset did not fix the issue
-						// something else must be wrong, declare the sensor faulty and stop the fusion
-						//_control_status.flags.mag_fault = true;
-						ECL_WARN("stopping %s fusion, starting conditions failing", AID_SRC_NAME);
-						stopMagFusion();
-
-					} else {
-						// A reset did not fix the issue but all the starting checks are not passing
-						// This could be a temporary issue, stop the fusion without declaring the sensor faulty
-						ECL_WARN("stopping %s, fusion failing", AID_SRC_NAME);
-						stopMagFusion();
-					}
-				}
-
-			} else {
-				// Stop fusion but do not declare it faulty
-				ECL_DEBUG("stopping %s fusion, continuing conditions no longer passing", AID_SRC_NAME);
-				stopMagFusion();
-			}
-
-		} else {
-			if (starting_conditions_passing) {
-
-				_control_status.flags.mag = true;
-
-				// activate fusion, reset mag states and initialize variance if first init or in flight reset
-				if (!_control_status.flags.yaw_align
-				    || wmm_updated
-				    || !_mag_decl_cov_reset
-				    || !_state.mag_I.longerThan(0.f)
-				    || (getStateVariance<State::mag_I>().min() < kMagVarianceMin)
-				    || (getStateVariance<State::mag_B>().min() < kMagVarianceMin)
-				   ) {
-					ECL_INFO("starting %s fusion, resetting states", AID_SRC_NAME);
-
-					bool reset_heading = !_control_status.flags.yaw_align;
-
-					resetMagStates(_mag_lpf.getState(), reset_heading);
-					aid_src.time_last_fuse = _time_delayed_us;
-					_nb_mag_3d_reset_available = 2;
-
-					if (reset_heading) {
-						_control_status.flags.yaw_align = true;
-					}
-
-				} else {
-					if (fuseMag(mag_sample.mag, R_MAG, H, aid_src)) {
-						ECL_INFO("starting %s fusion", AID_SRC_NAME);
-						_nb_mag_3d_reset_available = 2;
-					}
-				}
-			}
-		}
+		controlMag3DFusion(mag_sample, starting_conditions_passing, _aid_src_mag);
 
 	} else if (!isNewestSampleRecent(_time_last_mag_buffer_push, 2 * MAG_MAX_INTERVAL)) {
 		// No data anymore. Stop until it comes back.
@@ -304,37 +112,9 @@ void Ekf::controlMagFusion()
 	}
 }
 
-void Ekf::stopMagFusion()
-{
-	if (_control_status.flags.mag) {
-		ECL_INFO("stopping mag fusion");
-
-		_control_status.flags.mag = false;
-		_control_status.flags.mag_dec = false;
-
-		if (_control_status.flags.mag_3D) {
-			ECL_INFO("stopping mag 3D fusion");
-			_control_status.flags.mag_3D = false;
-		}
-
-		if (_control_status.flags.mag_hdg) {
-			ECL_INFO("stopping mag heading fusion");
-			_control_status.flags.mag_hdg = false;
-			_fault_status.flags.bad_hdg = false;
-		}
-
-		_fault_status.flags.bad_mag_x = false;
-		_fault_status.flags.bad_mag_y = false;
-		_fault_status.flags.bad_mag_z = false;
-
-		_fault_status.flags.bad_mag_decl = false;
-	}
-}
-
 bool Ekf::checkHaglYawResetReq() const
 {
 #if defined(CONFIG_EKF2_TERRAIN)
-
 	// We need to reset the yaw angle after climbing away from the ground to enable
 	// recovery from ground level magnetic interference.
 	if (_control_status.flags.in_air && _control_status.flags.yaw_align && !_control_status.flags.mag_aligned_in_flight) {
@@ -344,7 +124,6 @@ bool Ekf::checkHaglYawResetReq() const
 		const bool above_mag_anomalies = (getTerrainVPos() - _state.pos(2)) > mag_anomalies_max_hagl;
 		return above_mag_anomalies;
 	}
-
 #endif // CONFIG_EKF2_TERRAIN
 
 	return false;
@@ -368,7 +147,6 @@ void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading)
 
 		// mag_B: reset
 #if defined(CONFIG_EKF2_GNSS)
-
 		if (isYawEmergencyEstimateAvailable()) {
 
 			const Dcmf R_to_earth = updateYawInRotMat(_yawEstimator.getYaw(), _R_to_earth);
@@ -381,7 +159,6 @@ void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading)
 
 		} else if (!reset_heading && _control_status.flags.yaw_align) {
 #else
-
 		if (!reset_heading && _control_status.flags.yaw_align) {
 #endif
 			// mag_B: reset using WMM

src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp

@@ -43,6 +43,7 @@
 
 #include "ekf.h"
 
+#include <ekf_derivation/generated/compute_mag_innov_innov_var_and_hx.h>
 #include <ekf_derivation/generated/compute_mag_y_innov_var_and_h.h>
 #include <ekf_derivation/generated/compute_mag_z_innov_var_and_h.h>
 
@@ -50,14 +51,71 @@
 
 #include <mathlib/mathlib.h>
 
-bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estimator_aid_source3d_s &aid_src, bool update_all_states, bool update_tilt)
+bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states, bool update_tilt)
 {
-	// if any axis failed, abort the mag fusion
-	if (aid_src.innovation_rejected) {
+	// XYZ Measurement uncertainty. Need to consider timing errors for fast rotations
+	const float R_MAG = math::max(sq(_params.mag_noise), sq(0.01f));
+
+	// calculate intermediate variables used for X axis innovation variance, observation Jacobians and Kalman gains
+	Vector3f mag_innov;
+	Vector3f innov_var;
+
+	// Observation jacobian and Kalman gain vectors
+	VectorState H;
+	const auto state_vector = _state.vector();
+	sym::ComputeMagInnovInnovVarAndHx(state_vector, P, mag, R_MAG, FLT_EPSILON, &mag_innov, &innov_var, &H);
+
+	// do not use the synthesized measurement for the magnetomter Z component for 3D fusion
+	if (_control_status.flags.synthetic_mag_z) {
+		mag_innov(2) = 0.0f;
+	}
+
+	for (int i = 0; i < 3; i++) {
+		aid_src_mag.observation[i] = mag(i) - _state.mag_B(i);
+		aid_src_mag.observation_variance[i] = R_MAG;
+		aid_src_mag.innovation[i] = mag_innov(i);
+		aid_src_mag.innovation_variance[i] = innov_var(i);
+	}
+
+	const float innov_gate = math::max(_params.mag_innov_gate, 1.f);
+	setEstimatorAidStatusTestRatio(aid_src_mag, innov_gate);
+
+	if (update_all_states) {
+		_fault_status.flags.bad_mag_x = (aid_src_mag.innovation_variance[0] < aid_src_mag.observation_variance[0]);
+		_fault_status.flags.bad_mag_y = (aid_src_mag.innovation_variance[1] < aid_src_mag.observation_variance[1]);
+		_fault_status.flags.bad_mag_z = (aid_src_mag.innovation_variance[2] < aid_src_mag.observation_variance[2]);
+
+	} else {
+		_fault_status.flags.bad_mag_x = false;
+		_fault_status.flags.bad_mag_y = false;
+		_fault_status.flags.bad_mag_z = false;
+	}
+
+	// Perform an innovation consistency check and report the result
+	_innov_check_fail_status.flags.reject_mag_x = (aid_src_mag.test_ratio[0] > 1.f);
+	_innov_check_fail_status.flags.reject_mag_y = (aid_src_mag.test_ratio[1] > 1.f);
+	_innov_check_fail_status.flags.reject_mag_z = (aid_src_mag.test_ratio[2] > 1.f);
+
+	const char *numerical_error_covariance_reset_string = "numerical error - covariance reset";
+
+	// check innovation variances for being badly conditioned
+	if (innov_var.min() < R_MAG) {
+		// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
+		// we need to re-initialise covariances and abort this fusion step
+		if (update_all_states) {
+			resetQuatCov(_params.mag_heading_noise);
+		}
+
+		resetMagCov();
+
+		ECL_ERR("mag %s", numerical_error_covariance_reset_string);
 		return false;
 	}
 
-	const auto state_vector = _state.vector();
+	// if any axis fails, abort the mag fusion
+	if (aid_src_mag.innovation_rejected) {
+		return false;
+	}
 
 	bool fused[3] {false, false, false};
 
@@ -65,14 +123,29 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima
 	for (uint8_t index = 0; index <= 2; index++) {
 		// Calculate Kalman gains and observation jacobians
 		if (index == 0) {
-			// everything was already computed
+			// everything was already computed above
 
 		} else if (index == 1) {
 			// recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes)
-			sym::ComputeMagYInnovVarAndH(state_vector, P, R_MAG, FLT_EPSILON, &aid_src.innovation_variance[index], &H);
+			sym::ComputeMagYInnovVarAndH(state_vector, P, R_MAG, FLT_EPSILON, &aid_src_mag.innovation_variance[index], &H);
 
 			// recalculate innovation using the updated state
-			aid_src.innovation[index] = _state.quat_nominal.rotateVectorInverse(_state.mag_I)(index) + _state.mag_B(index) - mag(index);
+			aid_src_mag.innovation[index] = _state.quat_nominal.rotateVectorInverse(_state.mag_I)(index) + _state.mag_B(index) - mag(index);
+
+			if (aid_src_mag.innovation_variance[index] < R_MAG) {
+				// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
+				_fault_status.flags.bad_mag_y = true;
+
+				// we need to re-initialise covariances and abort this fusion step
+				if (update_all_states) {
+					resetQuatCov(_params.mag_heading_noise);
+				}
+
+				resetMagCov();
+
+				ECL_ERR("magY %s", numerical_error_covariance_reset_string);
+				return false;
+			}
 
 		} else if (index == 2) {
 			// we do not fuse synthesized magnetomter measurements when doing 3D fusion
@@ -82,26 +155,28 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima
 			}
 
 			// recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes)
-			sym::ComputeMagZInnovVarAndH(state_vector, P, R_MAG, FLT_EPSILON, &aid_src.innovation_variance[index], &H);
+			sym::ComputeMagZInnovVarAndH(state_vector, P, R_MAG, FLT_EPSILON, &aid_src_mag.innovation_variance[index], &H);
 
 			// recalculate innovation using the updated state
-			aid_src.innovation[index] = _state.quat_nominal.rotateVectorInverse(_state.mag_I)(index) + _state.mag_B(index) - mag(index);
-		}
+			aid_src_mag.innovation[index] = _state.quat_nominal.rotateVectorInverse(_state.mag_I)(index) + _state.mag_B(index) - mag(index);
 
-		if (aid_src.innovation_variance[index] < R_MAG) {
-			ECL_ERR("mag numerical error covariance reset");
+			if (aid_src_mag.innovation_variance[index] < R_MAG) {
+				// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
+				_fault_status.flags.bad_mag_z = true;
 
-			// we need to re-initialise covariances and abort this fusion step
-			if (update_all_states) {
-				resetQuatCov(_params.mag_heading_noise);
+				// we need to re-initialise covariances and abort this fusion step
+				if (update_all_states) {
+					resetQuatCov(_params.mag_heading_noise);
+				}
+
+				resetMagCov();
+
+				ECL_ERR("magZ %s", numerical_error_covariance_reset_string);
+				return false;
 			}
-
-			resetMagCov();
-
-			return false;
 		}
 
-		VectorState Kfusion = P * H / aid_src.innovation_variance[index];
+		VectorState Kfusion = P * H / aid_src_mag.innovation_variance[index];
 
 		if (!update_all_states) {
 			// zero non-mag Kalman gains if not updating all states
@@ -117,13 +192,16 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima
 		}
 
 		if (!update_tilt) {
-			Kfusion(State::quat_nominal.idx + 0) = 0.f;
+			Kfusion(State::quat_nominal.idx) = 0.f;
 			Kfusion(State::quat_nominal.idx + 1) = 0.f;
 		}
 
-		if (measurementUpdate(Kfusion, H, aid_src.observation_variance[index], aid_src.innovation[index])) {
+		if (measurementUpdate(Kfusion, H, aid_src_mag.observation_variance[index], aid_src_mag.innovation[index])) {
 			fused[index] = true;
 			limitDeclination();
+
+		} else {
+			fused[index] = false;
 		}
 	}
 
@@ -134,8 +212,8 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima
 	}
 
 	if (fused[0] && fused[1] && fused[2]) {
-		aid_src.fused = true;
-		aid_src.time_last_fuse = _time_delayed_us;
+		aid_src_mag.fused = true;
+		aid_src_mag.time_last_fuse = _time_delayed_us;
 
 		if (update_all_states) {
 			_time_last_heading_fuse = _time_delayed_us;
@@ -144,6 +222,7 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima
 		return true;
 	}
 
+	aid_src_mag.fused = false;
 	return false;
 }
 

src/modules/ekf2/EKF/ekf.h

@@ -758,7 +758,7 @@ private:
 
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 	// ekf sequential fusion of magnetometer measurements
-	bool fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estimator_aid_source3d_s &aid_src, bool update_all_states = false, bool update_tilt = false);
+	bool fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states = true, bool update_tilt = true);
 
 	// fuse magnetometer declination measurement
 	// argument passed in is the declination uncertainty in radians
@@ -1025,6 +1025,7 @@ private:
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 	// control fusion of magnetometer observations
 	void controlMagFusion();
+	void controlMag3DFusion(const magSample &mag_sample, const bool common_starting_conditions_passing, estimator_aid_source3d_s &aid_src);
 
 	bool checkHaglYawResetReq() const;
 

src/modules/ekf2/EKF/python/ekf_derivation/generated/predict_covariance.h

@@ -236,16 +236,57 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   // Output terms (1)
   matrix::Matrix<Scalar, 23, 23> _res;
 
-  _res.setZero();
-
   _res(0, 0) = std::pow(_tmp15, Scalar(2)) * gyro_var + _tmp15 * _tmp27 + _tmp18 * _tmp26 +
                std::pow(_tmp23, Scalar(2)) * gyro_var + _tmp23 * _tmp25 + _tmp24 * _tmp6 +
                std::pow(_tmp6, Scalar(2)) * gyro_var;
+  _res(1, 0) = 0;
+  _res(2, 0) = 0;
+  _res(3, 0) = 0;
+  _res(4, 0) = 0;
+  _res(5, 0) = 0;
+  _res(6, 0) = 0;
+  _res(7, 0) = 0;
+  _res(8, 0) = 0;
+  _res(9, 0) = 0;
+  _res(10, 0) = 0;
+  _res(11, 0) = 0;
+  _res(12, 0) = 0;
+  _res(13, 0) = 0;
+  _res(14, 0) = 0;
+  _res(15, 0) = 0;
+  _res(16, 0) = 0;
+  _res(17, 0) = 0;
+  _res(18, 0) = 0;
+  _res(19, 0) = 0;
+  _res(20, 0) = 0;
+  _res(21, 0) = 0;
+  _res(22, 0) = 0;
   _res(0, 1) = _tmp15 * _tmp38 + _tmp18 * _tmp35 + _tmp24 * _tmp34 + _tmp25 * _tmp29 +
                _tmp27 * _tmp36 + _tmp29 * _tmp37 + _tmp34 * _tmp39;
   _res(1, 1) = _tmp18 * _tmp43 + std::pow(_tmp29, Scalar(2)) * gyro_var + _tmp29 * _tmp40 +
                std::pow(_tmp34, Scalar(2)) * gyro_var + _tmp34 * _tmp41 +
                std::pow(_tmp36, Scalar(2)) * gyro_var + _tmp36 * _tmp42;
+  _res(2, 1) = 0;
+  _res(3, 1) = 0;
+  _res(4, 1) = 0;
+  _res(5, 1) = 0;
+  _res(6, 1) = 0;
+  _res(7, 1) = 0;
+  _res(8, 1) = 0;
+  _res(9, 1) = 0;
+  _res(10, 1) = 0;
+  _res(11, 1) = 0;
+  _res(12, 1) = 0;
+  _res(13, 1) = 0;
+  _res(14, 1) = 0;
+  _res(15, 1) = 0;
+  _res(16, 1) = 0;
+  _res(17, 1) = 0;
+  _res(18, 1) = 0;
+  _res(19, 1) = 0;
+  _res(20, 1) = 0;
+  _res(21, 1) = 0;
+  _res(22, 1) = 0;
   _res(0, 2) = _tmp15 * _tmp47 * gyro_var + _tmp18 * _tmp46 + _tmp24 * _tmp44 + _tmp25 * _tmp45 +
                _tmp27 * _tmp47 + _tmp37 * _tmp45 + _tmp39 * _tmp44;
   _res(1, 2) = _tmp18 * _tmp48 + _tmp29 * _tmp45 * gyro_var + _tmp34 * _tmp44 * gyro_var +
@@ -253,6 +294,26 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(2, 2) = _tmp18 * _tmp51 + std::pow(_tmp44, Scalar(2)) * gyro_var + _tmp44 * _tmp50 +
                std::pow(_tmp45, Scalar(2)) * gyro_var + _tmp45 * _tmp49 +
                std::pow(_tmp47, Scalar(2)) * gyro_var + _tmp47 * _tmp52;
+  _res(3, 2) = 0;
+  _res(4, 2) = 0;
+  _res(5, 2) = 0;
+  _res(6, 2) = 0;
+  _res(7, 2) = 0;
+  _res(8, 2) = 0;
+  _res(9, 2) = 0;
+  _res(10, 2) = 0;
+  _res(11, 2) = 0;
+  _res(12, 2) = 0;
+  _res(13, 2) = 0;
+  _res(14, 2) = 0;
+  _res(15, 2) = 0;
+  _res(16, 2) = 0;
+  _res(17, 2) = 0;
+  _res(18, 2) = 0;
+  _res(19, 2) = 0;
+  _res(20, 2) = 0;
+  _res(21, 2) = 0;
+  _res(22, 2) = 0;
   _res(0, 3) = _tmp35 * _tmp75 + _tmp46 * _tmp79 - _tmp53 * _tmp57 - _tmp58 * _tmp61 -
                _tmp62 * _tmp64 + _tmp80;
   _res(1, 3) = _tmp43 * _tmp75 + _tmp48 * _tmp79 - _tmp57 * _tmp81 - _tmp61 * _tmp82 -
@@ -263,6 +324,25 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
                std::pow(_tmp60, Scalar(2)) * _tmp97 - _tmp61 * _tmp93 +
                std::pow(_tmp63, Scalar(2)) * _tmp98 - _tmp64 * _tmp94 + _tmp75 * _tmp91 +
                _tmp79 * _tmp92 + _tmp99;
+  _res(4, 3) = 0;
+  _res(5, 3) = 0;
+  _res(6, 3) = 0;
+  _res(7, 3) = 0;
+  _res(8, 3) = 0;
+  _res(9, 3) = 0;
+  _res(10, 3) = 0;
+  _res(11, 3) = 0;
+  _res(12, 3) = 0;
+  _res(13, 3) = 0;
+  _res(14, 3) = 0;
+  _res(15, 3) = 0;
+  _res(16, 3) = 0;
+  _res(17, 3) = 0;
+  _res(18, 3) = 0;
+  _res(19, 3) = 0;
+  _res(20, 3) = 0;
+  _res(21, 3) = 0;
+  _res(22, 3) = 0;
   _res(0, 4) = -_tmp102 * _tmp58 + _tmp105 * _tmp46 - _tmp108 * _tmp62 - _tmp110 * _tmp53 +
                _tmp113 * _tmp114 + _tmp115;
   _res(1, 4) = -_tmp102 * _tmp82 + _tmp105 * _tmp48 - _tmp108 * _tmp83 - _tmp110 * _tmp81 +
@@ -278,6 +358,24 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
                std::pow(_tmp107, Scalar(2)) * _tmp98 - _tmp108 * _tmp128 +
                std::pow(_tmp109, Scalar(2)) * _tmp96 - _tmp110 * _tmp129 + _tmp117 * _tmp126 +
                _tmp130;
+  _res(5, 4) = 0;
+  _res(6, 4) = 0;
+  _res(7, 4) = 0;
+  _res(8, 4) = 0;
+  _res(9, 4) = 0;
+  _res(10, 4) = 0;
+  _res(11, 4) = 0;
+  _res(12, 4) = 0;
+  _res(13, 4) = 0;
+  _res(14, 4) = 0;
+  _res(15, 4) = 0;
+  _res(16, 4) = 0;
+  _res(17, 4) = 0;
+  _res(18, 4) = 0;
+  _res(19, 4) = 0;
+  _res(20, 4) = 0;
+  _res(21, 4) = 0;
+  _res(22, 4) = 0;
   _res(0, 5) = _tmp114 * _tmp136 - _tmp132 * _tmp62 + _tmp133 * _tmp35 - _tmp134 * _tmp58 -
                _tmp135 * _tmp53 + _tmp137;
   _res(1, 5) = _tmp116 * _tmp138 - _tmp132 * _tmp83 + _tmp133 * _tmp43 - _tmp134 * _tmp82 -
@@ -299,6 +397,23 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
                           P(13, 0) * _tmp134 - P(14, 0) * _tmp132 + P(5, 0)) +
                _tmp146 + std::pow(_tmp66, Scalar(2)) * _tmp96 +
                std::pow(_tmp73, Scalar(2)) * _tmp97;
+  _res(6, 5) = 0;
+  _res(7, 5) = 0;
+  _res(8, 5) = 0;
+  _res(9, 5) = 0;
+  _res(10, 5) = 0;
+  _res(11, 5) = 0;
+  _res(12, 5) = 0;
+  _res(13, 5) = 0;
+  _res(14, 5) = 0;
+  _res(15, 5) = 0;
+  _res(16, 5) = 0;
+  _res(17, 5) = 0;
+  _res(18, 5) = 0;
+  _res(19, 5) = 0;
+  _res(20, 5) = 0;
+  _res(21, 5) = 0;
+  _res(22, 5) = 0;
   _res(0, 6) =
       P(0, 6) * _tmp18 + P(10, 6) * _tmp23 + P(11, 6) * _tmp6 + P(9, 6) * _tmp15 + _tmp80 * dt;
   _res(1, 6) =
@@ -316,6 +431,22 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
                dt * (P(0, 3) * _tmp138 + P(1, 3) * _tmp133 - P(12, 3) * _tmp135 -
                      P(13, 3) * _tmp134 - P(14, 3) * _tmp132 + P(5, 3));
   _res(6, 6) = P(3, 6) * dt + P(6, 6) + dt * (P(3, 3) * dt + P(6, 3));
+  _res(7, 6) = 0;
+  _res(8, 6) = 0;
+  _res(9, 6) = 0;
+  _res(10, 6) = 0;
+  _res(11, 6) = 0;
+  _res(12, 6) = 0;
+  _res(13, 6) = 0;
+  _res(14, 6) = 0;
+  _res(15, 6) = 0;
+  _res(16, 6) = 0;
+  _res(17, 6) = 0;
+  _res(18, 6) = 0;
+  _res(19, 6) = 0;
+  _res(20, 6) = 0;
+  _res(21, 6) = 0;
+  _res(22, 6) = 0;
   _res(0, 7) =
       P(0, 7) * _tmp18 + P(10, 7) * _tmp23 + P(11, 7) * _tmp6 + P(9, 7) * _tmp15 + _tmp115 * dt;
   _res(1, 7) =
@@ -332,6 +463,21 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
                      P(13, 4) * _tmp134 - P(14, 4) * _tmp132 + P(5, 4));
   _res(6, 7) = P(3, 7) * dt + P(6, 7) + dt * (P(3, 4) * dt + P(6, 4));
   _res(7, 7) = P(4, 7) * dt + P(7, 7) + dt * (P(4, 4) * dt + P(7, 4));
+  _res(8, 7) = 0;
+  _res(9, 7) = 0;
+  _res(10, 7) = 0;
+  _res(11, 7) = 0;
+  _res(12, 7) = 0;
+  _res(13, 7) = 0;
+  _res(14, 7) = 0;
+  _res(15, 7) = 0;
+  _res(16, 7) = 0;
+  _res(17, 7) = 0;
+  _res(18, 7) = 0;
+  _res(19, 7) = 0;
+  _res(20, 7) = 0;
+  _res(21, 7) = 0;
+  _res(22, 7) = 0;
   _res(0, 8) =
       P(0, 8) * _tmp18 + P(10, 8) * _tmp23 + P(11, 8) * _tmp6 + P(9, 8) * _tmp15 + _tmp137 * dt;
   _res(1, 8) =
@@ -347,6 +493,20 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(6, 8) = P(3, 8) * dt + P(6, 8) + dt * (P(3, 5) * dt + P(6, 5));
   _res(7, 8) = P(4, 8) * dt + P(7, 8) + dt * (P(4, 5) * dt + P(7, 5));
   _res(8, 8) = P(5, 8) * dt + P(8, 8) + dt * (P(5, 5) * dt + P(8, 5));
+  _res(9, 8) = 0;
+  _res(10, 8) = 0;
+  _res(11, 8) = 0;
+  _res(12, 8) = 0;
+  _res(13, 8) = 0;
+  _res(14, 8) = 0;
+  _res(15, 8) = 0;
+  _res(16, 8) = 0;
+  _res(17, 8) = 0;
+  _res(18, 8) = 0;
+  _res(19, 8) = 0;
+  _res(20, 8) = 0;
+  _res(21, 8) = 0;
+  _res(22, 8) = 0;
   _res(0, 9) = _tmp27;
   _res(1, 9) = _tmp42;
   _res(2, 9) = _tmp52;
@@ -360,6 +520,19 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(7, 9) = P(4, 9) * dt + P(7, 9);
   _res(8, 9) = P(5, 9) * dt + P(8, 9);
   _res(9, 9) = P(9, 9);
+  _res(10, 9) = 0;
+  _res(11, 9) = 0;
+  _res(12, 9) = 0;
+  _res(13, 9) = 0;
+  _res(14, 9) = 0;
+  _res(15, 9) = 0;
+  _res(16, 9) = 0;
+  _res(17, 9) = 0;
+  _res(18, 9) = 0;
+  _res(19, 9) = 0;
+  _res(20, 9) = 0;
+  _res(21, 9) = 0;
+  _res(22, 9) = 0;
   _res(0, 10) = _tmp25;
   _res(1, 10) = _tmp40;
   _res(2, 10) = _tmp49;
@@ -374,6 +547,18 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(8, 10) = P(5, 10) * dt + P(8, 10);
   _res(9, 10) = P(9, 10);
   _res(10, 10) = P(10, 10);
+  _res(11, 10) = 0;
+  _res(12, 10) = 0;
+  _res(13, 10) = 0;
+  _res(14, 10) = 0;
+  _res(15, 10) = 0;
+  _res(16, 10) = 0;
+  _res(17, 10) = 0;
+  _res(18, 10) = 0;
+  _res(19, 10) = 0;
+  _res(20, 10) = 0;
+  _res(21, 10) = 0;
+  _res(22, 10) = 0;
   _res(0, 11) = _tmp24;
   _res(1, 11) = _tmp41;
   _res(2, 11) = _tmp50;
@@ -389,6 +574,17 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(9, 11) = P(9, 11);
   _res(10, 11) = P(10, 11);
   _res(11, 11) = P(11, 11);
+  _res(12, 11) = 0;
+  _res(13, 11) = 0;
+  _res(14, 11) = 0;
+  _res(15, 11) = 0;
+  _res(16, 11) = 0;
+  _res(17, 11) = 0;
+  _res(18, 11) = 0;
+  _res(19, 11) = 0;
+  _res(20, 11) = 0;
+  _res(21, 11) = 0;
+  _res(22, 11) = 0;
   _res(0, 12) = _tmp53;
   _res(1, 12) = _tmp81;
   _res(2, 12) = _tmp85;
@@ -402,6 +598,16 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(10, 12) = P(10, 12);
   _res(11, 12) = P(11, 12);
   _res(12, 12) = P(12, 12);
+  _res(13, 12) = 0;
+  _res(14, 12) = 0;
+  _res(15, 12) = 0;
+  _res(16, 12) = 0;
+  _res(17, 12) = 0;
+  _res(18, 12) = 0;
+  _res(19, 12) = 0;
+  _res(20, 12) = 0;
+  _res(21, 12) = 0;
+  _res(22, 12) = 0;
   _res(0, 13) = _tmp58;
   _res(1, 13) = _tmp82;
   _res(2, 13) = _tmp87;
@@ -416,6 +622,15 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(11, 13) = P(11, 13);
   _res(12, 13) = P(12, 13);
   _res(13, 13) = P(13, 13);
+  _res(14, 13) = 0;
+  _res(15, 13) = 0;
+  _res(16, 13) = 0;
+  _res(17, 13) = 0;
+  _res(18, 13) = 0;
+  _res(19, 13) = 0;
+  _res(20, 13) = 0;
+  _res(21, 13) = 0;
+  _res(22, 13) = 0;
   _res(0, 14) = _tmp62;
   _res(1, 14) = _tmp83;
   _res(2, 14) = _tmp88;
@@ -431,6 +646,14 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(12, 14) = P(12, 14);
   _res(13, 14) = P(13, 14);
   _res(14, 14) = P(14, 14);
+  _res(15, 14) = 0;
+  _res(16, 14) = 0;
+  _res(17, 14) = 0;
+  _res(18, 14) = 0;
+  _res(19, 14) = 0;
+  _res(20, 14) = 0;
+  _res(21, 14) = 0;
+  _res(22, 14) = 0;
   _res(0, 15) = P(0, 15) * _tmp18 + P(10, 15) * _tmp23 + P(11, 15) * _tmp6 + P(9, 15) * _tmp15;
   _res(1, 15) = P(1, 15) * _tmp18 + P(10, 15) * _tmp29 + P(11, 15) * _tmp34 + P(9, 15) * _tmp36;
   _res(2, 15) = P(10, 15) * _tmp45 + P(11, 15) * _tmp44 + P(2, 15) * _tmp18 + P(9, 15) * _tmp47;
@@ -450,6 +673,13 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(13, 15) = P(13, 15);
   _res(14, 15) = P(14, 15);
   _res(15, 15) = P(15, 15);
+  _res(16, 15) = 0;
+  _res(17, 15) = 0;
+  _res(18, 15) = 0;
+  _res(19, 15) = 0;
+  _res(20, 15) = 0;
+  _res(21, 15) = 0;
+  _res(22, 15) = 0;
   _res(0, 16) = P(0, 16) * _tmp18 + P(10, 16) * _tmp23 + P(11, 16) * _tmp6 + P(9, 16) * _tmp15;
   _res(1, 16) = P(1, 16) * _tmp18 + P(10, 16) * _tmp29 + P(11, 16) * _tmp34 + P(9, 16) * _tmp36;
   _res(2, 16) = P(10, 16) * _tmp45 + P(11, 16) * _tmp44 + P(2, 16) * _tmp18 + P(9, 16) * _tmp47;
@@ -470,6 +700,12 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(14, 16) = P(14, 16);
   _res(15, 16) = P(15, 16);
   _res(16, 16) = P(16, 16);
+  _res(17, 16) = 0;
+  _res(18, 16) = 0;
+  _res(19, 16) = 0;
+  _res(20, 16) = 0;
+  _res(21, 16) = 0;
+  _res(22, 16) = 0;
   _res(0, 17) = P(0, 17) * _tmp18 + P(10, 17) * _tmp23 + P(11, 17) * _tmp6 + P(9, 17) * _tmp15;
   _res(1, 17) = P(1, 17) * _tmp18 + P(10, 17) * _tmp29 + P(11, 17) * _tmp34 + P(9, 17) * _tmp36;
   _res(2, 17) = P(10, 17) * _tmp45 + P(11, 17) * _tmp44 + P(2, 17) * _tmp18 + P(9, 17) * _tmp47;
@@ -491,6 +727,11 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(15, 17) = P(15, 17);
   _res(16, 17) = P(16, 17);
   _res(17, 17) = P(17, 17);
+  _res(18, 17) = 0;
+  _res(19, 17) = 0;
+  _res(20, 17) = 0;
+  _res(21, 17) = 0;
+  _res(22, 17) = 0;
   _res(0, 18) = P(0, 18) * _tmp18 + P(10, 18) * _tmp23 + P(11, 18) * _tmp6 + P(9, 18) * _tmp15;
   _res(1, 18) = P(1, 18) * _tmp18 + P(10, 18) * _tmp29 + P(11, 18) * _tmp34 + P(9, 18) * _tmp36;
   _res(2, 18) = P(10, 18) * _tmp45 + P(11, 18) * _tmp44 + P(2, 18) * _tmp18 + P(9, 18) * _tmp47;
@@ -513,6 +754,10 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(16, 18) = P(16, 18);
   _res(17, 18) = P(17, 18);
   _res(18, 18) = P(18, 18);
+  _res(19, 18) = 0;
+  _res(20, 18) = 0;
+  _res(21, 18) = 0;
+  _res(22, 18) = 0;
   _res(0, 19) = P(0, 19) * _tmp18 + P(10, 19) * _tmp23 + P(11, 19) * _tmp6 + P(9, 19) * _tmp15;
   _res(1, 19) = P(1, 19) * _tmp18 + P(10, 19) * _tmp29 + P(11, 19) * _tmp34 + P(9, 19) * _tmp36;
   _res(2, 19) = P(10, 19) * _tmp45 + P(11, 19) * _tmp44 + P(2, 19) * _tmp18 + P(9, 19) * _tmp47;
@@ -536,6 +781,9 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(17, 19) = P(17, 19);
   _res(18, 19) = P(18, 19);
   _res(19, 19) = P(19, 19);
+  _res(20, 19) = 0;
+  _res(21, 19) = 0;
+  _res(22, 19) = 0;
   _res(0, 20) = P(0, 20) * _tmp18 + P(10, 20) * _tmp23 + P(11, 20) * _tmp6 + P(9, 20) * _tmp15;
   _res(1, 20) = P(1, 20) * _tmp18 + P(10, 20) * _tmp29 + P(11, 20) * _tmp34 + P(9, 20) * _tmp36;
   _res(2, 20) = P(10, 20) * _tmp45 + P(11, 20) * _tmp44 + P(2, 20) * _tmp18 + P(9, 20) * _tmp47;
@@ -560,6 +808,8 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(18, 20) = P(18, 20);
   _res(19, 20) = P(19, 20);
   _res(20, 20) = P(20, 20);
+  _res(21, 20) = 0;
+  _res(22, 20) = 0;
   _res(0, 21) = P(0, 21) * _tmp18 + P(10, 21) * _tmp23 + P(11, 21) * _tmp6 + P(9, 21) * _tmp15;
   _res(1, 21) = P(1, 21) * _tmp18 + P(10, 21) * _tmp29 + P(11, 21) * _tmp34 + P(9, 21) * _tmp36;
   _res(2, 21) = P(10, 21) * _tmp45 + P(11, 21) * _tmp44 + P(2, 21) * _tmp18 + P(9, 21) * _tmp47;
@@ -585,6 +835,7 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(19, 21) = P(19, 21);
   _res(20, 21) = P(20, 21);
   _res(21, 21) = P(21, 21);
+  _res(22, 21) = 0;
   _res(0, 22) = P(0, 22) * _tmp18 + P(10, 22) * _tmp23 + P(11, 22) * _tmp6 + P(9, 22) * _tmp15;
   _res(1, 22) = P(1, 22) * _tmp18 + P(10, 22) * _tmp29 + P(11, 22) * _tmp34 + P(9, 22) * _tmp36;
   _res(2, 22) = P(10, 22) * _tmp45 + P(11, 22) * _tmp44 + P(2, 22) * _tmp18 + P(9, 22) * _tmp47;

src/modules/ekf2/EKF/python/ekf_derivation/utils/derivation_utils.py

@@ -56,7 +56,7 @@ def generate_px4_function(function_name, output_names):
     codegen = Codegen.function(
             function_name,
             output_names=output_names,
-            config=CppConfig(zero_initialization_sparsity_threshold=1))
+            config=CppConfig())
     metadata = codegen.generate_function(
             output_dir="generated",
             skip_directory_nesting=True)

src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp

@@ -301,7 +301,8 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index, const Vector3f &delta_ang
 	// Use fixed values for delta angle process noise variances
 	const float d_ang_var = sq(_gyro_noise * delta_ang_dt);
 
-	_ekf_gsf[model_index].P = sym::YawEstPredictCovariance(_ekf_gsf[model_index].X, _ekf_gsf[model_index].P, Vector2f(dvx, dvy), d_vel_var, daz, d_ang_var);
+	sym::YawEstPredictCovariance(_ekf_gsf[model_index].X, _ekf_gsf[model_index].P,
+				     Vector2f(dvx, dvy), d_vel_var, daz, d_ang_var, &_ekf_gsf[model_index].P);
 
 	// covariance matrix is symmetrical, so copy upper half to lower half
 	_ekf_gsf[model_index].P(1, 0) = _ekf_gsf[model_index].P(0, 1);

src/modules/ekf2/EKF/yaw_estimator/derivation/derivation_yaw_estimator.py

@@ -171,5 +171,5 @@ def yaw_est_compute_measurement_update(
     return (S_inv, S_det_inv, K, P_new)
 
 print("Derive yaw estimator equations...")
-derivation_utils.generate_px4_function(yaw_est_predict_covariance, output_names=None)
+derivation_utils.generate_px4_function(yaw_est_predict_covariance, output_names=["P_new"])
 derivation_utils.generate_px4_function(yaw_est_compute_measurement_update, output_names=["S_inv", "S_det_inv", "K", "P_new"])

src/modules/ekf2/EKF/yaw_estimator/derivation/generated/yaw_est_compute_measurement_update.h

@@ -86,11 +86,12 @@ void YawEstComputeMeasurementUpdate(const matrix::Matrix<Scalar, 3, 3>& P, const
   if (P_new != nullptr) {
     matrix::Matrix<Scalar, 3, 3>& _p_new = (*P_new);
 
-    _p_new.setZero();
-
     _p_new(0, 0) = -P(0, 0) * _tmp9 + P(0, 0) - P(1, 0) * _tmp12;
+    _p_new(1, 0) = 0;
+    _p_new(2, 0) = 0;
     _p_new(0, 1) = -P(0, 1) * _tmp9 + P(0, 1) - P(1, 1) * _tmp12;
     _p_new(1, 1) = -P(0, 1) * _tmp10 - P(1, 1) * _tmp13 + P(1, 1);
+    _p_new(2, 1) = 0;
     _p_new(0, 2) = -P(0, 2) * _tmp9 + P(0, 2) - P(1, 2) * _tmp12;
     _p_new(1, 2) = -P(0, 2) * _tmp10 - P(1, 2) * _tmp13 + P(1, 2);
     _p_new(2, 2) = -P(0, 2) * _tmp11 - P(1, 2) * _tmp14 + P(2, 2);

src/modules/ekf2/EKF/yaw_estimator/derivation/generated/yaw_est_predict_covariance.h

@@ -24,14 +24,14 @@ namespace sym {
  *     d_ang_var: Scalar
  *
  * Outputs:
- *     res: Matrix33
+ *     P_new: Matrix33
  */
 template <typename Scalar>
-matrix::Matrix<Scalar, 3, 3> YawEstPredictCovariance(const matrix::Matrix<Scalar, 3, 1>& state,
-                                                    const matrix::Matrix<Scalar, 3, 3>& P,
-                                                    const matrix::Matrix<Scalar, 2, 1>& d_vel,
-                                                    const Scalar d_vel_var, const Scalar d_ang,
-                                                    const Scalar d_ang_var) {
+void YawEstPredictCovariance(const matrix::Matrix<Scalar, 3, 1>& state,
+                             const matrix::Matrix<Scalar, 3, 3>& P,
+                             const matrix::Matrix<Scalar, 2, 1>& d_vel, const Scalar d_vel_var,
+                             const Scalar d_ang, const Scalar d_ang_var,
+                             matrix::Matrix<Scalar, 3, 3>* const P_new = nullptr) {
   // Total ops: 39
 
   // Input arrays
@@ -48,18 +48,19 @@ matrix::Matrix<Scalar, 3, 3> YawEstPredictCovariance(const matrix::Matrix<Scalar
   const Scalar _tmp7 = std::pow(d_ang, Scalar(2)) + 1;
 
   // Output terms (1)
-  matrix::Matrix<Scalar, 3, 3> _res;
+  if (P_new != nullptr) {
+    matrix::Matrix<Scalar, 3, 3>& _p_new = (*P_new);
 
-  _res.setZero();
-
-  _res(0, 0) = P(0, 0) + P(2, 0) * _tmp2 + _tmp2 * _tmp3 + _tmp4;
-  _res(0, 1) = P(0, 1) + P(2, 1) * _tmp2 + _tmp3 * _tmp5;
-  _res(1, 1) = P(1, 1) + P(2, 1) * _tmp5 + _tmp4 + _tmp5 * _tmp6;
-  _res(0, 2) = _tmp3 * _tmp7;
-  _res(1, 2) = _tmp6 * _tmp7;
-  _res(2, 2) = P(2, 2) * std::pow(_tmp7, Scalar(2)) + d_ang_var;
-
-  return _res;
+    _p_new(0, 0) = P(0, 0) + P(2, 0) * _tmp2 + _tmp2 * _tmp3 + _tmp4;
+    _p_new(1, 0) = 0;
+    _p_new(2, 0) = 0;
+    _p_new(0, 1) = P(0, 1) + P(2, 1) * _tmp2 + _tmp3 * _tmp5;
+    _p_new(1, 1) = P(1, 1) + P(2, 1) * _tmp5 + _tmp4 + _tmp5 * _tmp6;
+    _p_new(2, 1) = 0;
+    _p_new(0, 2) = _tmp3 * _tmp7;
+    _p_new(1, 2) = _tmp6 * _tmp7;
+    _p_new(2, 2) = P(2, 2) * std::pow(_tmp7, Scalar(2)) + d_ang_var;
+  }
 }  // NOLINT(readability/fn_size)
 
 // NOLINTNEXTLINE(readability/fn_size)