CA testing hacks, external failure message, sitl tuning, disabling of rate controller saturation

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
matrix: change matrix.print() back to pring all elements, don't assume symmetric matrix
2026-06-13 18:10:05 +08:00 · 2024-06-10 11:50:58 +02:00 · 2024-06-10 11:16:32 +02:00 · 2024-06-10 11:16:32 +02:00 · 2024-06-10 11:16:32 +02:00 · 2024-06-10 11:16:32 +02:00
165 changed files with 6145 additions and 4177 deletions
@@ -151,16 +151,16 @@ CONFIG:
      buildType: MinSizeRel
      settings:
        CONFIG: ark_can-rtk-gps_canbootloader
-    ark_septentrio_gps_default:
-      short: ark_septentrio_gps_default
+    ark_septentrio-gps_default:
+      short: ark_septentrio-gps_default
      buildType: MinSizeRel
      settings:
-        CONFIG: ark_septentrio_gps_default
-    ark_septentrio_gps_canbootloader:
-      short: ark_septentrio_gps_canbootloader
+        CONFIG: ark_septentrio-gps_default
+    ark_septentrio-gps_canbootloader:
+      short: ark_septentrio-gps_canbootloader
      buildType: MinSizeRel
      settings:
-        CONFIG: ark_septentrio_gps_canbootloader
+        CONFIG: ark_septentrio-gps_canbootloader
    ark_cannode_default:
      short: ark_cannode_default
      buildType: MinSizeRel
@@ -1,6 +1,6 @@
 ############################################################################
 #
-# Copyright (c) 2015 - 2020 PX4 Development Team. All rights reserved.
+# Copyright (c) 2015 - 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
@@ -323,7 +323,32 @@ px4io_update:
 	cp build/cubepilot_io-v2_default/cubepilot_io-v2_default.bin boards/cubepilot/cubeyellow/extras/cubepilot_io-v2_default.bin
 	git status

-bootloaders_update: ark_fmu-v6x_bootloader cuav_nora_bootloader cuav_x7pro_bootloader cubepilot_cubeorange_bootloader holybro_durandal-v1_bootloader holybro_kakuteh7_bootloader matek_h743_bootloader matek_h743-mini_bootloader matek_h743-slim_bootloader modalai_fc-v2_bootloader mro_ctrl-zero-classic_bootloader mro_ctrl-zero-h7_bootloader mro_ctrl-zero-h7-oem_bootloader mro_pixracerpro_bootloader px4_fmu-v6c_bootloader px4_fmu-v6u_bootloader px4_fmu-v6x_bootloader
+bootloaders_update: \
+	ark_fmu-v6x_bootloader \
+	ark_pi6x_bootloader \
+	cuav_nora_bootloader \
+	cuav_x7pro_bootloader \
+	cubepilot_cubeorange_bootloader \
+	cubepilot_cubeorangeplus_bootloader \
+	hkust_nxt-dual_bootloader \
+	hkust_nxt-v1_bootloader \
+	holybro_durandal-v1_bootloader \
+	holybro_kakuteh7_bootloader \
+	holybro_kakuteh7mini_bootloader \
+	holybro_kakuteh7v2_bootloader \
+	matek_h743_bootloader \
+	matek_h743-mini_bootloader \
+	matek_h743-slim_bootloader \
+	modalai_fc-v2_bootloader \
+	mro_ctrl-zero-classic_bootloader \
+	mro_ctrl-zero-h7_bootloader \
+	mro_ctrl-zero-h7-oem_bootloader \
+	mro_pixracerpro_bootloader \
+	px4_fmu-v6c_bootloader \
+	px4_fmu-v6u_bootloader \
+	px4_fmu-v6x_bootloader \
+	px4_fmu-v6xrt_bootloader \
+	siyi_n7_bootloader
 	git status

 .PHONY: coverity_scan
@@ -21,7 +21,6 @@ param set-default VT_B_TRANS_DUR 5
 param set-default VT_ELEV_MC_LOCK 0
 param set-default VT_TYPE 0
 param set-default VT_FW_DIFTHR_EN 1
-param set-default VT_FW_DIFTHR_S_Y 0.3
 param set-default MPC_MAN_Y_MAX 60
 param set-default MC_PITCH_P 5

@@ -15,17 +15,17 @@ param set-default FD_ACT_MOT_TOUT 500
 param set-default CA_AIRFRAME 2

 param set-default CA_ROTOR_COUNT 5
-param set-default CA_ROTOR0_PX 0.1515
-param set-default CA_ROTOR0_PY 0.245
+param set-default CA_ROTOR0_PX 1
+param set-default CA_ROTOR0_PY 1
 param set-default CA_ROTOR0_KM 0.05
-param set-default CA_ROTOR1_PX -0.1515
-param set-default CA_ROTOR1_PY -0.1875
+param set-default CA_ROTOR1_PX -1
+param set-default CA_ROTOR1_PY -1
 param set-default CA_ROTOR1_KM 0.05
-param set-default CA_ROTOR2_PX 0.1515
-param set-default CA_ROTOR2_PY -0.245
+param set-default CA_ROTOR2_PX 1
+param set-default CA_ROTOR2_PY -1
 param set-default CA_ROTOR2_KM -0.05
-param set-default CA_ROTOR3_PX -0.1515
-param set-default CA_ROTOR3_PY 0.1875
+param set-default CA_ROTOR3_PX -1
+param set-default CA_ROTOR3_PY 1
 param set-default CA_ROTOR3_KM -0.05
 param set-default CA_ROTOR4_AX 1
 param set-default CA_ROTOR4_AZ 0
@@ -33,9 +33,9 @@ param set-default CA_ROTOR4_PX 0.2

 param set-default CA_SV_CS_COUNT 3
 param set-default CA_SV_CS0_TYPE 1
-param set-default CA_SV_CS0_TRQ_R -0.5
+param set-default CA_SV_CS0_TRQ_R -1
 param set-default CA_SV_CS1_TYPE 2
-param set-default CA_SV_CS1_TRQ_R 0.5
+param set-default CA_SV_CS1_TRQ_R 1
 param set-default CA_SV_CS2_TYPE 3
 param set-default CA_SV_CS2_TRQ_P 1
 param set-default PWM_MAIN_FUNC1 101
@@ -79,3 +79,18 @@ param set-default VT_FWD_THRUST_SC 1
 param set-default VT_F_TRANS_THR 0.75
 param set-default VT_TYPE 2

+param set-default CA_FW_DTHR_SC_R 1
+param set-default CA_FAILURE_MODE 1
+
+# Two ways to lock servo1 and servo2, either in allocation (will automatically notify about the failure), in in the output mixer
+# Only enable one of the two options
+
+# output mixer (NO automatic failure notification)
+# param set-default OUT_SRV_FAIL_IPT 1
+# param set-default OUT_SRV_FAIL_NR 2
+
+# allocation (automatic failure notification)
+param set-default COM_SRV_FAIL_IPT 1
+param set-default COM_SRV_FAIL_NR 2
+
+param set-default CA_FW_DTHR_AIRMD 1 # has to be enabled otheriwse the MC motors have no differential thrust capability
@@ -70,7 +70,6 @@ param set-default MC_PITCH_P 3
 param set-default VT_ARSP_TRANS 10
 param set-default VT_B_TRANS_DUR 5
 param set-default VT_FW_DIFTHR_EN 1
-param set-default VT_FW_DIFTHR_S_Y 1
 param set-default VT_F_TRANS_DUR 1.5
 param set-default VT_TYPE 0

@@ -67,7 +67,6 @@ param set-default MC_PITCHRATE_P 0.3
 param set-default VT_ARSP_TRANS 15
 param set-default VT_B_TRANS_DUR 5
 param set-default VT_FW_DIFTHR_EN 7
-param set-default VT_FW_DIFTHR_S_Y 1
 param set-default VT_F_TRANS_DUR 1.5
 param set-default VT_TYPE 0

@@ -84,6 +84,13 @@ if(CONFIG_MODULES_DIFFERENTIAL_DRIVE)
 	)
 endif()

+if(CONFIG_MODULES_ROVER_ACKERMANN)
+	px4_add_romfs_files(
+		rc.rover_ackermann_apps
+		rc.rover_ackermann_defaults
+	)
+endif()
+
 if(CONFIG_MODULES_UUV_ATT_CONTROL)
 	px4_add_romfs_files(
 		rc.uuv_apps
@@ -25,7 +25,6 @@ param set-default VT_ELEV_MC_LOCK 0
 param set-default VT_MOT_COUNT 2
 param set-default VT_TYPE 0
 param set-default VT_FW_DIFTHR_EN 1
-param set-default VT_FW_DIFTHR_S_Y 0.3
 param set-default MPC_MAN_Y_MAX 60
 param set-default MC_PITCH_P 5

@@ -0,0 +1,12 @@
+#!/bin/sh
+#
+# @name Generic ackermann rover
+#
+# @type Rover
+# @class Rover
+#
+# @board px4_fmu-v2 exclude
+# @board bitcraze_crazyflie exclude
+#
+
+. ${R}etc/init.d/rc.rover_ackermann_defaults
@@ -149,6 +149,12 @@ if(CONFIG_MODULES_DIFFERENTIAL_DRIVE)
 	)
 endif()

