rc_input: singlewire use push_pull

2026-05-27 09:50:05 +08:00 · 2024-06-01 19:15:42 +02:00
145 changed files with 3982 additions and 5498 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 - 2024 PX4 Development Team. All rights reserved.
+# Copyright (c) 2015 - 2020 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -323,32 +323,7 @@ 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 \
-	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
+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
 	git status

 .PHONY: coverity_scan
@@ -1,70 +0,0 @@
-#!/bin/sh
-#
-# @name Monocopter SITL
-#
-
-. ${R}etc/init.d/rc.fw_defaults
-
-param set-default FW_LND_ANG 8
-
-param set-default NPFG_PERIOD 12
-
-param set-default FW_PR_P 0.9
-param set-default FW_PR_FF 0.5
-param set-default FW_PR_I 0.5
-param set-default TRIM_PITCH -0.15
-
-param set-default FW_PSP_OFF 2
-param set-default FW_P_LIM_MIN -15
-
-param set-default FW_RR_FF 0.5
-param set-default FW_RR_P 0.3
-param set-default FW_RR_I 0.5
-
-param set-default FW_YR_FF 0.5
-param set-default FW_YR_P 0.6
-param set-default FW_YR_I 0.5
-
-param set-default FW_SPOILERS_LND 0.4
-
-param set-default FW_THR_MAX 0.6
-param set-default FW_THR_MIN 0.05
-param set-default FW_THR_TRIM 0.25
-
-param set-default FW_T_CLMB_MAX 8
-param set-default FW_T_SINK_MAX 2.7
-param set-default FW_T_SINK_MIN 2.2
-
-param set-default FW_W_EN 1
-
-param set-default MIS_TAKEOFF_ALT 30
-
-param set-default NAV_ACC_RAD 15
-param set-default NAV_DLL_ACT 2
-
-param set-default RWTO_TKOFF 1
-
-param set-default CA_AIRFRAME 1
-
-param set-default CA_ROTOR_COUNT 1
-param set-default CA_ROTOR0_PX 0.3
-
-param set-default CA_SV_CS_COUNT 6
-param set-default CA_SV_CS0_TRQ_R -0.5
-param set-default CA_SV_CS0_TYPE 1
-param set-default CA_SV_CS1_TRQ_R 0.5
-param set-default CA_SV_CS1_TYPE 2
-param set-default CA_SV_CS2_TRQ_P 1
-param set-default CA_SV_CS2_TYPE 3
-param set-default CA_SV_CS3_TRQ_Y 1
-param set-default CA_SV_CS3_TYPE 4
-param set-default CA_SV_CS4_TYPE 9
-param set-default CA_SV_CS5_TYPE 10
-param set-default PWM_MAIN_FUNC3 204
-param set-default PWM_MAIN_FUNC4 205
-param set-default PWM_MAIN_FUNC5 101
-param set-default PWM_MAIN_FUNC6 201
-param set-default PWM_MAIN_FUNC7 202
-param set-default PWM_MAIN_FUNC8 203
-param set-default PWM_MAIN_FUNC9 206
-param set-default PWM_MAIN_REV 256
@@ -52,4 +52,3 @@ param set-default SIM_GZ_EC_MAX3 1000
 param set-default SIM_GZ_EC_MAX4 1000

 param set-default MPC_THR_HOVER 0.60
-param set-default NAV_DLL_ACT 2
@@ -101,9 +101,8 @@ param set-default MPC_XY_VEL_I_ACC 4
 param set-default MPC_XY_VEL_D_ACC 0.1

 param set-default NAV_ACC_RAD 5
-param set-default NAV_DLL_ACT 2

 param set-default VT_FWD_THRUST_EN 4
-param set-default VT_F_TRANS_THR 0.3
+param set-default VT_F_TRANS_THR 0.75
 param set-default VT_TYPE 2
 param set-default FD_ESCS_EN 0
@@ -132,5 +132,3 @@ param set-default SIM_GZ_EC_MAX1 1100
 param set-default SIM_GZ_EC_MAX2 1100
 param set-default SIM_GZ_EC_MAX3 1100
 param set-default SIM_GZ_EC_MAX4 1100