+if(CONFIG_MODULES_ROVER_ACKERMANN)
+	px4_add_romfs_files(
+		50010_ackermann_rover_generic
+	)
+endif()
+
 if(CONFIG_MODULES_UUV_ATT_CONTROL)
 	px4_add_romfs_files(
 		# [60000, 61000] (Unmanned) Underwater Robots
@@ -0,0 +1,11 @@
+#!/bin/sh
+# Standard apps for a ackermann drive rover.
+
+# Start the attitude and position estimator.
+ekf2 start &
+
+# Start rover ackermann drive controller.
+rover_ackermann start
+
+# Start Land Detector.
+land_detector start rover
@@ -0,0 +1,13 @@
+#!/bin/sh
+# Ackermann rover parameters.
+
+set VEHICLE_TYPE rover_ackermann
+param set-default MAV_TYPE 10 # MAV_TYPE_GROUND_ROVER
+param set-default CA_AIRFRAME 5 # Rover (Ackermann)
+param set-default CA_R_REV 1 # Motor is assumed to be reversible
+param set-default EKF2_MAG_TYPE 1 # make sure magnetometer is fused even when not flying
+param set-default EKF2_GBIAS_INIT 0.01
+param set-default EKF2_ANGERR_INIT 0.01
+param set-default NAV_ACC_RAD 0.5 # Waypoint acceptance radius
+param set-default NAV_RCL_ACT 6 # Disarm on manual control loss
+param set-default COM_FAIL_ACT_T 1 # Delay before failsafe after rc loss
@@ -41,6 +41,15 @@ then
 	. ${R}etc/init.d/rc.rover_differential_apps
 fi

+#
+# Ackermann Rover setup.
+#
+if [ $VEHICLE_TYPE = rover_ackermann ]
+then
+	# Start ackermann drive rover apps.
+	. ${R}etc/init.d/rc.rover_ackermann_apps
+fi
+
 #
 # VTOL setup.
 #
@@ -95,7 +95,7 @@ if [[ $INSTALL_SIM == "true" ]]; then

 	# java (jmavsim)
 	sudo pacman -S --noconfirm --needed \
-		ant
+		ant \
 		;

 	# Gazebo setup
@@ -1,5 +1,4 @@
 argcomplete
-argparse>=1.2
 cerberus
 coverage
 empy==3.3.4
@@ -66,6 +66,8 @@ elif [[ "${UBUNTU_RELEASE}" == "20.04" ]]; then
 	echo "Ubuntu 20.04"
 elif [[ "${UBUNTU_RELEASE}" == "22.04" ]]; then
 	echo "Ubuntu 22.04"
+elif [[ "${UBUNTU_RELEASE}" == "21.3" ]]; then
+	echo "Linux Mint 21.3"
 fi


@@ -146,7 +148,7 @@ if [[ $INSTALL_NUTTX == "true" ]]; then
 		util-linux \
 		vim-common \
 		;
-	if [[ "${UBUNTU_RELEASE}" == "20.04" || "${UBUNTU_RELEASE}" == "22.04" ]]; then
+	if [[ "${UBUNTU_RELEASE}" == "20.04" || "${UBUNTU_RELEASE}" == "22.04" || "${UBUNTU_RELEASE}" == "21.3" ]]; then
 		sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
 		kconfig-frontends \
 		;
@@ -205,6 +207,8 @@ if [[ $INSTALL_SIM == "true" ]]; then
 		java_version=13
 	elif [[ "${UBUNTU_RELEASE}" == "22.04" ]]; then
 		java_version=11
+	elif [[ "${UBUNTU_RELEASE}" == "21.3" ]]; then
+		java_version=11
 	else
 		java_version=14
 	fi
@@ -228,6 +232,17 @@ if [[ $INSTALL_SIM == "true" ]]; then
 		echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | sudo tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null
 		sudo apt-get update -y --quiet

+		# Install Gazebo
+		gazebo_packages="gz-garden"
+	elif [[ "${UBUNTU_RELEASE}" == "21.3" ]]; then
+		echo "Gazebo (Garden) will be installed"
+		echo "Earlier versions will be removed"
+		# Add Gazebo binary repository
+		sudo wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg
+		echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable jammy main" | sudo tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null
+
+		sudo apt-get update -y --quiet
+
 		# Install Gazebo
 		gazebo_packages="gz-garden"
 	else
@@ -147,8 +147,8 @@ CONFIG_NETDB_DNSSERVER_NOADDR=y
 CONFIG_NETDEV_PHY_IOCTL=y
 CONFIG_NETINIT_DHCPC=y
 CONFIG_NETINIT_DNS=y
-CONFIG_NETINIT_DNSIPADDR=0XC0A800FE
-CONFIG_NETINIT_DRIPADDR=0XC0A800FE
+CONFIG_NETINIT_DNSIPADDR=0xA290AFE
+CONFIG_NETINIT_DRIPADDR=0xA290AFE
 CONFIG_NETINIT_MONITOR=y
 CONFIG_NETINIT_THREAD=y
 CONFIG_NETINIT_THREAD_PRIORITY=49
@@ -32,19 +32,10 @@ then
 	param set-default SENS_TEMP_ID 2490378
 fi

-param set-default EKF2_BARO_DELAY 39
-param set-default EKF2_BARO_NOISE 0.9
-param set-default EKF2_HGT_REF 0
-param set-default EKF2_MAG_DELAY 60
-param set-default EKF2_MAG_NOISE 0.06
 param set-default EKF2_MULTI_IMU 2
 param set-default EKF2_OF_CTRL 1
-param set-default EKF2_OF_DELAY 28
 param set-default EKF2_OF_N_MIN 0.05
 param set-default EKF2_RNG_A_HMAX 25
-param set-default EKF2_RNG_DELAY 105
-param set-default EKF2_RNG_NOISE 0.03
 param set-default EKF2_RNG_QLTY_T 0.1
-param set-default EKF2_RNG_SFE 0.03

 param set-default SENS_FLOW_RATE 150
@@ -139,8 +139,8 @@ CONFIG_NETDB_DNSSERVER_NOADDR=y
 CONFIG_NETDEV_PHY_IOCTL=y
 CONFIG_NETINIT_DHCPC=y
 CONFIG_NETINIT_DNS=y
-CONFIG_NETINIT_DNSIPADDR=0XC0A800FE
-CONFIG_NETINIT_DRIPADDR=0XC0A800FE
+CONFIG_NETINIT_DNSIPADDR=0xA290AFE
+CONFIG_NETINIT_DRIPADDR=0xA290AFE
 CONFIG_NETINIT_THREAD=y
 CONFIG_NETINIT_THREAD_PRIORITY=49
 CONFIG_NETUTILS_TELNETD=y
@@ -15,7 +15,7 @@ param set-default SYS_AUTOSTART 4050
 param set-default SYS_HAS_MAG 0

 # enable gravity fusion
-param set-default EKF2_IMU_CONTROL 7
+param set-default EKF2_IMU_CTRL 7

 # the startup tune is not great on a binary output buzzer, so disable it
 param set-default CBRK_BUZZER 782090
@@ -25,7 +25,7 @@ param set-default SYS_AUTOSTART 4050
 # use EKF2 without mag
 param set-default SYS_HAS_MAG 0
 # and enable gravity fusion
-param set-default EKF2_IMU_CONTROL 7
+param set-default EKF2_IMU_CTRL 7

 # the startup tune is not great on a binary output buzzer, so disable it
 param set-default CBRK_BUZZER 782090
@@ -25,7 +25,7 @@ param set-default SYS_AUTOSTART 4050
 # use EKF2 without mag
 param set-default SYS_HAS_MAG 0
 # and enable gravity fusion
-param set-default EKF2_IMU_CONTROL 7
+param set-default EKF2_IMU_CTRL 7

 # the startup tune is not great on a binary output buzzer, so disable it
 param set-default CBRK_BUZZER 782090
@@ -25,7 +25,7 @@ param set-default SYS_AUTOSTART 4050
 # use EKF2 without mag
 param set-default SYS_HAS_MAG 0
 # and enable gravity fusion
-param set-default EKF2_IMU_CONTROL 7
+param set-default EKF2_IMU_CTRL 7

 # the startup tune is not great on a binary output buzzer, so disable it
 param set-default CBRK_BUZZER 782090
@@ -158,8 +158,8 @@ CONFIG_NETDEV_LATEINIT=y
 CONFIG_NETDEV_PHY_IOCTL=y
 CONFIG_NETINIT_DHCPC=y
 CONFIG_NETINIT_DNS=y
-CONFIG_NETINIT_DNSIPADDR=0XC0A800FE
-CONFIG_NETINIT_DRIPADDR=0XC0A800FE
+CONFIG_NETINIT_DNSIPADDR=0xA290AFE
+CONFIG_NETINIT_DRIPADDR=0xA290AFE
 CONFIG_NETINIT_MONITOR=y
 CONFIG_NETINIT_THREAD=y
 CONFIG_NETINIT_THREAD_PRIORITY=49
@@ -158,8 +158,8 @@ CONFIG_NETDEV_LATEINIT=y
 CONFIG_NETDEV_PHY_IOCTL=y
 CONFIG_NETINIT_DHCPC=y
 CONFIG_NETINIT_DNS=y
-CONFIG_NETINIT_DNSIPADDR=0XC0A800FE
-CONFIG_NETINIT_DRIPADDR=0XC0A800FE
+CONFIG_NETINIT_DNSIPADDR=0xA290AFE
+CONFIG_NETINIT_DRIPADDR=0xA290AFE
 CONFIG_NETINIT_MONITOR=y
 CONFIG_NETINIT_THREAD=y
 CONFIG_NETINIT_THREAD_PRIORITY=49
@@ -19,3 +19,4 @@ CONFIG_MODULES_VTOL_ATT_CONTROL=n
 CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
 CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
+CONFIG_MODULES_ROVER_ACKERMANN=y
@@ -3,23 +3,25 @@
 # board specific defaults
 #------------------------------------------------------------------------------

-# Mavlink ethernet (CFG 1000)
-param set-default MAV_2_CONFIG 1000
-param set-default MAV_2_BROADCAST 1
-param set-default MAV_2_MODE 0
-param set-default MAV_2_RADIO_CTL 0
-param set-default MAV_2_RATE 100000
-param set-default MAV_2_REMOTE_PRT 14550
-param set-default MAV_2_UDP_PRT 14550
-
 param set-default SENS_EN_INA238 0
 param set-default SENS_EN_INA228 0
 param set-default SENS_EN_INA226 1

 if ver hwbasecmp 008 009 00a 010 011
 then
-	# Skynode: use the "custom participant" config for uxrce_dds_client
+	# Skynode: use the "custom participant", IP=10.41.10.1 config for uxrce_dds_client
 	param set-default UXRCE_DDS_PTCFG 2
+	param set-default UXRCE_DDS_AG_IP 170461697
+	param set-default UXRCE_DDS_CFG 1000
+else
+	# Mavlink ethernet (CFG 1000)
+	param set-default MAV_2_CONFIG 1000
+	param set-default MAV_2_BROADCAST 1
+	param set-default MAV_2_MODE 0
+	param set-default MAV_2_RADIO_CTL 0
+	param set-default MAV_2_RATE 100000
+	param set-default MAV_2_REMOTE_PRT 14550
+	param set-default MAV_2_UDP_PRT 14550
 fi

 safety_button start
@@ -143,8 +143,8 @@ CONFIG_NETDB_DNSSERVER_NOADDR=y
 CONFIG_NETDEV_PHY_IOCTL=y
 CONFIG_NETINIT_DHCPC=y
 CONFIG_NETINIT_DNS=y
-CONFIG_NETINIT_DNSIPADDR=0XC0A800FE
-CONFIG_NETINIT_DRIPADDR=0XC0A800FE
+CONFIG_NETINIT_DNSIPADDR=0xA290AFE
+CONFIG_NETINIT_DRIPADDR=0xA290AFE
 CONFIG_NETINIT_MONITOR=y
 CONFIG_NETINIT_THREAD=y
 CONFIG_NETINIT_THREAD_PRIORITY=49
@@ -143,8 +143,8 @@ CONFIG_NETDB_DNSSERVER_NOADDR=y
 CONFIG_NETDEV_PHY_IOCTL=y
 CONFIG_NETINIT_DHCPC=y
 CONFIG_NETINIT_DNS=y
-CONFIG_NETINIT_DNSIPADDR=0XC0A800FE
-CONFIG_NETINIT_DRIPADDR=0XC0A800FE
+CONFIG_NETINIT_DNSIPADDR=0xA290AFE
+CONFIG_NETINIT_DRIPADDR=0xA290AFE
 CONFIG_NETINIT_THREAD=y
 CONFIG_NETINIT_THREAD_PRIORITY=49
 CONFIG_NETUTILS_TELNETD=y
@@ -14,3 +14,5 @@ CONFIG_MODULES_MC_RATE_CONTROL=n
 CONFIG_MODULES_VTOL_ATT_CONTROL=n
 CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
+CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
+CONFIG_MODULES_ROVER_ACKERMANN=y
@@ -14,3 +14,4 @@ CONFIG_MODULES_VTOL_ATT_CONTROL=n
 CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
 CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
+CONFIG_MODULES_ROVER_ACKERMANN=y
@@ -14,3 +14,4 @@ CONFIG_MODULES_VTOL_ATT_CONTROL=n
 CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
 CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
+CONFIG_MODULES_ROVER_ACKERMANN=y
@@ -3,15 +3,6 @@
 # board specific defaults
 #------------------------------------------------------------------------------

-# Mavlink ethernet (CFG 1000)
-param set-default MAV_2_CONFIG 1000
-param set-default MAV_2_BROADCAST 1
-param set-default MAV_2_MODE 0
-param set-default MAV_2_RADIO_CTL 0
-param set-default MAV_2_RATE 100000
-param set-default MAV_2_REMOTE_PRT 14550
-param set-default MAV_2_UDP_PRT 14550
-
 # By disabling all 3 INA modules, we use the
 # i2c_launcher instead.
 param set-default SENS_EN_INA238 0
@@ -20,8 +11,19 @@ param set-default SENS_EN_INA226 0

 if ver hwbasecmp 009 010 011
 then
-	# Skynode: use the "custom participant" config for uxrce_dds_client
+	# Skynode: use the "custom participant", IP=10.41.10.1 config for uxrce_dds_client
 	param set-default UXRCE_DDS_PTCFG 2
+	param set-default UXRCE_DDS_AG_IP 170461697
+	param set-default UXRCE_DDS_CFG 1000
+else
+	# Mavlink ethernet (CFG 1000)
+	param set-default MAV_2_CONFIG 1000
+	param set-default MAV_2_BROADCAST 1
+	param set-default MAV_2_MODE 0
+	param set-default MAV_2_RADIO_CTL 0
+	param set-default MAV_2_RATE 100000
+	param set-default MAV_2_REMOTE_PRT 14550
+	param set-default MAV_2_UDP_PRT 14550
 fi

 safety_button start
@@ -147,8 +147,8 @@ CONFIG_NETDB_DNSSERVER_NOADDR=y
 CONFIG_NETDEV_PHY_IOCTL=y
 CONFIG_NETINIT_DHCPC=y
 CONFIG_NETINIT_DNS=y
-CONFIG_NETINIT_DNSIPADDR=0XC0A800FE
-CONFIG_NETINIT_DRIPADDR=0XC0A800FE
+CONFIG_NETINIT_DNSIPADDR=0xA290AFE
+CONFIG_NETINIT_DRIPADDR=0xA290AFE
 CONFIG_NETINIT_MONITOR=y
 CONFIG_NETINIT_THREAD=y
 CONFIG_NETINIT_THREAD_PRIORITY=49
@@ -14,3 +14,4 @@ CONFIG_MODULES_VTOL_ATT_CONTROL=n
 CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
 CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
+CONFIG_MODULES_ROVER_ACKERMANN=y
@@ -202,8 +202,8 @@ CONFIG_NETDEV_LATEINIT=y
 CONFIG_NETDEV_PHY_IOCTL=y
 CONFIG_NETINIT_DHCPC=y
 CONFIG_NETINIT_DNS=y
-CONFIG_NETINIT_DNSIPADDR=0XC0A800FE
-CONFIG_NETINIT_DRIPADDR=0XC0A800FE
+CONFIG_NETINIT_DNSIPADDR=0xA290AFE
+CONFIG_NETINIT_DRIPADDR=0xA290AFE
 CONFIG_NETINIT_MONITOR=y
 CONFIG_NETINIT_THREAD=y
 CONFIG_NETINIT_THREAD_PRIORITY=49
@@ -683,7 +683,6 @@
 *(.text._ZN18MavlinkStreamDebug8get_sizeEv)
 *(.text._ZN12GPSDriverUBX7receiveEj)
 *(.text._ZN13BatteryStatus21parameter_update_pollEb)
-*(.text._ZN3Ekf26checkYawAngleObservabilityEv)
 *(.text._ZN3RTL18updateDatamanCacheEv)
 *(.text.__ieee754_sqrtf)
 *(.text._ZThn24_N18mag_bias_estimator16MagBiasEstimator3RunEv)
@@ -15,3 +15,4 @@ CONFIG_MODULES_VTOL_ATT_CONTROL=n
 CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
 CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
+CONFIG_MODULES_ROVER_ACKERMANN=y
@@ -455,6 +455,7 @@
 #define RC_SERIAL_SINGLEWIRE            1 // Suport Single wire wiring
 #define RC_SERIAL_SWAP_RXTX             1 // Set Swap (but not supported in HW) to use Single wire
 #define RC_SERIAL_SWAP_USING_SINGLEWIRE 1 // Set to use Single wire swap as HW does not support swap
+#define BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT

 /* FLEXSPI4 */

@@ -104,9 +104,10 @@ const struct flexspi_nor_config_s g_flash_fast_config = {
 		.busyBitPolarity = 0u,
 		.lookupTable =
 		{
-			/* Read */// EEH+11H+32bit addr+20dummy cycles+ 4Bytes read data    //200Mhz 18 dummy=10+8
+			/* Read */// EEH+11H+32bit addr+20dummy cycles+ 4Bytes read data
+			/* Macronix manual says 20 dummy cycles @ 200Mhz, FlexSPI peripheral Operand value needs to be 2N in DDR mode hence 0x28 */
 			[0 + 0] = FLEXSPI_LUT_SEQ(CMD_DDR, FLEXSPI_8PAD, 0xEE, CMD_DDR, FLEXSPI_8PAD, 0x11),    //0x871187ee,
-			[0 + 1] = FLEXSPI_LUT_SEQ(RADDR_DDR, FLEXSPI_8PAD, 0x20, DUMMY_DDR, FLEXSPI_8PAD, 0x04),//0xb3048b20,
+			[0 + 1] = FLEXSPI_LUT_SEQ(RADDR_DDR, FLEXSPI_8PAD, 0x20, DUMMY_DDR, FLEXSPI_8PAD, 0x28),//0xb3288b20,
 			[0 + 2] = FLEXSPI_LUT_SEQ(READ_DDR, FLEXSPI_8PAD, 0x04, STOP_EXE, FLEXSPI_1PAD, 0x00), //0xa704,

 			/* Read status */
@@ -45,6 +45,7 @@ CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF=y
 CONFIG_MODULES_PAYLOAD_DELIVERER=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_REPLAY=y
+CONFIG_MODULES_ROVER_ACKERMANN=y
 CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_COMMON_SIMULATION=y
@@ -141,8 +141,8 @@ CONFIG_NETDB_DNSCLIENT_ENTRIES=8
 CONFIG_NETDB_DNSSERVER_NOADDR=y
 CONFIG_NETDEV_PHY_IOCTL=y
 CONFIG_NETINIT_DNS=y
-CONFIG_NETINIT_DNSIPADDR=0XC0A800FE
-CONFIG_NETINIT_DRIPADDR=0XC0A800FE
+CONFIG_NETINIT_DNSIPADDR=0xA290AFE
+CONFIG_NETINIT_DRIPADDR=0xA290AFE
 CONFIG_NETINIT_THREAD=y
 CONFIG_NETINIT_THREAD_PRIORITY=49
 CONFIG_NETUTILS_TELNETD=y
@@ -93,6 +93,7 @@ set(msg_files
 	Event.msg
 	FigureEightStatus.msg
 	FailsafeFlags.msg
+	FailureDetectorExtServo.msg
 	FailureDetectorStatus.msg
 	FlightPhaseEstimation.msg
 	FollowTarget.msg
@@ -178,6 +179,7 @@ set(msg_files
 	RcParameterMap.msg
 	RegisterExtComponentReply.msg
 	RegisterExtComponentRequest.msg
+	RoverAckermannGuidanceStatus.msg
 	Rpm.msg
 	RtlStatus.msg
 	RtlTimeEstimate.msg
@@ -1,5 +1,8 @@
 uint64 timestamp                        # time since system start (microseconds)

+float32[3] torque_setpoint              # Current torque setpoint (normalized) for specific allocation matrix.
+float32[3] thrust_setpoint              # Current thrust setpoint (normalized) for specific allocation matrix.
+
 bool torque_setpoint_achieved           # Boolean indicating whether the 3D torque setpoint was correctly allocated to actuators. 0 if not achieved, 1 if achieved.
 float32[3] unallocated_torque           # Unallocated torque. Equal to 0 if the setpoint was achieved.
                                        # Computed as: unallocated_torque = torque_setpoint - allocated_torque
@@ -0,0 +1,5 @@
+uint64 timestamp                    # time since system start (microseconds)
+
+bool fd_servo
+
+uint16 servo_failure_mask           # Bit-mask with servo indices, indicating critical servo failures
@@ -9,6 +9,10 @@ bool fd_arm_escs
 bool fd_battery
 bool fd_imbalanced_prop
 bool fd_motor
+bool fd_servo

 float32 imbalanced_prop_metric      # Metric of the imbalanced propeller check (low-passed)
 uint16 motor_failure_mask           # Bit-mask with motor indices, indicating critical motor failures
+uint16 servo_failure_mask           # Bit-mask with servo indices, indicating critical servo failures
+
+uint16 servo_to_center_mask	    # HACK to test allocation without some servos
@@ -0,0 +1,10 @@
+uint64 timestamp # time since system start (microseconds)
+
+float32 actual_speed # [m/s] Rover ground speed
+float32 desired_speed # [m/s] Rover desired ground speed
+float32 lookahead_distance # [m] Lookahead distance of pure the pursuit controller
+float32 heading_error # [deg] Heading error of the pure pursuit controller
+float32 pid_throttle_integral # [-1, 1] Integral of the PID for the normalized throttle to control the rover speed during missions
+float32 crosstrack_error # [m] Shortest distance from the vehicle to the path
+
+# TOPICS rover_ackermann_guidance_status
@@ -11,7 +11,7 @@ float32[4] q_d			# Desired quaternion for quaternion control

 # For clarification: For multicopters thrust_body[0] and thrust[1] are usually 0 and thrust[2] is the negative throttle demand.
 # For fixed wings thrust_x is the throttle demand and thrust_y, thrust_z will usually be zero.
-float32[3] thrust_body		# Normalized thrust command in body NED frame [-1,1]
+float32[3] thrust_body		# Normalized thrust command in body FRD frame [-1,1]

 bool reset_integral	# Reset roll/pitch/yaw integrals (navigation logic change)

@@ -74,6 +74,11 @@ bool SerialImpl::configure()
 	int speed;

 	switch (_baudrate) {
+	case 0:
+		// special case, if baudrate is 0 it hangs entire system
+		PX4_ERR("baudrate not specified");
+		return false;
+
 	case 9600:   speed = B9600;   break;

 	case 19200:  speed = B19200;  break;
@@ -153,9 +153,8 @@ Server::_server_main()
 			// Reboot command causes System Interrupt to stop poll(). This is not an error
 			if (errno != EINTR) {
 				PX4_ERR("poll() failed: %s", strerror(errno));
+				break;
 			}
-
-			break;
 		}

 		_lock();
@@ -37,17 +37,101 @@

 namespace Murata_SCH16T
 {
-static constexpr uint32_t SPI_SPEED = 5 * 1000 * 1000;  	// 5 MHz SPI serial interface
-static constexpr uint32_t SAMPLE_INTERVAL_US = 678;     	// 1500 Hz -- decimation factor 8, F_PRIM/16, 1.475 kHz
-static constexpr uint16_t EOI = (1 << 1); 			// End of Initialization
-static constexpr uint16_t EN_SENSOR = (1 << 0); 		// Enable RATE and ACC measurement
-static constexpr uint16_t DRY_DRV_EN = (1 << 5); 		// Enables Data ready function
-static constexpr uint16_t FILTER_BYPASS = (0b111); 		// Bypass filter
-static constexpr uint16_t RATE_300DPS_1475HZ = 0b0'001'001'011'011'011; // Gyro XYZ range 300 deg/s @ 1475Hz
-static constexpr uint16_t ACC12_8G_1475HZ = 0b0'001'001'011'011'011;  // Acc XYZ range 8 G and 1475 update rate
-static constexpr uint16_t ACC3_26G = (0b000 << 0);
+// General definitions
+static constexpr uint16_t EOI = (1 << 1);               // End of Initialization
+static constexpr uint16_t EN_SENSOR = (1 << 0);         // Enable RATE and ACC measurement
+static constexpr uint16_t DRY_DRV_EN = (1 << 5);        // Enables Data ready function
 static constexpr uint16_t SPI_SOFT_RESET = (0b1010);

+// Filter settings
+static constexpr uint8_t FILTER_13_HZ = (0b010);
+static constexpr uint8_t FILTER_30_HZ = (0b001);
+static constexpr uint8_t FILTER_68_HZ = (0b000);
+static constexpr uint8_t FILTER_280_HZ = (0b011);
+static constexpr uint8_t FILTER_370_HZ = (0b100);
+static constexpr uint8_t FILTER_235_HZ = (0b101);
+static constexpr uint8_t FILTER_BYPASS = (0b111);
+
+// Dynamic range settings
+static constexpr uint8_t RATE_RANGE_300 = (0b001);
+static constexpr uint8_t ACC12_RANGE_80 = (0b001);
+static constexpr uint8_t ACC3_RANGE_260 = (0b000);
+
+// Decimation ratio settings
+static constexpr uint8_t DECIMATION_NONE =	(0b000);
+static constexpr uint8_t DECIMATION_5900_HZ =	(0b001);
+static constexpr uint8_t DECIMATION_2950_HZ =	(0b010);
+static constexpr uint8_t DECIMATION_1475_HZ =	(0b011);
+static constexpr uint8_t DECIMATION_738_HZ =	(0b100);
+
+union CTRL_FILT_RATE_Register {
+	struct {
+		uint16_t FILT_SEL_RATE_X  : 3;
+		uint16_t FILT_SEL_RATE_Y  : 3;
+		uint16_t FILT_SEL_RATE_Z  : 3;
+		uint16_t reserved         : 7;
+	} bits;
+
+	uint16_t value;
+};
+
+union CTRL_FILT_ACC12_Register {
+	struct {
+		uint16_t FILT_SEL_ACC_X12  : 3;
+		uint16_t FILT_SEL_ACC_Y12  : 3;
+		uint16_t FILT_SEL_ACC_Z12  : 3;
+		uint16_t reserved         : 7;
+	} bits;
+
+	uint16_t value;
+};
+
+union CTRL_FILT_ACC3_Register {
+	struct {
+		uint16_t FILT_SEL_ACC_X3  : 3;
+		uint16_t FILT_SEL_ACC_Y3  : 3;
+		uint16_t FILT_SEL_ACC_Z3  : 3;
+		uint16_t reserved         : 7;
+	} bits;
+
+	uint16_t value;
+};
+
+union RATE_CTRL_Register {
+	struct {
+		uint16_t DEC_RATE_X2  : 3;
+		uint16_t DEC_RATE_Y2  : 3;
+		uint16_t DEC_RATE_Z2  : 3;
+		uint16_t DYN_RATE_XYZ2: 3;
+		uint16_t DYN_RATE_XYZ1: 3;
+		uint16_t reserved     : 1;
+	} bits;
+
+	uint16_t value;
+};
+
+union ACC12_CTRL_Register {
+	struct {
+		uint16_t DEC_ACC_X2  : 3;
+		uint16_t DEC_ACC_Y2  : 3;
+		uint16_t DEC_ACC_Z2  : 3;
+		uint16_t DYN_ACC_XYZ2: 3;
+		uint16_t DYN_ACC_XYZ1: 3;
+		uint16_t reserved     : 1;
+	} bits;
+
+	uint16_t value;
+};
+
+union ACC3_CTRL_Register {
+	struct {
+		uint16_t DYN_ACC_XYZ3  : 3;
+		uint16_t reserved     : 13;
+	} bits;
+
+	uint16_t value;
+};
+
 // Data registers
 #define RATE_X1         0x01 // 20 bit
 #define RATE_Y1         0x02 // 20 bit
@@ -44,6 +44,7 @@ static constexpr uint32_t POWER_ON_TIME = 250_ms;
 SCH16T::SCH16T(const I2CSPIDriverConfig &config) :
 	SPI(config),
 	I2CSPIDriver(config),
+	ModuleParams(nullptr),
 	_px4_accel(get_device_id(), config.rotation),
 	_px4_gyro(get_device_id(), config.rotation),
 	_drdy_gpio(config.drdy_gpio)
@@ -62,8 +63,11 @@ SCH16T::~SCH16T()
 	perf_free(_reset_perf);
 	perf_free(_bad_transfer_perf);
 	perf_free(_perf_crc_bad);
-	perf_free(_perf_frame_bad);
 	perf_free(_drdy_missed_perf);
+	perf_free(_perf_general_error);
+	perf_free(_perf_command_error);
+	perf_free(_perf_saturation_error);
+	perf_free(_perf_doing_initialization);
 }

 int SCH16T::init()
@@ -104,9 +108,7 @@ int SCH16T::probe()
 	PX4_INFO("COMP_ID:\t 0x%0x", comp_id);
 	PX4_INFO("ASIC_ID:\t 0x%0x", asic_id);

-	// SCH16T-K01 	- 	ID hex = 0x0020
-	// SCH1633-B13 	- 	ID hex = 0x0017
-	bool success = asic_id == 0x20 && comp_id == 0x17;
+	bool success = asic_id == 0x21 && comp_id == 0x23;

 	return success ? PX4_OK : PX4_ERROR;
 }
@@ -145,8 +147,11 @@ void SCH16T::print_status()
 	perf_print_counter(_reset_perf);
 	perf_print_counter(_bad_transfer_perf);
 	perf_print_counter(_perf_crc_bad);
-	perf_print_counter(_perf_frame_bad);
 	perf_print_counter(_drdy_missed_perf);
+	perf_print_counter(_perf_general_error);
+	perf_print_counter(_perf_command_error);
+	perf_print_counter(_perf_saturation_error);
+	perf_print_counter(_perf_doing_initialization);
 }

 void SCH16T::RunImpl()
@@ -186,6 +191,7 @@ void SCH16T::RunImpl()
 		}

 	case STATE::CONFIGURE: {
+			ConfigurationFromParameters();
 			Configure();

 			_state = STATE::LOCK_CONFIGURATION;
@@ -215,7 +221,7 @@ void SCH16T::RunImpl()
 					ScheduleDelayed(100_ms); // backup schedule as a watchdog timeout

 				} else {
-					ScheduleOnInterval(SAMPLE_INTERVAL_US, SAMPLE_INTERVAL_US);
+					ScheduleOnInterval(_sample_interval_us, _sample_interval_us);
 				}

 			} else {
@@ -233,7 +239,7 @@ void SCH16T::RunImpl()
 				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
 				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);

-				if ((now - drdy_timestamp_sample) < SAMPLE_INTERVAL_US) {
+				if ((now - drdy_timestamp_sample) < _sample_interval_us) {
 					timestamp_sample = drdy_timestamp_sample;

 				} else {
@@ -241,7 +247,7 @@ void SCH16T::RunImpl()
 				}

 				// push backup schedule back
-				ScheduleDelayed(SAMPLE_INTERVAL_US * 2);
+				ScheduleDelayed(_sample_interval_us * 2);
 			}

 			// Collect the data
@@ -279,48 +285,51 @@ void SCH16T::RunImpl()

 bool SCH16T::ReadData(SensorData *data)
 {
-	uint64_t temp = 0;
-	uint64_t gyro_x = 0;
-	uint64_t gyro_y = 0;
-	uint64_t gyro_z = 0;
-	uint64_t acc_x = 0;
-	uint64_t acc_y = 0;
-	uint64_t acc_z = 0;
+	// Register reads return 48bits. See SafeSpi 48bit out-of-frame protocol.
+	RegisterRead(RATE_X2);
+	uint64_t gyro_x = RegisterRead(RATE_Y2);
+	uint64_t gyro_y = RegisterRead(RATE_Z2);
+	uint64_t gyro_z = RegisterRead(ACC_X2);
+	uint64_t acc_x  = RegisterRead(ACC_Y2);
+	uint64_t acc_y  = RegisterRead(ACC_Z2);
+	uint64_t acc_z  = RegisterRead(TEMP);
+	uint64_t temp   = RegisterRead(TEMP);

-	// Data registers are 20bit 2s complement
-	RegisterRead(TEMP);
-	temp   = RegisterRead(STAT_SUM_SAT);
-	_sensor_status.saturation = SPI48_DATA_UINT16(RegisterRead(RATE_X2));
-	gyro_x = RegisterRead(RATE_Y2);
-	gyro_y = RegisterRead(RATE_Z2);
+	static constexpr uint64_t MASK48_GENERAL_ERROR = 	0b00000000'00010000'00000000'00000000'00000000'00000000;
+	static constexpr uint64_t MASK48_COMMAND_ERROR = 	0b00000000'00001000'00000000'00000000'00000000'00000000;
+	static constexpr uint64_t MASK48_SATURATION_ERROR = 0b00000000'00000100'00000000'00000000'00000000'00000000;
+	static constexpr uint64_t MASK48_DOING_INIT = 		0b00000000'00000110'00000000'00000000'00000000'00000000;

-	// Check if ACC2 is saturated, if so, use ACC3
-	if ((_sensor_status.saturation & STAT_SUM_SAT_ACC_X2) || (_sensor_status.saturation & STAT_SUM_SAT_ACC_Y2)
-	    || (_sensor_status.saturation & STAT_SUM_SAT_ACC_Z2)) {
-		gyro_z = RegisterRead(ACC_X3);
-		acc_x  = RegisterRead(ACC_Y3);
-		acc_y  = RegisterRead(ACC_Z3);
-		acc_z  = RegisterRead(TEMP);
-		_px4_accel.set_scale(1.f / 1600.f);
-		_px4_accel.set_range(260.f);
-
-	} else {
-		gyro_z = RegisterRead(ACC_X2);
-		acc_x  = RegisterRead(ACC_Y2);
-		acc_y  = RegisterRead(ACC_Z2);
-		acc_z  = RegisterRead(TEMP);
-		_px4_accel.set_scale(1.f / 3200.f);
-		_px4_accel.set_range(163.4f);
-	}
-
-	static constexpr uint64_t MASK48_ERROR = 0x001E00000000UL;
 	uint64_t values[] = { gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z, temp };

 	for (auto v : values) {
-		// Check for frame errors
-		if (v & MASK48_ERROR) {
-			perf_count(_perf_frame_bad);
+		// [1b ][1b][ 2b ]
+		// [IDS][CE][S1:0]
+		// IDS: Internal Data Status indication. SCH16T uses this field to indicate common cause error. This is redundant, more accurate info
+		// 		is seen from sensor status (S1:S0).
+		// CE: Command Error indication. SCH16T reports only semantically invalid frame content using this field. SPI protocol
+		// 		level errors are indicated with High-Z on MISO pin.
+		// S1,0: Sensor status indication.
+		// 		00: Normal operation
+		// 		01: Error status
+		// 		10: Saturation error
+		// 		11: Initialization running
+
+		if (v & MASK48_GENERAL_ERROR) {
+			perf_count(_perf_general_error);
 			return false;
+
+		} else if (v & MASK48_COMMAND_ERROR) {
+			perf_count(_perf_command_error);
+			return false;
+
+		} else if ((v & MASK48_DOING_INIT) == MASK48_DOING_INIT) {
+			perf_count(_perf_doing_initialization);
+			return false;
+
+		} else if (v & MASK48_SATURATION_ERROR) {
+			perf_count(_perf_saturation_error);
+			// Don't consider saturation an error
 		}

 		// Validate the CRC
@@ -339,7 +348,6 @@ bool SCH16T::ReadData(SensorData *data)
 	data->gyro_z   = SPI48_DATA_INT32(gyro_z);
 	// Temperature data is always 16 bits wide. Drop 4 LSBs as they are not used.
 	data->temp 	  = SPI48_DATA_INT32(temp) >> 4;
-
 	// Conver to PX4 coordinate system (FLU to FRD)
 	data->acc_x = data->acc_x;
 	data->acc_y = -data->acc_y;
@@ -347,10 +355,210 @@ bool SCH16T::ReadData(SensorData *data)
 	data->gyro_x = data->gyro_x;
 	data->gyro_y = -data->gyro_y;
 	data->gyro_z = -data->gyro_z;
-
 	return true;
 }

+void SCH16T::ConfigurationFromParameters()
+{
+	// NOTE: We use ACC2 and RATE2 which are both decimated without interpolation
+	CTRL_FILT_RATE_Register		filt_rate;
+	CTRL_FILT_ACC12_Register	filt_acc12;
+	CTRL_FILT_ACC3_Register 	filt_acc3;
+	RATE_CTRL_Register			rate_ctrl;
+	ACC12_CTRL_Register 		acc12_ctrl;
+	ACC3_CTRL_Register 			acc3_ctrl;
+
+	// We always use the maximum dynamic range for gyro and accel
+	rate_ctrl.bits.DYN_RATE_XYZ1 = 	RATE_RANGE_300;
+	rate_ctrl.bits.DYN_RATE_XYZ2 = 	RATE_RANGE_300;
+	acc12_ctrl.bits.DYN_ACC_XYZ1 = 	ACC12_RANGE_80;
+	acc12_ctrl.bits.DYN_ACC_XYZ2 = 	ACC12_RANGE_80;
+	acc3_ctrl.bits.DYN_ACC_XYZ3 = 	ACC3_RANGE_260;
+
+	_px4_gyro.set_range(math::radians(327.5f)); 		// 327.5 °/sec
+	_px4_gyro.set_scale(math::radians(1.f / 1600.f)); 	// 1600 LSB/(°/sec)
+	_px4_accel.set_range(163.4f); 		// 163.4 m/s2
+	_px4_accel.set_scale(1.f / 3200.f); // 3200 LSB/(m/s2)
+
+	// Gyro filter
+	switch (_sch16t_gyro_filt.get()) {
+	case 0:
+		filt_rate.bits.FILT_SEL_RATE_X = FILTER_13_HZ;
+		filt_rate.bits.FILT_SEL_RATE_Y = FILTER_13_HZ;
+		filt_rate.bits.FILT_SEL_RATE_Z = FILTER_13_HZ;
+		break;
+
+	case 1:
+		filt_rate.bits.FILT_SEL_RATE_X = FILTER_30_HZ;
+		filt_rate.bits.FILT_SEL_RATE_Y = FILTER_30_HZ;
+		filt_rate.bits.FILT_SEL_RATE_Z = FILTER_30_HZ;
+		break;
+
+	case 2:
+		filt_rate.bits.FILT_SEL_RATE_X = FILTER_68_HZ;
+		filt_rate.bits.FILT_SEL_RATE_Y = FILTER_68_HZ;
+		filt_rate.bits.FILT_SEL_RATE_Z = FILTER_68_HZ;
+		break;
+
+	case 3:
+		filt_rate.bits.FILT_SEL_RATE_X = FILTER_235_HZ;
+		filt_rate.bits.FILT_SEL_RATE_Y = FILTER_235_HZ;
+		filt_rate.bits.FILT_SEL_RATE_Z = FILTER_235_HZ;
+		break;
+
+	case 4:
+		filt_rate.bits.FILT_SEL_RATE_X = FILTER_280_HZ;
+		filt_rate.bits.FILT_SEL_RATE_Y = FILTER_280_HZ;
+		filt_rate.bits.FILT_SEL_RATE_Z = FILTER_280_HZ;
+		break;
+
+	case 5:
+		filt_rate.bits.FILT_SEL_RATE_X = FILTER_370_HZ;
+		filt_rate.bits.FILT_SEL_RATE_Y = FILTER_370_HZ;
+		filt_rate.bits.FILT_SEL_RATE_Z = FILTER_370_HZ;
+		break;
+
+	case 6:
+		filt_rate.bits.FILT_SEL_RATE_X = FILTER_BYPASS;
+		filt_rate.bits.FILT_SEL_RATE_Y = FILTER_BYPASS;
+		filt_rate.bits.FILT_SEL_RATE_Z = FILTER_BYPASS;
+		break;
+	}
+
+	// ACC12 / ACC3 filter
+	switch (_sch16t_acc_filt.get()) {
+	case 0:
+		filt_acc12.bits.FILT_SEL_ACC_X12 = FILTER_13_HZ;
+		filt_acc12.bits.FILT_SEL_ACC_Y12 = FILTER_13_HZ;
+		filt_acc12.bits.FILT_SEL_ACC_Z12 = FILTER_13_HZ;
+
+		filt_acc3.bits.FILT_SEL_ACC_X3 = FILTER_13_HZ;
+		filt_acc3.bits.FILT_SEL_ACC_Y3 = FILTER_13_HZ;
+		filt_acc3.bits.FILT_SEL_ACC_Z3 = FILTER_13_HZ;
+		break;
+
+	case 1:
+		filt_acc12.bits.FILT_SEL_ACC_X12 = FILTER_30_HZ;
+		filt_acc12.bits.FILT_SEL_ACC_Y12 = FILTER_30_HZ;
+		filt_acc12.bits.FILT_SEL_ACC_Z12 = FILTER_30_HZ;
+
+		filt_acc3.bits.FILT_SEL_ACC_X3 = FILTER_30_HZ;
+		filt_acc3.bits.FILT_SEL_ACC_Y3 = FILTER_30_HZ;
+		filt_acc3.bits.FILT_SEL_ACC_Z3 = FILTER_30_HZ;
+		break;
+
+	case 2:
+		filt_acc12.bits.FILT_SEL_ACC_X12 = FILTER_68_HZ;
+		filt_acc12.bits.FILT_SEL_ACC_Y12 = FILTER_68_HZ;
+		filt_acc12.bits.FILT_SEL_ACC_Z12 = FILTER_68_HZ;
+
+		filt_acc3.bits.FILT_SEL_ACC_X3 = FILTER_68_HZ;
+		filt_acc3.bits.FILT_SEL_ACC_Y3 = FILTER_68_HZ;
+		filt_acc3.bits.FILT_SEL_ACC_Z3 = FILTER_68_HZ;
+		break;
+
+	case 3:
+		filt_acc12.bits.FILT_SEL_ACC_X12 = FILTER_235_HZ;
+		filt_acc12.bits.FILT_SEL_ACC_Y12 = FILTER_235_HZ;
+		filt_acc12.bits.FILT_SEL_ACC_Z12 = FILTER_235_HZ;
+
+		filt_acc3.bits.FILT_SEL_ACC_X3 = FILTER_235_HZ;
+		filt_acc3.bits.FILT_SEL_ACC_Y3 = FILTER_235_HZ;
+		filt_acc3.bits.FILT_SEL_ACC_Z3 = FILTER_235_HZ;
+		break;
+
+	case 4:
+		filt_acc12.bits.FILT_SEL_ACC_X12 = FILTER_280_HZ;
+		filt_acc12.bits.FILT_SEL_ACC_Y12 = FILTER_280_HZ;
+		filt_acc12.bits.FILT_SEL_ACC_Z12 = FILTER_280_HZ;
+
+		filt_acc3.bits.FILT_SEL_ACC_X3 = FILTER_280_HZ;
+		filt_acc3.bits.FILT_SEL_ACC_Y3 = FILTER_280_HZ;
+		filt_acc3.bits.FILT_SEL_ACC_Z3 = FILTER_280_HZ;
+		break;
+
+	case 5:
+		filt_acc12.bits.FILT_SEL_ACC_X12 = FILTER_370_HZ;
+		filt_acc12.bits.FILT_SEL_ACC_Y12 = FILTER_370_HZ;
+		filt_acc12.bits.FILT_SEL_ACC_Z12 = FILTER_370_HZ;
+
+		filt_acc3.bits.FILT_SEL_ACC_X3 = FILTER_370_HZ;
+		filt_acc3.bits.FILT_SEL_ACC_Y3 = FILTER_370_HZ;
+		filt_acc3.bits.FILT_SEL_ACC_Z3 = FILTER_370_HZ;
+		break;
+
+	case 6:
+		filt_acc12.bits.FILT_SEL_ACC_X12 = FILTER_BYPASS;
+		filt_acc12.bits.FILT_SEL_ACC_Y12 = FILTER_BYPASS;
+		filt_acc12.bits.FILT_SEL_ACC_Z12 = FILTER_BYPASS;
+
+		filt_acc3.bits.FILT_SEL_ACC_X3 = FILTER_BYPASS;
+		filt_acc3.bits.FILT_SEL_ACC_Y3 = FILTER_BYPASS;
+		filt_acc3.bits.FILT_SEL_ACC_Z3 = FILTER_BYPASS;
+		break;
+	}
+
+	// Gyro decimation (only affects channel 2, ie RATE2)
+	switch (_sch16t_decim.get()) {
+	case 0:
+		_sample_interval_us = 85;
+		rate_ctrl.bits.DEC_RATE_X2 = DECIMATION_NONE;
+		rate_ctrl.bits.DEC_RATE_Y2 = DECIMATION_NONE;
+		rate_ctrl.bits.DEC_RATE_Z2 = DECIMATION_NONE;
+		acc12_ctrl.bits.DEC_ACC_X2 = DECIMATION_NONE;
+		acc12_ctrl.bits.DEC_ACC_Y2 = DECIMATION_NONE;
+		acc12_ctrl.bits.DEC_ACC_Z2 = DECIMATION_NONE;
+		break;
+
+	case 1:
+		_sample_interval_us = 169;
+		rate_ctrl.bits.DEC_RATE_X2 = DECIMATION_5900_HZ;
+		rate_ctrl.bits.DEC_RATE_Y2 = DECIMATION_5900_HZ;
+		rate_ctrl.bits.DEC_RATE_Z2 = DECIMATION_5900_HZ;
+		acc12_ctrl.bits.DEC_ACC_X2 = DECIMATION_5900_HZ;
+		acc12_ctrl.bits.DEC_ACC_Y2 = DECIMATION_5900_HZ;
+		acc12_ctrl.bits.DEC_ACC_Z2 = DECIMATION_5900_HZ;
+		break;
+
+	case 2:
+		_sample_interval_us = 338;
+		rate_ctrl.bits.DEC_RATE_X2 = DECIMATION_2950_HZ;
+		rate_ctrl.bits.DEC_RATE_Y2 = DECIMATION_2950_HZ;
+		rate_ctrl.bits.DEC_RATE_Z2 = DECIMATION_2950_HZ;
+		acc12_ctrl.bits.DEC_ACC_X2 = DECIMATION_2950_HZ;
+		acc12_ctrl.bits.DEC_ACC_Y2 = DECIMATION_2950_HZ;
+		acc12_ctrl.bits.DEC_ACC_Z2 = DECIMATION_2950_HZ;
+		break;
+
+	case 3:
+		_sample_interval_us = 678;
+		rate_ctrl.bits.DEC_RATE_X2 = DECIMATION_1475_HZ;
+		rate_ctrl.bits.DEC_RATE_Y2 = DECIMATION_1475_HZ;
+		rate_ctrl.bits.DEC_RATE_Z2 = DECIMATION_1475_HZ;
+		acc12_ctrl.bits.DEC_ACC_X2 = DECIMATION_1475_HZ;
+		acc12_ctrl.bits.DEC_ACC_Y2 = DECIMATION_1475_HZ;
+		acc12_ctrl.bits.DEC_ACC_Z2 = DECIMATION_1475_HZ;
+		break;
+
+	case 4:
+		_sample_interval_us = 1355;
+		rate_ctrl.bits.DEC_RATE_X2 = DECIMATION_738_HZ;
+		rate_ctrl.bits.DEC_RATE_Y2 = DECIMATION_738_HZ;
+		rate_ctrl.bits.DEC_RATE_Z2 = DECIMATION_738_HZ;
+		acc12_ctrl.bits.DEC_ACC_X2 = DECIMATION_738_HZ;
+		acc12_ctrl.bits.DEC_ACC_Y2 = DECIMATION_738_HZ;
+		acc12_ctrl.bits.DEC_ACC_Z2 = DECIMATION_738_HZ;
+		break;
+	}
+
+	_registers[0] = RegisterConfig(CTRL_FILT_RATE,  filt_rate.value);
+	_registers[1] = RegisterConfig(CTRL_FILT_ACC12, filt_acc12.value);
+	_registers[2] = RegisterConfig(CTRL_FILT_ACC3,  filt_acc3.value);
+	_registers[3] = RegisterConfig(CTRL_RATE,       rate_ctrl.value);
+	_registers[4] = RegisterConfig(CTRL_ACC12,      acc12_ctrl.value);
+	_registers[5] = RegisterConfig(CTRL_ACC3,       acc3_ctrl.value);
+}
+
 void SCH16T::Configure()
 {
 	for (auto &r : _registers) {
@@ -359,15 +567,6 @@ void SCH16T::Configure()

 	RegisterWrite(CTRL_USER_IF, DRY_DRV_EN); // Enable data ready
 	RegisterWrite(CTRL_MODE, EN_SENSOR); // Enable the sensor
-
-	// NOTE: we use ACC3 for the higher range. The DRDY frequency adjusts to whichever register bank is
-	// being sampled from (decimated vs interpolated outputs). RATE_XYZ2 is decimated and RATE_XYZ1 is interpolated.
-	_px4_gyro.set_range(math::radians(327.68f)); // +-/ 300°/sec calibrated range, 327.68°/sec electrical headroom (20bit)
-	_px4_gyro.set_scale(math::radians(1.f / 1600.f)); // scaling 1600 LSB/°/sec -> rad/s per LSB
-
-	// ACC12 range is 163.4 m/s^2, 3200 LSB/(m/s^2), ACC3 range is 260 m/s^2, 1600 LSB/(m/s^2)
-	_px4_accel.set_range(163.4f);
-	_px4_accel.set_scale(1.f / 3200.f);
 }

 bool SCH16T::ValidateRegisterConfiguration()
@@ -449,8 +648,6 @@ void SCH16T::RegisterWrite(uint8_t addr, uint16_t value)
 // The SPI protocol (SafeSPI) is 48bit out-of-frame. This means read return frames will be received on the next transfer.
 uint64_t SCH16T::TransferSpiFrame(uint64_t frame)
 {
-	set_frequency(SPI_SPEED);
-
 	uint16_t buf[3];

 	for (int index = 0; index < 3; index++) {
@@ -34,14 +34,14 @@
 #pragma once

 #include "Murata_SCH16T_registers.hpp"
-
+#include <px4_platform_common/module_params.h>
 #include <px4_platform_common/i2c_spi_buses.h>
 #include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
 #include <lib/drivers/gyroscope/PX4Gyroscope.hpp>

 using namespace Murata_SCH16T;

-class SCH16T : public device::SPI, public I2CSPIDriver<SCH16T>
+class SCH16T : public device::SPI, public I2CSPIDriver<SCH16T>, public ModuleParams
 {
 public:
 	SCH16T(const I2CSPIDriverConfig &config);
@@ -79,7 +79,7 @@ private:
 	};

 	struct RegisterConfig {
-		RegisterConfig(uint16_t a, uint16_t v)
+		RegisterConfig(uint16_t a = 0, uint16_t v = 0)
 			: addr(a)
 			, value(v)
 		{};
@@ -100,6 +100,8 @@ private:
 	void ReadStatusRegisters();

 	void Configure();
+	void ConfigurationFromParameters();
+
 	void SoftwareReset();

 	void RegisterWrite(uint8_t addr, uint16_t value);
@@ -131,19 +133,22 @@ private:
 		READ,
 	} _state{STATE::RESET_INIT};

-	RegisterConfig _registers[6] = {
-		RegisterConfig(CTRL_FILT_RATE,  FILTER_BYPASS),         // Bypass filter
-		RegisterConfig(CTRL_FILT_ACC12, FILTER_BYPASS),         // Bypass filter
-		RegisterConfig(CTRL_FILT_ACC3,  FILTER_BYPASS),         // Bypass filter
-		RegisterConfig(CTRL_RATE,       RATE_300DPS_1475HZ),    // +/- 300 deg/s, 1600 LSB/(deg/s) -- default, Decimation 8, 1475Hz
-		RegisterConfig(CTRL_ACC12,      ACC12_8G_1475HZ),       // +/- 80 m/s^2, 3200 LSB/(m/s^2) -- default, Decimation 8, 1475Hz
-		RegisterConfig(CTRL_ACC3,       ACC3_26G)               // +/- 260 m/s^2, 1600 LSB/(m/s^2) -- default
-	};
+	RegisterConfig _registers[6];
+
+	uint32_t _sample_interval_us = 678;

 	perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")};
 	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")};
 	perf_counter_t _perf_crc_bad{perf_counter_t(perf_alloc(PC_COUNT, MODULE_NAME": CRC8 bad"))};
-	perf_counter_t _perf_frame_bad{perf_counter_t(perf_alloc(PC_COUNT, MODULE_NAME": Frame bad"))};
 	perf_counter_t _drdy_missed_perf{nullptr};
+	perf_counter_t _perf_general_error{perf_counter_t(perf_alloc(PC_COUNT, MODULE_NAME": general error"))};
+	perf_counter_t _perf_command_error{perf_counter_t(perf_alloc(PC_COUNT, MODULE_NAME": command error"))};
+	perf_counter_t _perf_saturation_error{perf_counter_t(perf_alloc(PC_COUNT, MODULE_NAME": saturation error"))};
+	perf_counter_t _perf_doing_initialization{perf_counter_t(perf_alloc(PC_COUNT, MODULE_NAME": re-initializing"))};

+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::SCH16T_GYRO_FILT>) _sch16t_gyro_filt,
+		(ParamInt<px4::params::SCH16T_ACC_FILT>) _sch16t_acc_filt,
+		(ParamInt<px4::params::SCH16T_DECIM>) _sch16t_decim
+	)
 };
@@ -42,3 +42,49 @@
 * @value 1 Enabled
  */
 PARAM_DEFINE_INT32(SENS_EN_SCH16T, 0);
+
+/**
+ * Gyro filter settings
+ *
+ * @value 0 13 Hz
+ * @value 1 30 Hz
+ * @value 2 68 Hz
+ * @value 3 235 Hz
+ * @value 4 280 Hz
+ * @value 5 370 Hz
+ * @value 6 No filter
+ *
+ * @reboot_required true
+ *
+ */
+PARAM_DEFINE_INT32(SCH16T_GYRO_FILT, 2);
+
+/**
+ * Accel filter settings
+ *
+ * @value 0 13 Hz
+ * @value 1 30 Hz
+ * @value 2 68 Hz
+ * @value 3 235 Hz
+ * @value 4 280 Hz
+ * @value 5 370 Hz
+ * @value 6 No filter
+ *
+ * @reboot_required true
+ *
+ */
+PARAM_DEFINE_INT32(SCH16T_ACC_FILT, 6);
+
+/**
+ * Gyro and Accel decimation settings
+ *
+ * @value 0 None
+ * @value 1 5900 Hz
+ * @value 2 2950 Hz
+ * @value 3 1475 Hz
+ * @value 4 738 Hz
+ *
+ * @reboot_required true
+ *
+ */
+PARAM_DEFINE_INT32(SCH16T_DECIM, 4);
@@ -50,7 +50,7 @@ extern "C" int sch16t_main(int argc, char *argv[])
 	int ch;
 	using ThisDriver = SCH16T;
 	BusCLIArguments cli{false, true};
-	cli.default_spi_frequency = SPI_SPEED;
+	cli.default_spi_frequency = 5000000;
 	cli.spi_mode = SPIDEV_MODE0;

 	while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) {
@@ -314,7 +314,7 @@ void  RCInput::swap_rx_tx()
 #  ifdef TIOCSSINGLEWIRE

 	if (rv != OK) {
-		ioctl(_rcs_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED | SER_SINGLEWIRE_PUSHPULL);
+		ioctl(_rcs_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED);
 	}

 #  else
@@ -66,15 +66,12 @@ void FakeMagnetometer::Run()
 		if (_vehicle_gps_position_sub.copy(&gps)) {
 			if (gps.eph < 1000) {

-				const double lat = gps.latitude_deg;
-				const double lon = gps.longitude_deg;
-
 				// magnetic field data returned by the geo library using the current GPS position
-				const float mag_declination_gps = get_mag_declination_radians(lat, lon);
-				const float mag_inclination_gps = get_mag_inclination_radians(lat, lon);
-				const float mag_strength_gps = get_mag_strength_gauss(lat, lon);
+				const float declination_rad = math::radians(get_mag_declination_degrees(gps.latitude_deg, gps.longitude_deg));
+				const float inclination_rad = math::radians(get_mag_inclination_degrees(gps.latitude_deg, gps.longitude_deg));
+				const float field_strength_gauss = get_mag_strength_gauss(gps.latitude_deg, gps.longitude_deg);

-				_mag_earth_pred = Dcmf(Eulerf(0, -mag_inclination_gps, mag_declination_gps)) * Vector3f(mag_strength_gps, 0, 0);
+				_mag_earth_pred = Dcmf(Eulerf(0, -inclination_rad, declination_rad)) * Vector3f(field_strength_gauss, 0, 0);

 				_mag_earth_available = true;
 			}
@@ -305,19 +305,27 @@ void Battery::computeScale()
 float Battery::computeRemainingTime(float current_a)
 {
 	float time_remaining_s = NAN;
+	bool reset_current_avg_filter = false;

 	if (_vehicle_status_sub.updated()) {
 		vehicle_status_s vehicle_status;

 		if (_vehicle_status_sub.copy(&vehicle_status)) {
 			_armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
+
+			if (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && !_vehicle_status_is_fw) {
+				reset_current_avg_filter = true;
+			}
+
 			_vehicle_status_is_fw = (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING);
 		}
 	}

 	_flight_phase_estimation_sub.update();

-	if (!PX4_ISFINITE(_current_average_filter_a.getState()) || _current_average_filter_a.getState() < FLT_EPSILON) {
+	// reset filter if not feasible, negative or we did a VTOL transition to FW mode
+	if (!PX4_ISFINITE(_current_average_filter_a.getState()) || _current_average_filter_a.getState() < FLT_EPSILON
+	    || reset_current_avg_filter) {
 		_current_average_filter_a.reset(_params.bat_avrg_current);
 	}

@@ -387,7 +387,7 @@ public:
 		}
 	}

-	void print(float eps = 1e-9) const
+	void print_symmetric(float eps = 1e-9) const
 	{
 		// print column numbering
 		if (N > 1) {
@@ -435,6 +435,45 @@ public:
 		}
 	}

+	void print(float eps = 1e-9) const
+	{
+		// print column numbering
+		if (N > 1) {
+			printf("  ");
+
+			for (unsigned i = 0; i < N; i++) {
+				printf("|%2u      ", i);
+
+			}
+
+			printf("\n");
+		}
+
+		const Matrix<Type, M, N> &self = *this;
+
+		for (unsigned i = 0; i < M; i++) {
+			printf("%2u|", i); // print row numbering
+
+			for (unsigned j = 0; j < N; j++) {
+				double d = static_cast<double>(self(i, j));
+
+				// avoid -0.0 for display
+				if (fabs(d - 0.0) < (double)eps) {
+					// print fixed width zero
+					printf(" 0       ");
+
+				} else if ((fabs(d) < 1e-4) || (fabs(d) >= 10.0)) {
+					printf("% .1e ", d);
+
+				} else {
+					printf("% 6.5f ", d);
+				}
+			}
+
+			printf("\n");
+		}
+	}
+
 	Matrix<Type, N, M> transpose() const
 	{
 		Matrix<Type, N, M> res;
@@ -351,6 +351,21 @@ public:
 		return min_val;
 	}

+	// sum of absolute values
+	Type sum_abs() const
+	{
+		const SliceT<MatrixT, Type, P, Q, M, N> &self = *this;
+		Type accum(0);
+
+		for (size_t i = 0; i < P; i++) {
+			for (size_t j = 0; j < Q; j++) {
+				accum += std::abs(self(i, j));
+			}
+		}
+
+		return accum;
+	}
+
 private:
 	size_t _x0, _y0;
 	MatrixT *_data;
@@ -417,6 +417,28 @@ bool MixingOutput::update()
 		_throttle_armed = (_armed.armed && !_armed.lockdown) || _armed.in_esc_calibration_mode;
 	}

+	manual_control_setpoint_s manual_control_setpoint;
+
+	if (_manual_control_sp_sub.update(&manual_control_setpoint)) {
+
+		switch (_param_out_srv_fail_ipt.get()) {
+		case 0:
+			// disable
+			_servo_locking_injection_active = false;
+			break;
+
+		case 1:
+			// yaw stick above 60% to enable
+			_servo_locking_injection_active = manual_control_setpoint.yaw > 0.6f;
+			break;
+
+		case 2:
+			// aux1 above 60% to enable
+			_servo_locking_injection_active = manual_control_setpoint.aux1 > 0.6f;
+			break;
+		}
+	}
+
 	// only used for sitl with lockstep
 	bool has_updates = _subscription_callback && _subscription_callback->updated();

@@ -442,7 +464,21 @@ bool MixingOutput::update()
 			all_disabled = false;

 			if (_armed.armed || (_armed.prearmed && _functions[i]->allowPrearmControl())) {
-				outputs[i] = _functions[i]->value(_function_assignment[i]);
+
+				float factor = 1.f;
+
+				if (_servo_locking_injection_active) {
+					if (_function_assignment[i] == OutputFunction::Servo1 && (_param_out_srv_fail_nr.get() == 0
+							|| _param_out_srv_fail_nr.get() == 2)) {
+						factor = 0.f;
+
+					} else if (_function_assignment[i] == OutputFunction::Servo2 && (_param_out_srv_fail_nr.get() == 1
+							|| _param_out_srv_fail_nr.get() == 2)) {
+						factor = 0.f;
+					}
+				}
+
+				outputs[i] = _functions[i]->value(_function_assignment[i]) * factor;

 			} else {
 				outputs[i] = NAN;
@@ -59,6 +59,7 @@
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/manual_control_setpoint.h>

 using namespace time_literals;

@@ -258,6 +259,9 @@ private:

 	uORB::Subscription _armed_sub{ORB_ID(actuator_armed)};

+	uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)};
+	bool _servo_locking_injection_active{false};
+
 	uORB::PublicationMulti<actuator_outputs_s> _outputs_pub{ORB_ID(actuator_outputs)};

 	actuator_armed_s _armed{};
@@ -296,6 +300,8 @@ private:
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::MC_AIRMODE>) _param_mc_airmode,   ///< multicopter air-mode
 		(ParamFloat<px4::params::MOT_SLEW_MAX>) _param_mot_slew_max,
-		(ParamFloat<px4::params::THR_MDL_FAC>) _param_thr_mdl_fac ///< thrust to motor control signal modelling factor
+		(ParamFloat<px4::params::THR_MDL_FAC>) _param_thr_mdl_fac, ///< thrust to motor control signal modelling factor
+		(ParamInt<px4::params::OUT_SRV_FAIL_IPT>) _param_out_srv_fail_ipt,
+		(ParamInt<px4::params::OUT_SRV_FAIL_NR>) _param_out_srv_fail_nr
 	)
 };