-
-param set-default NAV_DLL_ACT 2
@@ -67,7 +67,6 @@ px4_add_romfs_files(
 	1061_gazebo-classic_r1_rover
 	1062_flightgear_tf-r1
 	1070_gazebo-classic_boat
-	1090_gazebo-classic_monocopter

 	2507_gazebo-classic_cloudship

@@ -84,13 +84,6 @@ 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
@@ -1,12 +0,0 @@
-#!/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,12 +149,6 @@ 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
@@ -1,11 +0,0 @@
-#!/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
@@ -1,13 +0,0 @@
-#!/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,15 +41,6 @@ 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,4 +1,5 @@
 argcomplete
+argparse>=1.2
 cerberus
 coverage
 empy==3.3.4
@@ -66,8 +66,6 @@ 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


@@ -148,7 +146,7 @@ if [[ $INSTALL_NUTTX == "true" ]]; then
 		util-linux \
 		vim-common \
 		;
-	if [[ "${UBUNTU_RELEASE}" == "20.04" || "${UBUNTU_RELEASE}" == "22.04" || "${UBUNTU_RELEASE}" == "21.3" ]]; then
+	if [[ "${UBUNTU_RELEASE}" == "20.04" || "${UBUNTU_RELEASE}" == "22.04" ]]; then
 		sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
 		kconfig-frontends \
 		;
@@ -207,8 +205,6 @@ 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
@@ -232,17 +228,6 @@ 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=0xA290AFE
-CONFIG_NETINIT_DRIPADDR=0xA290AFE
+CONFIG_NETINIT_DNSIPADDR=0XC0A800FE
+CONFIG_NETINIT_DRIPADDR=0XC0A800FE
 CONFIG_NETINIT_MONITOR=y
 CONFIG_NETINIT_THREAD=y
 CONFIG_NETINIT_THREAD_PRIORITY=49
@@ -32,10 +32,19 @@ 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
@@ -47,7 +47,7 @@
 #define GPIO_BTN_SAFETY       /* PB15 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTB|GPIO_PIN15)

 /* Safety LED */
-#define GPIO_LED_SAFETY       /* PA1  */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN1)
+#define GPIO_LED_SAFETY       /* PA1  */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN1)

 /* Tone alarm output. */
 #define TONE_ALARM_TIMER             2    /* timer 2 */
@@ -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=0xA290AFE
-CONFIG_NETINIT_DRIPADDR=0xA290AFE
+CONFIG_NETINIT_DNSIPADDR=0XC0A800FE
+CONFIG_NETINIT_DRIPADDR=0XC0A800FE
 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_CTRL 7
+param set-default EKF2_IMU_CONTROL 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_CTRL 7
+param set-default EKF2_IMU_CONTROL 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_CTRL 7
+param set-default EKF2_IMU_CONTROL 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_CTRL 7
+param set-default EKF2_IMU_CONTROL 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=0xA290AFE
-CONFIG_NETINIT_DRIPADDR=0xA290AFE
+CONFIG_NETINIT_DNSIPADDR=0XC0A800FE
+CONFIG_NETINIT_DRIPADDR=0XC0A800FE
 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=0xA290AFE
-CONFIG_NETINIT_DRIPADDR=0xA290AFE
+CONFIG_NETINIT_DNSIPADDR=0XC0A800FE
+CONFIG_NETINIT_DRIPADDR=0XC0A800FE
 CONFIG_NETINIT_MONITOR=y
 CONFIG_NETINIT_THREAD=y
 CONFIG_NETINIT_THREAD_PRIORITY=49
@@ -19,4 +19,3 @@ 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,25 +3,23 @@
 # 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", IP=10.41.10.1 config for uxrce_dds_client
+	# Skynode: use the "custom participant" 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=0xA290AFE
-CONFIG_NETINIT_DRIPADDR=0xA290AFE
+CONFIG_NETINIT_DNSIPADDR=0XC0A800FE
+CONFIG_NETINIT_DRIPADDR=0XC0A800FE
 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=0xA290AFE
-CONFIG_NETINIT_DRIPADDR=0xA290AFE
+CONFIG_NETINIT_DNSIPADDR=0XC0A800FE
+CONFIG_NETINIT_DRIPADDR=0XC0A800FE
 CONFIG_NETINIT_THREAD=y
 CONFIG_NETINIT_THREAD_PRIORITY=49
 CONFIG_NETUTILS_TELNETD=y
@@ -14,5 +14,3 @@ 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,4 +14,3 @@ 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,4 +14,3 @@ 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,6 +3,15 @@
 # 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
@@ -11,19 +20,8 @@ param set-default SENS_EN_INA226 0

 if ver hwbasecmp 009 010 011
 then
-	# Skynode: use the "custom participant", IP=10.41.10.1 config for uxrce_dds_client
+	# Skynode: use the "custom participant" 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=0xA290AFE
-CONFIG_NETINIT_DRIPADDR=0xA290AFE
+CONFIG_NETINIT_DNSIPADDR=0XC0A800FE
+CONFIG_NETINIT_DRIPADDR=0XC0A800FE
 CONFIG_NETINIT_MONITOR=y
 CONFIG_NETINIT_THREAD=y
 CONFIG_NETINIT_THREAD_PRIORITY=49
@@ -14,4 +14,3 @@ 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=0xA290AFE
-CONFIG_NETINIT_DRIPADDR=0xA290AFE
+CONFIG_NETINIT_DNSIPADDR=0XC0A800FE
+CONFIG_NETINIT_DRIPADDR=0XC0A800FE
 CONFIG_NETINIT_MONITOR=y
 CONFIG_NETINIT_THREAD=y
 CONFIG_NETINIT_THREAD_PRIORITY=49
@@ -683,6 +683,7 @@
 *(.text._ZN18MavlinkStreamDebug8get_sizeEv)
 *(.text._ZN12GPSDriverUBX7receiveEj)
 *(.text._ZN13BatteryStatus21parameter_update_pollEb)
+*(.text._ZN3Ekf26checkYawAngleObservabilityEv)
 *(.text._ZN3RTL18updateDatamanCacheEv)
 *(.text.__ieee754_sqrtf)
 *(.text._ZThn24_N18mag_bias_estimator16MagBiasEstimator3RunEv)
@@ -15,4 +15,3 @@ 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,7 +455,6 @@
 #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,10 +104,9 @@ const struct flexspi_nor_config_s g_flash_fast_config = {
 		.busyBitPolarity = 0u,
 		.lookupTable =
 		{
-			/* 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 */
+			/* Read */// EEH+11H+32bit addr+20dummy cycles+ 4Bytes read data    //200Mhz 18 dummy=10+8
 			[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, 0x28),//0xb3288b20,
+			[0 + 1] = FLEXSPI_LUT_SEQ(RADDR_DDR, FLEXSPI_8PAD, 0x20, DUMMY_DDR, FLEXSPI_8PAD, 0x04),//0xb3048b20,
 			[0 + 2] = FLEXSPI_LUT_SEQ(READ_DDR, FLEXSPI_8PAD, 0x04, STOP_EXE, FLEXSPI_1PAD, 0x00), //0xa704,

 			/* Read status */
@@ -45,7 +45,6 @@ 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=0xA290AFE
-CONFIG_NETINIT_DRIPADDR=0xA290AFE
+CONFIG_NETINIT_DNSIPADDR=0XC0A800FE
+CONFIG_NETINIT_DRIPADDR=0XC0A800FE
 CONFIG_NETINIT_THREAD=y
 CONFIG_NETINIT_THREAD_PRIORITY=49
 CONFIG_NETUTILS_TELNETD=y
@@ -178,7 +178,6 @@ set(msg_files
 	RcParameterMap.msg
 	RegisterExtComponentReply.msg
 	RegisterExtComponentRequest.msg
-	RoverAckermannGuidanceStatus.msg
 	Rpm.msg
 	RtlStatus.msg
 	RtlTimeEstimate.msg
@@ -1,10 +0,0 @@
-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 FRD frame [-1,1]
+float32[3] thrust_body		# Normalized thrust command in body NED frame [-1,1]

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

@@ -74,11 +74,6 @@ 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,8 +153,9 @@ 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,101 +37,17 @@

 namespace Murata_SCH16T
 {
-// 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 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);
 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,7 +44,6 @@ 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)
@@ -63,11 +62,8 @@ 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()
@@ -108,7 +104,9 @@ int SCH16T::probe()
 	PX4_INFO("COMP_ID:\t 0x%0x", comp_id);
 	PX4_INFO("ASIC_ID:\t 0x%0x", asic_id);

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

 	return success ? PX4_OK : PX4_ERROR;
 }
@@ -147,11 +145,8 @@ 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()
@@ -191,7 +186,6 @@ void SCH16T::RunImpl()
 		}

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

 			_state = STATE::LOCK_CONFIGURATION;
@@ -221,7 +215,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 {
@@ -239,7 +233,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 {
@@ -247,7 +241,7 @@ void SCH16T::RunImpl()
 				}

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

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

 bool SCH16T::ReadData(SensorData *data)
 {
-	// 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);
+	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;

-	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;
+	// 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);

+	// 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) {
-		// [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);
+		// Check for frame errors
+		if (v & MASK48_ERROR) {
+			perf_count(_perf_frame_bad);
 			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
@@ -348,6 +339,7 @@ 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;
@@ -355,210 +347,10 @@ 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) {
@@ -567,6 +359,15 @@ 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()
@@ -648,6 +449,8 @@ 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>, public ModuleParams
+class SCH16T : public device::SPI, public I2CSPIDriver<SCH16T>
 {
 public:
 	SCH16T(const I2CSPIDriverConfig &config);
@@ -79,7 +79,7 @@ private:
 	};

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

 	void Configure();
-	void ConfigurationFromParameters();
-
 	void SoftwareReset();

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

-	RegisterConfig _registers[6];
-
-	uint32_t _sample_interval_us = 678;
+	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
+	};

 	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,49 +42,3 @@
 * @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 = 5000000;
+	cli.default_spi_frequency = SPI_SPEED;
 	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);
+		ioctl(_rcs_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED | SER_SINGLEWIRE_PUSHPULL);
 	}

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

-				// magnetic field data returned by the geo library using the current GPS position
-				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);
+				const double lat = gps.latitude_deg;
+				const double lon = gps.longitude_deg;

-				_mag_earth_pred = Dcmf(Eulerf(0, -inclination_rad, declination_rad)) * Vector3f(field_strength_gauss, 0, 0);
+				// 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);
+
+				_mag_earth_pred = Dcmf(Eulerf(0, -mag_inclination_gps, mag_declination_gps)) * Vector3f(mag_strength_gps, 0, 0);

 				_mag_earth_available = true;
 			}
@@ -305,27 +305,19 @@ 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();

-	// 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) {
+	if (!PX4_ISFINITE(_current_average_filter_a.getState()) || _current_average_filter_a.getState() < FLT_EPSILON) {
 		_current_average_filter_a.reset(_params.bat_avrg_current);
 	}

@@ -402,7 +402,6 @@ public:
 		}

 		const Matrix<Type, M, N> &self = *this;
-		bool is_prev_symmetric = true; // assume symmetric until one element is not

 		for (unsigned i = 0; i < M; i++) {
 			printf("%2u|", i); // print row numbering
@@ -411,7 +410,7 @@ public:
 				double d = static_cast<double>(self(i, j));

 				// if symmetric don't print upper triangular elements
-				if (is_prev_symmetric && (M == N) && (j > i) && (i < N) && (j < M)
+				if ((M == N) && (j > i) && (i < N) && (j < M)
 				    && (fabs(d - static_cast<double>(self(j, i))) < (double)eps)
 				   ) {
 					// print empty space
@@ -429,8 +428,6 @@ public:
 					} else {
 						printf("% 6.5f ", d);
 					}
-
-					is_prev_symmetric = false; // not symmetric if once inside here
 				}
 			}