@@ -16,3 +16,23 @@
 * @group Mixer Output
 */
 PARAM_DEFINE_INT32(MC_AIRMODE, 0);
+
+/**
+ * Manual control source to inject servo failure
+ *
+ * @group Mixer Output
+ * @value 0 Disabled
+ * @value 1 yaw stick
+ * @value 2 aux1
+ */
+PARAM_DEFINE_INT32(OUT_SRV_FAIL_IPT, 0);
+
+/**
+ * Index of the servos to fail
+ *
+ * @group Mixer Output
+ * @value 0 Lock servo 1
+ * @value 1 Lock servo 2
+ * @value 2 Lock servo 1 and 2
+ */
+PARAM_DEFINE_INT32(OUT_SRV_FAIL_NR, 0);
@@ -52,6 +52,10 @@ void RateControl::setSaturationStatus(const Vector3<bool> &saturation_positive,
 {
 	_control_allocator_saturation_positive = saturation_positive;
 	_control_allocator_saturation_negative = saturation_negative;
+
+	const Vector3<bool> saturation_disabled(false, false, false);
+	_control_allocator_saturation_positive = saturation_disabled;
+	_control_allocator_saturation_negative = saturation_disabled;
 }

 void RateControl::setPositiveSaturationFlag(size_t axis, bool is_saturated)
@@ -1,7 +1,7 @@
 #!/usr/bin/env python3
 ############################################################################
 #
-#   Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2020-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
@@ -33,6 +33,7 @@
 ############################################################################

 import math
+import numpy
 import json
 import statistics
 import sys
@@ -49,7 +50,7 @@ def constrain(n, nmin, nmax):

 header = """/****************************************************************************
 *
- *   Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -80,6 +81,9 @@ header = """/*******************************************************************
 *
 ****************************************************************************/
 """
+
+key=sys.argv[1] # NOAA key (https://www.ngdc.noaa.gov/geomag/CalcSurvey.shtml)
+
 print(header)

 print('#include <stdint.h>\n')
@@ -97,188 +101,106 @@ print('static constexpr int LAT_DIM = {}'.format(LAT_DIM) + ';')
 print('static constexpr int LON_DIM = {}'.format(LON_DIM) + ';')
 print('\n')

-print('// *INDENT-OFF*')
+print('// *INDENT-OFF*\n\n\n')

-# Declination
-params = urllib.parse.urlencode({'lat1': 0, 'lat2': 0, 'lon1': 0, 'lon2': 0, 'latStepSize': 1, 'lonStepSize': 1, 'magneticComponent': 'd', 'resultFormat': 'json'})
-key=sys.argv[1] # NOAA key (https://www.ngdc.noaa.gov/geomag/CalcSurvey.shtml)
-f = urllib.request.urlopen("https://www.ngdc.noaa.gov/geomag-web/calculators/calculateIgrfgrid?key=%s&%s" % (key, params))
+
+
+# build the world magnetic model dictionary
+world_magnitude_model = {} # lat/lon dictionary with grid result
+
+params = urllib.parse.urlencode({'lat1': 0, 'lon1': 0, 'resultFormat': 'json'})
+f = urllib.request.urlopen("https://www.ngdc.noaa.gov/geomag-web/calculators/calculateIgrfwmm?key=%s&%s" % (key, params))
 data = json.loads(f.read())
-print("// Magnetic declination data in radians * 10^-4")
-print('// Model: {},'.format(data['model']))
-print('// Version: {},'.format(data['version']))
-print('// Date: {},'.format(data['result'][0]['date']))
-print('static constexpr const int16_t declination_table[{}][{}]'.format(LAT_DIM, LON_DIM) + " {")
-print('\t//    LONGITUDE: ', end='')
-for l in range(SAMPLING_MIN_LON, SAMPLING_MAX_LON+1, SAMPLING_RES):
-    print('{0:6d},'.format(l), end='')
-print('')

-declination_min = float('inf')
-declination_max = float('-inf')
-declination_min_lat_lon = ()
-declination_max_lat_lon = ()
+world_magnitude_model_units = data['units']

 for latitude in range(SAMPLING_MIN_LAT, SAMPLING_MAX_LAT+1, SAMPLING_RES):
-    params = urllib.parse.urlencode({'lat1': latitude, 'lat2': latitude, 'lon1': SAMPLING_MIN_LON, 'lon2': SAMPLING_MAX_LON, 'latStepSize': 1, 'lonStepSize': SAMPLING_RES, 'magneticComponent': 'd', 'resultFormat': 'json'})
-    f = urllib.request.urlopen("https://www.ngdc.noaa.gov/geomag-web/calculators/calculateIgrfgrid?key=%s&%s" % (key, params))
-    data = json.loads(f.read())
+    world_magnitude_model[latitude] = {}

-    latitude_blackout_zone = False
+    for longitude in range(SAMPLING_MIN_LON, SAMPLING_MAX_LON+1, SAMPLING_RES):
+        params = urllib.parse.urlencode({'lat1': latitude, 'lon1': longitude, 'lon2': SAMPLING_MAX_LON, 'resultFormat': 'json'})
+        f = urllib.request.urlopen("https://www.ngdc.noaa.gov/geomag-web/calculators/calculateIgrfwmm?key=%s&%s" % (key, params))
+        data = json.loads(f.read())
+        #print(json.dumps(data, indent = 4)) # debugging

-    print('\t/* LAT: {0:3d} */'.format(latitude) + ' { ', end='')
-    for p in data['result']:
-        declination_rad = math.radians(p['declination'])
-
-        warning = False
-
-        try:
-            if p['warning']:
-                warning = True
-                latitude_blackout_zone = True
-                #print("WARNING black out zone!", p)
-
-        except:
-            pass
-
-        # declination in radians * 10^-4
-        declination_int = constrain(int(round(declination_rad * 10000)), 32767, -32768)
-        print('{0:6d},'.format(declination_int), end='')
-
-        if (declination_rad > declination_max):
-            declination_max = declination_rad
-            declination_max_lat_lon = (p['latitude'], p['longitude'])
-
-        if (declination_rad < declination_min):
-            declination_min = declination_rad
-            declination_min_lat_lon = (p['latitude'], p['longitude'])
-
-    if latitude_blackout_zone:
-        print(' }, // WARNING! black out zone')
-    else:
-        print(' },')
-
-print("};")
-
-print('static constexpr float WMM_DECLINATION_MIN_RAD = {:.3f}; // latitude: {:.0f}, longitude: {:.0f}'.format(declination_min, declination_min_lat_lon[0], declination_min_lat_lon[1]))
-print('static constexpr float WMM_DECLINATION_MAX_RAD = {:.3f}; // latitude: {:.0f}, longitude: {:.0f}'.format(declination_max, declination_max_lat_lon[0], declination_max_lat_lon[1]))
-print("\n")
+        world_magnitude_model[latitude][longitude] = data['result'][0]
+        #print(world_magnitude_model[latitude][longitude])


-# Inclination
-params = urllib.parse.urlencode({'lat1': 0, 'lat2': 0, 'lon1': 0, 'lon2': 0, 'latStepSize': 1, 'lonStepSize': 1, 'magneticComponent': 'i', 'resultFormat': 'json'})
-f = urllib.request.urlopen("https://www.ngdc.noaa.gov/geomag-web/calculators/calculateIgrfgrid?key=%s&%s" % (key, params))
-data = json.loads(f.read())
-print("// Magnetic inclination data in radians * 10^-4")
-print('// Model: {},'.format(data['model']))
-print('// Version: {},'.format(data['version']))
-print('// Date: {},'.format(data['result'][0]['date']))
-print('static constexpr const int16_t inclination_table[{}][{}]'.format(LAT_DIM, LON_DIM) + " {")
-print('\t//    LONGITUDE: ', end='')
-for l in range(SAMPLING_MIN_LON, SAMPLING_MAX_LON+1, SAMPLING_RES):
-    print('{0:6d},'.format(l), end='')
-print('')
+def print_wmm_table(key_name):

-inclination_min = float('inf')
-inclination_max = float('-inf')
-inclination_min_lat_lon = ()
-inclination_max_lat_lon = ()
+    value_min = float('inf')
+    value_min_lat_lon = ()

-for latitude in range(SAMPLING_MIN_LAT, SAMPLING_MAX_LAT+1, SAMPLING_RES):
-    params = urllib.parse.urlencode({'lat1': latitude, 'lat2': latitude, 'lon1': SAMPLING_MIN_LON, 'lon2': SAMPLING_MAX_LON, 'latStepSize': 1, 'lonStepSize': SAMPLING_RES, 'magneticComponent': 'i', 'resultFormat': 'json'})
-    f = urllib.request.urlopen("https://www.ngdc.noaa.gov/geomag-web/calculators/calculateIgrfgrid?key=%s&%s" % (key, params))
-    data = json.loads(f.read())
+    value_max = float('-inf')
+    value_max_lat_lon = ()

-    latitude_blackout_zone = False
+    for latitude, lat_row in world_magnitude_model.items():
+        #print(latitude, lat_row)
+        for longitude, result in lat_row.items():
+            #print(result)

-    print('\t/* LAT: {0:3d} */'.format(latitude) + ' { ', end='')
-    for p in data['result']:
-        inclination_rad = math.radians(p['inclination'])
+            value = float(result[key_name])

-        warning = False
+            if (value > value_max):
+                value_max = value
+                value_max_lat_lon = (latitude, longitude)

-        try:
-            if p['warning']:
-                warning = True
-                latitude_blackout_zone = True
-                #print("WARNING black out zone!", p)
+            if (value < value_min):
+                value_min = value
+                value_min_lat_lon = (latitude, longitude)

-        except:
-            pass
+    # scale the values to fit into int16_t
+    value_scale_max = abs(numpy.iinfo(numpy.int16).max) / abs(value_max)
+    value_scale_min = abs(numpy.iinfo(numpy.int16).min) / abs(value_min)
+    value_scale = min(value_scale_max, value_scale_min)

-        # inclination in radians * 10^-4
-        inclination_int = constrain(int(round(inclination_rad * 10000)), 32767, -32768)
-        print('{0:6d},'.format(inclination_int), end='')
+    units_str = world_magnitude_model_units[key_name].split(' ')[0]

-        if (inclination_rad > inclination_max):
-            inclination_max = inclination_rad
-            inclination_max_lat_lon = (p['latitude'], p['longitude'])
+    # print the table
+    print('// Magnetic {} data in {:.4g} {}'.format(key_name, 1.0 / value_scale, units_str))
+    print('// Model: {},'.format(data['model']))
+    print('// Version: {},'.format(data['version']))
+    print('// Date: {},'.format(data['result'][0]['date']))
+    print('static constexpr const int16_t {}_table[{}][{}]'.format(key_name, LAT_DIM, LON_DIM) + " {")
+    print('\t//    LONGITUDE: ', end='')
+    for l in range(SAMPLING_MIN_LON, SAMPLING_MAX_LON+1, SAMPLING_RES):
+        print('{0:6d},'.format(l), end='')
+    print('')

-        if (inclination_rad < inclination_min):
-            inclination_min = inclination_rad
-            inclination_min_lat_lon = (p['latitude'], p['longitude'])
+    for latitude, lat_row in world_magnitude_model.items():
+        print('\t/* LAT: {0:3d} */'.format(latitude) + ' { ', end='')
+        latitude_blackout_zone = False

-    if latitude_blackout_zone:
-        print(' }, // WARNING! black out zone')
-    else:
-        print(' },')
+        for longitude, result in lat_row.items():

-print("};")
+            value = float(result[key_name])

-print('static constexpr float WMM_INCLINATION_MIN_RAD = {:.3f}; // latitude: {:.0f}, longitude: {:.0f}'.format(inclination_min, inclination_min_lat_lon[0], inclination_min_lat_lon[1]))
-print('static constexpr float WMM_INCLINATION_MAX_RAD = {:.3f}; // latitude: {:.0f}, longitude: {:.0f}'.format(inclination_max, inclination_max_lat_lon[0], inclination_max_lat_lon[1]))
-print("\n")
+            # value scaled to fit into int16_t
+            value_int = int(round(value * value_scale))
+            print('{0:6d},'.format(value_int), end='')

-# total intensity
-params = urllib.parse.urlencode({'lat1': 0, 'lat2': 0, 'lon1': 0, 'lon2': 0, 'latStepSize': 1, 'lonStepSize': 1, 'magneticComponent': 'i', 'resultFormat': 'json'})
-f = urllib.request.urlopen("https://www.ngdc.noaa.gov/geomag-web/calculators/calculateIgrfgrid?key=%s&%s" % (key, params))
-data = json.loads(f.read())
-print("// Magnetic strength data in milli-Gauss * 10")
-print('// Model: {},'.format(data['model']))
-print('// Version: {},'.format(data['version']))
-print('// Date: {},'.format(data['result'][0]['date']))
-print('static constexpr const int16_t strength_table[{}][{}]'.format(LAT_DIM, LON_DIM) + " {")
-print('\t//    LONGITUDE: ', end='')
-for l in range(SAMPLING_MIN_LON, SAMPLING_MAX_LON+1, SAMPLING_RES):
-    print('{0:5d},'.format(l), end='')
-print('')
+            # blackout warning at this latitude
+            try:
+                if result['warning']:
+                    latitude_blackout_zone = True

-strength_min = float('inf')
-strength_max = 0
-strength_min_lat_lon = ()
-strength_max_lat_lon = ()
-strength_sum = 0
-strength_sum_count = 0
+            except:
+                pass

-strength_list = []
+        if latitude_blackout_zone:
+            print(' }, // WARNING! black out zone')
+        else:
+            print(' },')

-for latitude in range(SAMPLING_MIN_LAT, SAMPLING_MAX_LAT+1, SAMPLING_RES):
-    params = urllib.parse.urlencode({'lat1': latitude, 'lat2': latitude, 'lon1': SAMPLING_MIN_LON, 'lon2': SAMPLING_MAX_LON, 'latStepSize': 1, 'lonStepSize': SAMPLING_RES, 'magneticComponent': 'f', 'resultFormat': 'json'})
-    f = urllib.request.urlopen("https://www.ngdc.noaa.gov/geomag-web/calculators/calculateIgrfgrid?key=%s&%s" % (key, params))
-    data = json.loads(f.read())
+    print("};")

-    print('\t/* LAT: {0:3d} */'.format(latitude) + ' { ', end='')
-    for p in data['result']:
+    print('static constexpr float WMM_{}_SCALE_TO_{} = {:.9g}f;'.format(key_name.upper(), units_str.upper(), 1.0 / value_scale))
+    print('static constexpr float WMM_{}_MIN_{} = {:.1f}f; // latitude: {:.0f}, longitude: {:.0f}'.format(key_name.upper(), units_str.upper(), value_min, value_min_lat_lon[0], value_min_lat_lon[1]))
+    print('static constexpr float WMM_{}_MAX_{} = {:.1f}f; // latitude: {:.0f}, longitude: {:.0f}'.format(key_name.upper(), units_str.upper(), value_max, value_max_lat_lon[0], value_max_lat_lon[1]))
+    print("\n")

-        strength_gauss = p['totalintensity'] * 1e-3
-        strength_list.append(strength_gauss)

-        totalintensity_int = int(round(p['totalintensity']/10))
-        print('{0:5d},'.format(totalintensity_int), end='')
-
-        if (strength_gauss > strength_max):
-            strength_max = strength_gauss
-            strength_max_lat_lon = (p['latitude'], p['longitude'])
-
-        if (strength_gauss < strength_min):
-            strength_min = strength_gauss
-            strength_min_lat_lon = (p['latitude'], p['longitude'])
-
-    print(' },')
-print("};")
-
-print('static constexpr float WMM_STRENGTH_MIN_GS = {:.1f}; // latitude: {:.0f}, longitude: {:.0f}'.format(strength_min, strength_min_lat_lon[0], strength_min_lat_lon[1]))
-print('static constexpr float WMM_STRENGTH_MAX_GS = {:.1f}; // latitude: {:.0f}, longitude: {:.0f}'.format(strength_max, strength_max_lat_lon[0], strength_max_lat_lon[1]))
-print('static constexpr float WMM_STRENGTH_MEAN_GS = {:.1f};'.format(statistics.mean(strength_list)))
-print('static constexpr float WMM_STRENGTH_MEDIAN_GS = {:.1f};'.format(statistics.median(strength_list)))
-print("\n")
+print_wmm_table('declination')
+print_wmm_table('inclination')
+print_wmm_table('totalintensity')
@@ -44,7 +44,7 @@ SAMPLING_MAX_LON = 180

 header = """/****************************************************************************
 *
- *   Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -66,21 +66,21 @@ static constexpr unsigned get_lookup_table_index(float *val, float min, float ma
 	return static_cast<unsigned>((-(min) + *val) / SAMPLING_RES);
 }

-static constexpr float get_table_data(float lat, float lon, const int16_t table[LAT_DIM][LON_DIM])
+static constexpr float get_table_data(float latitude_deg, float longitude_deg, const int16_t table[LAT_DIM][LON_DIM])
 {
-	lat = math::constrain(lat, SAMPLING_MIN_LAT, SAMPLING_MAX_LAT);
+	latitude_deg = math::constrain(latitude_deg, SAMPLING_MIN_LAT, SAMPLING_MAX_LAT);

-	if (lon > SAMPLING_MAX_LON) {
-		lon -= 360;
+	if (longitude_deg > SAMPLING_MAX_LON) {
+		longitude_deg -= 360.f;
 	}

-	if (lon < SAMPLING_MIN_LON) {
-		lon += 360;
+	if (longitude_deg < SAMPLING_MIN_LON) {
+		longitude_deg += 360.f;
 	}

 	/* round down to nearest sampling resolution */
-	float min_lat = floorf(lat / SAMPLING_RES) * SAMPLING_RES;
-	float min_lon = floorf(lon / SAMPLING_RES) * SAMPLING_RES;
+	float min_lat = floorf(latitude_deg / SAMPLING_RES) * SAMPLING_RES;
+	float min_lon = floorf(longitude_deg / SAMPLING_RES) * SAMPLING_RES;

 	/* find index of nearest low sampling point */
 	unsigned min_lat_index = get_lookup_table_index(&min_lat, SAMPLING_MIN_LAT, SAMPLING_MAX_LAT);
@@ -92,8 +92,8 @@ static constexpr float get_table_data(float lat, float lon, const int16_t table[
 	const float data_nw = table[min_lat_index + 1][min_lon_index];

 	/* perform bilinear interpolation on the four grid corners */
-	const float lat_scale = constrain((lat - min_lat) / SAMPLING_RES, 0.f, 1.f);
-	const float lon_scale = constrain((lon - min_lon) / SAMPLING_RES, 0.f, 1.f);
+	const float lat_scale = constrain((latitude_deg - min_lat) / SAMPLING_RES, 0.f, 1.f);
+	const float lon_scale = constrain((longitude_deg - min_lon) / SAMPLING_RES, 0.f, 1.f);

 	const float data_min = lon_scale * (data_se - data_sw) + data_sw;
 	const float data_max = lon_scale * (data_ne - data_nw) + data_nw;
@@ -101,32 +101,27 @@ static constexpr float get_table_data(float lat, float lon, const int16_t table[
 	return lat_scale * (data_max - data_min) + data_min;
 }

-float get_mag_declination_radians(float lat, float lon)
+float get_mag_declination_degrees(float latitude_deg, float longitude_deg)
 {
-	return get_table_data(lat, lon, declination_table) * 1e-4f; // declination table stored as 10^-4 radians
+	// table stored as scaled degrees
+	return get_table_data(latitude_deg, longitude_deg, declination_table) * WMM_DECLINATION_SCALE_TO_DEGREES;
 }

-float get_mag_declination_degrees(float lat, float lon)
+float get_mag_inclination_degrees(float latitude_deg, float longitude_deg)
 {
-	return math::degrees(get_mag_declination_radians(lat, lon));
+	// table stored as scaled degrees
+	return get_table_data(latitude_deg, longitude_deg, inclination_table) * WMM_INCLINATION_SCALE_TO_DEGREES;
 }

-float get_mag_inclination_radians(float lat, float lon)
+float get_mag_strength_gauss(float latitude_deg, float longitude_deg)
 {
-	return get_table_data(lat, lon, inclination_table) * 1e-4f; // inclination table stored as 10^-4 radians
+	// 1 Gauss = 1e4 Tesla
+	return get_mag_strength_tesla(latitude_deg, longitude_deg) * 1e4f;
 }

-float get_mag_inclination_degrees(float lat, float lon)
+float get_mag_strength_tesla(float latitude_deg, float longitude_deg)
 {
-	return math::degrees(get_mag_inclination_radians(lat, lon));
-}
-
-float get_mag_strength_gauss(float lat, float lon)
-{
-	return get_table_data(lat, lon, strength_table) * 1e-4f; // strength table table stored as milli-Gauss * 10
-}
-
-float get_mag_strength_tesla(float lat, float lon)
-{
-	return get_mag_strength_gauss(lat, lon) * 1e-4f; // 1 Gauss == 0.0001 Tesla
+	// table stored as scaled nanotesla
+	return get_table_data(latitude_deg, longitude_deg, totalintensity_table)
+	       * WMM_TOTALINTENSITY_SCALE_TO_NANOTESLA * 1e-9f;
 }
@@ -41,14 +41,12 @@

 #pragma once

-// Return magnetic declination in degrees or radians
-float get_mag_declination_degrees(float lat, float lon);
-float get_mag_declination_radians(float lat, float lon);
+// Return magnetic declination in degrees
+float get_mag_declination_degrees(float latitude_deg, float longitude_deg);

-// Return magnetic field inclination in degrees or radians
-float get_mag_inclination_degrees(float lat, float lon);
-float get_mag_inclination_radians(float lat, float lon);
+// Return magnetic field inclination in degrees
+float get_mag_inclination_degrees(float latitude_deg, float longitude_deg);

 // return magnetic field strength in Gauss or Tesla
-float get_mag_strength_gauss(float lat, float lon);
-float get_mag_strength_tesla(float lat, float lon);
+float get_mag_strength_gauss(float latitude_deg, float longitude_deg);
+float get_mag_strength_tesla(float latitude_deg, float longitude_deg);
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -44,95 +44,99 @@ static constexpr int LON_DIM = 37;


 // *INDENT-OFF*
-// Magnetic declination data in radians * 10^-4
+
+
+
+// Magnetic declination data in 0.005451 degrees
 // Model: WMM-2020,
 // Version: 0.5.1.11,
-// Date: 2024.0684,
+// Date: 2024.41257,
 static constexpr const int16_t declination_table[19][37] {
 	//    LONGITUDE:   -180,  -170,  -160,  -150,  -140,  -130,  -120,  -110,  -100,   -90,   -80,   -70,   -60,   -50,   -40,   -30,   -20,   -10,     0,    10,    20,    30,    40,    50,    60,    70,    80,    90,   100,   110,   120,   130,   140,   150,   160,   170,   180,
-	/* LAT: -90 */ {  25949, 24204, 22458, 20713, 18968, 17222, 15477, 13732, 11986, 10241,  8496,  6750,  5005,  3260,  1514,  -231, -1976, -3722, -5467, -7212, -8958,-10703,-12448,-14194,-15939,-17684,-19430,-21175,-22920,-24666,-26411,-28156,-29902, 31185, 29440, 27694, 25949, },
-	/* LAT: -80 */ {  22511, 20385, 18448, 16677, 15039, 13502, 12039, 10628,  9254,  7905,  6573,  5255,  3947,  2642,  1334,    13, -1329, -2704, -4116, -5571, -7068, -8608,-10188,-11810,-13476,-15197,-16985,-18860,-20845,-22966,-25242,-27677,-30245, 29950, 27336, 24836, 22511, },
-	/* LAT: -70 */ {  14988, 13587, 12457, 11491, 10618,  9783,  8939,  8049,  7095,  6075,  5005,  3913,  2830,  1777,   757,  -252, -1291, -2401, -3608, -4915, -6302, -7737, -9189,-10634,-12063,-13481,-14912,-16402,-18033,-19960,-22502,-26331, 30582, 24088, 19617, 16858, 14988, }, // WARNING! black out zone
-	/* LAT: -60 */ {   8460,  8207,  7918,  7636,  7376,  7117,  6803,  6366,  5747,  4924,  3920,  2809,  1692,   671,  -201,  -952, -1680, -2508, -3524, -4735, -6077, -7455, -8781, -9992,-11055,-11951,-12664,-13158,-13330,-12882,-10766, -3382,  5057,  7748,  8493,  8605,  8460, }, // WARNING! black out zone
-	/* LAT: -50 */ {   5516,  5549,  5488,  5392,  5313,  5271,  5232,  5100,  4751,  4082,  3066,  1790,   453,  -715, -1568, -2116, -2503, -2947, -3651, -4691, -5957, -7246, -8389, -9286, -9875,-10102, -9893, -9123, -7602, -5231, -2320,   430,  2541,  3966,  4841,  5314,  5516, },
-	/* LAT: -40 */ {   3977,  4068,  4072,  4021,  3956,  3918,  3919,  3905,  3727,  3185,  2151,   702,  -859, -2158, -2999, -3428, -3594, -3645, -3829, -4438, -5452, -6544, -7426, -7951, -8039, -7639, -6731, -5338, -3632, -1939,  -487,   732,  1779,  2650,  3311,  3745,  3977, },
-	/* LAT: -30 */ {   3003,  3088,  3113,  3092,  3028,  2945,  2881,  2845,  2716,  2229,  1178,  -353, -1958, -3193, -3909, -4234, -4292, -4059, -3621, -3439, -3855, -4626, -5316, -5645, -5497, -4889, -3917, -2714, -1520,  -590,    79,   677,  1305,  1916,  2433,  2801,  3003, },
-	/* LAT: -20 */ {   2360,  2404,  2416,  2410,  2361,  2262,  2149,  2071,  1924,  1420,   347, -1155, -2629, -3669, -4175, -4268, -4043, -3462, -2592, -1820, -1589, -1975, -2627, -3080, -3097, -2722, -2080, -1269,  -508,   -33,   221,   519,   966,  1455,  1887,  2202,  2360, },
-	/* LAT: -10 */ {   1966,  1959,  1931,  1919,  1885,  1794,  1677,  1582,  1396,   839,  -238, -1623, -2888, -3693, -3930, -3676, -3084, -2295, -1450,  -709,  -266,  -325,  -809, -1310, -1509, -1409, -1091,  -597,  -111,   118,   156,   311,   689,  1139,  1545,  1841,  1966, },
-	/* LAT:   0 */ {   1752,  1717,  1655,  1639,  1621,  1545,  1432,  1311,  1055,   430,  -618, -1847, -2888, -3450, -3420, -2898, -2123, -1344,  -699,  -169,   235,   331,    37,  -380,  -627,  -679,  -580,  -323,   -41,    38,   -35,    49,   397,   854,  1287,  1617,  1752, },
-	/* LAT:  10 */ {   1612,  1619,  1571,  1581,  1601,  1545,  1414,  1220,   842,   124,  -899, -1971, -2783, -3100, -2872, -2252, -1473,  -767,  -260,   116,   438,   577,   403,    80,  -149,  -257,  -285,  -210,  -113,  -163,  -313,  -290,    19,   487,   979,  1396,  1612, },
-	/* LAT:  20 */ {   1419,  1567,  1624,  1712,  1794,  1768,  1605,  1296,   751,  -104, -1144, -2086, -2666, -2757, -2413, -1803, -1092,  -447,     5,   310,   566,   706,   607,   362,   166,    46,   -53,  -127,  -214,  -411,  -655,  -715,  -474,   -16,   534,  1059,  1419, },
-	/* LAT:  30 */ {   1105,  1471,  1730,  1952,  2110,  2117,  1924,  1498,   769,  -255, -1364, -2227, -2625, -2550, -2143, -1560,  -904,  -289,   167,   468,   696,   837,   810,   660,   512,   385,   219,    -5,  -297,  -677, -1049, -1207, -1042,  -608,   -27,   584,  1105, },
-	/* LAT:  40 */ {    732,  1317,  1812,  2206,  2456,  2499,  2283,  1748,   839,  -380, -1603, -2452, -2759, -2599, -2149, -1556,  -902,  -275,   231,   591,   857,  1050,  1137,  1123,  1047,   895,   615,   189,  -362,  -974, -1493, -1732, -1610, -1189,  -591,    77,   732, },
-	/* LAT:  50 */ {    425,  1168,  1851,  2413,  2785,  2895,  2669,  2019,   887,  -593, -1994, -2892, -3177, -2982, -2491, -1845, -1135,  -440,   172,   674,  1088,  1437,  1711,  1876,  1891,  1695,  1235,   507,  -401, -1306, -1977, -2253, -2123, -1682, -1054,  -331,   425, },
-	/* LAT:  60 */ {    203,  1050,  1857,  2558,  3072,  3295,  3093,  2299,   813, -1110, -2799, -3769, -4020, -3763, -3195, -2453, -1630,  -793,    11,   759,  1447,  2073,  2610,  2999,  3148,  2946,  2284,  1141,  -289, -1606, -2460, -2757, -2588, -2101, -1421,  -635,   203, },
-	/* LAT:  70 */ {    -80,   847,  1743,  2546,  3170,  3479,  3255,  2178,    39, -2583, -4513, -5355, -5387, -4922, -4167, -3243, -2226, -1167,   -98,   958,  1979,  2941,  3805,  4505,  4941,  4955,  4314,  2796,   566, -1545, -2828, -3261, -3103, -2582, -1848,  -994,   -80, }, // WARNING! black out zone
-	/* LAT:  80 */ {   -944,   -26,   822,  1513,  1909,  1772,   716, -1590, -4521, -6612, -7444, -7396, -6828, -5954, -4893, -3716, -2466, -1173,   144,  1465,  2775,  4055,  5282,  6422,  7420,  8182,  8523,  8052,  6011,  1989, -1732, -3421, -3747, -3398, -2713, -1863,  -944, }, // WARNING! black out zone
-	/* LAT:  90 */ { -29255,-27510,-25765,-24019,-22274,-20529,-18783,-17038,-15293,-13547,-11802,-10057, -8311, -6566, -4821, -3075, -1330,   415,  2161,  3906,  5651,  7397,  9142, 10887, 12633, 14378, 16123, 17869, 19614, 21359, 23105, 24850, 26595, 28341, 30086,-31001,-29255, }, // WARNING! black out zone
+	/* LAT: -90 */ {  27264, 25429, 23595, 21761, 19926, 18092, 16258, 14423, 12589, 10754,  8920,  7086,  5251,  3417,  1583,  -252, -2086, -3921, -5755, -7589, -9424,-11258,-13092,-14927,-16761,-18596,-20430,-22264,-24099,-25933,-27768,-29602,-31436, 32767, 30933, 29098, 27264, },
+	/* LAT: -80 */ {  23650, 21416, 19382, 17521, 15799, 14184, 12647, 11165,  9721,  8303,  6904,  5519,  4143,  2772,  1397,     9, -1403, -2848, -4333, -5863, -7437, -9056,-10718,-12423,-14175,-15984,-17864,-19835,-21922,-24152,-26545,-29105,-31803, 31464, 28718, 26092, 23650, },
+	/* LAT: -70 */ {  15755, 14282, 13094, 12077, 11159, 10281,  9392,  8457,  7454,  6381,  5257,  4109,  2971,  1865,   794,  -267, -1359, -2527, -3797, -5172, -6632, -8142, -9669,-11188,-12690,-14181,-15686,-17253,-18969,-20997,-23675,-27707, 32110, 25303, 20617, 17720, 15755, }, // WARNING! black out zone
+	/* LAT: -60 */ {   8903,  8634,  8328,  8030,  7755,  7481,  7150,  6689,  6037,  5171,  4116,  2948,  1775,   704,  -211,  -998, -1763, -2635, -3706, -4982, -6396, -7846, -9240,-10513,-11630,-12572,-13320,-13839,-14019,-13549,-11322, -3523,  5353,  8167,  8943,  9057,  8903, }, // WARNING! black out zone
+	/* LAT: -50 */ {   5806,  5839,  5774,  5672,  5587,  5541,  5499,  5359,  4991,  4286,  3216,  1875,   470,  -754, -1647, -2218, -2623, -3091, -3835, -4934, -6270, -7627, -8830, -9771,-10388,-10623,-10400, -9587, -7986, -5492, -2431,   460,  2679,  4178,  5097,  5594,  5806, },
+	/* LAT: -40 */ {   4186,  4282,  4284,  4229,  4159,  4118,  4119,  4103,  3914,  3342,  2253,   728,  -912, -2273, -3151, -3597, -3767, -3819, -4016, -4665, -5740, -6891, -7817, -8365, -8453, -8028, -7069, -5603, -3811, -2034,  -509,   772,  1872,  2790,  3486,  3942,  4186, },
+	/* LAT: -30 */ {   3161,  3250,  3275,  3251,  3182,  3094,  3026,  2988,  2851,  2337,  1229,  -384, -2070, -3364, -4111, -4446, -4503, -4252, -3790, -3609, -4059, -4874, -5597, -5938, -5777, -5133, -4109, -2845, -1594,  -619,    83,   710,  1372,  2016,  2561,  2948,  3161, },
+	/* LAT: -20 */ {   2485,  2532,  2542,  2534,  2481,  2375,  2255,  2172,  2018,  1486,   353, -1228, -2777, -3865, -4390, -4481, -4240, -3623, -2707, -1902, -1669, -2082, -2768, -3241, -3254, -2856, -2179, -1328,  -531,   -35,   229,   542,  1013,  1529,  1985,  2318,  2485, },
+	/* LAT: -10 */ {   2072,  2064,  2032,  2018,  1980,  1883,  1758,  1657,  1461,   874,  -261, -1719, -3046, -3887, -4129, -3855, -3229, -2397, -1510,  -734,  -274,  -342,  -854, -1379, -1586, -1478, -1143,  -624,  -116,   121,   159,   321,   720,  1197,  1625,  1939,  2072, },
+	/* LAT:   0 */ {   1847,  1810,  1742,  1723,  1703,  1621,  1500,  1371,  1102,   444,  -660, -1952, -3043, -3628, -3589, -3035, -2219, -1401,  -724,  -169,   254,   350,    38,  -401,  -658,  -711,  -608,  -338,   -43,    36,   -42,    45,   413,   896,  1354,  1703,  1847, },
+	/* LAT:  10 */ {   1699,  1705,  1653,  1662,  1681,  1620,  1480,  1275,   878,   122,  -954, -2080, -2929, -3256, -3012, -2356, -1536,  -796,  -265,   129,   466,   610,   425,    85,  -155,  -268,  -299,  -222,  -121,  -175,  -335,  -312,    14,   509,  1030,  1470,  1699, },
+	/* LAT:  20 */ {   1494,  1649,  1708,  1799,  1884,  1855,  1681,  1355,   781,  -117, -1209, -2197, -2803, -2893, -2528, -1885, -1137,  -461,    13,   332,   601,   746,   640,   383,   178,    51,   -55,  -135,  -228,  -437,  -694,  -758,  -503,   -19,   561,  1114,  1494, },
+	/* LAT:  30 */ {   1160,  1544,  1816,  2050,  2215,  2221,  2017,  1567,   802,  -275, -1438, -2341, -2756, -2673, -2242, -1629,  -939,  -294,   184,   499,   738,   885,   855,   696,   542,   408,   230,    -8,  -317,  -718, -1109, -1275, -1099,  -642,   -30,   613,  1160, },
+	/* LAT:  40 */ {    765,  1379,  1900,  2314,  2577,  2622,  2393,  1831,   876,  -403, -1685, -2574, -2892, -2722, -2248, -1623,  -936,  -278,   252,   629,   908,  1109,  1199,  1184,  1104,   944,   647,   195,  -387, -1030, -1576, -1826, -1696, -1253,  -625,    77,   765, },
+	/* LAT:  50 */ {    437,  1218,  1936,  2528,  2919,  3036,  2799,  2116,   928,  -623, -2091, -3030, -3327, -3121, -2604, -1925, -1180,  -449,   193,   719,  1153,  1519,  1805,  1977,  1991,  1784,  1297,   527,  -430, -1381, -2085, -2373, -2234, -1771, -1112,  -355,   437, },
+	/* LAT:  60 */ {    198,  1087,  1936,  2674,  3216,  3452,  3243,  2411,   855, -1158, -2926, -3941, -4206, -3936, -3339, -2560, -1696,  -817,    27,   812,  1534,  2191,  2754,  3160,  3315,  3098,  2397,  1190,  -317, -1699, -2593, -2902, -2725, -2214, -1502,  -680,   198, },
+	/* LAT:  70 */ {   -111,   863,  1803,  2648,  3305,  3634,  3405,  2285,    58, -2678, -4700, -5589, -5627, -5143, -4352, -3383, -2317, -1205,   -83,  1026,  2098,  3108,  4014,  4748,  5204,  5214,  4533,  2927,   575, -1644, -2988, -3441, -3276, -2730, -1962, -1068,  -111, }, // WARNING! black out zone
+	/* LAT:  80 */ {  -1054,   -90,   801,  1527,  1946,  1812,   731, -1632, -4654, -6842, -7732, -7698, -7114, -6204, -5096, -3864, -2554, -1197,   184,  1572,  2947,  4292,  5581,  6779,  7828,  8630,  8988,  8486,  6313,  2026, -1910, -3677, -4009, -3636, -2914, -2019, -1054, }, // WARNING! black out zone
+	/* LAT:  90 */ { -30607,-28773,-26938,-25104,-23269,-21435,-19601,-17766,-15932,-14097,-12263,-10429, -8594, -6760, -4926, -3091, -1257,   578,  2412,  4246,  6081,  7915,  9749, 11584, 13418, 15253, 17087, 18921, 20756, 22590, 24424, 26259, 28093, 29928, 31762,-32441,-30607, }, // WARNING! black out zone
 };
-static constexpr float WMM_DECLINATION_MIN_RAD = -3.100; // latitude: 90, longitude: 170
-static constexpr float WMM_DECLINATION_MAX_RAD = 3.118; // latitude: -90, longitude: 150
+static constexpr float WMM_DECLINATION_SCALE_TO_DEGREES = 0.00545143529f;
+static constexpr float WMM_DECLINATION_MIN_DEGREES = -176.9f; // latitude: 90, longitude: 170
+static constexpr float WMM_DECLINATION_MAX_DEGREES = 178.6f; // latitude: -90, longitude: 150


-// Magnetic inclination data in radians * 10^-4
+// Magnetic inclination data in 0.002699 degrees
 // Model: WMM-2020,
 // Version: 0.5.1.11,
-// Date: 2024.0684,
+// Date: 2024.41257,
 static constexpr const int16_t inclination_table[19][37] {
 	//    LONGITUDE:   -180,  -170,  -160,  -150,  -140,  -130,  -120,  -110,  -100,   -90,   -80,   -70,   -60,   -50,   -40,   -30,   -20,   -10,     0,    10,    20,    30,    40,    50,    60,    70,    80,    90,   100,   110,   120,   130,   140,   150,   160,   170,   180,
-	/* LAT: -90 */ { -12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562, },
-	/* LAT: -80 */ { -13644,-13510,-13349,-13169,-12975,-12774,-12571,-12371,-12181,-12005,-11849,-11715,-11605,-11519,-11455,-11414,-11394,-11396,-11423,-11477,-11561,-11676,-11821,-11995,-12194,-12412,-12641,-12874,-13102,-13314,-13500,-13649,-13752,-13802,-13798,-13743,-13644, },
-	/* LAT: -70 */ { -14091,-13772,-13453,-13130,-12799,-12455,-12102,-11746,-11404,-11097,-10846,-10664,-10553,-10501,-10487,-10488,-10492,-10499,-10521,-10576,-10685,-10861,-11110,-11427,-11802,-12221,-12670,-13135,-13601,-14053,-14469,-14811,-14995,-14937,-14705,-14407,-14091, }, // WARNING! black out zone
-	/* LAT: -60 */ { -13509,-13155,-12816,-12483,-12140,-11768,-11353,-10900,-10434,-10006, -9680, -9510, -9511, -9650, -9853,-10040,-10158,-10193,-10174,-10157,-10205,-10369,-10664,-11078,-11581,-12140,-12727,-13324,-13913,-14474,-14971,-15259,-15076,-14687,-14279,-13884,-13509, }, // WARNING! black out zone
-	/* LAT: -50 */ { -12492,-12148,-11816,-11493,-11170,-10823,-10425, -9954, -9426, -8909, -8526, -8410, -8620, -9086, -9648,-10152,-10499,-10648,-10604,-10443,-10301,-10315,-10547,-10974,-11522,-12117,-12703,-13240,-13683,-13979,-14086,-14012,-13807,-13523,-13193,-12844,-12492, },
-	/* LAT: -40 */ { -11239,-10888,-10538,-10191, -9852, -9514, -9155, -8732, -8213, -7652, -7235, -7210, -7686, -8518, -9438,-10259,-10906,-11324,-11443,-11256,-10906,-10641,-10650,-10946,-11416,-11922,-12363,-12679,-12833,-12837,-12748,-12613,-12435,-12204,-11917,-11588,-11239, },
-	/* LAT: -30 */ {  -9603, -9218, -8834, -8442, -8050, -7677, -7323, -6936, -6425, -5817, -5378, -5486, -6282, -7498, -8746, -9831,-10733,-11426,-11801,-11765,-11376,-10868,-10548,-10564,-10824,-11146,-11397,-11501,-11434,-11258,-11088,-10959,-10818,-10614,-10333, -9985, -9603, },
-	/* LAT: -20 */ {  -7374, -6924, -6499, -6068, -5624, -5196, -4810, -4404, -3842, -3160, -2729, -3025, -4173, -5803, -7426, -8778, -9823,-10578,-10993,-11002,-10618, -9994, -9442, -9212, -9270, -9434, -9570, -9580, -9403, -9133, -8945, -8869, -8775, -8573, -8256, -7840, -7374, },
-	/* LAT: -10 */ {  -4420, -3869, -3405, -2963, -2506, -2057, -1649, -1205,  -588,   107,   447,   -20, -1391, -3330, -5297, -6877, -7942, -8550, -8793, -8710, -8280, -7581, -6919, -6585, -6553, -6646, -6760, -6778, -6587, -6293, -6148, -6175, -6151, -5943, -5559, -5026, -4420, },
-	/* LAT:   0 */ {   -913,  -272,   205,   613,  1033,  1448,  1832,  2262,  2833,  3397,  3578,  3047,  1712,  -222, -2259, -3876, -4847, -5247, -5286, -5101, -4641, -3909, -3204, -2843, -2786, -2852, -2974, -3044, -2913, -2681, -2639, -2805, -2890, -2713, -2288, -1650,  -913, },
-	/* LAT:  10 */ {   2555,  3197,  3642,  3988,  4344,  4710,  5056,  5430,  5867,  6224,  6243,  5728,  4617,  3031,  1345,    -3,  -774,  -994,  -886,  -639,  -213,   438,  1072,  1402,  1466,  1428,  1331,  1240,  1281,  1380,  1291,  1002,   789,   851,  1205,  1815,  2555, },
-	/* LAT:  20 */ {   5412,  5951,  6338,  6637,  6951,  7293,  7631,  7968,  8290,  8479,  8378,  7898,  7045,  5939,  4818,  3927,  3420,  3323,  3486,  3737,  4077,  4551,  5012,  5264,  5324,  5314,  5270,  5211,  5196,  5172,  4987,  4639,  4326,  4229,  4402,  4831,  5412, },
-	/* LAT:  30 */ {   7567,  7945,  8266,  8551,  8860,  9206,  9559,  9893, 10162, 10266, 10112,  9680,  9044,  8331,  7675,  7174,  6895,  6867,  7019,  7236,  7488,  7793,  8083,  8256,  8315,  8334,  8340,  8330,  8304,  8215,  7986,  7624,  7260,  7036,  7020,  7218,  7567, },
-	/* LAT:  40 */ {   9266,  9488,  9744, 10030, 10356, 10715, 11080, 11414, 11660, 11735, 11580, 11216, 10741, 10270,  9878,  9601,  9460,  9464,  9581,  9747,  9926, 10114, 10287, 10412, 10492, 10556, 10614, 10648, 10628, 10512, 10265,  9909,  9538,  9251,  9108,  9122,  9266, },
-	/* LAT:  50 */ {  10802, 10922, 11123, 11391, 11713, 12065, 12416, 12729, 12947, 13002, 12861, 12563, 12200, 11857, 11585, 11403, 11315, 11317, 11388, 11494, 11611, 11729, 11847, 11964, 12083, 12208, 12324, 12396, 12383, 12253, 12003, 11672, 11331, 11046, 10857, 10777, 10802, },
-	/* LAT:  60 */ {  12320, 12390, 12538, 12754, 13021, 13319, 13619, 13883, 14057, 14084, 13950, 13702, 13416, 13147, 12929, 12775, 12687, 12659, 12678, 12728, 12798, 12885, 12993, 13127, 13288, 13465, 13630, 13737, 13739, 13614, 13387, 13107, 12829, 12593, 12423, 12331, 12320, },
-	/* LAT:  70 */ {  13757, 13796, 13888, 14027, 14204, 14404, 14610, 14790, 14895, 14879, 14747, 14549, 14334, 14132, 13960, 13826, 13734, 13680, 13663, 13677, 13720, 13794, 13899, 14037, 14205, 14393, 14577, 14716, 14759, 14683, 14520, 14321, 14127, 13963, 13842, 13772, 13757, }, // WARNING! black out zone
-	/* LAT:  80 */ {  14991, 15001, 15036, 15092, 15166, 15249, 15325, 15371, 15359, 15287, 15180, 15059, 14938, 14826, 14727, 14646, 14584, 14544, 14526, 14531, 14558, 14608, 14680, 14774, 14886, 15014, 15150, 15283, 15390, 15432, 15389, 15299, 15201, 15115, 15049, 15007, 14991, }, // WARNING! black out zone
-	/* LAT:  90 */ {  15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, }, // WARNING! black out zone
+	/* LAT: -90 */ { -26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663, },
+	/* LAT: -80 */ { -28959,-28675,-28333,-27951,-27540,-27113,-26682,-26258,-25854,-25482,-25150,-24867,-24633,-24450,-24315,-24226,-24183,-24188,-24245,-24361,-24539,-24783,-25092,-25462,-25884,-26346,-26833,-27328,-27812,-28262,-28656,-28973,-29191,-29297,-29287,-29170,-28959, },
+	/* LAT: -70 */ { -29906,-29230,-28552,-27867,-27164,-26436,-25685,-24930,-24205,-23555,-23023,-22638,-22403,-22293,-22261,-22264,-22272,-22285,-22330,-22447,-22678,-23053,-23582,-24256,-25053,-25944,-26898,-27885,-28874,-29833,-30717,-31441,-31829,-31704,-31211,-30578,-29906, }, // WARNING! black out zone
+	/* LAT: -60 */ { -28675,-27922,-27202,-26495,-25766,-24977,-24097,-23135,-22147,-21240,-20551,-20191,-20196,-20492,-20922,-21317,-21564,-21635,-21592,-21554,-21657,-22006,-22635,-23517,-24587,-25775,-27023,-28290,-29540,-30732,-31788,-32398,-32004,-31178,-30310,-29471,-28675, }, // WARNING! black out zone
+	/* LAT: -50 */ { -26518,-25787,-25081,-24395,-23709,-22973,-22127,-21129,-20009,-18913,-18103,-17862,-18313,-19305,-20497,-21563,-22295,-22602,-22503,-22157,-21856,-21888,-22387,-23298,-24466,-25730,-26974,-28112,-29052,-29680,-29904,-29747,-29313,-28709,-28008,-27265,-26518, },
+	/* LAT: -40 */ { -23859,-23111,-22367,-21631,-20911,-20195,-19433,-18535,-17434,-16245,-15366,-15320,-16340,-18110,-20062,-21801,-23168,-24048,-24290,-23883,-23136,-22578,-22605,-23240,-24242,-25316,-26251,-26918,-27243,-27250,-27062,-26776,-26401,-25910,-25300,-24601,-23859, },
+	/* LAT: -30 */ { -20386,-19567,-18750,-17917,-17085,-16293,-15544,-14724,-13640,-12351,-11424,-11666,-13370,-15956,-18605,-20904,-22812,-24277,-25061,-24972,-24136,-23057,-22385,-22426,-22983,-23667,-24196,-24414,-24269,-23896,-23536,-23265,-22968,-22536,-21939,-21200,-20386, },
+	/* LAT: -20 */ { -15655,-14695,-13789,-12872,-11930,-11023,-10206, -9346, -8156, -6711, -5803, -6449, -8904,-12373,-15815,-18676,-20887,-22480,-23348,-23356,-22530,-21203,-20034,-19551,-19678,-20026,-20313,-20331,-19954,-19380,-18986,-18828,-18633,-18206,-17535,-16650,-15655, },
+	/* LAT: -10 */ {  -9385, -8209, -7218, -6278, -5306, -4355, -3491, -2554, -1249,   222,   935,   -75, -3005, -7132,-11304,-14645,-16891,-18169,-18676,-18490,-17568,-16077,-14672,-13969,-13905,-14103,-14342,-14377,-13970,-13348,-13045,-13109,-13066,-12627,-11813,-10677, -9385, },
+	/* LAT:   0 */ {  -1941,  -572,   447,  1316,  2208,  3089,  3901,  4808,  6012,  7204,  7577,  6434,  3584,  -534, -4855, -8273,-10317,-11152,-11228,-10827, -9842, -8280, -6784, -6023, -5903, -6042, -6299, -6446, -6167, -5676, -5592, -5955, -6145, -5775, -4872, -3514, -1941, },
+	/* LAT:  10 */ {   5421,  6791,  7742,  8478,  9236, 10013, 10743, 11532, 12453, 13205, 13235, 12131,  9759,  6384,  2805,   -45, -1666, -2121, -1883, -1354,  -444,   946,  2291,  2989,  3125,  3048,  2844,  2651,  2737,  2945,  2751,  2128,  1664,  1792,  2543,  3843,  5421, },
+	/* LAT:  20 */ {  11488, 12637, 13462, 14098, 14766, 15492, 16206, 16917, 17594, 17991, 17771, 16744, 14925, 12573, 10194,  8309,  7245,  7046,  7400,  7937,  8662,  9672, 10651, 11186, 11314, 11296, 11203, 11079, 11045, 10991, 10595,  9848,  9176,  8967,  9335, 10249, 11488, },
+	/* LAT:  30 */ {  16064, 16869, 17552, 18157, 18813, 19547, 20295, 21001, 21567, 21785, 21452, 20532, 19178, 17663, 16272, 15213, 14627, 14573, 14900, 15364, 15901, 16550, 17167, 17534, 17662, 17704, 17719, 17697, 17639, 17449, 16959, 16185, 15409, 14931, 14899, 15320, 16064, },
+	/* LAT:  40 */ {  19670, 20142, 20688, 21294, 21986, 22747, 23519, 24226, 24746, 24903, 24570, 23795, 22787, 21787, 20958, 20373, 20075, 20088, 20340, 20695, 21077, 21477, 21845, 22111, 22280, 22419, 22544, 22615, 22571, 22323, 21795, 21038, 20247, 19638, 19335, 19364, 19670, },
+	/* LAT:  50 */ {  22933, 23187, 23612, 24182, 24864, 25609, 26354, 27016, 27477, 27593, 27291, 26658, 25888, 25162, 24586, 24201, 24016, 24023, 24176, 24404, 24654, 24906, 25156, 25404, 25658, 25925, 26171, 26324, 26295, 26017, 25483, 24780, 24056, 23451, 23049, 22878, 22933, },
+	/* LAT:  60 */ {  26155, 26303, 26615, 27072, 27639, 28270, 28905, 29465, 29833, 29890, 29605, 29081, 28474, 27904, 27442, 27117, 26932, 26874, 26915, 27023, 27173, 27359, 27588, 27873, 28215, 28592, 28943, 29169, 29170, 28904, 28420, 27827, 27237, 26736, 26376, 26179, 26155, },
+	/* LAT:  70 */ {  29204, 29286, 29480, 29773, 30146, 30572, 31008, 31388, 31612, 31580, 31301, 30883, 30427, 29998, 29634, 29351, 29155, 29043, 29006, 29037, 29130, 29287, 29511, 29805, 30163, 30562, 30953, 31246, 31335, 31173, 30826, 30404, 29992, 29643, 29386, 29237, 29204, }, // WARNING! black out zone
+	/* LAT:  80 */ {  31820, 31841, 31914, 32033, 32188, 32362, 32524, 32622, 32598, 32449, 32224, 31969, 31713, 31474, 31265, 31093, 30963, 30878, 30840, 30850, 30908, 31015, 31169, 31368, 31607, 31878, 32167, 32450, 32679, 32767, 32672, 32479, 32271, 32087, 31945, 31855, 31820, }, // WARNING! black out zone
+	/* LAT:  90 */ {  32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, }, // WARNING! black out zone
 };
-static constexpr float WMM_INCLINATION_MIN_RAD = -1.526; // latitude: -60, longitude: 130
-static constexpr float WMM_INCLINATION_MAX_RAD = 1.543; // latitude: 80, longitude: 110
+static constexpr float WMM_INCLINATION_SCALE_TO_DEGREES = 0.00269892697f;
+static constexpr float WMM_INCLINATION_MIN_DEGREES = -87.4f; // latitude: -60, longitude: 130
+static constexpr float WMM_INCLINATION_MAX_DEGREES = 88.4f; // latitude: 80, longitude: 110


-// Magnetic strength data in milli-Gauss * 10
+// Magnetic totalintensity data in 2.042 nanoTesla
 // Model: WMM-2020,
 // Version: 0.5.1.11,
-// Date: 2024.0684,
-static constexpr const int16_t strength_table[19][37] {
-	//    LONGITUDE:  -180, -170, -160, -150, -140, -130, -120, -110, -100,  -90,  -80,  -70,  -60,  -50,  -40,  -30,  -20,  -10,    0,   10,   20,   30,   40,   50,   60,   70,   80,   90,  100,  110,  120,  130,  140,  150,  160,  170,  180,
-	/* LAT: -90 */ {  5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, },
-	/* LAT: -80 */ {  6049, 5985, 5905, 5812, 5709, 5596, 5478, 5356, 5234, 5114, 5000, 4895, 4802, 4722, 4660, 4615, 4591, 4590, 4612, 4659, 4730, 4825, 4940, 5072, 5216, 5367, 5517, 5660, 5791, 5905, 5997, 6065, 6109, 6128, 6123, 6096, 6049, },
-	/* LAT: -70 */ {  6294, 6159, 6007, 5841, 5661, 5468, 5263, 5048, 4829, 4615, 4413, 4231, 4074, 3943, 3840, 3764, 3718, 3706, 3735, 3813, 3942, 4124, 4354, 4623, 4919, 5227, 5531, 5815, 6064, 6268, 6418, 6512, 6553, 6544, 6493, 6407, 6294, },
-	/* LAT: -60 */ {  6179, 5985, 5782, 5572, 5352, 5116, 4860, 4581, 4289, 4000, 3734, 3508, 3330, 3196, 3098, 3025, 2974, 2954, 2981, 3074, 3245, 3499, 3829, 4217, 4640, 5075, 5496, 5880, 6204, 6451, 6613, 6690, 6691, 6626, 6511, 6358, 6179, },
-	/* LAT: -50 */ {  5838, 5606, 5373, 5140, 4907, 4661, 4390, 4087, 3758, 3427, 3126, 2888, 2728, 2638, 2590, 2555, 2520, 2492, 2498, 2572, 2750, 3045, 3445, 3920, 4428, 4934, 5408, 5826, 6163, 6403, 6540, 6579, 6535, 6423, 6260, 6060, 5838, },
-	/* LAT: -40 */ {  5389, 5142, 4895, 4654, 4419, 4181, 3925, 3641, 3327, 3001, 2704, 2483, 2369, 2344, 2363, 2383, 2385, 2371, 2357, 2387, 2523, 2806, 3230, 3748, 4296, 4819, 5283, 5669, 5960, 6146, 6233, 6234, 6164, 6031, 5849, 5629, 5389, },
-	/* LAT: -30 */ {  4876, 4633, 4393, 4157, 3930, 3711, 3491, 3260, 3004, 2728, 2471, 2291, 2224, 2250, 2317, 2387, 2451, 2498, 2517, 2528, 2600, 2807, 3178, 3672, 4204, 4700, 5114, 5427, 5628, 5727, 5752, 5725, 5645, 5511, 5331, 5113, 4876, },
-	/* LAT: -20 */ {  4320, 4105, 3895, 3689, 3492, 3310, 3141, 2977, 2798, 2599, 2408, 2277, 2239, 2286, 2376, 2487, 2614, 2739, 2823, 2856, 2882, 2985, 3234, 3624, 4077, 4503, 4847, 5077, 5180, 5186, 5156, 5109, 5027, 4899, 4732, 4534, 4320, },
-	/* LAT: -10 */ {  3789, 3627, 3472, 3325, 3190, 3069, 2965, 2872, 2773, 2657, 2535, 2437, 2396, 2424, 2514, 2644, 2799, 2955, 3075, 3134, 3147, 3177, 3307, 3565, 3894, 4216, 4478, 4637, 4669, 4616, 4548, 4485, 4398, 4273, 4123, 3959, 3789, },
-	/* LAT:   0 */ {  3412, 3317, 3232, 3159, 3103, 3064, 3037, 3017, 2990, 2941, 2861, 2768, 2690, 2664, 2710, 2815, 2947, 3082, 3194, 3267, 3297, 3320, 3399, 3560, 3772, 3986, 4165, 4268, 4272, 4203, 4115, 4023, 3912, 3781, 3646, 3521, 3412, },
-	/* LAT:  10 */ {  3282, 3250, 3229, 3224, 3248, 3294, 3348, 3399, 3432, 3421, 3352, 3238, 3113, 3022, 3000, 3044, 3126, 3225, 3325, 3409, 3473, 3536, 3626, 3746, 3884, 4024, 4143, 4212, 4212, 4149, 4038, 3895, 3733, 3573, 3437, 3339, 3282, },
-	/* LAT:  20 */ {  3399, 3400, 3425, 3477, 3568, 3688, 3815, 3930, 4009, 4020, 3946, 3805, 3640, 3504, 3432, 3423, 3460, 3535, 3632, 3730, 3821, 3921, 4033, 4145, 4256, 4369, 4472, 4537, 4546, 4485, 4347, 4143, 3915, 3705, 3541, 3439, 3399, },
-	/* LAT:  30 */ {  3722, 3726, 3778, 3876, 4017, 4186, 4361, 4515, 4622, 4648, 4575, 4421, 4234, 4074, 3972, 3929, 3935, 3988, 4077, 4176, 4275, 4382, 4499, 4617, 4737, 4866, 4988, 5074, 5098, 5038, 4880, 4639, 4364, 4109, 3909, 3779, 3722, },
-	/* LAT:  40 */ {  4222, 4216, 4278, 4399, 4564, 4750, 4932, 5088, 5192, 5218, 5151, 5004, 4820, 4649, 4524, 4451, 4427, 4452, 4515, 4595, 4683, 4781, 4897, 5030, 5182, 5345, 5497, 5606, 5643, 5586, 5428, 5190, 4916, 4657, 4446, 4299, 4222, },
-	/* LAT:  50 */ {  4832, 4820, 4872, 4979, 5123, 5281, 5431, 5552, 5627, 5637, 5575, 5450, 5290, 5128, 4993, 4898, 4845, 4836, 4862, 4914, 4984, 5076, 5196, 5346, 5522, 5707, 5873, 5989, 6030, 5984, 5851, 5656, 5432, 5218, 5038, 4906, 4832, },
-	/* LAT:  60 */ {  5393, 5376, 5400, 5459, 5543, 5637, 5724, 5792, 5827, 5820, 5768, 5675, 5556, 5430, 5312, 5218, 5153, 5120, 5119, 5148, 5205, 5293, 5412, 5559, 5726, 5894, 6040, 6142, 6183, 6160, 6077, 5951, 5807, 5665, 5543, 5450, 5393, },
-	/* LAT:  70 */ {  5726, 5703, 5697, 5707, 5727, 5753, 5778, 5793, 5795, 5777, 5740, 5684, 5616, 5541, 5468, 5405, 5358, 5331, 5327, 5348, 5393, 5463, 5555, 5664, 5781, 5895, 5993, 6065, 6103, 6106, 6077, 6024, 5957, 5887, 5821, 5766, 5726, },
-	/* LAT:  80 */ {  5790, 5772, 5756, 5743, 5733, 5723, 5713, 5702, 5688, 5670, 5649, 5625, 5599, 5573, 5549, 5529, 5516, 5511, 5517, 5532, 5558, 5594, 5636, 5684, 5734, 5782, 5824, 5858, 5882, 5895, 5897, 5889, 5875, 5855, 5833, 5811, 5790, },
-	/* LAT:  90 */ {  5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, },
+// Date: 2024.41257,
+static constexpr const int16_t totalintensity_table[19][37] {
+	//    LONGITUDE:   -180,  -170,  -160,  -150,  -140,  -130,  -120,  -110,  -100,   -90,   -80,   -70,   -60,   -50,   -40,   -30,   -20,   -10,     0,    10,    20,    30,    40,    50,    60,    70,    80,    90,   100,   110,   120,   130,   140,   150,   160,   170,   180,
+	/* LAT: -90 */ {  26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, },
+	/* LAT: -80 */ {  29615, 29299, 28908, 28453, 27945, 27395, 26815, 26218, 25619, 25032, 24474, 23959, 23502, 23115, 22808, 22592, 22476, 22469, 22578, 22807, 23157, 23621, 24187, 24836, 25543, 26279, 27014, 27716, 28358, 28914, 29365, 29700, 29913, 30004, 29979, 29846, 29615, },
+	/* LAT: -70 */ {  30814, 30152, 29407, 28592, 27711, 26765, 25759, 24706, 23635, 22584, 21596, 20706, 19938, 19300, 18794, 18422, 18195, 18139, 18285, 18666, 19302, 20195, 21322, 22642, 24092, 25601, 27090, 28480, 29701, 30696, 31432, 31894, 32090, 32046, 31794, 31371, 30814, }, // WARNING! black out zone
+	/* LAT: -60 */ {  30256, 29303, 28305, 27273, 26195, 25041, 23783, 22419, 20989, 19575, 18274, 17168, 16296, 15644, 15162, 14803, 14555, 14459, 14593, 15046, 15888, 17136, 18753, 20656, 22732, 24862, 26925, 28803, 30388, 31598, 32391, 32767, 32766, 32449, 31882, 31131, 30256, }, // WARNING! black out zone
+	/* LAT: -50 */ {  28585, 27448, 26301, 25162, 24017, 22813, 21484, 19998, 18389, 16768, 15296, 14132, 13352, 12914, 12677, 12503, 12330, 12195, 12223, 12590, 13464, 14914, 16881, 19207, 21698, 24175, 26497, 28540, 30193, 31368, 32034, 32224, 32006, 31456, 30654, 29673, 28585, },
+	/* LAT: -40 */ {  26388, 25175, 23965, 22783, 21629, 20463, 19212, 17819, 16281, 14687, 13231, 12153, 11593, 11476, 11568, 11662, 11671, 11598, 11530, 11680, 12349, 13743, 15828, 18371, 21056, 23615, 25886, 27774, 29196, 30107, 30531, 30536, 30188, 29538, 28642, 27566, 26388, },
+	/* LAT: -30 */ {  23875, 22687, 21506, 20350, 19238, 18163, 17088, 15953, 14699, 13348, 12090, 11211, 10886, 11019, 11346, 11688, 11997, 12225, 12312, 12367, 12723, 13749, 15575, 17999, 20611, 23034, 25059, 26587, 27567, 28051, 28176, 28039, 27646, 26992, 26106, 25040, 23875, },
+	/* LAT: -20 */ {  21154, 20100, 19069, 18060, 17097, 16201, 15373, 14569, 13693, 12716, 11782, 11141, 10963, 11194, 11640, 12182, 12805, 13413, 13818, 13974, 14106, 14617, 15845, 17765, 19985, 22068, 23750, 24872, 25370, 25402, 25252, 25023, 24621, 23994, 23174, 22204, 21154, },
+	/* LAT: -10 */ {  18558, 17760, 17002, 16280, 15615, 15025, 14516, 14058, 13572, 13001, 12403, 11927, 11727, 11873, 12314, 12952, 13712, 14475, 15055, 15339, 15405, 15556, 16199, 17471, 19086, 20662, 21941, 22713, 22869, 22606, 22275, 21968, 21540, 20930, 20196, 19388, 18558, },
+	/* LAT:   0 */ {  16709, 16246, 15828, 15466, 15193, 15003, 14869, 14765, 14634, 14389, 14000, 13543, 13167, 13043, 13274, 13791, 14440, 15095, 15645, 15999, 16143, 16257, 16647, 17443, 18484, 19534, 20407, 20910, 20925, 20587, 20154, 19705, 19162, 18518, 17858, 17244, 16709, },
+	/* LAT:  10 */ {  16075, 15916, 15810, 15786, 15900, 16127, 16390, 16638, 16796, 16740, 16399, 15842, 15234, 14793, 14691, 14911, 15314, 15798, 16286, 16698, 17008, 17318, 17762, 18355, 19031, 19718, 20303, 20638, 20637, 20325, 19783, 19078, 18284, 17502, 16834, 16355, 16075, },
+	/* LAT:  20 */ {  16646, 16650, 16769, 17024, 17466, 18051, 18672, 19233, 19618, 19672, 19310, 18619, 17813, 17152, 16804, 16764, 16949, 17315, 17793, 18272, 18719, 19208, 19759, 20311, 20853, 21411, 21916, 22235, 22275, 21976, 21295, 20297, 19178, 18148, 17345, 16845, 16646, },
+	/* LAT:  30 */ {  18228, 18245, 18498, 18973, 19662, 20491, 21341, 22097, 22620, 22746, 22388, 21634, 20724, 19941, 19447, 19240, 19272, 19535, 19972, 20460, 20944, 21470, 22044, 22622, 23213, 23845, 24444, 24864, 24980, 24684, 23906, 22724, 21376, 20127, 19146, 18510, 18228, },
+	/* LAT:  40 */ {  20676, 20646, 20945, 21535, 22339, 23247, 24138, 24900, 25411, 25538, 25208, 24491, 23592, 22759, 22150, 21796, 21685, 21808, 22117, 22515, 22946, 23429, 23995, 24649, 25392, 26192, 26939, 27472, 27650, 27367, 26593, 25423, 24081, 22812, 21778, 21055, 20676, },
+	/* LAT:  50 */ {  23666, 23603, 23852, 24371, 25074, 25848, 26580, 27175, 27541, 27593, 27291, 26681, 25898, 25111, 24452, 23986, 23733, 23688, 23819, 24073, 24420, 24871, 25459, 26197, 27059, 27964, 28776, 29343, 29542, 29310, 28661, 27703, 26610, 25561, 24680, 24034, 23666, },
+	/* LAT:  60 */ {  26412, 26324, 26436, 26723, 27132, 27589, 28019, 28351, 28524, 28493, 28240, 27789, 27209, 26590, 26018, 25556, 25239, 25081, 25078, 25221, 25503, 25932, 26515, 27239, 28055, 28877, 29590, 30087, 30289, 30171, 29764, 29152, 28446, 27753, 27153, 26695, 26412, },
+	/* LAT:  70 */ {  28043, 27927, 27896, 27940, 28040, 28166, 28285, 28364, 28372, 28290, 28109, 27838, 27503, 27138, 26784, 26477, 26247, 26116, 26097, 26198, 26422, 26766, 27217, 27750, 28323, 28879, 29358, 29708, 29894, 29908, 29765, 29507, 29181, 28836, 28513, 28242, 28043, }, // WARNING! black out zone
+	/* LAT:  80 */ {  28358, 28266, 28189, 28125, 28072, 28025, 27977, 27922, 27854, 27769, 27667, 27549, 27422, 27295, 27178, 27083, 27020, 26999, 27025, 27102, 27229, 27402, 27613, 27847, 28090, 28324, 28531, 28697, 28813, 28875, 28885, 28848, 28776, 28680, 28572, 28462, 28358, }, // WARNING! black out zone
+	/* LAT:  90 */ {  27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, }, // WARNING! black out zone
 };
-static constexpr float WMM_STRENGTH_MIN_GS = 22.2; // latitude: -30, longitude: -60
-static constexpr float WMM_STRENGTH_MAX_GS = 66.9; // latitude: -60, longitude: 140
-static constexpr float WMM_STRENGTH_MEAN_GS = 46.4;
-static constexpr float WMM_STRENGTH_MEDIAN_GS = 48.8;
+static constexpr float WMM_TOTALINTENSITY_SCALE_TO_NANOTESLA = 2.04183477f;
+static constexpr float WMM_TOTALINTENSITY_MIN_NANOTESLA = 22226.9f; // latitude: -30, longitude: -60
+static constexpr float WMM_TOTALINTENSITY_MAX_NANOTESLA = 66904.8f; // latitude: -60, longitude: 130


@@ -227,8 +227,8 @@ void AttitudeEstimatorQ::update_gps_position()
 		if (_vehicle_gps_position_sub.update(&gps)) {
 			if (_param_att_mag_decl_a.get() && (gps.eph < 20.0f)) {
 				// set magnetic declination automatically
-				update_mag_declination(get_mag_declination_radians((float)gps.latitude_deg,
-						       (float)gps.longitude_deg));
+				float mag_decl_deg = get_mag_declination_degrees(gps.latitude_deg, gps.longitude_deg);
+				update_mag_declination(math::radians(mag_decl_deg));
 			}
 		}
 	}
@@ -74,6 +74,8 @@
 #include <uORB/topics/mavlink_log.h>
 #include <uORB/topics/tune_control.h>

+using namespace time_literals;
+
 typedef enum VEHICLE_MODE_FLAG {
 	VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED  = 1,   /* 0b00000001 Reserved for future use. | */
 	VEHICLE_MODE_FLAG_TEST_ENABLED         = 2,   /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */
@@ -1907,6 +1909,77 @@ void Commander::run()
 			_vehicle_status.timestamp = hrt_absolute_time();
 			_vehicle_status_pub.publish(_vehicle_status);

+			// HACK: inject aileron failure (stuck to center) through here. Assume servo 0 is an aileron.
+			manual_control_setpoint_s manual_control_setpoint;
+			_manual_control_setpoint_sub.copy(&manual_control_setpoint);
+
+			bool aileron_failure_injected = false;
+			bool servo_failure_detected = false;
+			int failed_servo_bitmask = 0;
+
+			if (!_param_com_srv_fail_cl.get()) {
+
+				if (_param_com_srv_fail_ipt.get() == 1) {
+
+					aileron_failure_injected =  manual_control_setpoint.yaw > 0.6f;
+
+				} else if (_param_com_srv_fail_ipt.get() == 2) {
+					aileron_failure_injected =  manual_control_setpoint.aux1 > 0.6f;
+				}
+
+				if (aileron_failure_injected) {
+					if (_time_failure_injected == 0) {
+						_time_failure_injected = hrt_absolute_time();
+					}
+
+					servo_failure_detected = hrt_elapsed_time(&_time_failure_injected) > 1_s; // trigger detection 2s after injection
+
+				} else {
+					_time_failure_injected = 0;
+					servo_failure_detected = false;
+				}
+
+				if (_param_com_srv_fail_nr.get() == 0) {
+					failed_servo_bitmask = 1 << 0;
+
+				} else if (_param_com_srv_fail_nr.get() == 1) {
+					failed_servo_bitmask = 1 << 1;
+
+				} else if (_param_com_srv_fail_nr.get() == 2) {
+					failed_servo_bitmask = (1 << 0) + (1 << 1);
+				}
+
+			} else {
+				_time_failure_injected = 0; // reset open loop variable
+				aileron_failure_injected = false;
+
+				if (_failure_detector_ext_servo.updated()) {
+					_failure_detector_ext_servo.update();
+					_time_last_ext_fail_topic = now;
+
+				}
+
+				if ((_time_last_ext_fail_topic > 0UL) && ((now - _time_last_ext_fail_topic) < 500_ms)) {
+					// aileron_failure_injected = _failure_detector_ext_servo.get().fd_servo;
+					servo_failure_detected = _failure_detector_ext_servo.get().fd_servo;
+					failed_servo_bitmask = _failure_detector_ext_servo.get().servo_failure_mask;
+				}
+			}
+
+			// Inform operator about detected servo failures
+			if (servo_failure_detected && failed_servo_bitmask > _reported_servo_fail) {
+				_reported_servo_fail = failed_servo_bitmask;
+
+				if ((_reported_servo_fail == (1 << 0)) || (_reported_servo_fail == (1 << 1))) {
+					events::send(events::ID("single_aileron_failure"), events::Log::Critical,
+						     "Actuator failure detected: single aileron fault");
+
+				} else if (_reported_servo_fail == ((1 << 1) + (1 << 0))) {
+					events::send(events::ID("double_aileron_failure"), events::Log::Critical,
+						     "Actuator failure detected: double aileron fault");
+				}
+			}
+
 			// failure_detector_status publish
 			failure_detector_status_s fd_status{};
 			fd_status.fd_roll = _failure_detector.getStatusFlags().roll;
@@ -1917,8 +1990,11 @@ void Commander::run()
 			fd_status.fd_battery = _failure_detector.getStatusFlags().battery;
 			fd_status.fd_imbalanced_prop = _failure_detector.getStatusFlags().imbalanced_prop;
 			fd_status.fd_motor = _failure_detector.getStatusFlags().motor;
+			fd_status.fd_servo = servo_failure_detected;
 			fd_status.imbalanced_prop_metric = _failure_detector.getImbalancedPropMetric();
 			fd_status.motor_failure_mask = _failure_detector.getMotorFailures();
+			fd_status.servo_failure_mask = servo_failure_detected ? failed_servo_bitmask : 0;
+			fd_status.servo_to_center_mask = aileron_failure_injected ? failed_servo_bitmask : 0;
 			fd_status.timestamp = hrt_absolute_time();
 			_failure_detector_status_pub.publish(fd_status);
 		}
@@ -69,6 +69,7 @@
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/cpuload.h>
 #include <uORB/topics/distance_sensor.h>
+#include <uORB/topics/failure_detector_ext_servo.h>
 #include <uORB/topics/iridiumsbd_status.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/mission_result.h>
@@ -280,6 +281,10 @@ private:
 	bool _have_taken_off_since_arming{false};
 	bool _status_changed{true};

+	hrt_abstime _time_failure_injected{0};
+	hrt_abstime _time_last_ext_fail_topic{0U};
+	uint16_t _reported_servo_fail{UINT16_C(0)};
+
 	vehicle_land_detected_s	_vehicle_land_detected{};

 	// commander publications
@@ -308,6 +313,7 @@ private:

 	uORB::SubscriptionData<mission_result_s>		_mission_result_sub{ORB_ID(mission_result)};
 	uORB::SubscriptionData<offboard_control_mode_s>		_offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
+	uORB::SubscriptionData<failure_detector_ext_servo_s> 	_failure_detector_ext_servo{ORB_ID(failure_detector_ext_servo)};

 	// Publications
 	uORB::Publication<actuator_armed_s>			_actuator_armed_pub{ORB_ID(actuator_armed)};
@@ -349,6 +355,9 @@ private:
 		(ParamInt<px4::params::COM_RC_OVERRIDE>)    _param_com_rc_override,
 		(ParamInt<px4::params::COM_FLIGHT_UUID>)    _param_flight_uuid,
 		(ParamInt<px4::params::COM_TAKEOFF_ACT>)    _param_takeoff_finished_action,
-		(ParamFloat<px4::params::COM_CPU_MAX>)      _param_com_cpu_max
+		(ParamFloat<px4::params::COM_CPU_MAX>)      _param_com_cpu_max,
+		(ParamInt<px4::params::COM_SRV_FAIL_IPT>)   _param_com_srv_fail_ipt,
+		(ParamInt<px4::params::COM_SRV_FAIL_NR>)    _param_com_srv_fail_nr,
+		(ParamBool<px4::params::COM_SRV_FAIL_CL>)   _param_com_srv_fail_cl
 	)
 };
@@ -1044,3 +1044,31 @@ PARAM_DEFINE_FLOAT(COM_THROW_SPEED, 5);
 * @increment 1
 */
 PARAM_DEFINE_INT32(COM_FLTT_LOW_ACT, 3);
+
+/**
+ * Manual control source to inject servo failure
+ *
+ * @group Commander
+ * @value 0 Disabled
+ * @value 1 yaw stick
+ * @value 2 aux1
+ */
+PARAM_DEFINE_INT32(COM_SRV_FAIL_IPT, 0);
+
+/**
+ * Index of the servos to fail
+ *
+ * @group Commander
+ * @value 0 Lock servo 0
+ * @value 1 Lock servo 1
+ * @value 2 Lock servo 1 and 2
+ */
+PARAM_DEFINE_INT32(COM_SRV_FAIL_NR, 0);
+
+/**
+ * Flag if closed loop servo detection and allocation shall be used.
+ *
+ * @group Commander
+ * @boolean
+ */
+PARAM_DEFINE_INT32(COM_SRV_FAIL_CL, 0);
@@ -237,11 +237,8 @@ static float get_sphere_radius()

 		if (gps_sub.copy(&gps)) {
 			if (hrt_elapsed_time(&gps.timestamp) < 100_s && (gps.fix_type >= 2) && (gps.eph < 1000)) {
-				const double lat = gps.latitude_deg;
-				const double lon = gps.longitude_deg;
-
 				// magnetic field data returned by the geo library using the current GPS position
-				return get_mag_strength_gauss(lat, lon);
+				return get_mag_strength_gauss(gps.latitude_deg, gps.longitude_deg);
 			}
 		}
 	}
@@ -954,13 +951,14 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
 	return result;
 }