@@ -1,7 +1,7 @@
 #!/usr/bin/env python3
 ############################################################################
 #
-#   Copyright (c) 2020-2024 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -33,7 +33,6 @@
 ############################################################################

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

 header = """/****************************************************************************
 *
- *   Copyright (c) 2020-2024 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -81,9 +80,6 @@ header = """/*******************************************************************
 *
 ****************************************************************************/
 """
-
-key=sys.argv[1] # NOAA key (https://www.ngdc.noaa.gov/geomag/CalcSurvey.shtml)
-
 print(header)

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

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

-
-
-# 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))
+# 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))
 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('')

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

 for latitude in range(SAMPLING_MIN_LAT, SAMPLING_MAX_LAT+1, SAMPLING_RES):
-    world_magnitude_model[latitude] = {}
+    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())

-    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
+    latitude_blackout_zone = False

-        world_magnitude_model[latitude][longitude] = data['result'][0]
-        #print(world_magnitude_model[latitude][longitude])
+    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")


-def print_wmm_table(key_name):
+# 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('')

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

-    value_max = float('-inf')
-    value_max_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())

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

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

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

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

-    # 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)
+        except:
+            pass

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

-    # 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_max):
+            inclination_max = inclination_rad
+            inclination_max_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 (inclination_rad < inclination_min):
+            inclination_min = inclination_rad
+            inclination_min_lat_lon = (p['latitude'], p['longitude'])

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

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

-            # value scaled to fit into int16_t
-            value_int = int(round(value * value_scale))
-            print('{0:6d},'.format(value_int), end='')
+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")

-            # blackout warning at this latitude
-            try:
-                if result['warning']:
-                    latitude_blackout_zone = True
+# 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('')

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

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

-    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('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")
+    print('\t/* LAT: {0:3d} */'.format(latitude) + ' { ', end='')
+    for p in data['result']:

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

-print_wmm_table('declination')
-print_wmm_table('inclination')
-print_wmm_table('totalintensity')
+        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")
@@ -44,7 +44,7 @@ SAMPLING_MAX_LON = 180

 header = """/****************************************************************************
 *
- *   Copyright (c) 2020-2024 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -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 latitude_deg, float longitude_deg, const int16_t table[LAT_DIM][LON_DIM])
+static constexpr float get_table_data(float lat, float lon, const int16_t table[LAT_DIM][LON_DIM])
 {
-	latitude_deg = math::constrain(latitude_deg, SAMPLING_MIN_LAT, SAMPLING_MAX_LAT);
+	lat = math::constrain(lat, SAMPLING_MIN_LAT, SAMPLING_MAX_LAT);

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

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

 	/* round down to nearest sampling resolution */
-	float min_lat = floorf(latitude_deg / SAMPLING_RES) * SAMPLING_RES;
-	float min_lon = floorf(longitude_deg / SAMPLING_RES) * SAMPLING_RES;
+	float min_lat = floorf(lat / SAMPLING_RES) * SAMPLING_RES;
+	float min_lon = floorf(lon / 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 latitude_deg, float longitude_deg, c
 	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((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 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 data_min = lon_scale * (data_se - data_sw) + data_sw;
 	const float data_max = lon_scale * (data_ne - data_nw) + data_nw;
@@ -101,27 +101,32 @@ static constexpr float get_table_data(float latitude_deg, float longitude_deg, c
 	return lat_scale * (data_max - data_min) + data_min;
 }

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

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

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

-float get_mag_strength_tesla(float latitude_deg, float longitude_deg)
+float get_mag_inclination_degrees(float lat, float lon)
 {
-	// table stored as scaled nanotesla
-	return get_table_data(latitude_deg, longitude_deg, totalintensity_table)
-	       * WMM_TOTALINTENSITY_SCALE_TO_NANOTESLA * 1e-9f;
+	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
 }
@@ -41,12 +41,14 @@

 #pragma once

-// Return magnetic declination in degrees
-float get_mag_declination_degrees(float latitude_deg, float longitude_deg);
+// 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 field inclination in degrees
-float get_mag_inclination_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 strength in Gauss or Tesla
-float get_mag_strength_gauss(float latitude_deg, float longitude_deg);
-float get_mag_strength_tesla(float latitude_deg, float longitude_deg);
+float get_mag_strength_gauss(float lat, float lon);
+float get_mag_strength_tesla(float lat, float lon);
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020-2024 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -44,99 +44,95 @@ static constexpr int LON_DIM = 37;


 // *INDENT-OFF*
-
-
-
-// Magnetic declination data in 0.005451 degrees
+// Magnetic declination data in radians * 10^-4
 // Model: WMM-2020,
 // Version: 0.5.1.11,
-// Date: 2024.41257,
+// Date: 2024.0684,
 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 */ {  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
+	/* 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
 };
-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
+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


-// Magnetic inclination data in 0.002699 degrees
+// Magnetic inclination data in radians * 10^-4
 // Model: WMM-2020,
 // Version: 0.5.1.11,
-// Date: 2024.41257,
+// Date: 2024.0684,
 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 */ { -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
+	/* 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
 };
-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
+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


-// Magnetic totalintensity data in 2.042 nanoTesla
+// Magnetic strength data in milli-Gauss * 10
 // Model: WMM-2020,
 // Version: 0.5.1.11,
-// 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
+// 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, },
 };
-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
+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;


@@ -222,13 +222,16 @@ AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_
 	}

 	// reset states if check is disabled, we are not flying or wind estimator was just initialized/reset
-	if (!_innovation_check_enabled || !_in_fixed_wing_flight || (time_now - _time_wind_estimator_initialized) < 5_s) {
+	if (!_innovation_check_enabled || !_in_fixed_wing_flight || (time_now - _time_wind_estimator_initialized) < 5_s
+	    || _tas_innov_integ_threshold <= 0.f) {
 		_innovations_check_failed = false;
+		_time_last_tas_pass = time_now;
 		_aspd_innov_integ_state = 0.f;

 	} else if (!lpos_valid || estimator_status_vel_test_ratio > 1.f || estimator_status_mag_test_ratio > 1.f) {
 		//nav velocity data is likely not good
 		//don't run the test but don't reset the check if it had previously failed when nav velocity data was still likely good
+		_time_last_tas_pass = time_now;
 		_aspd_innov_integ_state = 0.f;

 	} else {
@@ -246,7 +249,11 @@ AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_
 			_aspd_innov_integ_state = 0.f;
 		}

-		_innovations_check_failed = _aspd_innov_integ_state > _tas_innov_integ_threshold;
+		if (_tas_innov_integ_threshold > 0.f && _aspd_innov_integ_state < _tas_innov_integ_threshold) {
+			_time_last_tas_pass = time_now;
+		}
+
+		_innovations_check_failed = (time_now - _time_last_tas_pass) > TAS_INNOV_FAIL_DELAY;
 	}

 	_time_last_aspd_innov_check = time_now;
@@ -150,7 +150,9 @@ private:
 	float _tas_innov_threshold{1.0}; ///< innovation error threshold for triggering innovation check failure
 	float _tas_innov_integ_threshold{-1.0}; ///< integrator innovation error threshold for triggering innovation check failure
 	uint64_t	_time_last_aspd_innov_check{0};	///< time airspeed innovation was last checked (uSec)
+	uint64_t	_time_last_tas_pass{0};		///< last time innovation checks passed
 	float		_aspd_innov_integ_state{0.0f};	///< integral of excess normalised airspeed innovation (sec)
+	static constexpr uint64_t TAS_INNOV_FAIL_DELAY{1_s};	///< time required for innovation levels to pass or fail (usec)
 	uint64_t	_time_wind_estimator_initialized{0};		///< time last time wind estimator was initialized (uSec)

 	// states of load factor check
@@ -160,8 +162,8 @@ private:

 	// states of airspeed valid declaration
 	bool _airspeed_valid{true}; ///< airspeed valid (pitot or groundspeed-windspeed)
-	float _checks_fail_delay{2.f}; ///< delay for airspeed invalid declaration after single check failure (Sec)
-	float _checks_clear_delay{-1.f}; ///< delay for airspeed valid declaration after all checks passed again (Sec)
+	int _checks_fail_delay{3}; ///< delay for airspeed invalid declaration after single check failure (Sec)
+	int _checks_clear_delay{-1}; ///< delay for airspeed valid declaration after all checks passed again (Sec)
 	uint64_t	_time_checks_passed{0};	///< time the checks have last passed (uSec)
 	uint64_t	_time_checks_failed{0};	///< time the checks have last not passed (uSec)

@@ -182,8 +182,8 @@ private:

 		(ParamFloat<px4::params::ASPD_FS_INNOV>) _tas_innov_threshold, /**< innovation check threshold */
 		(ParamFloat<px4::params::ASPD_FS_INTEG>) _tas_innov_integ_threshold, /**< innovation check integrator threshold */
-		(ParamFloat<px4::params::ASPD_FS_T_STOP>) _checks_fail_delay, /**< delay to declare airspeed invalid */
-		(ParamFloat<px4::params::ASPD_FS_T_START>) _checks_clear_delay, /**<  delay to declare airspeed valid again */
+		(ParamInt<px4::params::ASPD_FS_T_STOP>) _checks_fail_delay, /**< delay to declare airspeed invalid */
+		(ParamInt<px4::params::ASPD_FS_T_START>) _checks_clear_delay, /**<  delay to declare airspeed valid again */

 		(ParamFloat<px4::params::FW_AIRSPD_STALL>) _param_fw_airspd_stall,
 		(ParamFloat<px4::params::ASPD_WERR_THR>) _param_wind_sigma_max_synth_tas
@@ -208,10 +208,10 @@ PARAM_DEFINE_FLOAT(ASPD_FS_INTEG, 10.f);
 *
 * @unit s
 * @group Airspeed Validator
- * @min 0.0
- * @decimal 1
+ * @min 1
+ * @max 10
 */
-PARAM_DEFINE_FLOAT(ASPD_FS_T_STOP, 1.f);
+PARAM_DEFINE_INT32(ASPD_FS_T_STOP, 2);

 /**
 * Airspeed failsafe start delay
@@ -221,10 +221,10 @@ PARAM_DEFINE_FLOAT(ASPD_FS_T_STOP, 1.f);
 *
 * @unit s
 * @group Airspeed Validator
- * @min -1.0
- * @decimal 1
+ * @min -1
+ * @max 1000
 */
-PARAM_DEFINE_FLOAT(ASPD_FS_T_START, -1.f);
+PARAM_DEFINE_INT32(ASPD_FS_T_START, -1);

 /**
 * Horizontal wind uncertainty threshold for synthetic airspeed.
@@ -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
-				float mag_decl_deg = get_mag_declination_degrees(gps.latitude_deg, gps.longitude_deg);
-				update_mag_declination(math::radians(mag_decl_deg));
+				update_mag_declination(get_mag_declination_radians((float)gps.latitude_deg,
+						       (float)gps.longitude_deg));
 			}
 		}
 	}
@@ -237,8 +237,11 @@ 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(gps.latitude_deg, gps.longitude_deg);
+				return get_mag_strength_gauss(lat, lon);
 			}
 		}
 	}
@@ -951,14 +954,13 @@ 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_deg, float longitude_deg)
+int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radians, float latitude, float longitude)
 {
 	// magnetometer quick calibration
 	//  if GPS available use world magnetic model to zero mag offsets
 	bool mag_earth_available = false;

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

 	} else {
@@ -967,8 +969,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_deg = (float)gps.latitude_deg;
-				longitude_deg = (float)gps.longitude_deg;
+				latitude = (float)gps.latitude_deg;
+				longitude = (float)gps.longitude_deg;
 				mag_earth_available = true;
 			}
 		}
@@ -979,13 +981,14 @@ 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 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, -inclination_rad, declination_rad))
-						* Vector3f(field_strength_gauss, 0, 0);
+		// 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 Vector3f mag_earth_pred = Dcmf(Eulerf(0, -mag_inclination_gps, mag_declination_gps)) * Vector3f(mag_strength_gps,
+						0, 0);

 		uORB::Subscription vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
 		vehicle_attitude_s attitude{};
@@ -393,7 +393,7 @@ ControlAllocator::Run()
 	if (_vehicle_thrust_setpoint_sub.update(&vehicle_thrust_setpoint)) {
 		_thrust_sp = matrix::Vector3f(vehicle_thrust_setpoint.xyz);

-		if (dt > 0.005f) {
+		if (dt > 5_ms) {
 			do_update = true;
 			_timestamp_sample = vehicle_thrust_setpoint.timestamp_sample;
 		}
@@ -66,7 +66,6 @@ void DifferentialDrive::Run()
 	if (should_exit()) {
 		ScheduleClear();
 		exit_and_cleanup();
-		return;
 	}

 	hrt_abstime now = hrt_absolute_time();
@@ -99,12 +99,15 @@ void Ekf::collect_gps(const gnssSample &gps)
 		if (gps_rough_2d_fix && (_gps_checks_passed || !_NED_origin_initialised)) {

 			// If we have good GPS data set the origin's WGS-84 position to the last gps fix
+			const double lat = gps.lat;
+
 #if defined(CONFIG_EKF2_MAGNETOMETER)
+			const double lon = gps.lon;

 			// set the magnetic field data returned by the geo library using the current GPS position
-			const float mag_declination_gps = math::radians(get_mag_declination_degrees(gps.lat, gps.lon));
-			const float mag_inclination_gps = math::radians(get_mag_inclination_degrees(gps.lat, gps.lon));
-			const float mag_strength_gps = get_mag_strength_gauss(gps.lat, gps.lon);
+			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);

 			if (PX4_ISFINITE(mag_declination_gps) && PX4_ISFINITE(mag_inclination_gps) && PX4_ISFINITE(mag_strength_gps)) {

@@ -127,7 +130,7 @@ void Ekf::collect_gps(const gnssSample &gps)
 			}
 #endif // CONFIG_EKF2_MAGNETOMETER

-			_earth_rate_NED = calcEarthRateNED((float)math::radians(gps.lat));
+			_earth_rate_NED = calcEarthRateNED((float)math::radians(lat));
 		}

 		_wmm_gps_time_last_checked = _time_delayed_us;
@@ -52,6 +52,8 @@ void Ekf::controlMagFusion()
 		_control_status.flags.mag_aligned_in_flight = false;
 	}

+	checkYawAngleObservability();
+
 	if (_params.mag_fusion_type == MagFuseType::NONE) {
 		stopMagFusion();
 		return;
@@ -128,9 +130,7 @@ void Ekf::controlMagFusion()
 		_innov_check_fail_status.flags.reject_mag_z = (aid_src.test_ratio[2] > 1.f);

 		// determine if we should use mag fusion
-		bool continuing_conditions_passing = ((_params.mag_fusion_type == MagFuseType::INIT)
-						      || (_params.mag_fusion_type == MagFuseType::AUTO)
-						      || (_params.mag_fusion_type == MagFuseType::HEADING))
+		bool continuing_conditions_passing = (_params.mag_fusion_type != MagFuseType::NONE)
 						     && _control_status.flags.tilt_align
 						     && (_control_status.flags.yaw_align || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align))
 						     && mag_sample.mag.longerThan(0.f)
@@ -275,15 +275,6 @@ void Ekf::stopMagFusion()
 	if (_control_status.flags.mag) {
 		ECL_INFO("stopping mag fusion");

-		if (_control_status.flags.yaw_align && (_control_status.flags.mag_3D || _control_status.flags.mag_hdg)) {
-			// reset yaw alignment from mag unless using GNSS aiding
-			const bool using_ne_aiding = _control_status.flags.gps || _control_status.flags.aux_gpos;
-
-			if (!using_ne_aiding) {
-				_control_status.flags.yaw_align = false;
-			}
-		}
-
 		_control_status.flags.mag = false;
 		_control_status.flags.mag_dec = false;

@@ -298,8 +289,6 @@ void Ekf::stopMagFusion()
 			_fault_status.flags.bad_hdg = false;
 		}

-		_control_status.flags.mag_aligned_in_flight = false;
-
 		_fault_status.flags.bad_mag_x = false;
 		_fault_status.flags.bad_mag_y = false;
 		_fault_status.flags.bad_mag_z = false;
@@ -400,6 +389,23 @@ void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading)
 	}
 }

+void Ekf::checkYawAngleObservability()
+{
+	if (_control_status.flags.gps) {
+		// Check if there has been enough change in horizontal velocity to make yaw observable
+		// Apply hysteresis to check to avoid rapid toggling
+		if (_yaw_angle_observable) {
+			_yaw_angle_observable = _accel_lpf_NE.norm() > _params.mag_acc_gate;
+
+		} else {
+			_yaw_angle_observable = _accel_lpf_NE.norm() > _params.mag_acc_gate * 2.f;
+		}
+
+	} else {
+		_yaw_angle_observable = false;
+	}
+}
+
 void Ekf::checkMagHeadingConsistency(const magSample &mag_sample)
 {
 	// use mag bias if variance good
@@ -429,10 +435,7 @@ void Ekf::checkMagHeadingConsistency(const magSample &mag_sample)
 	}

 	if (fabsf(_mag_heading_innov_lpf.getState()) < _params.mag_heading_noise) {
-		// Check if there has been enough change in horizontal velocity to make yaw observable
-		const bool using_ne_aiding = _control_status.flags.gps || _control_status.flags.aux_gpos;
-
-		if (using_ne_aiding && (_accel_lpf_NE.norm() > _params.mag_acc_gate)) {
+		if (_yaw_angle_observable) {
 			// yaw angle must be observable to consider consistency
 			_control_status.flags.mag_heading_consistent = true;
 		}
@@ -55,13 +55,9 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 					       && !_flow_sample_delayed.flow_rate.longerThan(_flow_max_rate);
 		const bool is_tilt_good = (_R_to_earth(2, 2) > _params.range_cos_max_tilt);

-		// don't enforce this condition if terrain estimate is not valid as we have logic below to coast through bad range finder data
-		const bool is_within_max_sensor_dist = isTerrainEstimateValid() ? (_terrain_vpos - _state.pos(2) <= _flow_max_distance) : true;
-
 		if (is_quality_good
 		    && is_magnitude_good
-		    && is_tilt_good
-		    && is_within_max_sensor_dist) {
+		    && is_tilt_good) {
 			// compensate for body motion to give a LOS rate
 			calcOptFlowBodyRateComp(imu_delayed);
 			_flow_rate_compensated = _flow_sample_delayed.flow_rate - _flow_sample_delayed.gyro_rate.xy();
--- a/Show More
+++ b/Show More