-int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radians, float latitude, float longitude)
+int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radians,
+			     float latitude_deg, float longitude_deg)
 {
 	// magnetometer quick calibration
 	//  if GPS available use world magnetic model to zero mag offsets
 	bool mag_earth_available = false;

-	if (PX4_ISFINITE(latitude) && PX4_ISFINITE(longitude)) {
+	if (PX4_ISFINITE(latitude_deg) && PX4_ISFINITE(longitude_deg)) {
 		mag_earth_available = true;

 	} else {
@@ -969,8 +967,8 @@ int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radian

 		if (vehicle_gps_position_sub.copy(&gps)) {
 			if ((gps.timestamp != 0) && (gps.eph < 1000)) {
-				latitude = (float)gps.latitude_deg;
-				longitude = (float)gps.longitude_deg;
+				latitude_deg = (float)gps.latitude_deg;
+				longitude_deg = (float)gps.longitude_deg;
 				mag_earth_available = true;
 			}
 		}
@@ -981,14 +979,13 @@ int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radian
 		return PX4_ERROR;

 	} else {
-
 		// magnetic field data returned by the geo library using the current GPS position
-		const float mag_declination_gps = get_mag_declination_radians(latitude, longitude);
-		const float mag_inclination_gps = get_mag_inclination_radians(latitude, longitude);
-		const float mag_strength_gps = get_mag_strength_gauss(latitude, longitude);
+		const float declination_rad = math::radians(get_mag_declination_degrees(latitude_deg, longitude_deg));
+		const float inclination_rad = math::radians(get_mag_inclination_degrees(latitude_deg, longitude_deg));
+		const float field_strength_gauss = get_mag_strength_gauss(latitude_deg, longitude_deg);

-		const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -mag_inclination_gps, mag_declination_gps)) * Vector3f(mag_strength_gps,
-						0, 0);
+		const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -inclination_rad, declination_rad))
+						* Vector3f(field_strength_gauss, 0, 0);

 		uORB::Subscription vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
 		vehicle_attitude_s attitude{};
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
Silvan Fuhrer	619812cbdd	CA testing hacks, external failure message, sitl tuning, disabling of rate controller saturation Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-06-10 11:50:58 +02:00
Silvan Fuhrer	94e98c2457	matrix: change matrix.print() back to pring all elements, don't assume symmetric matrix Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-06-10 11:16:32 +02:00
Silvan Fuhrer	f717f5bcc9	CA: update params always, also when armed Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-06-10 11:16:32 +02:00
Silvan Fuhrer	2ef7c45d97	CA: log torque setpoints Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-06-10 11:16:32 +02:00
Silvan Fuhrer	9bdc898ceb	CA: extend status print Print not only effectiveness matrix, but also raw and normalized mixing matrix (inverted effectiveness). Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-06-10 11:16:32 +02:00
Silvan Fuhrer	1fcd5b466f	CA: handle servo failure detection - add fd_servo to failure detector status - inside control allocation: if one axis is no longer controllable with servos then enable auxiliary motors for it's control (VTOL only) Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-06-10 11:16:32 +02:00
Silvan Fuhrer	d27ac8a2cb	CA: add fixed-wing airmode Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-06-10 11:16:32 +02:00
Silvan Fuhrer	3370b4583a	CA: enable differential thrust on matrix 1 in VTOL FW Introduce differential thrust SCALE and WEIGHT params that are used to configure differential thrust on a VTOL vehicle. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-06-10 11:16:32 +02:00
Silvan Fuhrer	1695babe8a	CA: Differentiate between STOPPED and DISABLED actuator STOPPED: actuators are NOT remoted from the allocation, but output is set to NAN. Intended for completely stopping a motor that anyway already had a very low thrust setpoint. DISABLED: Remove actuator from effectiveness matrix and set output to NAN. Intended to tell the allocation when an actuator shouldn't be used in the current flight state to allocate torque. Additionally added a method to enable/disable motors that are considered auxiliary (e.g. hover motors in FW flight on a Standard VTOL). Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-06-10 11:15:27 +02:00
Silvan Fuhrer	41393f0618	CA: rename ControlAllocation::getEffectivenessMatrix to getEffectivenessMatrixAllocation Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-06-10 11:15:27 +02:00
Silvan Fuhrer	6aa2c0b6dc	CA: change how normalization is done Instead of using the same normalization algorithm for multicopters and planes, this commit splits it up. The multicopter one is left unchanged in it's core (has special yaw handling e.g.), while the new (FW one) looks at all axes separately and scales the mixer matrix such that an input of 1 results in maximum actuator control setpoint. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-06-10 11:15:27 +02:00
Silvan Fuhrer	f75a0bec33	CA: make flight_phase class variable Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-06-10 11:15:27 +02:00
Silvan Fuhrer	fd9d0211aa	CA: rename normalize_rpy to normalize_as_planar_mc To make clear that this normalization method is optimized for planar multicopters. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-06-10 11:15:27 +02:00
Ruoyu Wang	b9aa8818a4	kakute f7/h7/h7mini/h7v2: fix EKF2_IMU_CTRL typo	2024-06-10 11:08:33 +02:00
Beat Küng	52ac9336c4	boards: change default IP from 192.168.0.3 to 10.41.10.2 192.168.0.x is often used by routers for WIFI/ethernet networks, and thus can create conflicts. This can happen for example if a companion is connected to the FMU via ethernet and at the same time connects to a WIFI network as DHCP client.	2024-06-10 11:05:50 +02:00
Beat Küng	a90cdcfe80	boards/px4/fmu-v{5,6}x: enable uxrce_dds_client on ethernet by default	2024-06-10 11:05:50 +02:00
Beat Küng	2524ac8c2b	boards/px4/fmu-v{5,6}x: do not enable mavlink on ethernet for skynode	2024-06-10 11:05:50 +02:00
asifpatankar	746322d6d2	Update ubuntu.sh with Linux Mint 21.3 version	2024-06-10 16:20:48 +12:00
Jacob Dahl	2882e5c4e1	platforms: nuttx: SerialImpl: fix hang if baudrate is 0 (#23238 )	2024-06-07 22:08:21 -06:00
chfriedrich98	5c64a3ed93	Rover Ackermann module (#23024 ) New module handling Ackermann rover guidance and control. Only enabled in SITL and in the rover builds for now.	2024-06-07 17:15:12 +02:00
Silvan Fuhrer	831160389e	VTOL Tailsitter: remove throttle spike also for quad-chute Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-06-07 16:11:59 +02:00
Silvan Fuhrer	199a2f43be	VTOL Tailsitter: treat back transition abort like a front transition for throttle blending. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-06-07 16:11:59 +02:00
Silvan Fuhrer	b5988ed38f	VTOL Tailsitter: add back transition throttle blending Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-06-07 16:11:59 +02:00
Silvan Fuhrer	f119b15ff1	VTOL Tailsitter: set differential thrust to 0 in first 50ms of front transition Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-06-07 16:11:59 +02:00
Silvan Fuhrer	77a3099154	VTOL Tailsitter: fix motor spikes to 0 when starting back transition Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-06-07 16:11:59 +02:00
Silvan Fuhrer	3a2b973aba	VTOL Tailsitter: add front transition throttle blending Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-06-07 16:11:59 +02:00
Silvan Fuhrer	a081354933	VTOL tailsitter: fix motor spike to 0 when finishing front transition Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-06-07 16:11:59 +02:00
Silvan Fuhrer	1f7b4843dd	FW position control: specify modes where FW_PN_R_SLEW_MAX applies Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-06-07 09:32:23 +02:00
Silvan Fuhrer	b8998933c9	AttitudeSetpoint.msg: FRD instead of NED for body frame Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-06-07 09:32:23 +02:00
Silvan Fuhrer	b53d2cdf39	battery: reset current filter when transitioning to FW (#22256 ) VTOLs consume a lot more power in hover copared to fixed-wing fligt. The remaining flight time thus should reset if one switches from MC to FW, as otherwise it takes several minutes until the estimate goes down.	2024-06-07 09:28:37 +02:00
Joao Mario Lago	2ce92a678d	arch.sh: Fix syntax error Fix error of script failing with following error: PX4-Autopilot/Tools/setup/arch.sh: line 99: syntax error near unexpected token `;'	2024-06-06 21:55:05 -04:00
Julian Oes	f6b65e68cc	boards: update all bootloaders	2024-06-07 13:16:17 +12:00
Julian Oes	7137780654	Makefile: add missing bootloader targets Quite a few were missing, and everything on one line was hard to diff.	2024-06-07 13:16:17 +12:00
Chris Lalancette	b1bf0ff888	Remove argparse from the requirements.txt. The argparse module has been builtin to Python since Python 3.2, released in 2011 (see https://docs.python.org/3/whatsnew/3.2.html). Further, the argparse pip module has not been released or updated since 2015, and lacks some of the features of the modern, built-in argparse. Drop the pip installed version in favor of the built-in version. Signed-off-by: Chris Lalancette <clalancette@gmail.com>	2024-06-06 10:09:11 -04:00
Daniel Agar	fb659ae200	ekf2: stopMagFusion() reset yaw_align if mag heading was active - we also need to clear mag_aligned_in_flight	2024-06-06 10:08:20 -04:00
Daniel Agar	bfe3c86aeb	ekf2: merge mag yaw angle observability into heading consistency - the additional hyteresis logic for "yaw angle observable" was needed when it controlled time dependent mag_3d	2024-06-06 10:07:31 -04:00
Rowan Dempster	bc51eb37eb	Only close server when errno is not EINTR	2024-06-06 07:40:12 +02:00
Matthias Grob	68769ea0ec	mavlink: use reference instead of pointer to access the MAVLink instance from protocol classes	2024-06-05 16:13:52 +02:00
Matthias Grob	9a7a977625	mavlink_receiver: put all message handling in the same function	2024-06-05 16:13:52 +02:00
Daniel Agar	157f7cf40b	simplify world_magnetic_model interface (degrees in, degrees out) - this hopefully helps avoid accidental mis-use - try to clarify units everywhere	2024-06-04 09:14:36 -04:00
Daniel Agar	5fa3b9d86a	lib/world_magnetic_model: fetch_noaa_table.py refactor and scaling improvements - upate to NOAA grid API to build WMM table in one pass - refactor declination/inclination/totalintensity table printing to shared method - compute scaling factor to maximize resolution	2024-06-04 09:14:36 -04:00
Jacob Dahl	22a38c0c6d	drivers/imu: sch16t improvements (#23221 ) * individual perf counters for frame errors * don't treat saturation as an error * added parameters for gyro/accel LPF and decimation ratio	2024-06-03 22:15:00 -04:00
Alex Klimaj	36ec576c0f	boards: ark-pi6x remove ekf delay param defaults (#23177 )	2024-06-03 17:52:17 -08:00
Matteo Del Seppia	a8617cf681	Fix float and uint64_t comparison (#23199 ) fix: ControlAllocator float and int comparison bug There was an incorrect comparison between a float variable `dt` and a `uint64_t` value representing 5 milliseconds (`5_ms`). As a result, `do_update` could never become true even if the last torque setpoint was received more than 5 milliseconds before. To solve this, the `5_ms` value has been converted to seconds (0.005f) for the comparison with `dt`.	2024-06-03 17:48:38 -08:00
Alex Klimaj	de0e73d505	vscode cmake-variants fix ark_septentrio-gps (#23222 )	2024-06-03 19:43:26 -06:00
Matteo Del Seppia	de5087ae3e	Patch for issue #22818 (#23170 ) When calling "differential_drive stop" a missing return statement was causing a segmentation fault due to access to already freed memory.	2024-06-03 21:10:28 +02:00
Peter van der Perk	c22f725d85	fmu-v6xrt: Support RC telemetry	2024-06-03 08:31:18 -04:00
Peter van der Perk	7fab93ede8	fmu-v6xrt: Fix flash configuration Fixes correct dummy cycle count of 20	2024-06-03 08:26:48 -04:00
bresch	0a665a526c	ekf2: add mag type init In this mode, the mag is used to inilialize the heading. During flight, the heading is predicted using gyros and corrected by GNSS measurements if available.	2024-06-03 13:28:12 +02:00