fixup! Update submodule nuttx

uavcan_v1: Add Publishers dir to includes
uavcan_v1: SocketCAN set 'can_fd' based on CONFIG
2026-05-30 19:30:05 +08:00 · 2022-03-18 10:56:44 -07:00 · 2022-03-18 09:03:48 -07:00 · 2022-03-18 09:03:06 -07:00 · 2022-03-18 09:02:00 -07:00 · 2022-03-17 17:01:54 -07:00
184 changed files with 8808 additions and 7237 deletions
@@ -24,8 +24,8 @@
 	branch = master
 [submodule "platforms/nuttx/NuttX/nuttx"]
 	path = platforms/nuttx/NuttX/nuttx
-	url = https://github.com/PX4/NuttX.git
-	branch = px4_firmware_nuttx-10.1.0+
+	url = https://github.com/JacobCrabill/incubator-nuttx.git
+	branch = dev-h7-socketcan
 [submodule "platforms/nuttx/NuttX/apps"]
 	path = platforms/nuttx/NuttX/apps
 	url = https://github.com/PX4/NuttX-apps.git
@@ -10,7 +10,7 @@
 # param set-default SYS_CTRL_ALLOC 1
 param set-default CA_AIRFRAME 2

-param set-default CA_ROTOR_COUNT 4
+param set-default CA_ROTOR_COUNT 5
 param set-default CA_ROTOR0_PX 0.1515
 param set-default CA_ROTOR0_PY 0.245
 param set-default CA_ROTOR0_KM 0.05
@@ -23,6 +23,9 @@ param set-default CA_ROTOR2_KM -0.05
 param set-default CA_ROTOR3_PX -0.1515
 param set-default CA_ROTOR3_PY 0.1875
 param set-default CA_ROTOR3_KM -0.05
+param set-default CA_ROTOR4_AX 1.0
+param set-default CA_ROTOR4_AZ 0.0
+param set-default CA_ROTOR4_PX 0.2

 param set-default CA_SV_CS_COUNT 3
 param set-default CA_SV_CS0_TYPE 1
@@ -39,6 +39,26 @@ param set-default FW_RR_P 0.3

 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 4
+param set-default CA_SV_CS0_TRQ_R 0.5
+param set-default CA_SV_CS0_TYPE 2
+param set-default CA_SV_CS1_TRQ_P 1.0
+param set-default CA_SV_CS1_TYPE 3
+param set-default CA_SV_CS2_TRQ_Y 1.0
+param set-default CA_SV_CS2_TYPE 4
+param set-default CA_SV_CS3_TYPE 10
+
+param set-default HIL_ACT_REV 2
+param set-default HIL_ACT_FUNC1 201
+param set-default HIL_ACT_FUNC2 202
+param set-default HIL_ACT_FUNC3 203
+param set-default HIL_ACT_FUNC4 101
+param set-default HIL_ACT_FUNC5 204
+param set-default HIL_ACT_FUNC6 400
+
 param set SYS_HITL 1

 # disable some checks to allow to fly
@@ -15,6 +15,23 @@ set MIXER quad_x

 param set SYS_HITL 1

+param set-default CA_ROTOR_COUNT 4
+param set-default CA_ROTOR0_PX 0.15
+param set-default CA_ROTOR0_PY 0.15
+param set-default CA_ROTOR1_PX -0.15
+param set-default CA_ROTOR1_PY -0.15
+param set-default CA_ROTOR2_PX 0.15
+param set-default CA_ROTOR2_PY -0.15
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX -0.15
+param set-default CA_ROTOR3_PY 0.15
+param set-default CA_ROTOR3_KM -0.05
+
+param set-default HIL_ACT_FUNC1 101
+param set-default HIL_ACT_FUNC2 102
+param set-default HIL_ACT_FUNC3 103
+param set-default HIL_ACT_FUNC4 104
+
 # disable some checks to allow to fly
 # - with usb
 param set-default CBRK_USB_CHK 197848
@@ -61,6 +61,37 @@ param set-default VT_MOT_ID 1234
 param set-default VT_FW_MOT_OFFID 1234
 param set-default VT_TYPE 2

+param set-default CA_AIRFRAME 2
+param set-default CA_ROTOR_COUNT 5
+param set-default CA_ROTOR0_PX 0.15
+param set-default CA_ROTOR0_PY 0.15
+param set-default CA_ROTOR1_PX -0.15
+param set-default CA_ROTOR1_PY -0.15
+param set-default CA_ROTOR2_PX 0.15
+param set-default CA_ROTOR2_PY -0.15
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX -0.15
+param set-default CA_ROTOR3_PY 0.15
+param set-default CA_ROTOR3_KM -0.05
+param set-default CA_ROTOR4_AX 1.0
+param set-default CA_ROTOR4_AZ 0.0
+param set-default CA_ROTOR4_PX 0.2
+
+param set-default CA_SV_CS_COUNT 3
+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.0
+param set-default CA_SV_CS2_TYPE 3
+
+param set-default HIL_ACT_FUNC1 101
+param set-default HIL_ACT_FUNC2 102
+param set-default HIL_ACT_FUNC3 103
+param set-default HIL_ACT_FUNC4 104
+param set-default HIL_ACT_FUNC5 105
+param set-default HIL_ACT_FUNC6 201
+param set-default HIL_ACT_FUNC7 202
+param set-default HIL_ACT_FUNC8 203
+

 param set SYS_HITL 1

@@ -17,6 +17,23 @@ set PWM_OUT 1234

 param set UAVCAN_ENABLE 0

+param set-default CA_ROTOR_COUNT 4
+param set-default CA_ROTOR0_PX 0.15
+param set-default CA_ROTOR0_PY 0.15
+param set-default CA_ROTOR1_PX -0.15
+param set-default CA_ROTOR1_PY -0.15
+param set-default CA_ROTOR2_PX 0.15
+param set-default CA_ROTOR2_PY -0.15
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX -0.15
+param set-default CA_ROTOR3_PY 0.15
+param set-default CA_ROTOR3_KM -0.05
+
+param set-default HIL_ACT_FUNC1 101
+param set-default HIL_ACT_FUNC2 102
+param set-default HIL_ACT_FUNC3 103
+param set-default HIL_ACT_FUNC4 104
+
 # set SYS_HITL to 2 to start the SIH and avoid sensors startup
 param set SYS_HITL 2

@@ -17,6 +17,26 @@ set PWM_OUT 1234

 param set UAVCAN_ENABLE 0

+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 4
+param set-default CA_SV_CS0_TRQ_R 0.5
+param set-default CA_SV_CS0_TYPE 2
+param set-default CA_SV_CS1_TRQ_P 1.0
+param set-default CA_SV_CS1_TYPE 3
+param set-default CA_SV_CS2_TRQ_Y 1.0
+param set-default CA_SV_CS2_TYPE 4
+param set-default CA_SV_CS3_TYPE 10
+
+param set-default HIL_ACT_REV 2
+param set-default HIL_ACT_FUNC1 201
+param set-default HIL_ACT_FUNC2 202
+param set-default HIL_ACT_FUNC3 203
+param set-default HIL_ACT_FUNC4 101
+param set-default HIL_ACT_FUNC5 204
+param set-default HIL_ACT_FUNC6 400
+
 # set SYS_HITL to 2 to start the SIH and avoid sensors startup
 param set-default SYS_HITL 2

@@ -26,6 +26,26 @@ param set-default VT_FW_DIFTHR_SC 0.3
 param set-default MPC_MAN_Y_MAX 60
 param set-default MC_PITCH_P 5

+param set-default CA_AIRFRAME 4
+param set-default CA_ROTOR_COUNT 2
+param set-default CA_ROTOR0_KM -0.05
+param set-default CA_ROTOR0_PY 0.2
+param set-default CA_ROTOR1_KM -0.05
+param set-default CA_ROTOR1_PY -0.2
+param set-default CA_SV_CS_COUNT 2
+param set-default CA_SV_CS0_TRQ_P 0.3
+param set-default CA_SV_CS0_TRQ_Y 0.3
+param set-default CA_SV_CS0_TYPE 5
+param set-default CA_SV_CS1_TRQ_P 0.3
+param set-default CA_SV_CS1_TRQ_Y -0.3
+param set-default CA_SV_CS1_TYPE 6
+
+param set-default HIL_ACT_FUNC1 101
+param set-default HIL_ACT_FUNC2 102
+param set-default HIL_ACT_FUNC5 202
+param set-default HIL_ACT_FUNC6 201
+param set-default HIL_ACT_REV 32
+
 param set-default MAV_TYPE 19
 set MAV_TYPE 19
 set MIXER vtol_tailsitter_duo_sat
@@ -25,7 +25,7 @@
 . ${R}etc/init.d/rc.vtol_defaults

 param set-default CA_AIRFRAME 2
-param set-default CA_ROTOR_COUNT 4
+param set-default CA_ROTOR_COUNT 5
 param set-default CA_ROTOR0_PX 0.15
 param set-default CA_ROTOR0_PY 0.15
 param set-default CA_ROTOR1_PX -0.15
@@ -36,6 +36,9 @@ param set-default CA_ROTOR2_KM -0.05
 param set-default CA_ROTOR3_PX -0.15
 param set-default CA_ROTOR3_PY 0.15
 param set-default CA_ROTOR3_KM -0.05
+param set-default CA_ROTOR4_AX 1.0
+param set-default CA_ROTOR4_AZ 0.0
+param set-default CA_ROTOR4_PX 0.2
 param set-default CA_SV_CS_COUNT 4
 param set-default CA_SV_CS0_TYPE 1
 param set-default CA_SV_CS0_TRQ_R -0.5
@@ -164,6 +164,8 @@ param set-default RC_MAP_AUX2       5
 param set-default RC_MAP_AUX3       10
 param set-default RC_MAP_AUX4       8
 param set-default RC_MAP_FLTMODE    6
+param set-default RC_MAP_RETURN_SW  7
+
 param set-default RC1_TRIM          1000


@@ -330,9 +330,11 @@ def get_mixers(yaml_config, output_functions, verbose):
        option = select_param + '==' + str(type_index)
        mixer_config = {
                'option': option,
+                'help-url': 'https://docs.px4.io/master/en/config/actuators.html',
            }
-        if 'type' in current_type:
-            mixer_config['type'] = current_type['type']
+        for optional in ['type', 'title']:
+            if optional in current_type:
+                mixer_config[optional] = current_type[optional]
        actuators = []
        for actuator_conf in current_type['actuators']:
            actuator = {
@@ -7,7 +7,8 @@ import os
 import math
 import matplotlib.pyplot as plt
 import numpy as np
-import scipy as sp
+
+from scipy.signal import medfilt

 from pyulog import *

@@ -65,7 +66,7 @@ def resampleWithDeltaX(x,y):
    return resampledX,resampledY

 def median_filter(data):
-    return sp.signal.medfilt(data, 31)
+    return medfilt(data, 31)

 parser = argparse.ArgumentParser(description='Reads in IMU data from a static thermal calibration test and performs a curve fit of gyro, accel and baro bias vs temperature')
 parser.add_argument('filename', metavar='file.ulg', help='ULog input file')
@@ -16,7 +16,7 @@ class ModuleDocumentation(object):
    valid_categories = ['driver', 'estimator', 'controller', 'system',
                        'communication', 'command', 'template', 'simulation', 'autotune']
    valid_subcategories = ['', 'distance_sensor', 'imu', 'airspeed_sensor',
-                           'magnetometer', 'baro', 'optical_flow']
+                           'magnetometer', 'baro', 'optical_flow', 'rpm_sensor']

    max_line_length = 80 # wrap lines that are longer than this

@@ -0,0 +1,245 @@
+#
+# This file is autogenerated: PLEASE DO NOT EDIT IT.
+#
+# You can use "make menuconfig" to make any modifications to the installed .config file.
+# You can then do "make savedefconfig" to generate a new defconfig file that includes your
+# modifications.
+#
+# CONFIG_DISABLE_ENVIRON is not set
+# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set
+# CONFIG_MMCSD_HAVE_CARDDETECT is not set
+# CONFIG_MMCSD_HAVE_WRITEPROTECT is not set
+# CONFIG_MMCSD_MMCSUPPORT is not set
+# CONFIG_MMCSD_SPI is not set
+# CONFIG_NET_CAN_CANFD is not set
+# CONFIG_NET_ETHERNET is not set
+# CONFIG_NET_IPv4 is not set
+# CONFIG_NSH_DISABLEBG is not set
+# CONFIG_NSH_DISABLESCRIPT is not set
+# CONFIG_NSH_DISABLE_DF is not set
+# CONFIG_NSH_DISABLE_EXEC is not set
+# CONFIG_NSH_DISABLE_EXIT is not set
+# CONFIG_NSH_DISABLE_GET is not set
+# CONFIG_NSH_DISABLE_ITEF is not set
+# CONFIG_NSH_DISABLE_LOOPS is not set
+# CONFIG_NSH_DISABLE_MKFATFS is not set
+# CONFIG_NSH_DISABLE_REBOOT is not set
+# CONFIG_NSH_DISABLE_SEMICOLON is not set
+# CONFIG_NSH_DISABLE_SHUTDOWN is not set
+# CONFIG_NSH_DISABLE_TIME is not set
+CONFIG_ARCH="arm"
+CONFIG_ARCH_BOARD_CUSTOM=y
+CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/cubepilot/cubeorange/nuttx-config"
+CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
+CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
+CONFIG_ARCH_CHIP="stm32h7"
+CONFIG_ARCH_CHIP_STM32H743ZI=y
+CONFIG_ARCH_CHIP_STM32H7=y
+CONFIG_ARCH_INTERRUPTSTACK=512
+CONFIG_ARCH_STACKDUMP=y
+CONFIG_ARMV7M_BASEPRI_WAR=y
+CONFIG_ARMV7M_DCACHE=y
+CONFIG_ARMV7M_DTCM=y
+CONFIG_ARMV7M_ICACHE=y
+CONFIG_ARMV7M_MEMCPY=y
+CONFIG_ARMV7M_USEBASEPRI=y
+CONFIG_ARM_MPU_EARLY_RESET=y
+CONFIG_BOARDCTL_RESET=y
+CONFIG_BOARD_CRASHDUMP=y
+CONFIG_BOARD_LOOPSPERMSEC=79954
+CONFIG_BOARD_RESET_ON_ASSERT=2
+CONFIG_BUILTIN=y
+CONFIG_C99_BOOL8=y
+CONFIG_CANUTILS_CANDUMP=y
+CONFIG_CANUTILS_CANSEND=y
+CONFIG_CDCACM=y
+CONFIG_CDCACM_IFLOWCONTROL=y
+CONFIG_CDCACM_PRODUCTID=0x1016
+CONFIG_CDCACM_PRODUCTSTR="CubeOrange"
+CONFIG_CDCACM_RXBUFSIZE=600
+CONFIG_CDCACM_TXBUFSIZE=12000
+CONFIG_CDCACM_VENDORID=0x2DAE
+CONFIG_CDCACM_VENDORSTR="CubePilot"
+CONFIG_DEBUG_FULLOPT=y
+CONFIG_DEBUG_HARDFAULT_ALERT=y
+CONFIG_DEBUG_SYMBOLS=y
+CONFIG_DEFAULT_SMALL=y
+CONFIG_DEV_FIFO_SIZE=0
+CONFIG_DEV_PIPE_MAXSIZE=1024
+CONFIG_DEV_PIPE_SIZE=70
+CONFIG_EXAMPLES_CALIB_UDELAY=y
+CONFIG_EXPERIMENTAL=y
+CONFIG_FAT_DMAMEMORY=y
+CONFIG_FAT_LCNAMES=y
+CONFIG_FAT_LFN=y
+CONFIG_FAT_LFN_ALIAS_HASH=y
+CONFIG_FDCLONE_STDIO=y
+CONFIG_FS_BINFS=y
+CONFIG_FS_CROMFS=y
+CONFIG_FS_FAT=y
+CONFIG_FS_FATTIME=y
+CONFIG_FS_PROCFS=y
+CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
+CONFIG_FS_PROCFS_MAX_TASKS=64
+CONFIG_FS_PROCFS_REGISTER=y
+CONFIG_FS_ROMFS=y
+CONFIG_GRAN=y
+CONFIG_GRAN_INTR=y
+CONFIG_HAVE_CXX=y
+CONFIG_HAVE_CXXINITIALIZE=y
+CONFIG_I2C=y
+CONFIG_I2C_RESET=y
+CONFIG_IDLETHREAD_STACKSIZE=750
+CONFIG_LIBC_LONG_LONG=y
+CONFIG_LIBC_STRERROR=y
+CONFIG_MEMSET_64BIT=y
+CONFIG_MEMSET_OPTSPEED=y
+CONFIG_MMCSD=y
+CONFIG_MMCSD_SDIO=y
+CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y
+CONFIG_MM_REGIONS=4
+CONFIG_MTD=y
+CONFIG_MTD_BYTE_WRITE=y
+CONFIG_MTD_PARTITION=y
+CONFIG_MTD_PROGMEM=y
+CONFIG_MTD_RAMTRON=y
+CONFIG_NAME_MAX=40
+CONFIG_NET=y
+CONFIG_NETDEV_CAN_BITRATE_IOCTL=y
+CONFIG_NETDEV_IFINDEX=y
+CONFIG_NETDEV_PHY_IOCTL=y
+CONFIG_NET_CAN=y
+CONFIG_NET_CAN_RAW_TX_DEADLINE=y
+CONFIG_NET_CAN_SOCK_OPTS=y
+CONFIG_NET_TIMESTAMP=y
+CONFIG_NSH_ARCHINIT=y
+CONFIG_NSH_ARGCAT=y
+CONFIG_NSH_BUILTIN_APPS=y
+CONFIG_NSH_CMDPARMS=y
+CONFIG_NSH_CROMFSETC=y
+CONFIG_NSH_DISABLE_TELNETD=y
+CONFIG_NSH_LINELEN=128
+CONFIG_NSH_MAXARGUMENTS=15
+CONFIG_NSH_NESTDEPTH=8
+CONFIG_NSH_QUOTE=y
+CONFIG_NSH_ROMFSETC=y
+CONFIG_NSH_ROMFSSECTSIZE=128
+CONFIG_NSH_STRERROR=y
+CONFIG_NSH_VARS=y
+CONFIG_OTG_ID_GPIO_DISABLE=y
+CONFIG_PIPES=y
+CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
+CONFIG_PTHREAD_MUTEX_ROBUST=y
+CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_SETSPEED=y
+CONFIG_RAM_SIZE=245760
+CONFIG_RAM_START=0x20010000
+CONFIG_RAW_BINARY=y
+CONFIG_READLINE_CMD_HISTORY=y
+CONFIG_READLINE_TABCOMPLETION=y
+CONFIG_RTC_DATETIME=y
+CONFIG_SCHED_ATEXIT=y
+CONFIG_SCHED_HPWORK=y
+CONFIG_SCHED_HPWORKPRIORITY=249
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_INSTRUMENTATION=y
+CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
+CONFIG_SCHED_WAITPID=y
+CONFIG_SDCLONE_DISABLE=y
+CONFIG_SDMMC1_SDIO_PULLUP=y
+CONFIG_SEM_NNESTPRIO=8
+CONFIG_SEM_PREALLOCHOLDERS=0
+CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
+CONFIG_SERIAL_TERMIOS=y
+CONFIG_SIG_DEFAULT=y
+CONFIG_SIG_SIGALRM_ACTION=y
+CONFIG_SIG_SIGUSR1_ACTION=y
+CONFIG_SIG_SIGUSR2_ACTION=y
+CONFIG_SIG_SIGWORK=4
+CONFIG_STACK_COLORATION=y
+CONFIG_START_DAY=30
+CONFIG_START_MONTH=11
+CONFIG_STDIO_BUFFER_SIZE=256
+CONFIG_STM32H7_ADC1=y
+CONFIG_STM32H7_ADC3=y
+CONFIG_STM32H7_BBSRAM=y
+CONFIG_STM32H7_BBSRAM_FILES=5
+CONFIG_STM32H7_BKPSRAM=y
+CONFIG_STM32H7_DMA1=y
+CONFIG_STM32H7_DMA2=y
+CONFIG_STM32H7_DMACAPABLE=y
+CONFIG_STM32H7_FDCAN1=y
+CONFIG_STM32H7_FLOWCONTROL_BROKEN=y
+CONFIG_STM32H7_I2C1=y
+CONFIG_STM32H7_I2C2=y
+CONFIG_STM32H7_I2C_DYNTIMEO=y
+CONFIG_STM32H7_I2C_DYNTIMEO_STARTSTOP=10
+CONFIG_STM32H7_OTGFS=y
+CONFIG_STM32H7_PROGMEM=y
+CONFIG_STM32H7_RTC=y
+CONFIG_STM32H7_RTC_HSECLOCK=y
+CONFIG_STM32H7_RTC_MAGIC_REG=1
+CONFIG_STM32H7_SAVE_CRASHDUMP=y
+CONFIG_STM32H7_SDMMC1=y
+CONFIG_STM32H7_SERIALBRK_BSDCOMPAT=y
+CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y
+CONFIG_STM32H7_SPI1=y
+CONFIG_STM32H7_SPI1_DMA=y
+CONFIG_STM32H7_SPI1_DMA_BUFFER=1024
+CONFIG_STM32H7_SPI2=y
+CONFIG_STM32H7_SPI4=y
+CONFIG_STM32H7_SPI4_DMA=y
+CONFIG_STM32H7_SPI4_DMA_BUFFER=1024
+CONFIG_STM32H7_SPI_DMA=y
+CONFIG_STM32H7_TIM1=y
+CONFIG_STM32H7_TIM3=y
+CONFIG_STM32H7_TIM4=y
+CONFIG_STM32H7_UART4=y
+CONFIG_STM32H7_UART7=y
+CONFIG_STM32H7_UART8=y
+CONFIG_STM32H7_USART2=y
+CONFIG_STM32H7_USART3=y
+CONFIG_STM32H7_USART6=y
+CONFIG_STM32H7_USART_BREAKS=y
+CONFIG_STM32H7_USART_INVERT=y
+CONFIG_STM32H7_USART_SINGLEWIRE=y
+CONFIG_STM32H7_USART_SWAP=y
+CONFIG_SYSTEM_CDCACM=y
+CONFIG_SYSTEM_NSH=y
+CONFIG_TASK_NAME_SIZE=24
+CONFIG_TTY_SIGINT=y
+CONFIG_TTY_SIGTSTP=y
+CONFIG_UART4_BAUD=57600
+CONFIG_UART4_RXBUFSIZE=600
+CONFIG_UART4_TXBUFSIZE=1500
+CONFIG_UART7_BAUD=57600
+CONFIG_UART7_RXBUFSIZE=600
+CONFIG_UART7_SERIAL_CONSOLE=y
+CONFIG_UART7_TXBUFSIZE=1500
+CONFIG_UART8_BAUD=57600
+CONFIG_UART8_RXBUFSIZE=600
+CONFIG_UART8_TXBUFSIZE=1500
+CONFIG_USART2_BAUD=57600
+CONFIG_USART2_IFLOWCONTROL=y
+CONFIG_USART2_OFLOWCONTROL=y
+CONFIG_USART2_RXBUFSIZE=600
+CONFIG_USART2_TXBUFSIZE=1500
+CONFIG_USART3_BAUD=57600
+CONFIG_USART3_IFLOWCONTROL=y
+CONFIG_USART3_OFLOWCONTROL=y
+CONFIG_USART3_RXBUFSIZE=600
+CONFIG_USART3_TXBUFSIZE=3000
+CONFIG_USART6_BAUD=57600
+CONFIG_USART6_RXBUFSIZE=600
+CONFIG_USART6_TXBUFSIZE=1500
+CONFIG_USBDEV=y
+CONFIG_USBDEV_BUSPOWERED=y
+CONFIG_USBDEV_MAXPOWER=500
+CONFIG_USEC_PER_TICK=1000
+CONFIG_USERMAIN_STACKSIZE=2944
+CONFIG_USER_ENTRYPOINT="nsh_main"
+CONFIG_WATCHDOG=y
@@ -0,0 +1,22 @@
+CONFIG_DRIVERS_ADC_ADS1115=n
+CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n
+CONFIG_DRIVERS_IRLOCK=n
+CONFIG_DRIVERS_PCA9685=n
+CONFIG_DRIVERS_PCA9685_PWM_OUT=n
+CONFIG_DRIVERS_RPM=n
+CONFIG_DRIVERS_UAVCAN=n
+CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n
+CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n
+CONFIG_MODULES_ROVER_POS_CONTROL=n
+CONFIG_MODULES_TEMPERATURE_COMPENSATION=n
+CONFIG_DRIVERS_UAVCAN_V1=y
+CONFIG_UAVCAN_V1_BMS_SUBSCRIBER=y
+CONFIG_UAVCAN_V1_ESC_CONTROLLER=y
+CONFIG_UAVCAN_V1_ESC_SUBSCRIBER=y
+CONFIG_UAVCAN_V1_GNSS_PUBLISHER=y
+CONFIG_UAVCAN_V1_GNSS_SUBSCRIBER_0=y
+CONFIG_UAVCAN_V1_GNSS_SUBSCRIBER_1=y
+CONFIG_UAVCAN_V1_READINESS_PUBLISHER=y
+CONFIG_UAVCAN_V1_UORB_ACTUATOR_OUTPUTS_PUBLISHER=y
+CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_PUBLISHER=y
+CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_SUBSCRIBER=y
@@ -15,7 +15,6 @@ CONFIG_DRIVERS_OSD=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
 CONFIG_DRIVERS_TELEMETRY_FRSKY_TELEMETRY=y
-CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_COMMANDER=y
@@ -27,7 +26,6 @@ CONFIG_MODULES_LOGGER=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
-CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
 CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
@@ -133,6 +133,8 @@

 #define GPIO_RSSI_IN                       /* PC5  */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTC|GPIO_PIN5)

+#define GPIO_RF_SWITCH                     /* PE13  */ (GPIO_OUTPUT|GPIO_PULLDOWN|GPIO_PORTE|GPIO_PIN13)
+
 /* Power switch controls ******************************************************/

 #define SDIO_SLOTNO                    0  /* Only one slot */
@@ -175,6 +177,7 @@
 		PX4_ADC_GPIO,                     \
 		GPIO_TONE_ALARM_IDLE,             \
 		GPIO_RSSI_IN,                     \
+		GPIO_RF_SWITCH,                   \
 	}

 #define BOARD_ENABLE_CONSOLE_BUFFER
@@ -160,6 +160,9 @@ stm32_boardinitialize(void)

 	board_control_spi_sensors_power_configgpio();

+	/* Turn bluetooth off by default (no mavlink support yet) */
+	px4_arch_gpiowrite(GPIO_RF_SWITCH, 0);
+
 	/* configure USB interfaces */

 	stm32_usbinitialize();
@@ -0,0 +1,249 @@
+#
+# This file is autogenerated: PLEASE DO NOT EDIT IT.
+#
+# You can use "make menuconfig" to make any modifications to the installed .config file.
+# You can then do "make savedefconfig" to generate a new defconfig file that includes your
+# modifications.
+#
+# CONFIG_DISABLE_ENVIRON is not set
+# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set
+# CONFIG_MMCSD_HAVE_CARDDETECT is not set
+# CONFIG_MMCSD_HAVE_WRITEPROTECT is not set
+# CONFIG_MMCSD_MMCSUPPORT is not set
+# CONFIG_MMCSD_SPI is not set
+# CONFIG_NET_CAN_CANFD is not set
+# CONFIG_NET_ETHERNET is not set
+# CONFIG_NET_IPv4 is not set
+# CONFIG_NSH_DISABLEBG is not set
+# CONFIG_NSH_DISABLESCRIPT is not set
+# CONFIG_NSH_DISABLE_DF is not set
+# CONFIG_NSH_DISABLE_EXEC is not set
+# CONFIG_NSH_DISABLE_EXIT is not set
+# CONFIG_NSH_DISABLE_GET is not set
+# CONFIG_NSH_DISABLE_ITEF is not set
+# CONFIG_NSH_DISABLE_LOOPS is not set
+# CONFIG_NSH_DISABLE_MKFATFS is not set
+# CONFIG_NSH_DISABLE_SEMICOLON is not set
+# CONFIG_NSH_DISABLE_TIME is not set
+CONFIG_ARCH="arm"
+CONFIG_ARCH_BOARD_CUSTOM=y
+CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/mro/ctrl-zero-h7-oem/nuttx-config"
+CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
+CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
+CONFIG_ARCH_CHIP="stm32h7"
+CONFIG_ARCH_CHIP_STM32H743II=y
+CONFIG_ARCH_CHIP_STM32H7=y
+CONFIG_ARCH_INTERRUPTSTACK=512
+CONFIG_ARCH_STACKDUMP=y
+CONFIG_ARMV7M_BASEPRI_WAR=y
+CONFIG_ARMV7M_DCACHE=y
+CONFIG_ARMV7M_DTCM=y
+CONFIG_ARMV7M_ICACHE=y
+CONFIG_ARMV7M_MEMCPY=y
+CONFIG_ARMV7M_USEBASEPRI=y
+CONFIG_ARM_MPU_EARLY_RESET=y
+CONFIG_BOARDCTL_RESET=y
+CONFIG_BOARD_CRASHDUMP=y
+CONFIG_BOARD_LOOPSPERMSEC=95150
+CONFIG_BOARD_RESET_ON_ASSERT=2
+CONFIG_BUILTIN=y
+CONFIG_C99_BOOL8=y
+CONFIG_CDCACM=y
+CONFIG_CDCACM_IFLOWCONTROL=y
+CONFIG_CDCACM_PRODUCTID=0x1024
+CONFIG_CDCACM_PRODUCTSTR="mRoControlZeroH7 OEM"
+CONFIG_CDCACM_RXBUFSIZE=600
+CONFIG_CDCACM_TXBUFSIZE=12000
+CONFIG_CDCACM_VENDORID=0x26ac
+CONFIG_CDCACM_VENDORSTR="mRo"
+CONFIG_CLOCK_MONOTONIC=y
+CONFIG_DEBUG_FULLOPT=y
+CONFIG_DEBUG_HARDFAULT_ALERT=y
+CONFIG_DEBUG_MEMFAULT=y
+CONFIG_DEBUG_SYMBOLS=y
+CONFIG_DEFAULT_SMALL=y
+CONFIG_DEV_FIFO_SIZE=0
+CONFIG_DEV_PIPE_MAXSIZE=1024
+CONFIG_DEV_PIPE_SIZE=70
+CONFIG_DISABLE_MQUEUE=y
+CONFIG_EXPERIMENTAL=y
+CONFIG_FAT_DMAMEMORY=y
+CONFIG_FAT_LCNAMES=y
+CONFIG_FAT_LFN=y
+CONFIG_FAT_LFN_ALIAS_HASH=y
+CONFIG_FDCLONE_STDIO=y
+CONFIG_FS_BINFS=y
+CONFIG_FS_CROMFS=y
+CONFIG_FS_FAT=y
+CONFIG_FS_FATTIME=y
+CONFIG_FS_PROCFS=y
+CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
+CONFIG_FS_PROCFS_MAX_TASKS=64
+CONFIG_FS_PROCFS_REGISTER=y
+CONFIG_FS_ROMFS=y
+CONFIG_GRAN=y
+CONFIG_GRAN_INTR=y
+CONFIG_HAVE_CXX=y
+CONFIG_HAVE_CXXINITIALIZE=y
+CONFIG_I2C=y
+CONFIG_I2C_RESET=y
+CONFIG_IDLETHREAD_STACKSIZE=750
+CONFIG_LIBC_FLOATINGPOINT=y
+CONFIG_LIBC_LONG_LONG=y
+CONFIG_LIBC_STRERROR=y
+CONFIG_MEMSET_64BIT=y
+CONFIG_MEMSET_OPTSPEED=y
+CONFIG_MMCSD=y
+CONFIG_MMCSD_SDIO=y
+CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y
+CONFIG_MM_REGIONS=4
+CONFIG_MTD=y
+CONFIG_MTD_BYTE_WRITE=y
+CONFIG_MTD_PARTITION=y
+CONFIG_MTD_PROGMEM=y
+CONFIG_MTD_RAMTRON=y
+CONFIG_NAME_MAX=40
+CONFIG_NET=y
+CONFIG_NETDEV_CAN_BITRATE_IOCTL=y
+CONFIG_NETDEV_IFINDEX=y
+CONFIG_NETDEV_PHY_IOCTL=y
+CONFIG_NET_CAN=y
+CONFIG_NET_CAN_RAW_TX_DEADLINE=y
+CONFIG_NET_CAN_SOCK_OPTS=y
+CONFIG_NET_TIMESTAMP=y
+CONFIG_NSH_ARCHINIT=y
+CONFIG_NSH_ARGCAT=y
+CONFIG_NSH_BUILTIN_APPS=y
+CONFIG_NSH_CMDPARMS=y
+CONFIG_NSH_CROMFSETC=y
+CONFIG_NSH_DISABLE_IFCONFIG=y
+CONFIG_NSH_DISABLE_IFUPDOWN=y
+CONFIG_NSH_DISABLE_TELNETD=y
+CONFIG_NSH_LINELEN=128
+CONFIG_NSH_MAXARGUMENTS=15
+CONFIG_NSH_NESTDEPTH=8
+CONFIG_NSH_QUOTE=y
+CONFIG_NSH_ROMFSETC=y
+CONFIG_NSH_ROMFSSECTSIZE=128
+CONFIG_NSH_STRERROR=y
+CONFIG_NSH_VARS=y
+CONFIG_OTG_ID_GPIO_DISABLE=y
+CONFIG_PIPES=y
+CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
+CONFIG_PTHREAD_MUTEX_ROBUST=y
+CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_SETSPEED=y
+CONFIG_RAM_SIZE=245760
+CONFIG_RAM_START=0x20010000
+CONFIG_RAW_BINARY=y
+CONFIG_READLINE_CMD_HISTORY=y
+CONFIG_READLINE_TABCOMPLETION=y
+CONFIG_RTC_DATETIME=y
+CONFIG_SCHED_ATEXIT=y
+CONFIG_SCHED_HPWORK=y
+CONFIG_SCHED_HPWORKPRIORITY=249
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_INSTRUMENTATION=y
+CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
+CONFIG_SCHED_WAITPID=y
+CONFIG_SDCLONE_DISABLE=y
+CONFIG_SDMMC1_SDIO_PULLUP=y
+CONFIG_SEM_NNESTPRIO=8
+CONFIG_SEM_PREALLOCHOLDERS=0
+CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
+CONFIG_SERIAL_TERMIOS=y
+CONFIG_SIG_DEFAULT=y
+CONFIG_SIG_SIGALRM_ACTION=y
+CONFIG_SIG_SIGUSR1_ACTION=y
+CONFIG_SIG_SIGUSR2_ACTION=y
+CONFIG_SIG_SIGWORK=4
+CONFIG_STACK_COLORATION=y
+CONFIG_START_DAY=30
+CONFIG_START_MONTH=11
+CONFIG_STDIO_BUFFER_SIZE=256
+CONFIG_STM32H7_ADC1=y
+CONFIG_STM32H7_ADC3=y
+CONFIG_STM32H7_BBSRAM=y
+CONFIG_STM32H7_BBSRAM_FILES=5
+CONFIG_STM32H7_BDMA=y
+CONFIG_STM32H7_BKPSRAM=y
+CONFIG_STM32H7_DMA1=y
+CONFIG_STM32H7_DMA2=y
+CONFIG_STM32H7_DMACAPABLE=y
+CONFIG_STM32H7_FDCAN1=y
+CONFIG_STM32H7_FLOWCONTROL_BROKEN=y
+CONFIG_STM32H7_I2C1=y
+CONFIG_STM32H7_I2C3=y
+CONFIG_STM32H7_I2C4=y
+CONFIG_STM32H7_I2C_DYNTIMEO=y
+CONFIG_STM32H7_I2C_DYNTIMEO_STARTSTOP=10
+CONFIG_STM32H7_OTGFS=y
+CONFIG_STM32H7_PROGMEM=y
+CONFIG_STM32H7_RTC=y
+CONFIG_STM32H7_RTC_HSECLOCK=y
+CONFIG_STM32H7_RTC_MAGIC_REG=1
+CONFIG_STM32H7_SAVE_CRASHDUMP=y
+CONFIG_STM32H7_SDMMC1=y
+CONFIG_STM32H7_SERIALBRK_BSDCOMPAT=y
+CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y
+CONFIG_STM32H7_SPI1=y
+CONFIG_STM32H7_SPI1_DMA=y
+CONFIG_STM32H7_SPI1_DMA_BUFFER=1024
+CONFIG_STM32H7_SPI2=y
+CONFIG_STM32H7_SPI5=y
+CONFIG_STM32H7_SPI5_DMA=y
+CONFIG_STM32H7_SPI5_DMA_BUFFER=1024
+CONFIG_STM32H7_SPI_DMA=y
+CONFIG_STM32H7_TIM1=y
+CONFIG_STM32H7_TIM2=y
+CONFIG_STM32H7_TIM3=y
+CONFIG_STM32H7_TIM4=y
+CONFIG_STM32H7_TIM8=y
+CONFIG_STM32H7_UART4=y
+CONFIG_STM32H7_UART7=y
+CONFIG_STM32H7_UART8=y
+CONFIG_STM32H7_USART2=y
+CONFIG_STM32H7_USART3=y
+CONFIG_STM32H7_USART6=y
+CONFIG_STM32H7_USART_BREAKS=y
+CONFIG_STM32H7_USART_INVERT=y
+CONFIG_STM32H7_USART_SINGLEWIRE=y
+CONFIG_STM32H7_USART_SWAP=y
+CONFIG_SYSTEM_CDCACM=y
+CONFIG_SYSTEM_NSH=y
+CONFIG_TASK_NAME_SIZE=24
+CONFIG_TTY_SIGINT=y
+CONFIG_TTY_SIGTSTP=y
+CONFIG_UART4_BAUD=57600
+CONFIG_UART4_RXBUFSIZE=600
+CONFIG_UART4_TXBUFSIZE=1500
+CONFIG_UART7_BAUD=57600
+CONFIG_UART7_RXBUFSIZE=600
+CONFIG_UART7_TXBUFSIZE=1500
+CONFIG_UART8_BAUD=57600
+CONFIG_UART8_RXBUFSIZE=600
+CONFIG_UART8_SERIAL_CONSOLE=y
+CONFIG_UART8_TXBUFSIZE=1500
+CONFIG_USART2_BAUD=57600
+CONFIG_USART2_IFLOWCONTROL=y
+CONFIG_USART2_OFLOWCONTROL=y
+CONFIG_USART2_RXBUFSIZE=600
+CONFIG_USART2_TXBUFSIZE=1500
+CONFIG_USART3_BAUD=57600
+CONFIG_USART3_IFLOWCONTROL=y
+CONFIG_USART3_OFLOWCONTROL=y
+CONFIG_USART3_RXBUFSIZE=600
+CONFIG_USART3_TXBUFSIZE=3000
+CONFIG_USART6_BAUD=57600
+CONFIG_USART6_RXBUFSIZE=600
+CONFIG_USART6_TXBUFSIZE=1500
+CONFIG_USBDEV=y
+CONFIG_USBDEV_BUSPOWERED=y
+CONFIG_USBDEV_MAXPOWER=500
+CONFIG_USEC_PER_TICK=1000
+CONFIG_USERMAIN_STACKSIZE=2944
+CONFIG_USER_ENTRYPOINT="nsh_main"
@@ -0,0 +1,13 @@
+CONFIG_DRIVERS_UAVCAN=n
+CONFIG_DRIVERS_UAVCAN_V1=y
+CONFIG_UAVCAN_V1_BMS_SUBSCRIBER=y
+CONFIG_UAVCAN_V1_CLIENT=y
+CONFIG_UAVCAN_V1_ESC_CONTROLLER=y
+CONFIG_UAVCAN_V1_ESC_SUBSCRIBER=y
+CONFIG_UAVCAN_V1_GNSS_PUBLISHER=y
+CONFIG_UAVCAN_V1_GNSS_SUBSCRIBER_0=y
+CONFIG_UAVCAN_V1_GNSS_SUBSCRIBER_1=y
+CONFIG_UAVCAN_V1_READINESS_PUBLISHER=y
+CONFIG_UAVCAN_V1_UORB_ACTUATOR_OUTPUTS_PUBLISHER=y
+CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_PUBLISHER=y
+CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_SUBSCRIBER=y
@@ -22,7 +22,6 @@ CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
-CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LOGGER=y
@@ -34,7 +33,7 @@ CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
-CONFIG_SYSTEMCMDS_DMESG=y
+CONFIG_SYSTEMCMDS_DMESG=n
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
 CONFIG_SYSTEMCMDS_MIXER=y
 CONFIG_SYSTEMCMDS_PARAM=y
@@ -0,0 +1,3 @@
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=n
+CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=n
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM20608G=y
@@ -8,7 +8,11 @@ board_adc start
 if ! mpu6000 -R 6 -s start
 then
 	# some boards such as the Hobbywing XRotor F4 G2 use the ICM-20602
-	icm20602 -s -R 6 start
+	if ! icm20602 -s -R 6 start
+	then
+		# some clones of Omnibus F4 use the ICM-20608G (such as F4 PDB for Martian II FPV Drone)
+		icm20608g -s -R 6 start
+	fi
 fi

 bmp280 -s start
@@ -39,6 +39,7 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
 	initSPIBus(SPI::Bus::SPI1, {
 		initSPIDevice(DRV_IMU_DEVTYPE_MPU6000, SPI::CS{GPIO::PortA, GPIO::Pin4}),
 		initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortA, GPIO::Pin4}),
+		initSPIDevice(DRV_IMU_DEVTYPE_ICM20608G, SPI::CS{GPIO::PortA, GPIO::Pin4}),
 	}),
 	initSPIBus(SPI::Bus::SPI2, {
 		initSPIDevice(SPIDEV_MMCSD(0), SPI::CS{GPIO::PortB, GPIO::Pin12})
@@ -12,7 +12,6 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-CONFIG_SYSTEMCMDS_I2CDETECT=y
 CONFIG_SYSTEMCMDS_PERF=y
 CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_SYSTEMCMDS_USB_CONNECTED=y
@@ -14,10 +14,12 @@ CONFIG_DRIVERS_PCA9685=n
 CONFIG_DRIVERS_PCA9685_PWM_OUT=n
 CONFIG_DRIVERS_POWER_MONITOR_INA226=n
 CONFIG_DRIVERS_PWM_INPUT=n
+CONFIG_DRIVERS_PWM_OUT_SIM=n
 CONFIG_DRIVERS_ROBOCLAW=n
 CONFIG_DRIVERS_RPM=n
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=n
 CONFIG_DRIVERS_UAVCAN=n
+CONFIG_EXAMPLES_FAKE_GPS=n
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n
 CONFIG_MODULES_CAMERA_FEEDBACK=n
 CONFIG_MODULES_ESC_BATTERY=n
@@ -258,6 +258,7 @@ CONFIG_UART4_TXBUFSIZE=1500
 CONFIG_UART5_IFLOWCONTROL=y
 CONFIG_UART5_OFLOWCONTROL=y
 CONFIG_UART5_RXDMA=y
+CONFIG_UART5_TXBUFSIZE=3000
 CONFIG_UART5_TXDMA=y
 CONFIG_UART7_BAUD=57600
 CONFIG_UART7_IFLOWCONTROL=y
@@ -13,7 +13,7 @@ if(REPLAY_FILE)

 	message(STATUS "Building without lockstep for replay")
 	set(ENABLE_LOCKSTEP_SCHEDULER no)
-elif(CMAKE_BUILD_TYPE STREQUAL FuzzTesting)
+elseif(CMAKE_BUILD_TYPE STREQUAL FuzzTesting)
 	set(ENABLE_LOCKSTEP_SCHEDULER no)
 else()
 	set(ENABLE_LOCKSTEP_SCHEDULER yes)
@@ -3,3 +3,6 @@ uint8 NUM_ACTUATOR_OUTPUTS		= 16
 uint8 NUM_ACTUATOR_OUTPUT_GROUPS	= 4	# for sanity checking
 uint32 noutputs				# valid outputs
 float32[16] output				# output data, in natural output units
+
+# actuator_outputs_sim is used for SITL, HITL & SIH (with an output range of [-1, 1])
+# TOPICS actuator_outputs actuator_outputs_sim
@@ -33,6 +33,8 @@ bool cs_synthetic_mag_z       # 25 - true when we are using a synthesized measur
 bool cs_vehicle_at_rest       # 26 - true when the vehicle is at rest
 bool cs_gps_yaw_fault         # 27 - true when the GNSS heading has been declared faulty and is no longer being used
 bool cs_rng_fault             # 28 - true when the range finder has been declared faulty and is no longer being used
+bool cs_inertial_dead_reckoning # 29 - true if we are no longer fusing measurements that constrain horizontal velocity drift
+bool cs_wind_dead_reckoning     # 30 - true if we are navigationg reliant on wind relative measurements

 # fault status
 uint32 fault_status_changes   # number of filter fault status (fs) changes
@@ -33,7 +33,7 @@ uint32 silence       # in us
 uint8 volume         # value between 0-100 if supported by backend

 uint8 VOLUME_LEVEL_MIN = 0
-uint8 VOLUME_LEVEL_DEFAULT = 40
+uint8 VOLUME_LEVEL_DEFAULT = 20
 uint8 VOLUME_LEVEL_MAX = 100

 uint8 ORB_QUEUE_LENGTH = 4
@@ -13,12 +13,19 @@ bool condition_local_altitude_valid
 bool condition_local_position_valid		# set to true by the commander app if the quality of the local position estimate is good enough to use for navigation
 bool condition_local_velocity_valid		# set to true by the commander app if the quality of the local horizontal velocity data is good enough to use for navigation
 bool condition_global_position_valid		# set to true by the commander app if the quality of the global position estimate is good enough to use for navigation
+bool condition_gps_position_valid
 bool condition_home_position_valid		# indicates a valid home position (a valid home position is not always a valid launch)
 bool condition_power_input_valid                # set if input power is valid
 bool condition_battery_healthy                # set if battery is available and not low
 bool condition_escs_error		      # set to true if one or more ESCs reporting esc_status are offline
 bool condition_escs_failure		      # set to true if one or more ESCs reporting esc_status has a failure

+bool position_reliant_on_gps
+bool position_reliant_on_optical_flow
+bool position_reliant_on_vision_position
+
+bool dead_reckoning
+
 bool circuit_breaker_engaged_power_check
 bool circuit_breaker_engaged_airspd_check
 bool circuit_breaker_engaged_enginefailure_check
@@ -136,19 +136,40 @@ public:
 	/**
 	 * Check if there is a new update.
 	 */
-	bool updated() { return advertised() && Manager::updates_available(_node, _last_generation); }
+	bool updated()
+	{
+		if (!valid()) {
+			subscribe();
+		}
+
+		return valid() ? Manager::updates_available(_node, _last_generation) : false;
+	}

 	/**
 	 * Update the struct
 	 * @param dst The uORB message struct we are updating.
 	 */
-	bool update(void *dst) { return updated() && Manager::orb_data_copy(_node, dst, _last_generation); }
+	bool update(void *dst)
+	{
+		if (!valid()) {
+			subscribe();
+		}
+
+		return valid() ? Manager::orb_data_copy(_node, dst, _last_generation, true) : false;
+	}

 	/**
 	 * Copy the struct
 	 * @param dst The uORB message struct we are updating.
 	 */
-	bool copy(void *dst) { return advertised() && Manager::orb_data_copy(_node, dst, _last_generation); }
+	bool copy(void *dst)
+	{
+		if (!valid()) {
+			subscribe();
+		}
+
+		return valid() ? Manager::orb_data_copy(_node, dst, _last_generation, false) : false;
+	}

 	/**
 	 * Change subscription instance
@@ -172,7 +172,7 @@ int	uORB::Manager::orb_ioctl(unsigned int cmd, unsigned long arg)

 	case ORBIOCDEVDATACOPY: {
 			orbiocdevdatacopy_t *data = (orbiocdevdatacopy_t *)arg;
-			data->ret = uORB::Manager::orb_data_copy(data->handle, data->dst, data->generation);
+			data->ret = uORB::Manager::orb_data_copy(data->handle, data->dst, data->generation, data->only_if_updated);
 		}
 		break;

@@ -479,8 +479,16 @@ void uORB::Manager::orb_remove_internal_subscriber(void *node_handle)

 uint8_t uORB::Manager::orb_get_queue_size(const void *node_handle) { return static_cast<const DeviceNode *>(node_handle)->get_queue_size(); }

-bool uORB::Manager::orb_data_copy(void *node_handle, void *dst, unsigned &generation)
+bool uORB::Manager::orb_data_copy(void *node_handle, void *dst, unsigned &generation, bool only_if_updated)
 {
+	if (!is_advertised(node_handle)) {
+		return false;
+	}
+
+	if (only_if_updated && !static_cast<const uORB::DeviceNode *>(node_handle)->updates_available(generation)) {
+		return false;
+	}
+
 	return static_cast<DeviceNode *>(node_handle)->copy(dst, generation);
 }

@@ -509,7 +517,8 @@ uint8_t uORB::Manager::orb_get_instance(const void *node_handle)
 #if !defined(CONFIG_BUILD_FLAT)
 unsigned uORB::Manager::updates_available(const void *node_handle, unsigned last_generation)
 {
-	return static_cast<const uORB::DeviceNode *>(node_handle)->updates_available(last_generation);
+	return is_advertised(node_handle) ? static_cast<const uORB::DeviceNode *>(node_handle)->updates_available(
+		       last_generation) : 0;
 }

 bool uORB::Manager::is_advertised(const void *node_handle)
@@ -117,6 +117,7 @@ typedef struct {
 	void *handle;
 	void *dst;
 	unsigned generation;
+	bool only_if_updated;
 	bool ret;
 } orbiocdevdatacopy_t;

@@ -450,7 +451,7 @@ public:

 	static uint8_t orb_get_queue_size(const void *node_handle);

-	static bool orb_data_copy(void *node_handle, void *dst, unsigned &generation);
+	static bool orb_data_copy(void *node_handle, void *dst, unsigned &generation, bool only_if_updated);

 	static bool register_callback(void *node_handle, SubscriptionCallback *callback_sub);

@@ -460,9 +461,10 @@ public:

 #if defined(CONFIG_BUILD_FLAT)
 	/* These are optimized by inlining in NuttX Flat build */
-	static unsigned updates_available(const void *node_handle, unsigned last_generation) { return static_cast<const DeviceNode *>(node_handle)->updates_available(last_generation); }
+	static unsigned updates_available(const void *node_handle, unsigned last_generation) { return is_advertised(node_handle) ? static_cast<const DeviceNode *>(node_handle)->updates_available(last_generation) : 0; }

 	static bool is_advertised(const void *node_handle) { return static_cast<const DeviceNode *>(node_handle)->is_advertised(); }
+
 #else
 	static unsigned updates_available(const void *node_handle, unsigned last_generation);

@@ -303,9 +303,9 @@ uint8_t uORB::Manager::orb_get_queue_size(const void *node_handle)
 	return data.size;
 }

-bool uORB::Manager::orb_data_copy(void *node_handle, void *dst, unsigned &generation)
+bool uORB::Manager::orb_data_copy(void *node_handle, void *dst, unsigned &generation, bool only_if_updated)
 {
-	orbiocdevdatacopy_t data = {node_handle, dst, generation, false};
+	orbiocdevdatacopy_t data = {node_handle, dst, generation, only_if_updated, false};
 	boardctl(ORBIOCDEVDATACOPY, reinterpret_cast<unsigned long>(&data));
 	generation = data.generation;

@@ -21,6 +21,24 @@
                "set print pretty",
            ]
        },
+        {
+            "name": "stlink (@PX4_BOARD@)",
+            "device": "@DEBUG_DEVICE@",
+            "svdFile": "@DEBUG_SVD_FILE_PATH@",
+            "executable": "${command:cmake.launchTargetPath}",
+            "request": "launch",
+            "type": "cortex-debug",
+            "servertype": "stutil",
+            "cwd": "${workspaceFolder}",
+            "internalConsoleOptions": "openOnSessionStart",
+            "preLaunchCommands": [
+                "source ${workspaceFolder}/platforms/nuttx/Debug/PX4",
+                "source ${workspaceFolder}/platforms/nuttx/Debug/NuttX",
+                "source ${workspaceFolder}/platforms/nuttx/Debug/ARMv7M",
+                "set mem inaccessible-by-default off",
+                "set print pretty",
+            ]
+        },
        {
            "name": "blackmagic (@PX4_BOARD@)",
            "device": "@DEBUG_DEVICE@",
@@ -8,6 +8,7 @@ add_library(px4_layer
 	cdc_acm_check.cpp
 	${PX4_SOURCE_DIR}/platforms/posix/src/px4/common/print_load.cpp
 	${PX4_SOURCE_DIR}/platforms/posix/src/px4/common/cpuload.cpp
+	usr_hrt.cpp
 )

 target_link_libraries(px4_layer
@@ -0,0 +1,221 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file usr_hrt.c
+ *
+ * Userspace High-resolution timer callouts and timekeeping.
+ *
+ * This can be used with nuttx userspace
+ *
+ */
+
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/shutdown.h>
+
+#include <sys/ioctl.h>
+#include <sys/types.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <fcntl.h>
+#include <sched.h>
+#include <errno.h>
+
+#include <assert.h>
+#include <debug.h>
+
+#include <board_config.h>
+#include <drivers/drv_hrt.h>
+#include <sys/boardctl.h>
+
+static px4_task_t g_usr_hrt_task = -1;
+
+/**
+ * Fetch a never-wrapping absolute time value in microseconds from
+ * some arbitrary epoch shortly after system start.
+ */
+hrt_abstime
+hrt_absolute_time(void)
+{
+	hrt_abstime abstime = 0;
+	boardctl(HRT_ABSOLUTE_TIME, (uintptr_t)&abstime);
+	return abstime;
+}
+
+/**
+ * Store the absolute time in an interrupt-safe fashion
+ */
+void
+hrt_store_absolute_time(volatile hrt_abstime *t)
+{
+	irqstate_t flags = px4_enter_critical_section();
+	*t = hrt_absolute_time();
+	px4_leave_critical_section(flags);
+}
+
+/**
+ * Event dispatcher thread
+ */
+int
+event_thread(int argc, char *argv[])
+{
+	struct hrt_call *entry = NULL;
+
+	while (1) {
+		/* Wait for hrt tick */
+		boardctl(HRT_WAITEVENT, (uintptr_t)&entry);
+
+		/* HRT event received, dispatch */
+		if (entry) {
+			entry->usr_callout(entry->usr_arg);
+		}
+	}
+
+	return 0;
+}
+
+/**
+ * Request stop.
+ */
+bool hrt_request_stop()
+{
+	px4_task_delete(g_usr_hrt_task);
+	return true;
+}
+
+/**
+ * Initialise the high-resolution timing module.
+ */
+void
+hrt_init(void)
+{
+	px4_register_shutdown_hook(hrt_request_stop);
+	g_usr_hrt_task = px4_task_spawn_cmd("usr_hrt", SCHED_DEFAULT, SCHED_PRIORITY_MAX, 1000, event_thread, NULL);
+}
+
+/**
+ * Call callout(arg) after interval has elapsed.
+ */
+void
+hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg)
+{
+	hrt_boardctl_t ioc_parm;
+	ioc_parm.entry = entry;
+	ioc_parm.time = delay;
+	ioc_parm.callout = callout;
+	ioc_parm.arg = arg;
+	entry->usr_callout = callout;
+	entry->usr_arg = arg;
+
+	boardctl(HRT_CALL_AFTER, (uintptr_t)&ioc_parm);
+}
+
+/**
+ * Call callout(arg) at calltime.
+ */
+void
+hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg)
+{
+	hrt_boardctl_t ioc_parm;
+	ioc_parm.entry = entry;
+	ioc_parm.time = calltime;
+	ioc_parm.interval = 0;
+	ioc_parm.callout = callout;
+	ioc_parm.arg = arg;
+	entry->usr_callout = callout;
+	entry->usr_arg = arg;
+
+	boardctl(HRT_CALL_AT, (uintptr_t)&ioc_parm);
+}
+
+/**
+ * Call callout(arg) every period.
+ */
+void
+hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg)
+{
+	hrt_boardctl_t ioc_parm;
+	ioc_parm.entry = entry;
+	ioc_parm.time = delay;
+	ioc_parm.interval = interval;
+	ioc_parm.callout = callout;
+	ioc_parm.arg = arg;
+	entry->usr_callout = callout;
+	entry->usr_arg = arg;
+
+	boardctl(HRT_CALL_EVERY, (uintptr_t)&ioc_parm);
+}
+
+/**
+ * Remove the entry from the callout list.
+ */
+void
+hrt_cancel(struct hrt_call *entry)
+{
+	boardctl(HRT_CANCEL, (uintptr_t)entry);
+}
+
+void
+hrt_call_init(struct hrt_call *entry)
+{
+	memset(entry, 0, sizeof(*entry));
+}
+
+/**
+ * If this returns true, the call has been invoked and removed from the callout list.
+ *
+ * Always returns false for repeating callouts.
+ */
+bool
+hrt_called(struct hrt_call *entry)
+{
+	return (entry->deadline == 0);
+}
+
+latency_info_t
+get_latency(uint16_t bucket_idx, uint16_t counter_idx)
+{
+	latency_boardctl_t latency_ioc;
+	latency_ioc.bucket_idx = bucket_idx;
+	latency_ioc.counter_idx = counter_idx;
+	latency_ioc.latency = {0, 0};
+	boardctl(HRT_GET_LATENCY, (uintptr_t)&latency_ioc);
+	return latency_ioc.latency;
+}
+
+void reset_latency_counters()
+{
+	boardctl(HRT_RESET_LATENCY, NULL);
+}
@@ -561,31 +561,6 @@ hrt_absolute_time(void)
 	return abstime;
 }

-/**
- * Convert a timespec to absolute time
- */
-hrt_abstime
-ts_to_abstime(const struct timespec *ts)
-{
-	hrt_abstime	result;
-
-	result = (hrt_abstime)(ts->tv_sec) * 1000000;
-	result += ts->tv_nsec / 1000;
-
-	return result;
-}
-
-/**
- * Convert absolute time to a timespec.
- */
-void
-abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
-{
-	ts->tv_sec = abstime / 1000000;
-	abstime -= ts->tv_sec * 1000000;
-	ts->tv_nsec = abstime * 1000;
-}
-
 /**
 * Store the absolute time in an interrupt-safe fashion
 */
@@ -573,31 +573,6 @@ hrt_absolute_time(void)
 	return abstime;
 }

-/**
- * Convert a timespec to absolute time
- */
-hrt_abstime
-ts_to_abstime(const struct timespec *ts)
-{
-	hrt_abstime	result;
-
-	result = (hrt_abstime)(ts->tv_sec) * 1000000;
-	result += ts->tv_nsec / 1000;
-
-	return result;
-}
-
-/**
- * Convert absolute time to a timespec.
- */
-void
-abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
-{
-	ts->tv_sec = abstime / 1000000;
-	abstime -= ts->tv_sec * 1000000;
-	ts->tv_nsec = abstime * 1000;
-}
-
 /**
 * Store the absolute time in an interrupt-safe fashion
 */
@@ -609,31 +609,6 @@ hrt_absolute_time(void)
 	return abstime;
 }

-/**
- * Convert a timespec to absolute time
- */
-hrt_abstime
-ts_to_abstime(const struct timespec *ts)
-{
-	hrt_abstime	result;
-
-	result = (hrt_abstime)(ts->tv_sec) * 1000000;
-	result += ts->tv_nsec / 1000;
-
-	return result;
-}
-
-/**
- * Convert absolute time to a timespec.
- */
-void
-abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
-{
-	ts->tv_sec = abstime / 1000000;
-	abstime -= ts->tv_sec * 1000000;
-	ts->tv_nsec = abstime * 1000;
-}
-
 /**
 * Store the absolute time in an interrupt-safe fashion
 */
@@ -497,43 +497,6 @@ hrt_abstime hrt_absolute_time(void)
 	return ((uint64_t) hi << 32) | lo;
 }

-/**
- * Convert a timespec to absolute time
- */
-hrt_abstime ts_to_abstime(const struct timespec *ts)
-{
-	hrt_abstime	result;
-
-	result = (hrt_abstime)(ts->tv_sec) * 1000000;
-	result += ts->tv_nsec / 1000;
-
-	return result;
-}
-
-/**
- * Convert absolute time to a timespec.
- */
-void abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
-{
-	ts->tv_sec = abstime / 1000000;
-	abstime -= ts->tv_sec * 1000000;
-	ts->tv_nsec = abstime * 1000;
-}
-
-/**
- * Compare a time value with the current time as atomic operation
- */
-hrt_abstime hrt_elapsed_time_atomic(const volatile hrt_abstime *then)
-{
-	irqstate_t flags = px4_enter_critical_section();
-
-	hrt_abstime delta = hrt_absolute_time() - *then;
-
-	px4_leave_critical_section(flags);
-
-	return delta;
-}
-
 /**
 * Store the absolute time in an interrupt-safe fashion
 */
@@ -72,6 +72,33 @@
 #  define hrtinfo(x...)
 #endif

+#if !defined(CONFIG_BUILD_FLAT)
+#include <px4_platform_common/defines.h>
+#include <px4_platform/board_ctrl.h>
+#include <px4_platform_common/sem.h>
+
+#define HRT_ENTRY_QUEUE_MAX_SIZE 3
+static px4_sem_t g_wait_sem;
+static struct hrt_call *next_hrt_entry[HRT_ENTRY_QUEUE_MAX_SIZE];
+static int hrt_entry_queued = 0;
+static bool suppress_entry_queue_error = false;
+static bool hrt_entry_queue_error = false;
+
+void hrt_usr_call(void *arg)
+{
+	// This is called from hrt interrupt
+	if (hrt_entry_queued < HRT_ENTRY_QUEUE_MAX_SIZE) {
+		next_hrt_entry[hrt_entry_queued++] = (struct hrt_call *)arg;
+
+	} else {
+		hrt_entry_queue_error = true;
+	}
+
+	px4_sem_post(&g_wait_sem);
+}
+
+#endif
+
 #ifdef HRT_TIMER

 /* HRT configuration */
@@ -275,6 +302,8 @@ static void		hrt_call_enter(struct hrt_call *entry);
 static void		hrt_call_reschedule(void);
 static void		hrt_call_invoke(void);

+
+int hrt_ioctl(unsigned int cmd, unsigned long arg);
 /*
 * Specific registers and bits used by PPM sub-functions
 */
@@ -694,31 +723,6 @@ hrt_absolute_time(void)
 	return abstime;
 }

-/**
- * Convert a timespec to absolute time
- */
-hrt_abstime
-ts_to_abstime(const struct timespec *ts)
-{
-	hrt_abstime	result;
-
-	result = (hrt_abstime)(ts->tv_sec) * 1000000;
-	result += ts->tv_nsec / 1000;
-
-	return result;
-}
-
-/**
- * Convert absolute time to a timespec.
- */
-void
-abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
-{
-	ts->tv_sec = abstime / 1000000;
-	abstime -= ts->tv_sec * 1000000;
-	ts->tv_nsec = abstime * 1000;
-}
-
 /**
 * Store the absolute time in an interrupt-safe fashion
 */
@@ -743,6 +747,16 @@ hrt_init(void)
 	/* configure the PPM input pin */
 	px4_arch_configgpio(GPIO_PPM_IN);
 #endif
+
+#if !defined(CONFIG_BUILD_FLAT)
+	/* Create a semaphore for handling hrt driver callbacks */
+	px4_sem_init(&g_wait_sem, 0, 0);
+	/* this is a signalling semaphore */
+	px4_sem_setprotocol(&g_wait_sem, SEM_PRIO_NONE);
+
+	/* register ioctl callbacks */
+	px4_register_boardct_ioctl(_HRTIOCBASE, hrt_ioctl);
+#endif
 }

 /**
@@ -1003,6 +1017,81 @@ void reset_latency_counters(void)
 		latency_counters[i] = 0;
 	}
 }
+
+/* board_ioctl interface for user-space hrt driver */
+int
+hrt_ioctl(unsigned int cmd, unsigned long arg)
+{
+	hrt_boardctl_t *h = (hrt_boardctl_t *)arg;
+
+	switch (cmd) {
+	case HRT_WAITEVENT: {
+			irqstate_t flags;
+			px4_sem_wait(&g_wait_sem);
+			/* Atomically update the pointer to user side hrt entry */
+			flags = px4_enter_critical_section();
+
+			/* This should be always true, but check it anyway */
+			if (hrt_entry_queued > 0) {
+				*(struct hrt_call **)arg = next_hrt_entry[--hrt_entry_queued];
+				next_hrt_entry[hrt_entry_queued] = NULL;
+
+			} else {
+				hrt_entry_queue_error = true;
+			}
+
+			px4_leave_critical_section(flags);
+
+			/* Warn once for entry queue being full */
+			if (hrt_entry_queue_error && !suppress_entry_queue_error) {
+				PX4_ERR("HRT entry error, queue size now %d", hrt_entry_queued);
+				suppress_entry_queue_error = true;
+			}
+		}
+		break;
+
+	case HRT_ABSOLUTE_TIME:
+		*(hrt_abstime *)arg = hrt_absolute_time();
+		break;
+
+	case HRT_CALL_AFTER:
+		hrt_call_after(h->entry, h->time, (hrt_callout)hrt_usr_call, h->entry);
+		break;
+
+	case HRT_CALL_AT:
+		hrt_call_at(h->entry, h->time, (hrt_callout)hrt_usr_call, h->entry);
+		break;
+
+	case HRT_CALL_EVERY:
+		hrt_call_every(h->entry, h->time, h->interval, (hrt_callout)hrt_usr_call, h->entry);
+		break;
+
+	case HRT_CANCEL:
+		if (h && h->entry) {
+			hrt_cancel(h->entry);
+
+		} else {
+			PX4_ERR("HRT_CANCEL called with NULL entry");
+		}
+
+		break;
+
+	case HRT_GET_LATENCY: {
+			latency_boardctl_t *latency = (latency_boardctl_t *)arg;
+			latency->latency = get_latency(latency->bucket_idx, latency->counter_idx);
+		}
+		break;
+
+	case HRT_RESET_LATENCY:
+		reset_latency_counters();
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return OK;
+}
 #endif

 #endif /* HRT_TIMER */
@@ -144,19 +144,6 @@ hrt_abstime hrt_absolute_time()
 #endif // defined(ENABLE_LOCKSTEP_SCHEDULER)
 }

-/*
- * Convert a timespec to absolute time.
- */
-hrt_abstime ts_to_abstime(const struct timespec *ts)
-{
-	hrt_abstime	result;
-
-	result = (hrt_abstime)(ts->tv_sec) * 1000000;
-	result += ts->tv_nsec / 1000;
-
-	return result;
-}
-
 /*
 * Store the absolute time in an interrupt-safe fashion.
 *
@@ -484,13 +471,6 @@ hrt_call_invoke()
 	hrt_unlock();
 }

-void abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
-{
-	ts->tv_sec = abstime / 1000000;
-	abstime -= ts->tv_sec * 1000000;
-	ts->tv_nsec = abstime * 1000;
-}
-
 int px4_clock_gettime(clockid_t clk_id, struct timespec *tp)
 {
 	if (clk_id == CLOCK_MONOTONIC) {
@@ -47,6 +47,10 @@
 #include <px4_platform_common/time.h>
 #include <queue.h>

+#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT)
+#include <px4_platform/board_ctrl.h>
+#endif
+
 __BEGIN_DECLS

 /**
@@ -76,6 +80,10 @@ typedef struct hrt_call {
 	hrt_abstime		period;
 	hrt_callout		callout;
 	void			*arg;
+#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT)
+	hrt_callout		usr_callout;
+	void			*usr_arg;
+#endif
 } *hrt_call_t;


@@ -89,6 +97,35 @@ typedef struct latency_info {
 	uint32_t                counter;
 } latency_info_t;

+#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT)
+
+typedef struct hrt_boardctl {
+	hrt_call_t		entry;
+	hrt_abstime		time; /* delay or calltime */
+	hrt_abstime		interval;
+	hrt_callout		callout;
+	void			*arg;
+} hrt_boardctl_t;
+
+typedef struct latency_boardctl {
+	uint16_t		bucket_idx;
+	uint16_t		counter_idx;
+	latency_info_t	latency;
+} latency_boardctl_t;
+
+#define _HRTIOC(_n)		(_PX4_IOC(_HRTIOCBASE, _n))
+
+#define HRT_WAITEVENT		_HRTIOC(1)
+#define HRT_ABSOLUTE_TIME	_HRTIOC(2)
+#define HRT_CALL_AFTER		_HRTIOC(3)
+#define HRT_CALL_AT		_HRTIOC(4)
+#define HRT_CALL_EVERY		_HRTIOC(5)
+#define HRT_CANCEL		_HRTIOC(6)
+#define HRT_GET_LATENCY		_HRTIOC(7)
+#define HRT_RESET_LATENCY	_HRTIOC(8)
+
+#endif
+
 /**
 * Get absolute time in [us] (does not wrap).
 */
@@ -97,12 +134,25 @@ __EXPORT extern hrt_abstime hrt_absolute_time(void);
 /**
 * Convert a timespec to absolute time.
 */
-__EXPORT extern hrt_abstime ts_to_abstime(const struct timespec *ts);
+static inline hrt_abstime ts_to_abstime(const struct timespec *ts)
+{
+	hrt_abstime	result;
+
+	result = (hrt_abstime)(ts->tv_sec) * 1000000;
+	result += ts->tv_nsec / 1000;
+
+	return result;
+}

 /**
 * Convert absolute time to a timespec.
 */
-__EXPORT extern void	abstime_to_ts(struct timespec *ts, hrt_abstime abstime);
+static inline void abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
+{
+	ts->tv_sec = abstime / 1000000;
+	abstime -= ts->tv_sec * 1000000;
+	ts->tv_nsec = abstime * 1000;
+}

 /**
 * Compute the delta between a timestamp taken in the past
@@ -478,7 +478,7 @@ bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 		_current_command.clear();
 	}

-	if (stop_motors || num_control_groups_updated > 0) {
+	if (stop_motors || num_control_groups_updated > 0 || _mixing_output.useDynamicMixing()) {
 		up_dshot_trigger();
 	}

@@ -377,7 +377,7 @@ bool ADIS16470::Configure()

 	_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
 	_px4_accel.set_range(40.f * CONSTANTS_ONE_G);
-	_px4_gyro.set_scale(math::radians(2000.f / 32768.f));
+	_px4_gyro.set_scale(math::radians(1.f / 0.1f)); // 1 LSB = 0.1°/sec
 	_px4_gyro.set_range(math::radians(2000.f));

 	_px4_accel.set_scale(1.25f * CONSTANTS_ONE_G / 1000.0f); // accel 1.25 mg/LSB
@@ -0,0 +1,5 @@
+menuconfig DRIVERS_IMU_INVENSENSE_ICM42670P
+	bool "icm42670p"
+	default n
+	---help---
+		Enable support for icm42670p
@@ -124,7 +124,12 @@ int INA228::read(uint8_t address, int32_t &data)
 	const int ret = transfer(&address, 1, (uint8_t *)&received_bytes, sizeof(received_bytes) - 1);

 	if (ret == PX4_OK) {
-		data = swap32(received_bytes) >> ((32 - 24) + 4);
+		data = swap32(received_bytes) >> ((32 - 24) + 4); // Convert to 20bit value
+
+		// Handle negative 20bit twos complement
+		if (data & 0x80000) {
+			data = -((0x000FFFFF & ~data) + 1);
+		}

 	} else {
 		perf_count(_comms_errors);
@@ -81,9 +81,40 @@ bool
 PWMSim::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
 		      unsigned num_control_groups_updated)
 {
-	// Nothing to do, as we are only interested in the actuator_outputs topic publication.
-	// That should only be published once we receive actuator_controls (important for lock-step to work correctly)
-	return num_control_groups_updated > 0;
+	// Only publish once we receive actuator_controls (important for lock-step to work correctly)
+	if (num_control_groups_updated > 0) {
+		actuator_outputs_s actuator_outputs{};
+		actuator_outputs.noutputs = num_outputs;
+
+		const uint32_t reversible_outputs = _mixing_output.reversibleOutputs();
+
+		for (int i = 0; i < (int)num_outputs; i++) {
+			if (outputs[i] != PWM_SIM_DISARMED_MAGIC) {
+
+				OutputFunction function = _mixing_output.outputFunction(i);
+				bool is_reversible = reversible_outputs & (1u << i);
+				float output = outputs[i];
+
+				if (((int)function >= (int)OutputFunction::Motor1 && (int)function <= (int)OutputFunction::MotorMax)
+				    && !is_reversible) {
+					// Scale non-reversible motors to [0, 1]
+					actuator_outputs.output[i] = (output - PWM_SIM_PWM_MIN_MAGIC) / (PWM_SIM_PWM_MAX_MAGIC - PWM_SIM_PWM_MIN_MAGIC);
+
+				} else {
+					// Scale everything else to [-1, 1]
+					const float pwm_center = (PWM_SIM_PWM_MAX_MAGIC + PWM_SIM_PWM_MIN_MAGIC) / 2.f;
+					const float pwm_delta = (PWM_SIM_PWM_MAX_MAGIC - PWM_SIM_PWM_MIN_MAGIC) / 2.f;
+					actuator_outputs.output[i] = (output - pwm_center) / pwm_delta;
+				}
+			}
+		}
+
+		actuator_outputs.timestamp = hrt_absolute_time();
+		_actuator_outputs_sim_pub.publish(actuator_outputs);
+		return true;
+	}
+
+	return false;
 }

 int
@@ -43,6 +43,9 @@
 #include <px4_platform_common/tasks.h>
 #include <px4_platform_common/time.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/actuator_outputs.h>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>

 #if defined(CONFIG_ARCH_BOARD_PX4_SITL)
 #define PARAM_PREFIX "PWM_MAIN"
@@ -86,5 +89,7 @@ private:

 	MixingOutput _mixing_output{PARAM_PREFIX, MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+
+	uORB::Publication<actuator_outputs_s> _actuator_outputs_sim_pub{ORB_ID(actuator_outputs_sim)};
 };

@@ -58,7 +58,7 @@ class UavcanEscController
 {
 public:
 	static constexpr int MAX_ACTUATORS = esc_status_s::CONNECTED_ESC_MAX;
-	static constexpr unsigned MAX_RATE_HZ = 200;			///< XXX make this configurable
+	static constexpr unsigned MAX_RATE_HZ = 400;
 	static constexpr uint16_t DISARMED_OUTPUT_VALUE = UINT16_MAX;

 	static_assert(uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize >= MAX_ACTUATORS, "Too many actuators");
@@ -112,6 +112,7 @@ px4_add_module(
 	INCLUDES
 		${LIBCANARD_DIR}/libcanard/
 		${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated
+		${CMAKE_CURRENT_SOURCE_DIR}/Publishers
 	SRCS
 		PublicationManager.cpp
 		PublicationManager.hpp
@@ -124,6 +125,7 @@ px4_add_module(
 		Publishers/uORB/uorb_publisher.cpp
 		Subscribers/uORB/uorb_subscriber.cpp
 		${SRCS}
+		Publishers/Publisher.cpp
 		o1heap/o1heap.c
 		o1heap/o1heap.h
 		${LIBCANARD_DIR}/libcanard/canard.c
@@ -56,8 +56,11 @@ int CanardSocketCAN::init()
 	struct ifreq ifr;

 	//FIXME HOTFIX to make this code compile
+#ifdef CONFIG_NET_CAN_CANFD
+	bool can_fd = 1;
+#else
 	bool can_fd = 0;
-
+#endif
 	_can_fd = can_fd;

 	/* open socket */
@@ -45,7 +45,9 @@
 #include <reg/drone/physics/kinematics/geodetic/Point_0_1.h>
 #include <reg/drone/service/gnss/DilutionOfPrecision_0_1.h>

-#include "../Publisher.hpp"
+class UavcanNode;
+
+#include "Publisher.hpp"

 class UavcanGnssPublisher : public UavcanPublisher
 {
@@ -124,6 +126,8 @@ public:
 				// set the data ready in the buffer and chop if needed
 				++_transfer_id_2;  // The transfer-ID shall be incremented after every transmission on this subject.
 				result = canardTxPush(&_canard_instance, &transfer2);
+
+				transmit();
 			}
 		}
 	};
@@ -95,6 +95,8 @@ public:
 				// set the data ready in the buffer and chop if needed
 				++_transfer_id;  // The transfer-ID shall be incremented after every transmission on this subject.
 				result = canardTxPush(&_canard_instance, &transfer);
+
+				transmit();
 			}
 		}
 	};
@@ -0,0 +1,49 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Publisher.cpp
+ *
+ * Defines basic functionality of UAVCAN v1 publisher class
+ *
+ * @author Jacob Crabill <jacob@flyvoly.com>
+ */
+
+#include "Publisher.hpp"
+
+#include "../Uavcan.hpp"
+
+void UavcanPublisher::transmit()
+{
+	UavcanNode::do_transmit();
+}
@@ -142,4 +142,6 @@ protected:
 	CanardTransferID _transfer_id {0};

 	UavcanPublisher *_next_pub {nullptr};
+
+	void transmit();
 };
@@ -81,6 +81,7 @@ public:
 			// set the data ready in the buffer and chop if needed
 			++_transfer_id;  // The transfer-ID shall be incremented after every transmission on this subject.
 			canardTxPush(&_canard_instance, &transfer);
+			transmit();
 		}
 	};

@@ -140,6 +140,8 @@ public:

 	static UavcanNode *instance() { return _instance; }

+	static void do_transmit() { if (_instance) _instance->transmit(); };
+
 	/* The bit rate that can be passed back to the bootloader */
 	int32_t active_bitrate{0};

@@ -110,6 +110,10 @@
                        "6": {
                            "name": "no_rc_and_no_datalink",
                            "description": "No RC and no datalink"
+                        },
+                        "7": {
+                            "name": "no_gps",
+                            "description": "No valid GPS"
                        }
                    }
                },
@@ -485,7 +485,7 @@ public:
 	 * @param vec vector to rotate in frame 1 (typically body frame)
 	 * @return rotated vector in frame 2 (typically reference frame)
 	 */
-	Vector3<Type> conjugate(const Vector3<Type> &vec) const
+	Vector3<Type> rotateVector(const Vector3<Type> &vec) const
 	{
 		const Quaternion &q = *this;
 		Quaternion v(Type(0), vec(0), vec(1), vec(2));
@@ -502,7 +502,7 @@ public:
 	 * @param vec vector to rotate in frame 2 (typically reference frame)
 	 * @return rotated vector in frame 1 (typically body frame)
 	 */
-	Vector3<Type> conjugate_inversed(const Vector3<Type> &vec) const
+	Vector3<Type> rotateVectorInverse(const Vector3<Type> &vec) const
 	{
 		const Quaternion &q = *this;
 		Quaternion v(Type(0), vec(0), vec(1), vec(2));
@@ -123,6 +123,35 @@ Type wrap_2pi(Type x)
 	return wrap(x, Type(0), Type(M_TWOPI));
 }

+/**
+ * Unwrap value that was wrapped with range [low, high)
+ *
+ * @param[in] last_x Last unwrapped value
+ * @param[in] new_x New value in range
+ * @param low lower limit of the wrapping range
+ * @param high upper limit of the wrapping range
+ * @return New unwrapped value
+ */
+template<typename Type>
+Type unwrap(const Type last_x, const Type new_x, const Type low, const Type high)
+{
+	return last_x + wrap(new_x - last_x, low, high);
+}
+
+/**
+ * Unwrap value with range [-π, π)
+ *
+ * @param[in] last_angle Last unwrapped angle [rad]
+ * @param[in] new_angle New angle in [-pi, pi] [rad]
+ * @param
+ * @return New unwrapped angle [rad]
+ */
+template<typename Type>
+Type unwrap_pi(const Type last_angle, const Type new_angle)
+{
+	return unwrap(last_angle, new_angle, Type(-M_PI), Type(M_PI));
+}
+
 template<typename T>
 int sign(T val)
 {
@@ -44,3 +44,4 @@ foreach(test_name ${tests})
 endforeach()

 px4_add_unit_gtest(SRC sparseVector.cpp)
+px4_add_unit_gtest(SRC unwrap.cpp)
@@ -57,27 +57,27 @@ int main()
 	// quaternion ctor: vector to vector
 	// identity test
 	Quatf quat_v(v, v);
-	TEST(isEqual(quat_v.conjugate(v), v));
+	TEST(isEqual(quat_v.rotateVector(v), v));
 	// random test (vector norm can not be preserved with a pure rotation)
 	Vector3f v1(-80.1f, 1.5f, -6.89f);
 	quat_v = Quatf(v1, v);
-	TEST(isEqual(quat_v.conjugate(v1).normalized() * v.norm(), v));
+	TEST(isEqual(quat_v.rotateVector(v1).normalized() * v.norm(), v));
 	// special 180 degree case 1
 	v1 = Vector3f(0.f, 1.f, 1.f);
 	quat_v = Quatf(v1, -v1);
-	TEST(isEqual(quat_v.conjugate(v1), -v1));
+	TEST(isEqual(quat_v.rotateVector(v1), -v1));
 	// special 180 degree case 2
 	v1 = Vector3f(1.f, 2.f, 0.f);
 	quat_v = Quatf(v1, -v1);
-	TEST(isEqual(quat_v.conjugate(v1), -v1));
+	TEST(isEqual(quat_v.rotateVector(v1), -v1));
 	// special 180 degree case 3
 	v1 = Vector3f(0.f, 0.f, 1.f);
 	quat_v = Quatf(v1, -v1);
-	TEST(isEqual(quat_v.conjugate(v1), -v1));
+	TEST(isEqual(quat_v.rotateVector(v1), -v1));
 	// special 180 degree case 4
 	v1 = Vector3f(1.f, 1.f, 1.f);
 	quat_v = Quatf(v1, -v1);
-	TEST(isEqual(quat_v.conjugate(v1), -v1));
+	TEST(isEqual(quat_v.rotateVector(v1), -v1));

 	// quat normalization
 	q.normalize();
@@ -447,8 +447,8 @@ int main()

 	// conjugate
 	v = Vector3f(1.5f, 2.2f, 3.2f);
-	TEST(isEqual(q.conjugate_inversed(v1), Dcmf(q).T()*v1));
-	TEST(isEqual(q.conjugate(v1), Dcmf(q)*v1));
+	TEST(isEqual(q.rotateVectorInverse(v1), Dcmf(q).T()*v1));
+	TEST(isEqual(q.rotateVector(v1), Dcmf(q)*v1));

 	AxisAnglef aa_q_init(q);
 	TEST(isEqual(aa_q_init, AxisAnglef(1.0f, 2.0f, 3.0f)));
@@ -28,7 +28,7 @@ Vector<Scalar, 3> positionError(const Vector<Scalar, 3> &positionState,
 				Scalar dt
 			       )
 {
-	return positionMeasurement - (positionState +  bodyOrientation.conjugate(velocityStateBody) * dt);
+	return positionMeasurement - (positionState +  bodyOrientation.rotateVector(velocityStateBody) * dt);
 }

 int main()
@@ -0,0 +1,62 @@
+#include <matrix/math.hpp>
+#include <gtest/gtest.h>
+
+using namespace matrix;
+
+TEST(Unwrap, UnwrapFloats)
+{
+	const float M_TWO_PI_F = float(M_PI * 2);
+
+	float unwrapped_angles[6] = {0.0, 0.25, 0.5, 0.75, 1.0, 1.25};
+	float wrapped_angles[6] = {0.0, 0.25, 0.5, -0.25, 0.0, 0.25};
+
+	for (int i = 0; i < 6; i++) {
+		unwrapped_angles[i] *= M_TWO_PI_F;
+		wrapped_angles[i] *= M_TWO_PI_F;
+	}
+
+	// positive unwrapping
+	float last_angle = wrapped_angles[0];
+
+	for (int i = 1; i < 6; i++) {
+		last_angle = unwrap_pi(last_angle, wrapped_angles[i]);
+		EXPECT_FLOAT_EQ(last_angle, unwrapped_angles[i]);
+	}
+
+	// negative unwrapping
+	last_angle = -wrapped_angles[0];
+
+	for (int i = 1; i < 6; i++) {
+		last_angle = unwrap_pi(last_angle, -wrapped_angles[i]);
+		EXPECT_FLOAT_EQ(last_angle, -unwrapped_angles[i]);
+	}
+}
+
+TEST(Unwrap, UnwrapDoubles)
+{
+	const double M_TWO_PI = M_PI * 2;
+
+	double unwrapped_angles[6] = {0.0, 0.25, 0.5, 0.75, 1.0, 1.25};
+	double wrapped_angles[6] = {0.0, 0.25, 0.5, -0.25, 0.0, 0.25};
+
+	for (int i = 0; i < 6; i++) {
+		unwrapped_angles[i] *= M_TWO_PI;
+		wrapped_angles[i] *= M_TWO_PI;
+	}
+
+	// positive unwrapping
+	double last_angle = wrapped_angles[0];
+
+	for (int i = 1; i < 6; i++) {
+		last_angle = unwrap_pi(last_angle, wrapped_angles[i]);
+		EXPECT_DOUBLE_EQ(last_angle, unwrapped_angles[i]);
+	}
+
+	// negative unwrapping
+	last_angle = -wrapped_angles[0];
+
+	for (int i = 1; i < 6; i++) {
+		last_angle = unwrap_pi(last_angle, -wrapped_angles[i]);
+		EXPECT_DOUBLE_EQ(last_angle, -unwrapped_angles[i]);
+	}
+}
@@ -140,12 +140,28 @@ private:

 void FunctionMotors::updateValues(uint32_t reversible, float thrust_factor, float *values, int num_values)
 {
-	if (thrust_factor > FLT_EPSILON) {
+	if (thrust_factor > 0.f && thrust_factor <= 1.f) {
+		// thrust factor
+		//  rel_thrust = factor * x^2 + (1-factor) * x,
+		const float a = thrust_factor;
+		const float b = (1.f - thrust_factor);
+
+		// don't recompute for all values (ax^2+bx+c=0)
+		const float tmp1 = b / (2.f * a);
+		const float tmp2 = b * b / (4.f * a * a);
+
 		for (int i = 0; i < num_values; ++i) {
 			float control = values[i];
-			control = matrix::sign(control) * (-(1.0f - thrust_factor) / (2.0f * thrust_factor) + sqrtf((1.0f - thrust_factor) *
-							   (1.0f - thrust_factor) / (4.0f * thrust_factor * thrust_factor) + (fabsf(control) / thrust_factor)));
-			values[i] = control;
+
+			if (control > 0.f) {
+				values[i] = -tmp1 + sqrtf(tmp2 + (control / a));
+
+			} else if (control < -0.f) {
+				values[i] =  tmp1 - sqrtf(tmp2 - (control / a));
+
+			} else {
+				values[i] = 0.f;
+			}
 		}
 	}

@@ -159,9 +159,9 @@ void MixingOutput::printStatus() const

 	if (_use_dynamic_mixing) {
 		for (unsigned i = 0; i < _max_num_outputs; i++) {
-			PX4_INFO_RAW("Channel %i: func: %i, value: %i, failsafe: %d, disarmed: %d, min: %d, max: %d\n", i,
+			PX4_INFO_RAW("Channel %i: func: %3i, value: %i, failsafe: %d, disarmed: %d, min: %d, max: %d\n", i,
 				     (int)_function_assignment[i], _current_output_value[i],
-				     _failsafe_value[i], _disarmed_value[i], _min_value[i], _max_value[i]);
+				     actualFailsafeValue(i), _disarmed_value[i], _min_value[i], _max_value[i]);
 		}

 	} else {
@@ -1103,10 +1103,10 @@ MixingOutput::updateLatencyPerfCounter(const actuator_outputs_s &actuator_output
 }

 uint16_t
-MixingOutput::actualFailsafeValue(int index)
+MixingOutput::actualFailsafeValue(int index) const
 {
 	if (!_use_dynamic_mixing) {
-		return failsafeValue(index);
+		return _failsafe_value[index];
 	}

 	uint16_t value = 0;
@@ -194,7 +194,7 @@ public:
 	/**
 	 * Returns the actual failsafe value taking into account the assigned function
 	 */
-	uint16_t actualFailsafeValue(int index);
+	uint16_t actualFailsafeValue(int index) const;

 	/**
 	 * Get the motor index that maps from PX4 convention to the configured one
@@ -207,16 +207,6 @@ bool Accelerometer::ParametersLoad()
 			_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
 		}

-		// CAL_ACCx_TEMP
-		float cal_temp = GetCalibrationParamFloat(SensorString(), "TEMP", _calibration_index);
-
-		if (cal_temp > TEMPERATURE_INVALID) {
-			set_temperature(cal_temp);
-
-		} else {
-			set_temperature(NAN);
-		}
-
 		// CAL_ACCx_OFF{X,Y,Z}
 		set_offset(GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index));

@@ -243,7 +233,6 @@ void Accelerometer::Reset()
 	_scale = Vector3f{1.f, 1.f, 1.f};

 	_thermal_offset.zero();
-	_temperature = NAN;

 	_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;

@@ -285,13 +274,6 @@ bool Accelerometer::ParametersSave(int desired_calibration_index, bool force)
 			success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1); // internal
 		}

-		if (PX4_ISFINITE(_temperature)) {
-			success &= SetCalibrationParam(SensorString(), "TEMP", _calibration_index, _temperature);
-
-		} else {
-			success &= SetCalibrationParam(SensorString(), "TEMP", _calibration_index, TEMPERATURE_INVALID);
-		}
-
 		return success;
 	}

@@ -302,20 +284,18 @@ void Accelerometer::PrintStatus()
 {
 	if (external()) {
 		PX4_INFO_RAW("%s %" PRIu32
-			     " EN: %d, offset: [%05.3f %05.3f %05.3f], scale: [%05.3f %05.3f %05.3f], %.1f degC, Ext ROT: %d\n",
+			     " EN: %d, offset: [%05.3f %05.3f %05.3f], scale: [%05.3f %05.3f %05.3f], Ext ROT: %d\n",
 			     SensorString(), device_id(), enabled(),
 			     (double)_offset(0), (double)_offset(1), (double)_offset(2),
 			     (double)_scale(0), (double)_scale(1), (double)_scale(2),
-			     (double)_temperature,
 			     rotation_enum());

 	} else {
 		PX4_INFO_RAW("%s %" PRIu32
-			     " EN: %d, offset: [%05.3f %05.3f %05.3f], scale: [%05.3f %05.3f %05.3f], %.1f degC, Internal\n",
+			     " EN: %d, offset: [%05.3f %05.3f %05.3f], scale: [%05.3f %05.3f %05.3f], Internal\n",
 			     SensorString(), device_id(), enabled(),
 			     (double)_offset(0), (double)_offset(1), (double)_offset(2),
-			     (double)_scale(0), (double)_scale(1), (double)_scale(2),
-			     (double)_temperature);
+			     (double)_scale(0), (double)_scale(1), (double)_scale(2));
 	}

 	if (_thermal_offset.norm() > 0.f) {
@@ -64,7 +64,6 @@ public:
 	bool set_offset(const matrix::Vector3f &offset);
 	bool set_scale(const matrix::Vector3f &scale);
 	void set_rotation(Rotation rotation);
-	void set_temperature(float temperature) { _temperature = temperature; };

 	bool calibrated() const { return (_device_id != 0) && (_calibration_index >= 0); }
 	uint8_t calibration_count() const { return _calibration_count; }
@@ -100,8 +99,6 @@ public:
 	void SensorCorrectionsUpdate(bool force = false);

 private:
-	static constexpr float TEMPERATURE_INVALID = -1000.f;
-
 	uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};

 	Rotation _rotation_enum{ROTATION_NONE};
@@ -110,7 +107,6 @@ private:
 	matrix::Vector3f _offset;
 	matrix::Vector3f _scale;
 	matrix::Vector3f _thermal_offset;
-	float _temperature{NAN};

 	int8_t _calibration_index{-1};
 	uint32_t _device_id{0};
@@ -192,16 +192,6 @@ bool Gyroscope::ParametersLoad()
 			_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
 		}

-		// CAL_GYROx_TEMP
-		float cal_temp = GetCalibrationParamFloat(SensorString(), "TEMP", _calibration_index);
-
-		if (cal_temp > TEMPERATURE_INVALID) {
-			set_temperature(cal_temp);
-
-		} else {
-			set_temperature(NAN);
-		}
-
 		// CAL_GYROx_OFF{X,Y,Z}
 		set_offset(GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index));

@@ -224,7 +214,6 @@ void Gyroscope::Reset()
 	_offset.zero();

 	_thermal_offset.zero();
-	_temperature = NAN;

 	_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;

@@ -265,13 +254,6 @@ bool Gyroscope::ParametersSave(int desired_calibration_index, bool force)
 			success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1); // internal
 		}

-		if (PX4_ISFINITE(_temperature)) {
-			success &= SetCalibrationParam(SensorString(), "TEMP", _calibration_index, _temperature);
-
-		} else {
-			success &= SetCalibrationParam(SensorString(), "TEMP", _calibration_index, TEMPERATURE_INVALID);
-		}
-
 		return success;
 	}

@@ -282,17 +264,15 @@ void Gyroscope::PrintStatus()
 {
 	if (external()) {
 		PX4_INFO_RAW("%s %" PRIu32
-			     " EN: %d, offset: [%05.3f %05.3f %05.3f], %.1f degC, Ext ROT: %d\n",
+			     " EN: %d, offset: [%05.3f %05.3f %05.3f], Ext ROT: %d\n",
 			     SensorString(), device_id(), enabled(),
 			     (double)_offset(0), (double)_offset(1), (double)_offset(2),
-			     (double)_temperature,
 			     rotation_enum());

 	} else {
-		PX4_INFO_RAW("%s %" PRIu32 " EN: %d, offset: [%05.3f %05.3f %05.3f], %.1f degC, Internal\n",
+		PX4_INFO_RAW("%s %" PRIu32 " EN: %d, offset: [%05.3f %05.3f %05.3f], Internal\n",
 			     SensorString(), device_id(), enabled(),
-			     (double)_offset(0), (double)_offset(1), (double)_offset(2),
-			     (double)_temperature);
+			     (double)_offset(0), (double)_offset(1), (double)_offset(2));
 	}

 	if (_thermal_offset.norm() > 0.f) {
@@ -63,7 +63,6 @@ public:
 	void set_device_id(uint32_t device_id);
 	bool set_offset(const matrix::Vector3f &offset);
 	void set_rotation(Rotation rotation);
-	void set_temperature(float temperature) { _temperature = temperature; };

 	bool calibrated() const { return (_device_id != 0) && (_calibration_index >= 0); }
 	uint8_t calibration_count() const { return _calibration_count; }
@@ -104,8 +103,6 @@ public:
 	void SensorCorrectionsUpdate(bool force = false);

 private:
-	static constexpr float TEMPERATURE_INVALID = -1000.f;
-
 	uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};

 	Rotation _rotation_enum{ROTATION_NONE};
@@ -113,7 +110,6 @@ private:
 	matrix::Dcmf _rotation;
 	matrix::Vector3f _offset;
 	matrix::Vector3f _thermal_offset;
-	float _temperature{NAN};

 	int8_t _calibration_index{-1};
 	uint32_t _device_id{0};
@@ -192,16 +192,6 @@ bool Magnetometer::ParametersLoad()
 			_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
 		}

-		// CAL_MAGx_TEMP
-		float cal_temp = GetCalibrationParamFloat(SensorString(), "TEMP", _calibration_index);
-
-		if (cal_temp > TEMPERATURE_INVALID) {
-			set_temperature(cal_temp);
-
-		} else {
-			set_temperature(NAN);
-		}
-
 		// CAL_MAGx_OFF{X,Y,Z}
 		set_offset(GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index));

@@ -236,8 +226,6 @@ void Magnetometer::Reset()
 	_power_compensation.zero();
 	_power = 0.f;

-	_temperature = NAN;
-
 	_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;

 	_calibration_index = -1;
@@ -285,13 +273,6 @@ bool Magnetometer::ParametersSave(int desired_calibration_index, bool force)
 			success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1); // internal
 		}

-		if (PX4_ISFINITE(_temperature)) {
-			success &= SetCalibrationParam(SensorString(), "TEMP", _calibration_index, _temperature);
-
-		} else {
-			success &= SetCalibrationParam(SensorString(), "TEMP", _calibration_index, TEMPERATURE_INVALID);
-		}
-
 		return success;
 	}

@@ -302,20 +283,18 @@ void Magnetometer::PrintStatus()
 {
 	if (external()) {
 		PX4_INFO_RAW("%s %" PRIu32
-			     " EN: %d, offset: [%05.3f %05.3f %05.3f], scale: [%05.3f %05.3f %05.3f], %.1f degC, Ext ROT: %d\n",
+			     " EN: %d, offset: [%05.3f %05.3f %05.3f], scale: [%05.3f %05.3f %05.3f], Ext ROT: %d\n",
 			     SensorString(), device_id(), enabled(),
 			     (double)_offset(0), (double)_offset(1), (double)_offset(2),
 			     (double)_scale(0, 0), (double)_scale(1, 1), (double)_scale(2, 2),
-			     (double)_temperature,
 			     rotation_enum());

 	} else {
 		PX4_INFO_RAW("%s %" PRIu32
-			     " EN: %d, offset: [%05.3f %05.3f %05.3f], scale: [%05.3f %05.3f %05.3f], %.1f degC, Internal\n",
+			     " EN: %d, offset: [%05.3f %05.3f %05.3f], scale: [%05.3f %05.3f %05.3f], Internal\n",
 			     SensorString(), device_id(), enabled(),
 			     (double)_offset(0), (double)_offset(1), (double)_offset(2),
-			     (double)_scale(0, 0), (double)_scale(1, 1), (double)_scale(2, 2),
-			     (double)_temperature);
+			     (double)_scale(0, 0), (double)_scale(1, 1), (double)_scale(2, 2));
 	}

 #if defined(DEBUG_BUILD)
@@ -66,7 +66,6 @@ public:
 	bool set_scale(const matrix::Vector3f &scale);
 	bool set_offdiagonal(const matrix::Vector3f &offdiagonal);
 	void set_rotation(Rotation rotation);
-	void set_temperature(float temperature) { _temperature = temperature; };

 	bool calibrated() const { return (_device_id != 0) && (_calibration_index >= 0); }
 	uint8_t calibration_count() const { return _calibration_count; }
@@ -102,8 +101,6 @@ public:
 	void UpdatePower(float power) { _power = power; }

 private:
-	static constexpr float TEMPERATURE_INVALID = -1000.f;
-
 	Rotation _rotation_enum{ROTATION_NONE};

 	matrix::Dcmf _rotation;
@@ -111,7 +108,6 @@ private:
 	matrix::Matrix3f _scale;
 	matrix::Vector3f _power_compensation;
 	float _power{0.f};
-	float _temperature{NAN};

 	int8_t _calibration_index{-1};
 	uint32_t _device_id{0};
@@ -149,21 +149,6 @@ int32_t GetCalibrationParamInt32(const char *sensor_type, const char *cal_type,
 	return value;
 }

-float GetCalibrationParamFloat(const char *sensor_type, const char *cal_type, uint8_t instance)
-{
-	// eg CAL_MAGn_TEMP
-	char str[20] {};
-	sprintf(str, "CAL_%s%" PRIu8 "_%s", sensor_type, instance, cal_type);
-
-	float value = NAN;
-
-	if (param_get(param_find(str), &value) != 0) {
-		PX4_ERR("failed to get %s", str);
-	}
-
-	return value;
-}
-
 Vector3f GetCalibrationParamsVector3f(const char *sensor_type, const char *cal_type, uint8_t instance)
 {
 	Vector3f values{0.f, 0.f, 0.f};
@@ -71,7 +71,6 @@ int8_t FindAvailableCalibrationIndex(const char *sensor_type, uint32_t device_id
 * @return int32_t The calibration value.
 */
 int32_t GetCalibrationParamInt32(const char *sensor_type, const char *cal_type, uint8_t instance);
-float GetCalibrationParamFloat(const char *sensor_type, const char *cal_type, uint8_t instance);

 /**
 * @brief Set a single calibration paramter.
@@ -47,80 +47,80 @@ static constexpr int LON_DIM = 37;
 // Magnetic declination data in radians * 10^-4
 // Model: WMM-2020,
 // Version: 0.5.1.11,
-// Date: 2022.0082,
+// Date: 2022.1534,
 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 */ {  26000, 24255, 22510, 20764, 19019, 17274, 15528, 13783, 12038, 10292,  8547,  6802,  5057,  3311,  1566,  -179, -1925, -3670, -5415, -7160, -8906,-10651,-12396,-14142,-15887,-17632,-19378,-21123,-22868,-24614,-26359,-28105,-29850, 31236, 29491, 27746, 26000, },
-	/* LAT: -80 */ {  22568, 20435, 18493, 16718, 15076, 13537, 12073, 10661,  9285,  7935,  6603,  5284,  3975,  2670,  1363,    44, -1297, -2668, -4076, -5527, -7020, -8556,-10132,-11750,-13413,-15130,-16915,-18786,-20767,-22884,-25158,-27591,-30160, 30031, 27410, 24901, 22568, },
-	/* LAT: -70 */ {  14972, 13575, 12450, 11491, 10624,  9794,  8954,  8067,  7116,  6097,  5027,  3933,  2847,  1791,   768,  -242, -1278, -2382, -3580, -4877, -6255, -7683, -9129,-10570,-11995,-13410,-14837,-16321,-17942,-19850,-22361,-26145, 30768, 24168, 19626, 16844, 14972, },
-	/* LAT: -60 */ {   8399,  8158,  7881,  7611,  7361,  7111,  6805,  6375,  5762,  4944,  3945,  2833,  1712,   681,  -204,  -964, -1694, -2514, -3512, -4704, -6030, -7396, -8716, -9926,-10992,-11892,-12609,-13104,-13276,-12827,-10732, -3551,  4841,  7612,  8395,  8528,  8399, },
-	/* LAT: -50 */ {   5468,  5507,  5454,  5368,  5297,  5263,  5230,  5106,  4765,  4105,  3098,  1827,   485,  -700, -1578, -2149, -2549, -2987, -3665, -4668, -5904, -7177, -8318, -9222, -9825,-10069, -9881, -9129, -7625, -5268, -2366,   381,  2490,  3914,  4788,  5263,  5468, },
-	/* LAT: -40 */ {   3942,  4036,  4046,  4005,  3947,  3916,  3923,  3914,  3743,  3213,  2195,   756,  -809, -2129, -3001, -3461, -3651, -3714, -3877, -4432, -5396, -6468, -7355, -7899, -8014, -7645, -6763, -5382, -3669, -1963,  -501,   721,  1763,  2627,  3281,  3711,  3942, },
-	/* LAT: -30 */ {   2973,  3060,  3093,  3082,  3027,  2951,  2892,  2860,  2735,  2262,  1233,  -280, -1889, -3150, -3899, -4252, -4339, -4138, -3706, -3470, -3816, -4557, -5258, -5616, -5499, -4920, -3965, -2758, -1545,  -596,    84,   684,  1307,  1907,  2414,  2775,  2973, },
-	/* LAT: -20 */ {   2330,  2377,  2398,  2404,  2365,  2274,  2168,  2093,  1949,  1458,   409, -1074, -2555, -3624, -4167, -4293, -4098, -3549, -2691, -1885, -1593, -1937, -2586, -3061, -3104, -2751, -2118, -1300,  -520,   -27,   238,   540,   979,  1454,  1874,  2179,  2330, },
-	/* LAT: -10 */ {   1937,  1932,  1914,  1915,  1890,  1809,  1702,  1613,  1430,   883,  -172, -1546, -2823, -3662, -3941, -3724, -3155, -2377, -1531,  -772,  -299,  -322,  -787, -1295, -1513, -1427, -1115,  -615,  -115,   130,   181,   342,   711,  1145,  1535,  1819,  1937, },
-	/* LAT:   0 */ {   1724,  1691,  1639,  1634,  1626,  1561,  1460,  1349,  1097,   479,  -558, -1784, -2843, -3439, -3450, -2959, -2195, -1413,  -758,  -219,   197,   315,    42,  -375,  -633,  -690,  -592,  -329,   -37,    55,    -3,    87,   425,   864,  1280,  1597,  1724, },
-	/* LAT:  10 */ {   1590,  1598,  1559,  1578,  1607,  1562,  1444,  1261,   887,   171,  -848, -1926, -2759, -3108, -2914, -2315, -1540,  -826,  -307,    76,   403,   555,   398,    76,  -160,  -268,  -291,  -208,  -102,  -141,  -278,  -251,    48,   499,   976,  1381,  1590, },
-	/* LAT:  20 */ {   1409,  1558,  1621,  1715,  1804,  1787,  1636,  1337,   794,   -62, -1105, -2060, -2664, -2782, -2462, -1864, -1154,  -502,   -39,   273,   533,   682,   594,   352,   151,    33,   -56,  -119,  -197,  -385,  -620,  -679,  -447,    -2,   536,  1053,  1409, },
-	/* LAT:  30 */ {   1112,  1479,  1740,  1965,  2127,  2139,  1955,  1535,   809,  -220, -1340, -2223, -2646, -2591, -2197, -1620,  -965,  -345,   120,   428,   661,   810,   793,   645,   494,   370,   216,     8,  -273,  -646, -1014, -1174, -1017,  -593,   -18,   591,  1112, },
-	/* LAT:  40 */ {    760,  1347,  1841,  2233,  2483,  2528,  2314,  1783,   873,  -356, -1600, -2474, -2803, -2656, -2212, -1621,  -968,  -337,   175,   543,   815,  1016,  1113,  1104,  1027,   880,   614,   207,  -328,  -933, -1456, -1702, -1588, -1173,  -574,    99,   760, },
-	/* LAT:  50 */ {    475,  1224,  1904,  2461,  2827,  2933,  2704,  2051,   908,  -594, -2024, -2947, -3247, -3058, -2570, -1924, -1213,  -514,   104,   613,  1034,  1391,  1674,  1847,  1868,  1683,  1242,   539,  -351, -1253, -1935, -2226, -2103, -1662, -1026,  -291,   475, },
-	/* LAT:  60 */ {    288,  1142,  1948,  2642,  3145,  3356,  3140,  2326,   805, -1166, -2893, -3880, -4135, -3875, -3301, -2555, -1727,  -884,   -74,   680,  1375,  2008,  2553,  2952,  3116,  2934,  2303,  1194,  -216, -1539, -2413, -2726, -2563, -2067, -1370,  -565,   288, },
-	/* LAT:  70 */ {     69,  1008,  1906,  2705,  3318,  3609,  3351,  2204,   -60, -2799, -4760, -5583, -5587, -5099, -4324, -3386, -2358, -1289,  -211,   851,  1879,  2847,  3718,  4428,  4880,  4919,  4321,  2862,   678, -1431, -2735, -3183, -3024, -2490, -1736,  -862,    69, },
-	/* LAT:  80 */ {   -584,   340,  1195,  1888,  2272,  2078,   846, -1840, -5119, -7245, -7976, -7827, -7184, -6256, -5157, -3952, -2681, -1371,   -43,  1288,  2604,  3888,  5116,  6255,  7253,  8014,  8361,  7928,  6040,  2336, -1234, -2965, -3346, -3027, -2355, -1507,  -584, },
-	/* LAT:  90 */ { -30042,-28297,-26551,-24806,-23060,-21315,-19570,-17824,-16079,-14334,-12588,-10843, -9098, -7353, -5608, -3863, -2117,  -372,  1373,  3118,  4863,  6609,  8354, 10099, 11844, 13590, 15335, 17081, 18826, 20571, 22317, 24062, 25808, 27553, 29299, 31044,-30042, },
+	/* LAT: -90 */ {  25997, 24251, 22506, 20761, 19015, 17270, 15525, 13779, 12034, 10289,  8543,  6798,  5053,  3308,  1562,  -183, -1928, -3674, -5419, -7164, -8909,-10655,-12400,-14145,-15891,-17636,-19381,-21127,-22872,-24617,-26363,-28108,-29854, 31233, 29487, 27742, 25997, },
+	/* LAT: -80 */ {  22564, 20431, 18490, 16715, 15073, 13535, 12070, 10659,  9283,  7933,  6601,  5282,  3973,  2668,  1361,    42, -1299, -2670, -4079, -5530, -7024, -8559,-10136,-11754,-13418,-15135,-16920,-18791,-20773,-22890,-25164,-27598,-30166, 30025, 27405, 24897, 22564, },
+	/* LAT: -70 */ {  14973, 13576, 12451, 11491, 10623,  9793,  8953,  8066,  7114,  6095,  5025,  3932,  2846,  1790,   767,  -243, -1279, -2383, -3582, -4880, -6258, -7687, -9133,-10574,-12000,-13415,-14843,-16327,-17948,-19857,-22371,-26158, 30755, 24163, 19625, 16845, 14973, },
+	/* LAT: -60 */ {   8403,  8161,  7884,  7613,  7362,  7111,  6805,  6374,  5761,  4943,  3943,  2832,  1710,   680,  -204,  -964, -1693, -2513, -3513, -4706, -6033, -7400, -8720, -9931,-10996,-11896,-12613,-13108,-13280,-12831,-10735, -3540,  4856,  7622,  8402,  8534,  8403, },
+	/* LAT: -50 */ {   5471,  5510,  5456,  5369,  5298,  5263,  5230,  5105,  4764,  4104,  3096,  1825,   483,  -701, -1577, -2147, -2546, -2984, -3664, -4670, -5908, -7182, -8323, -9227, -9828,-10072, -9882, -9129, -7624, -5265, -2363,   384,  2493,  3918,  4792,  5266,  5471, },
+	/* LAT: -40 */ {   3944,  4038,  4048,  4006,  3948,  3916,  3923,  3913,  3742,  3211,  2192,   753,  -812, -2131, -3001, -3459, -3647, -3709, -3874, -4432, -5400, -6473, -7360, -7903, -8016, -7645, -6761, -5379, -3666, -1962,  -500,   721,  1764,  2628,  3283,  3713,  3944, },
+	/* LAT: -30 */ {   2975,  3062,  3094,  3083,  3027,  2950,  2892,  2859,  2734,  2259,  1230,  -286, -1894, -3153, -3900, -4251, -4336, -4132, -3700, -3468, -3818, -4562, -5262, -5618, -5499, -4918, -3962, -2755, -1543,  -596,    83,   684,  1307,  1908,  2416,  2776,  2975, },
+	/* LAT: -20 */ {   2333,  2379,  2399,  2405,  2365,  2273,  2167,  2092,  1947,  1455,   405, -1080, -2560, -3628, -4168, -4291, -4094, -3543, -2684, -1880, -1593, -1940, -2589, -3062, -3104, -2749, -2116, -1298,  -519,   -28,   236,   539,   978,  1454,  1875,  2180,  2333, },
+	/* LAT: -10 */ {   1939,  1934,  1915,  1915,  1890,  1808,  1700,  1611,  1427,   880,  -177, -1552, -2828, -3664, -3940, -3720, -3150, -2372, -1525,  -767,  -297,  -322,  -789, -1296, -1513, -1426, -1113,  -614,  -114,   129,   179,   340,   709,  1144,  1535,  1821,  1939, },
+	/* LAT:   0 */ {   1726,  1693,  1640,  1634,  1626,  1560,  1458,  1347,  1094,   475,  -562, -1788, -2846, -3440, -3448, -2955, -2190, -1408,  -754,  -215,   200,   316,    42,  -375,  -632,  -690,  -591,  -329,   -37,    54,    -5,    84,   424,   863,  1280,  1598,  1726, },
+	/* LAT:  10 */ {   1591,  1600,  1560,  1579,  1607,  1560,  1442,  1258,   884,   167,  -852, -1929, -2760, -3107, -2911, -2310, -1535,  -822,  -304,    79,   405,   557,   399,    77,  -159,  -268,  -290,  -208,  -103,  -143,  -281,  -254,    46,   498,   977,  1382,  1591, },
+	/* LAT:  20 */ {   1409,  1559,  1621,  1715,  1803,  1785,  1634,  1334,   791,   -65, -1107, -2062, -2664, -2780, -2458, -1860, -1149,  -498,   -35,   276,   536,   684,   595,   352,   152,    34,   -56,  -119,  -198,  -387,  -623,  -681,  -449,    -3,   536,  1053,  1409, },
+	/* LAT:  30 */ {   1112,  1479,  1739,  1964,  2126,  2138,  1953,  1533,   807,  -222, -1342, -2223, -2644, -2588, -2193, -1615,  -960,  -341,   123,   431,   664,   812,   794,   646,   496,   371,   216,     7,  -274,  -648, -1016, -1177, -1019,  -594,   -19,   590,  1112, },
+	/* LAT:  40 */ {    758,  1345,  1839,  2232,  2481,  2526,  2312,  1781,   870,  -358, -1600, -2473, -2800, -2652, -2208, -1617,  -963,  -333,   179,   546,   818,  1019,  1115,  1105,  1029,   881,   614,   206,  -330,  -936, -1458, -1704, -1589, -1174,  -575,    98,   758, },
+	/* LAT:  50 */ {    472,  1220,  1900,  2457,  2824,  2930,  2702,  2048,   907,  -593, -2022, -2943, -3242, -3053, -2564, -1918, -1208,  -509,   109,   617,  1037,  1395,  1677,  1849,  1869,  1684,  1242,   537,  -354, -1257, -1938, -2227, -2105, -1664, -1028,  -294,   472, },
+	/* LAT:  60 */ {    282,  1136,  1942,  2636,  3140,  3352,  3137,  2324,   805, -1162, -2886, -3872, -4126, -3867, -3294, -2547, -1720,  -877,   -68,   686,  1380,  2012,  2557,  2956,  3118,  2935,  2302,  1190,  -222, -1543, -2416, -2729, -2564, -2069, -1374,  -570,   282, },
+	/* LAT:  70 */ {     59,   996,  1895,  2694,  3307,  3600,  3344,  2202,   -53, -2783, -4742, -5567, -5573, -5086, -4313, -3376, -2349, -1280,  -203,   859,  1886,  2854,  3724,  4434,  4885,  4922,  4320,  2857,   670, -1439, -2742, -3189, -3030, -2496, -1744,  -871,    59, },
+	/* LAT:  80 */ {   -610,   314,  1168,  1861,  2245,  2056,   836, -1821, -5074, -7200, -7938, -7797, -7159, -6235, -5139, -3935, -2666, -1357,   -30,  1300,  2616,  3899,  5128,  6267,  7264,  8025,  8372,  7936,  6038,  2313, -1268, -2997, -3374, -3053, -2381, -1532,  -610, },
+	/* LAT:  90 */ { -29988,-28242,-26497,-24751,-23006,-21260,-19515,-17770,-16025,-14279,-12534,-10789, -9044, -7298, -5553, -3808, -2063,  -318,  1427,  3173,  4918,  6663,  8408, 10154, 11899, 13644, 15390, 17135, 18880, 20626, 22371, 24117, 25862, 27608, 29353, 31099,-29988, },
 };

 // Magnetic inclination data in radians * 10^-4
 // Model: WMM-2020,
 // Version: 0.5.1.11,
-// Date: 2022.0082,
+// Date: 2022.1534,
 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 */ { -12575,-12575,-12575,-12575,-12575,-12575,-12575,-12575,-12575,-12575,-12575,-12575,-12575,-12575,-12575,-12575,-12575,-12575,-12575,-12575,-12575,-12575,-12575,-12575,-12575,-12575,-12575,-12575,-12575,-12575,-12575,-12575,-12575,-12575,-12575,-12575,-12575, },
-	/* LAT: -80 */ { -13661,-13527,-13366,-13186,-12992,-12790,-12586,-12385,-12194,-12018,-11861,-11726,-11616,-11530,-11466,-11425,-11405,-11408,-11435,-11489,-11573,-11687,-11831,-12005,-12203,-12420,-12649,-12882,-13109,-13322,-13509,-13659,-13764,-13816,-13813,-13759,-13661, },
-	/* LAT: -70 */ { -14110,-13791,-13472,-13149,-12817,-12472,-12117,-11759,-11415,-11106,-10852,-10668,-10555,-10502,-10489,-10493,-10501,-10511,-10535,-10591,-10699,-10873,-11118,-11432,-11803,-12220,-12668,-13132,-13598,-14051,-14469,-14815,-15006,-14954,-14723,-14426,-14110, },
-	/* LAT: -60 */ { -13521,-13168,-12830,-12497,-12154,-11781,-11366,-10911,-10443,-10011, -9680, -9502, -9497, -9633, -9837,-10031,-10159,-10206,-10195,-10182,-10228,-10385,-10672,-11078,-11575,-12130,-12715,-13311,-13900,-14461,-14958,-15248,-15075,-14692,-14286,-13893,-13521, },
-	/* LAT: -50 */ { -12497,-12156,-11825,-11503,-11180,-10833,-10434, -9962, -9432, -8909, -8516, -8387, -8584, -9042, -9607,-10121,-10485,-10653,-10628,-10479,-10337,-10339,-10556,-10968,-11508,-12099,-12686,-13225,-13673,-13971,-14080,-14007,-13804,-13521,-13193,-12846,-12497, },
-	/* LAT: -40 */ { -11240,-10893,-10547,-10202, -9862, -9524, -9162, -8736, -8215, -7648, -7218, -7170, -7623, -8444, -9366,-10198,-10861,-11301,-11449,-11289,-10949,-10674,-10661,-10937,-11396,-11901,-12348,-12672,-12833,-12839,-12749,-12611,-12431,-12199,-11913,-11585,-11240, },
-	/* LAT: -30 */ {  -9601, -9225, -8848, -8458, -8066, -7690, -7332, -6940, -6426, -5814, -5357, -5431, -6190, -7388, -8640, -9738,-10656,-11372,-11779,-11779,-11414,-10906,-10568,-10563,-10811,-11132,-11391,-11507,-11446,-11270,-11095,-10959,-10811,-10604,-10322, -9978, -9601, },
-	/* LAT: -20 */ {  -7370, -6934, -6520, -6093, -5649, -5218, -4825, -4410, -3842, -3155, -2702, -2950, -4049, -5655, -7285, -8660, -9732,-10514,-10960,-11003,-10646,-10036, -9476, -9227, -9273, -9435, -9577, -9597, -9426, -9154, -8958, -8869, -8762, -8553, -8237, -7826, -7370, },
-	/* LAT: -10 */ {  -4414, -3883, -3434, -2999, -2542, -2091, -1675, -1217,  -587,   119,   488,    73, -1243, -3154, -5131, -6748, -7854, -8495, -8767, -8710, -8307, -7627, -6963, -6614, -6573, -6665, -6784, -6809, -6622, -6326, -6169, -6174, -6130, -5912, -5528, -5003, -4414, },
-	/* LAT:   0 */ {   -905,  -288,   172,   572,   990,  1407,  1799,  2247,  2836,  3418,  3627,  3141,  1857,   -46, -2091, -3750, -4770, -5207, -5271, -5104, -4669, -3958, -3253, -2879, -2818, -2888, -3014, -3089, -2959, -2723, -2666, -2804, -2861, -2672, -2248, -1621,  -905, },
-	/* LAT:  10 */ {   2563,  3183,  3613,  3951,  4306,  4673,  5027,  5418,  5875,  6249,  6290,  5809,  4736,  3176,  1485,   103,  -711,  -966,  -879,  -647,  -239,   395,  1028,  1367,  1429,  1383,  1281,  1190,  1233,  1338,  1265,  1004,   817,   892,  1244,  1844,  2563, },
-	/* LAT:  20 */ {   5417,  5941,  6317,  6611,  6924,  7268,  7612,  7963,  8301,  8504,  8421,  7963,  7133,  6041,  4915,  4002,  3467,  3342,  3488,  3730,  4058,  4521,  4980,  5236,  5292,  5272,  5223,  5166,  5156,  5139,  4968,  4639,  4346,  4258,  4430,  4850,  5417, },
-	/* LAT:  30 */ {   7569,  7940,  8256,  8537,  8846,  9194,  9553,  9896, 10176, 10293, 10151,  9733,  9106,  8396,  7734,  7221,  6926,  6880,  7020,  7229,  7474,  7772,  8060,  8234,  8290,  8301,  8302,  8293,  8272,  8191,  7972,  7621,  7269,  7050,  7033,  7227,  7569, },
-	/* LAT:  40 */ {   9266,  9486,  9741, 10027, 10353, 10715, 11084, 11425, 11679, 11763, 11616, 11257, 10785, 10311,  9914,  9629,  9478,  9472,  9580,  9740,  9914, 10097, 10269, 10394, 10471, 10530, 10584, 10617, 10602, 10493, 10253,  9905,  9539,  9255,  9112,  9124,  9266, },
-	/* LAT:  50 */ {  10801, 10923, 11125, 11395, 11718, 12073, 12429, 12746, 12970, 13030, 12892, 12595, 12230, 11884, 11608, 11420, 11325, 11320, 11385, 11487, 11600, 11715, 11831, 11946, 12064, 12185, 12298, 12371, 12363, 12240, 11995, 11668, 11329, 11044, 10855, 10775, 10801, },
-	/* LAT:  60 */ {  12319, 12392, 12544, 12763, 13034, 13335, 13638, 13905, 14082, 14110, 13974, 13724, 13435, 13163, 12941, 12784, 12692, 12660, 12675, 12721, 12789, 12873, 12978, 13110, 13268, 13443, 13608, 13718, 13725, 13607, 13383, 13105, 12826, 12588, 12418, 12327, 12319, },
-	/* LAT:  70 */ {  13758, 13802, 13898, 14041, 14222, 14426, 14635, 14817, 14922, 14901, 14762, 14560, 14341, 14137, 13963, 13828, 13733, 13678, 13658, 13670, 13712, 13783, 13886, 14022, 14188, 14375, 14559, 14700, 14748, 14677, 14516, 14318, 14123, 13958, 13838, 13770, 13758, },
-	/* LAT:  80 */ {  15000, 15013, 15051, 15112, 15189, 15275, 15354, 15399, 15380, 15299, 15185, 15060, 14937, 14822, 14723, 14640, 14578, 14537, 14518, 14522, 14548, 14597, 14668, 14760, 14872, 14998, 15133, 15265, 15372, 15417, 15380, 15295, 15200, 15116, 15052, 15013, 15000, },
+	/* LAT: -90 */ { -12574,-12574,-12574,-12574,-12574,-12574,-12574,-12574,-12574,-12574,-12574,-12574,-12574,-12574,-12574,-12574,-12574,-12574,-12574,-12574,-12574,-12574,-12574,-12574,-12574,-12574,-12574,-12574,-12574,-12574,-12574,-12574,-12574,-12574,-12574,-12574,-12574, },
+	/* LAT: -80 */ { -13659,-13526,-13365,-13185,-12991,-12789,-12585,-12384,-12193,-12017,-11860,-11726,-11615,-11529,-11466,-11424,-11404,-11407,-11434,-11488,-11572,-11686,-11831,-12004,-12202,-12419,-12648,-12881,-13109,-13322,-13508,-13659,-13763,-13815,-13812,-13758,-13659, },
+	/* LAT: -70 */ { -14108,-13790,-13471,-13148,-12815,-12471,-12116,-11758,-11414,-11105,-10852,-10668,-10555,-10502,-10489,-10493,-10500,-10510,-10534,-10590,-10698,-10872,-11117,-11431,-11803,-12221,-12668,-13132,-13598,-14051,-14469,-14815,-15005,-14952,-14722,-14424,-14108, },
+	/* LAT: -60 */ { -13520,-13167,-12829,-12496,-12153,-11781,-11365,-10910,-10442,-10011, -9680, -9502, -9498, -9634, -9838,-10032,-10159,-10205,-10194,-10180,-10227,-10384,-10671,-11078,-11575,-12130,-12716,-13312,-13901,-14462,-14959,-15249,-15075,-14691,-14286,-13893,-13520, },
+	/* LAT: -50 */ { -12496,-12155,-11825,-11503,-11179,-10833,-10433, -9962, -9431, -8909, -8517, -8388, -8586, -9045, -9610,-10123,-10486,-10652,-10626,-10477,-10335,-10338,-10555,-10969,-11509,-12100,-12687,-13226,-13673,-13972,-14080,-14008,-13804,-13521,-13193,-12846,-12496, },
+	/* LAT: -40 */ { -11240,-10892,-10546,-10201, -9862, -9523, -9162, -8736, -8214, -7648, -7219, -7173, -7628, -8449, -9371,-10202,-10864,-11303,-11449,-11286,-10946,-10672,-10660,-10938,-11398,-11902,-12349,-12673,-12833,-12839,-12749,-12611,-12431,-12199,-11913,-11586,-11240, },
+	/* LAT: -30 */ {  -9602, -9225, -8847, -8457, -8065, -7689, -7331, -6939, -6426, -5814, -5359, -5435, -6197, -7396, -8647, -9745,-10662,-11376,-11781,-11778,-11411,-10904,-10566,-10563,-10812,-11133,-11391,-11506,-11445,-11269,-11094,-10959,-10812,-10604,-10323, -9978, -9602, },
+	/* LAT: -20 */ {  -7371, -6933, -6518, -6091, -5647, -5216, -4824, -4409, -3842, -3156, -2703, -2956, -4057, -5666, -7295, -8668, -9738,-10518,-10962,-11003,-10644,-10033, -9473, -9226, -9273, -9435, -9577, -9596, -9425, -9152, -8957, -8869, -8763, -8555, -8238, -7827, -7371, },
+	/* LAT: -10 */ {  -4414, -3882, -3432, -2997, -2539, -2089, -1673, -1216,  -587,   118,   485,    67, -1253, -3166, -5143, -6757, -7860, -8499, -8769, -8710, -8305, -7624, -6960, -6612, -6571, -6664, -6782, -6807, -6620, -6324, -6167, -6174, -6132, -5914, -5530, -5004, -4414, },
+	/* LAT:   0 */ {   -905,  -287,   174,   575,   993,  1410,  1802,  2248,  2836,  3417,  3623,  3135,  1847,   -58, -2103, -3759, -4775, -5210, -5272, -5104, -4667, -3955, -3250, -2877, -2816, -2886, -3012, -3085, -2956, -2720, -2664, -2804, -2863, -2675, -2251, -1623,  -905, },
+	/* LAT:  10 */ {   2562,  3184,  3615,  3954,  4308,  4676,  5029,  5419,  5874,  6247,  6287,  5804,  4727,  3166,  1475,    95,  -716,  -968,  -880,  -646,  -237,   398,  1031,  1369,  1432,  1386,  1285,  1194,  1237,  1341,  1267,  1004,   815,   889,  1241,  1842,  2562, },
+	/* LAT:  20 */ {   5417,  5942,  6319,  6613,  6926,  7269,  7614,  7964,  8300,  8502,  8418,  7959,  7127,  6034,  4908,  3996,  3464,  3341,  3488,  3730,  4059,  4523,  4983,  5238,  5294,  5275,  5226,  5169,  5158,  5141,  4969,  4639,  4344,  4256,  4428,  4849,  5417, },
+	/* LAT:  30 */ {   7569,  7940,  8256,  8538,  8847,  9195,  9553,  9896, 10175, 10291, 10148,  9729,  9102,  8391,  7730,  7217,  6924,  6879,  7020,  7230,  7475,  7774,  8062,  8235,  8292,  8304,  8305,  8296,  8274,  8193,  7973,  7622,  7268,  7049,  7033,  7226,  7569, },
+	/* LAT:  40 */ {   9266,  9486,  9742, 10027, 10354, 10715, 11084, 11424, 11678, 11761, 11613, 11255, 10782, 10308,  9912,  9627,  9477,  9471,  9580,  9741,  9915, 10098, 10270, 10395, 10472, 10532, 10586, 10619, 10604, 10495, 10254,  9905,  9539,  9254,  9111,  9123,  9266, },
+	/* LAT:  50 */ {  10801, 10923, 11124, 11394, 11718, 12072, 12428, 12745, 12968, 13028, 12890, 12592, 12228, 11882, 11606, 11419, 11324, 11320, 11385, 11487, 11600, 11716, 11832, 11947, 12065, 12187, 12300, 12372, 12364, 12241, 11996, 11669, 11329, 11045, 10855, 10775, 10801, },
+	/* LAT:  60 */ {  12319, 12392, 12543, 12762, 13033, 13334, 13637, 13904, 14080, 14108, 13972, 13723, 13433, 13162, 12940, 12783, 12692, 12660, 12675, 12722, 12789, 12874, 12979, 13111, 13270, 13445, 13610, 13719, 13726, 13607, 13383, 13105, 12826, 12589, 12419, 12327, 12319, },
+	/* LAT:  70 */ {  13758, 13802, 13898, 14040, 14220, 14425, 14634, 14815, 14920, 14899, 14761, 14559, 14341, 14137, 13963, 13828, 13733, 13678, 13659, 13671, 13713, 13784, 13887, 14023, 14189, 14376, 14560, 14701, 14748, 14677, 14516, 14318, 14124, 13959, 13838, 13771, 13758, },
+	/* LAT:  80 */ {  14999, 15012, 15050, 15110, 15187, 15273, 15352, 15397, 15378, 15298, 15185, 15060, 14937, 14823, 14723, 14641, 14578, 14538, 14519, 14522, 14549, 14598, 14669, 14761, 14873, 14999, 15134, 15266, 15373, 15418, 15381, 15295, 15200, 15116, 15052, 15012, 14999, },
 	/* LAT:  90 */ {  15394, 15394, 15394, 15394, 15394, 15394, 15394, 15394, 15394, 15394, 15394, 15394, 15394, 15394, 15394, 15394, 15394, 15394, 15394, 15394, 15394, 15394, 15394, 15394, 15394, 15394, 15394, 15394, 15394, 15394, 15394, 15394, 15394, 15394, 15394, 15394, 15394, },
 };

 // Magnetic strength data in milli-Gauss * 10
 // Model: WMM-2020,
 // Version: 0.5.1.11,
-// Date: 2022.0082,
+// Date: 2022.1534,
 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 */ {  5455, 5455, 5455, 5455, 5454, 5454, 5454, 5454, 5454, 5454, 5454, 5454, 5454, 5454, 5454, 5454, 5454, 5454, 5454, 5454, 5454, 5454, 5454, 5454, 5454, 5454, 5454, 5454, 5454, 5455, 5455, 5455, 5455, 5455, 5455, 5455, 5455, },
-	/* LAT: -80 */ {  6061, 5998, 5919, 5827, 5725, 5613, 5495, 5374, 5251, 5131, 5017, 4912, 4818, 4738, 4675, 4630, 4605, 4603, 4624, 4670, 4740, 4833, 4948, 5079, 5223, 5372, 5522, 5665, 5796, 5910, 6003, 6072, 6116, 6136, 6132, 6107, 6061, },
-	/* LAT: -70 */ {  6305, 6173, 6023, 5859, 5681, 5489, 5284, 5069, 4850, 4634, 4432, 4249, 4091, 3960, 3855, 3778, 3731, 3718, 3746, 3822, 3949, 4129, 4357, 4624, 4918, 5225, 5529, 5813, 6063, 6267, 6418, 6514, 6556, 6549, 6500, 6416, 6305, },
-	/* LAT: -60 */ {  6190, 5999, 5798, 5590, 5372, 5137, 4881, 4603, 4310, 4020, 3753, 3525, 3345, 3210, 3111, 3038, 2987, 2967, 2992, 3082, 3249, 3500, 3825, 4210, 4632, 5066, 5487, 5872, 6197, 6446, 6610, 6689, 6691, 6629, 6516, 6366, 6190, },
-	/* LAT: -50 */ {  5847, 5618, 5386, 5156, 4924, 4680, 4409, 4106, 3778, 3446, 3143, 2902, 2740, 2648, 2600, 2566, 2533, 2507, 2512, 2583, 2755, 3043, 3437, 3907, 4414, 4920, 5395, 5814, 6154, 6396, 6535, 6576, 6534, 6425, 6263, 6066, 5847, },
-	/* LAT: -40 */ {  5395, 5150, 4906, 4667, 4433, 4196, 3942, 3658, 3344, 3018, 2719, 2495, 2377, 2351, 2370, 2392, 2397, 2386, 2374, 2402, 2530, 2802, 3217, 3730, 4276, 4801, 5269, 5658, 5951, 6139, 6228, 6231, 6162, 6032, 5851, 5633, 5395, },
-	/* LAT: -30 */ {  4880, 4640, 4401, 4167, 3941, 3723, 3504, 3273, 3018, 2742, 2484, 2301, 2230, 2253, 2320, 2392, 2458, 2509, 2533, 2545, 2611, 2806, 3164, 3650, 4181, 4679, 5099, 5417, 5621, 5721, 5748, 5722, 5643, 5511, 5332, 5116, 4880, },
-	/* LAT: -20 */ {  4322, 4110, 3902, 3698, 3502, 3319, 3151, 2988, 2811, 2613, 2422, 2287, 2245, 2286, 2375, 2486, 2614, 2743, 2834, 2871, 2896, 2989, 3225, 3606, 4055, 4482, 4832, 5069, 5176, 5184, 5154, 5107, 5024, 4897, 4731, 4534, 4322, },
-	/* LAT: -10 */ {  3790, 3631, 3478, 3332, 3197, 3077, 2974, 2882, 2786, 2672, 2550, 2450, 2403, 2425, 2510, 2638, 2794, 2953, 3079, 3143, 3158, 3183, 3304, 3553, 3877, 4199, 4465, 4630, 4666, 4615, 4547, 4483, 4394, 4269, 4121, 3957, 3790, },
-	/* LAT:   0 */ {  3412, 3320, 3237, 3164, 3109, 3071, 3045, 3027, 3004, 2957, 2878, 2784, 2702, 2668, 2708, 2810, 2942, 3078, 3194, 3270, 3302, 3323, 3396, 3552, 3760, 3973, 4153, 4260, 4267, 4201, 4113, 4020, 3908, 3777, 3643, 3519, 3412, },
-	/* LAT:  10 */ {  3283, 3252, 3232, 3229, 3254, 3302, 3358, 3413, 3449, 3440, 3371, 3256, 3129, 3032, 3003, 3043, 3123, 3222, 3322, 3407, 3471, 3533, 3620, 3738, 3872, 4010, 4129, 4200, 4203, 4142, 4033, 3890, 3729, 3569, 3434, 3338, 3283, },
-	/* LAT:  20 */ {  3400, 3403, 3430, 3485, 3577, 3699, 3829, 3947, 4029, 4041, 3968, 3826, 3658, 3517, 3439, 3425, 3459, 3531, 3627, 3724, 3815, 3912, 4023, 4134, 4242, 4353, 4454, 4521, 4532, 4475, 4338, 4137, 3910, 3701, 3538, 3438, 3400, },
-	/* LAT:  30 */ {  3723, 3731, 3786, 3887, 4030, 4202, 4379, 4536, 4644, 4671, 4598, 4443, 4253, 4086, 3979, 3932, 3934, 3984, 4070, 4167, 4264, 4369, 4485, 4603, 4722, 4848, 4968, 5055, 5082, 5025, 4870, 4631, 4357, 4103, 3904, 3777, 3723, },
-	/* LAT:  40 */ {  4222, 4222, 4288, 4413, 4581, 4769, 4953, 5110, 5215, 5240, 5172, 5023, 4835, 4659, 4529, 4453, 4426, 4446, 4506, 4585, 4670, 4767, 4882, 5015, 5166, 5327, 5478, 5588, 5628, 5575, 5421, 5183, 4909, 4650, 4440, 4295, 4222, },
-	/* LAT:  50 */ {  4832, 4826, 4883, 4995, 5142, 5302, 5452, 5573, 5646, 5655, 5591, 5463, 5300, 5135, 4996, 4898, 4842, 4830, 4853, 4903, 4971, 5062, 5180, 5330, 5505, 5690, 5856, 5975, 6020, 5977, 5847, 5651, 5426, 5210, 5030, 4901, 4832, },
-	/* LAT:  60 */ {  5392, 5381, 5411, 5475, 5562, 5656, 5743, 5809, 5841, 5832, 5777, 5682, 5561, 5432, 5312, 5215, 5148, 5113, 5111, 5138, 5194, 5280, 5397, 5544, 5711, 5879, 6027, 6132, 6177, 6155, 6073, 5946, 5800, 5656, 5534, 5444, 5392, },
-	/* LAT:  70 */ {  5726, 5707, 5706, 5718, 5741, 5767, 5791, 5805, 5804, 5784, 5744, 5686, 5615, 5539, 5465, 5400, 5352, 5323, 5318, 5338, 5382, 5452, 5543, 5652, 5769, 5883, 5983, 6056, 6096, 6100, 6071, 6017, 5950, 5879, 5815, 5762, 5726, },
-	/* LAT:  80 */ {  5789, 5772, 5758, 5746, 5736, 5727, 5717, 5705, 5690, 5672, 5649, 5623, 5596, 5569, 5544, 5523, 5509, 5504, 5508, 5523, 5549, 5584, 5627, 5675, 5724, 5772, 5815, 5850, 5874, 5887, 5890, 5883, 5869, 5850, 5829, 5808, 5789, },
-	/* LAT:  90 */ {  5681, 5680, 5680, 5680, 5680, 5680, 5680, 5680, 5680, 5680, 5680, 5680, 5680, 5680, 5680, 5680, 5680, 5680, 5680, 5680, 5680, 5680, 5680, 5680, 5680, 5680, 5680, 5680, 5680, 5680, 5681, 5681, 5681, 5681, 5681, 5681, 5681, },
+	/* LAT: -90 */ {  5454, 5454, 5454, 5454, 5454, 5454, 5454, 5454, 5454, 5454, 5454, 5454, 5454, 5454, 5454, 5454, 5454, 5454, 5454, 5454, 5454, 5454, 5454, 5454, 5454, 5454, 5454, 5454, 5454, 5454, 5454, 5454, 5454, 5454, 5454, 5454, 5454, },
+	/* LAT: -80 */ {  6060, 5997, 5918, 5826, 5724, 5612, 5494, 5372, 5250, 5130, 5016, 4911, 4817, 4737, 4674, 4629, 4604, 4602, 4623, 4669, 4739, 4833, 4947, 5079, 5222, 5372, 5522, 5665, 5796, 5910, 6002, 6072, 6116, 6136, 6132, 6106, 6060, },
+	/* LAT: -70 */ {  6305, 6172, 6022, 5858, 5679, 5487, 5282, 5067, 4848, 4633, 4430, 4248, 4090, 3959, 3854, 3777, 3730, 3717, 3746, 3821, 3949, 4128, 4356, 4624, 4918, 5226, 5529, 5813, 6063, 6267, 6418, 6514, 6556, 6549, 6500, 6416, 6305, },
+	/* LAT: -60 */ {  6190, 5998, 5797, 5589, 5370, 5136, 4880, 4601, 4309, 4019, 3752, 3524, 3344, 3209, 3110, 3037, 2986, 2966, 2992, 3081, 3249, 3500, 3826, 4211, 4633, 5067, 5488, 5872, 6197, 6446, 6610, 6689, 6691, 6629, 6516, 6365, 6190, },
+	/* LAT: -50 */ {  5846, 5617, 5385, 5155, 4923, 4678, 4408, 4105, 3776, 3444, 3142, 2901, 2739, 2648, 2599, 2565, 2532, 2506, 2511, 2582, 2754, 3043, 3438, 3908, 4415, 4921, 5396, 5815, 6154, 6396, 6535, 6576, 6534, 6424, 6263, 6066, 5846, },
+	/* LAT: -40 */ {  5395, 5149, 4905, 4666, 4432, 4195, 3941, 3657, 3343, 3017, 2718, 2494, 2376, 2350, 2369, 2391, 2396, 2385, 2373, 2401, 2530, 2803, 3218, 3731, 4277, 4802, 5270, 5659, 5951, 6139, 6228, 6231, 6162, 6032, 5851, 5633, 5395, },
+	/* LAT: -30 */ {  4879, 4639, 4401, 4167, 3941, 3722, 3503, 3272, 3017, 2741, 2483, 2300, 2229, 2253, 2320, 2392, 2458, 2509, 2532, 2544, 2610, 2806, 3165, 3652, 4183, 4681, 5100, 5418, 5622, 5722, 5749, 5722, 5643, 5511, 5332, 5115, 4879, },
+	/* LAT: -20 */ {  4322, 4109, 3901, 3697, 3501, 3319, 3150, 2987, 2810, 2612, 2421, 2287, 2244, 2286, 2375, 2486, 2614, 2743, 2833, 2870, 2895, 2988, 3225, 3607, 4056, 4484, 4833, 5070, 5176, 5184, 5154, 5107, 5025, 4897, 4731, 4534, 4322, },
+	/* LAT: -10 */ {  3790, 3630, 3478, 3332, 3196, 3076, 2973, 2881, 2785, 2671, 2549, 2449, 2402, 2425, 2510, 2638, 2794, 2954, 3079, 3142, 3157, 3183, 3304, 3554, 3879, 4201, 4466, 4630, 4667, 4615, 4547, 4483, 4394, 4270, 4121, 3957, 3790, },
+	/* LAT:   0 */ {  3412, 3320, 3236, 3164, 3109, 3071, 3045, 3027, 3003, 2956, 2877, 2782, 2701, 2668, 2708, 2810, 2942, 3078, 3194, 3270, 3302, 3323, 3397, 3552, 3760, 3974, 4154, 4261, 4268, 4201, 4113, 4020, 3908, 3777, 3643, 3519, 3412, },
+	/* LAT:  10 */ {  3283, 3252, 3232, 3229, 3254, 3301, 3357, 3412, 3448, 3438, 3369, 3255, 3127, 3031, 3003, 3043, 3123, 3222, 3322, 3408, 3471, 3533, 3621, 3738, 3873, 4011, 4130, 4201, 4204, 4143, 4034, 3890, 3729, 3570, 3435, 3338, 3283, },
+	/* LAT:  20 */ {  3400, 3403, 3430, 3484, 3577, 3698, 3828, 3946, 4027, 4040, 3967, 3825, 3657, 3516, 3439, 3425, 3459, 3531, 3628, 3725, 3815, 3913, 4024, 4135, 4243, 4354, 4456, 4522, 4533, 4475, 4339, 4137, 3910, 3701, 3538, 3438, 3400, },
+	/* LAT:  30 */ {  3723, 3730, 3786, 3886, 4030, 4201, 4377, 4534, 4643, 4670, 4597, 4441, 4251, 4085, 3979, 3931, 3934, 3984, 4070, 4168, 4265, 4370, 4486, 4604, 4723, 4849, 4969, 5056, 5083, 5026, 4871, 4632, 4358, 4104, 3905, 3777, 3723, },
+	/* LAT:  40 */ {  4222, 4222, 4288, 4412, 4580, 4768, 4951, 5108, 5213, 5239, 5171, 5022, 4834, 4658, 4529, 4453, 4426, 4447, 4507, 4585, 4671, 4768, 4883, 5016, 5167, 5328, 5479, 5589, 5629, 5576, 5421, 5184, 4910, 4651, 4440, 4295, 4222, },
+	/* LAT:  50 */ {  4832, 4826, 4883, 4994, 5141, 5300, 5450, 5571, 5645, 5654, 5590, 5462, 5299, 5134, 4996, 4898, 4842, 4830, 4854, 4904, 4972, 5063, 5182, 5331, 5507, 5691, 5857, 5976, 6021, 5977, 5847, 5651, 5426, 5210, 5030, 4901, 4832, },
+	/* LAT:  60 */ {  5392, 5381, 5410, 5474, 5560, 5655, 5742, 5807, 5840, 5831, 5776, 5682, 5561, 5432, 5312, 5215, 5148, 5114, 5111, 5139, 5195, 5281, 5398, 5545, 5712, 5880, 6028, 6133, 6177, 6156, 6073, 5947, 5800, 5657, 5535, 5444, 5392, },
+	/* LAT:  70 */ {  5726, 5707, 5705, 5718, 5740, 5766, 5790, 5804, 5803, 5784, 5744, 5686, 5615, 5539, 5465, 5400, 5352, 5324, 5319, 5338, 5383, 5452, 5544, 5652, 5770, 5884, 5984, 6057, 6096, 6100, 6071, 6018, 5950, 5880, 5815, 5762, 5726, },
+	/* LAT:  80 */ {  5789, 5772, 5758, 5746, 5736, 5727, 5717, 5705, 5690, 5671, 5649, 5624, 5596, 5569, 5544, 5523, 5509, 5504, 5509, 5524, 5549, 5585, 5627, 5675, 5725, 5773, 5816, 5850, 5875, 5888, 5890, 5883, 5869, 5851, 5830, 5809, 5789, },
+	/* LAT:  90 */ {  5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, },
 };
@@ -327,7 +327,7 @@ AttitudeEstimatorQ::Run()
 					if (_vel_prev_t != 0 && lpos.timestamp != _vel_prev_t) {
 						float vel_dt = (lpos.timestamp - _vel_prev_t) / 1e6f;
 						/* calculate acceleration in body frame */
-						_pos_acc = _q.conjugate_inversed((vel - _vel_prev) / vel_dt);
+						_pos_acc = _q.rotateVectorInverse((vel - _vel_prev) / vel_dt);
 					}

 					_vel_prev_t = lpos.timestamp;
@@ -452,26 +452,26 @@ AttitudeEstimatorQ::update(float dt)
 		if (_param_att_ext_hdg_m.get() == 1) {
 			// Vision heading correction
 			// Project heading to global frame and extract XY component
-			Vector3f vision_hdg_earth = _q.conjugate(_vision_hdg);
+			Vector3f vision_hdg_earth = _q.rotateVector(_vision_hdg);
 			float vision_hdg_err = wrap_pi(atan2f(vision_hdg_earth(1), vision_hdg_earth(0)));
 			// Project correction to body frame
-			corr += _q.conjugate_inversed(Vector3f(0.0f, 0.0f, -vision_hdg_err)) * _param_att_w_ext_hdg.get();
+			corr += _q.rotateVectorInverse(Vector3f(0.0f, 0.0f, -vision_hdg_err)) * _param_att_w_ext_hdg.get();
 		}

 		if (_param_att_ext_hdg_m.get() == 2) {
 			// Mocap heading correction
 			// Project heading to global frame and extract XY component
-			Vector3f mocap_hdg_earth = _q.conjugate(_mocap_hdg);
+			Vector3f mocap_hdg_earth = _q.rotateVector(_mocap_hdg);
 			float mocap_hdg_err = wrap_pi(atan2f(mocap_hdg_earth(1), mocap_hdg_earth(0)));
 			// Project correction to body frame
-			corr += _q.conjugate_inversed(Vector3f(0.0f, 0.0f, -mocap_hdg_err)) * _param_att_w_ext_hdg.get();
+			corr += _q.rotateVectorInverse(Vector3f(0.0f, 0.0f, -mocap_hdg_err)) * _param_att_w_ext_hdg.get();
 		}
 	}

 	if (_param_att_ext_hdg_m.get() == 0 || !_ext_hdg_good) {
 		// Magnetometer correction
 		// Project mag field vector to global frame and extract XY component
-		Vector3f mag_earth = _q.conjugate(_mag);
+		Vector3f mag_earth = _q.rotateVector(_mag);
 		float mag_err = wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);
 		float gainMult = 1.0f;
 		const float fifty_dps = 0.873f;
@@ -481,7 +481,7 @@ AttitudeEstimatorQ::update(float dt)
 		}

 		// Project magnetometer correction to body frame
-		corr += _q.conjugate_inversed(Vector3f(0.0f, 0.0f, -mag_err)) * _param_att_w_mag.get() * gainMult;
+		corr += _q.rotateVectorInverse(Vector3f(0.0f, 0.0f, -mag_err)) * _param_att_w_mag.get() * gainMult;
 	}

 	_q.normalize();
@@ -489,7 +489,7 @@ AttitudeEstimatorQ::update(float dt)

 	// Accelerometer correction
 	// Project 'k' unit vector of earth frame to body frame
-	// Vector3f k = _q.conjugate_inversed(Vector3f(0.0f, 0.0f, 1.0f));
+	// Vector3f k = _q.rotateVectorInverse(Vector3f(0.0f, 0.0f, 1.0f));
 	// Optimized version with dropped zeros
 	Vector3f k(
 		2.0f * (_q(1) * _q(3) - _q(0) * _q(2)),
@@ -47,6 +47,7 @@ px4_add_library(PreFlightCheck
 	checks/ekf2Check.cpp
 	checks/failureDetectorCheck.cpp
 	checks/manualControlCheck.cpp
+	checks/modeCheck.cpp
 	checks/cpuResourceCheck.cpp
 	checks/sdcardCheck.cpp
 	checks/parachuteCheck.cpp
@@ -271,6 +271,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 	}

 	failed = failed || !manualControlCheck(mavlink_log_pub, report_failures);
+	failed = failed || !modeCheck(mavlink_log_pub, report_failures, status);
 	failed = failed || !cpuResourceCheck(mavlink_log_pub, report_failures);
 	failed = failed || !parachuteCheck(mavlink_log_pub, report_failures, status_flags);

@@ -119,6 +119,7 @@ private:
 					 const bool prearm);

 	static bool manualControlCheck(orb_advert_t *mavlink_log_pub, const bool report_fail);
+	static bool modeCheck(orb_advert_t *mavlink_log_pub, const bool report_fail, const vehicle_status_s &status);
 	static bool airframeCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status);
 	static bool cpuResourceCheck(orb_advert_t *mavlink_log_pub, const bool report_fail);
 	static bool sdcardCheck(orb_advert_t *mavlink_log_pub, bool &sd_card_detected_once, const bool report_fail);
@@ -0,0 +1,69 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "../PreFlightCheck.hpp"
+
+#include <systemlib/mavlink_log.h>
+
+using namespace time_literals;
+
+bool PreFlightCheck::modeCheck(orb_advert_t *mavlink_log_pub, const bool report_fail,
+			       const vehicle_status_s &vehicle_status)
+{
+	bool success = true;
+
+	switch (vehicle_status.nav_state) {
+	case vehicle_status_s::NAVIGATION_STATE_MANUAL:
+	case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
+	case vehicle_status_s::NAVIGATION_STATE_POSCTL:
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
+	case vehicle_status_s::NAVIGATION_STATE_ACRO:
+	case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
+	case vehicle_status_s::NAVIGATION_STATE_STAB:
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF:
+		break;
+
+	default:
+		success = false;
+
+		if (report_fail) {
+			mavlink_log_critical(mavlink_log_pub, "Mode not suitable for takeoff");
+		}
+
+		break;
+	}
+
+	return success;
+}
@@ -677,7 +677,8 @@ static constexpr const char *main_state_str(uint8_t main_state)
 transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks)
 {
 	// allow a grace period for re-arming: preflight checks don't need to pass during that time, for example for accidential in-air disarming
-	if (_param_com_rearm_grace.get() && (hrt_elapsed_time(&_last_disarmed_timestamp) < 5_s)) {
+	if (calling_reason == arm_disarm_reason_t::rc_switch
+	    && (hrt_elapsed_time(&_last_disarmed_timestamp) < 5_s)) {
 		run_preflight_checks = false;
 	}

@@ -811,6 +812,9 @@ Commander::Commander() :

 	// default for vtol is rotary wing
 	_vtol_status.vtol_in_rw_mode = true;
+
+	_vehicle_gps_position_valid.set_hysteresis_time_from(false, GPS_VALID_TIME);
+	_vehicle_gps_position_valid.set_hysteresis_time_from(true, GPS_VALID_TIME);
 }

 bool
@@ -3248,8 +3252,8 @@ Commander::reset_posvel_validity()
 	_lpos_probation_time_us = POSVEL_PROBATION_MIN;
 	_lvel_probation_time_us = POSVEL_PROBATION_MIN;

-	// recheck validity
-	UpdateEstimateValidity();
+	// recheck validity (force update)
+	estimator_check(true);
 }

 bool
@@ -3965,7 +3969,7 @@ void Commander::battery_status_check()
 	}
 }

-void Commander::estimator_check()
+void Commander::estimator_check(bool force)
 {
 	// Check if quality checking of position accuracy and consistency is to be performed
 	const bool run_quality_checks = !_status_flags.circuit_breaker_engaged_posfailure_check;
@@ -3983,86 +3987,112 @@ void Commander::estimator_check()
 		_heading_reset_counter = lpos.heading_reset_counter;
 	}

-	const bool mag_fault_prev = (_estimator_status_sub.get().control_mode_flags & (1 << estimator_status_s::CS_MAG_FAULT));
-	const bool gnss_heading_fault_prev = (_estimator_status_sub.get().control_mode_flags &
-					      (1 << estimator_status_s::CS_GPS_YAW_FAULT));
+	const bool mag_fault_prev = _estimator_status_flags_sub.get().cs_mag_fault;
+	const bool gnss_heading_fault_prev = _estimator_status_flags_sub.get().cs_gps_yaw_fault;

 	// use primary estimator_status
-	if (_estimator_selector_status_sub.updated()) {
+	if (_estimator_selector_status_sub.updated() || force) {
 		estimator_selector_status_s estimator_selector_status;

 		if (_estimator_selector_status_sub.copy(&estimator_selector_status)) {
 			if (estimator_selector_status.primary_instance != _estimator_status_sub.get_instance()) {
 				_estimator_status_sub.ChangeInstance(estimator_selector_status.primary_instance);
+				_estimator_status_flags_sub.ChangeInstance(estimator_selector_status.primary_instance);
 			}
 		}
 	}

-	if (_estimator_status_sub.update()) {
-		const estimator_status_s &estimator_status = _estimator_status_sub.get();
+	if (_estimator_status_flags_sub.update()) {
+		const estimator_status_flags_s &estimator_status_flags = _estimator_status_flags_sub.get();
+
+		_status_flags.dead_reckoning = estimator_status_flags.cs_wind_dead_reckoning
+					       || estimator_status_flags.cs_inertial_dead_reckoning;
+
+		if (!(estimator_status_flags.cs_inertial_dead_reckoning || estimator_status_flags.cs_wind_dead_reckoning)) {
+			// position requirements (update if not dead reckoning)
+			bool gps             = estimator_status_flags.cs_gps;
+			bool optical_flow    = estimator_status_flags.cs_opt_flow;
+			bool vision_position = estimator_status_flags.cs_ev_pos;
+
+			_status_flags.position_reliant_on_gps             =  gps && !optical_flow && !vision_position;
+			_status_flags.position_reliant_on_optical_flow    = !gps &&  optical_flow && !vision_position;
+			_status_flags.position_reliant_on_vision_position = !gps && !optical_flow &&  vision_position;
+		}

 		// Check for a magnetomer fault and notify the user
-		const bool mag_fault = (estimator_status.control_mode_flags & (1 << estimator_status_s::CS_MAG_FAULT));
-		const bool gnss_heading_fault = (estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GPS_YAW_FAULT));
-
-		if (!mag_fault_prev && mag_fault) {
+		if (!mag_fault_prev && estimator_status_flags.cs_mag_fault) {
 			mavlink_log_critical(&_mavlink_log_pub, "Compass needs calibration - Land now!\t");
 			events::send(events::ID("commander_stopping_mag_use"), events::Log::Critical,
 				     "Stopping compass use! Land now and calibrate the compass");
 			set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG, true, true, false, _status);
 		}

-		if (!gnss_heading_fault_prev && gnss_heading_fault) {
+		if (!gnss_heading_fault_prev && estimator_status_flags.cs_gps_yaw_fault) {
 			mavlink_log_critical(&_mavlink_log_pub, "GNSS heading not reliable - Land now!\t");
 			events::send(events::ID("commander_stopping_gnss_heading_use"), events::Log::Critical,
 				     "GNSS heading not reliable. Land now!");
 			set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GPS, true, true, false, _status);
 		}
+	}

-		/* Check estimator status for signs of bad yaw induced post takeoff navigation failure
-		 * for a short time interval after takeoff.
-		 * Most of the time, the drone can recover from a bad initial yaw using GPS-inertial
-		 * heading estimation (yaw emergency estimator) or GPS heading (fixed wings only), but
-		 * if this does not fix the issue we need to stop using a position controlled
-		 * mode to prevent flyaway crashes.
-		 */

-		if (run_quality_checks && _status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+	/* Check estimator status for signs of bad yaw induced post takeoff navigation failure
+	* for a short time interval after takeoff.
+	* Most of the time, the drone can recover from a bad initial yaw using GPS-inertial
+	* heading estimation (yaw emergency estimator) or GPS heading (fixed wings only), but
+	* if this does not fix the issue we need to stop using a position controlled
+	* mode to prevent flyaway crashes.
+	*/
+	bool pre_flt_fail_innov_heading = false;
+	bool pre_flt_fail_innov_vel_horiz = false;

-			if (_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
-				_nav_test_failed = false;
-				_nav_test_passed = false;
+	if (_estimator_status_sub.updated() || force) {

-			} else {
-				if (!_nav_test_passed) {
-					// Both test ratios need to pass/fail together to change the nav test status
-					const bool innovation_pass = (estimator_status.vel_test_ratio < 1.0f) && (estimator_status.pos_test_ratio < 1.0f)
-								     && (estimator_status.vel_test_ratio > FLT_EPSILON) && (estimator_status.pos_test_ratio > FLT_EPSILON);
-					const bool innovation_fail = (estimator_status.vel_test_ratio >= 1.0f) && (estimator_status.pos_test_ratio >= 1.0f);
+		estimator_status_s estimator_status;

-					if (innovation_pass) {
-						_time_last_innov_pass = hrt_absolute_time();
+		if (_estimator_status_sub.copy(&estimator_status)) {

-						// if nav status is unconfirmed, confirm yaw angle as passed after 30 seconds or achieving 5 m/s of speed
-						const bool sufficient_time = (_status.takeoff_time != 0) && (hrt_elapsed_time(&_status.takeoff_time) > 30_s);
-						const bool sufficient_speed = matrix::Vector2f(lpos.vx, lpos.vy).longerThan(5.f);
+			pre_flt_fail_innov_heading = estimator_status.pre_flt_fail_innov_heading;
+			pre_flt_fail_innov_vel_horiz = estimator_status.pre_flt_fail_innov_vel_horiz;

-						// Even if the test already failed, allow it to pass if it did not fail during the last 10 seconds
-						if (hrt_elapsed_time(&_time_last_innov_fail) > 10_s
-						    && (sufficient_time || sufficient_speed)) {
-							_nav_test_passed = true;
-							_nav_test_failed = false;
-						}
+			if (run_quality_checks && _status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {

-					} else if (innovation_fail) {
-						_time_last_innov_fail = hrt_absolute_time();
+				if (_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
+					_nav_test_failed = false;
+					_nav_test_passed = false;

-						if (!_nav_test_failed && hrt_elapsed_time(&_time_last_innov_pass) > 2_s) {
-							// if the innovation test has failed continuously, declare the nav as failed
-							_nav_test_failed = true;
-							mavlink_log_emergency(&_mavlink_log_pub, "Navigation failure! Land and recalibrate sensors\t");
-							events::send(events::ID("commander_navigation_failure"), events::Log::Emergency,
-								     "Navigation failure! Land and recalibrate the sensors");
+				} else {
+					if (!_nav_test_passed) {
+						// Both test ratios need to pass/fail together to change the nav test status
+						const bool innovation_pass = (estimator_status.vel_test_ratio < 1.f) && (estimator_status.pos_test_ratio < 1.f)
+									     && (estimator_status.vel_test_ratio > FLT_EPSILON) && (estimator_status.pos_test_ratio > FLT_EPSILON);
+
+						const bool innovation_fail = (estimator_status.vel_test_ratio >= 1.f) && (estimator_status.pos_test_ratio >= 1.f);
+
+						if (innovation_pass) {
+							_time_last_innov_pass = hrt_absolute_time();
+
+							// if nav status is unconfirmed, confirm yaw angle as passed after 30 seconds or achieving 5 m/s of speed
+							const bool sufficient_time = (_status.takeoff_time != 0) && (hrt_elapsed_time(&_status.takeoff_time) > 30_s);
+							const bool sufficient_speed = matrix::Vector2f(lpos.vx, lpos.vy).longerThan(5.f);
+
+							// Even if the test already failed, allow it to pass if it did not fail during the last 10 seconds
+							if (hrt_elapsed_time(&_time_last_innov_fail) > 10_s
+							    && (sufficient_time || sufficient_speed)) {
+								_nav_test_passed = true;
+								_nav_test_failed = false;
+							}
+
+						} else if (innovation_fail) {
+							_time_last_innov_fail = hrt_absolute_time();
+
+							if (!_nav_test_failed && hrt_elapsed_time(&_time_last_innov_pass) > 2_s) {
+								// if the innovation test has failed continuously, declare the nav as failed
+								_nav_test_failed = true;
+								mavlink_log_emergency(&_mavlink_log_pub, "Navigation failure! Land and recalibrate sensors\t");
+								events::send(events::ID("commander_navigation_failure"), events::Log::Emergency,
+									     "Navigation failure! Land and recalibrate the sensors");
+							}
 						}
 					}
 				}
@@ -4073,9 +4103,51 @@ void Commander::estimator_check()
 	// run position and velocity accuracy checks
 	// Check if quality checking of position accuracy and consistency is to be performed
 	if (run_quality_checks) {
-		UpdateEstimateValidity();
+		float lpos_eph_threshold_adj = _param_com_pos_fs_eph.get();
+
+		// relax local position eph threshold in operator controlled position mode
+		if (_internal_state.main_state == commander_state_s::MAIN_STATE_POSCTL &&
+		    ((_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ALTCTL)
+		     || (_status.nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL))) {
+
+			// Set the allowable position uncertainty based on combination of flight and estimator state
+			// When we are in a operator demanded position control mode and are solely reliant on optical flow,
+			// do not check position error because it will gradually increase throughout flight and the operator will compensate for the drift
+			if (_status_flags.position_reliant_on_optical_flow) {
+				lpos_eph_threshold_adj = INFINITY;
+			}
+		}
+
+		bool xy_valid = lpos.xy_valid && !_nav_test_failed;
+		bool v_xy_valid = lpos.v_xy_valid && !_nav_test_failed;
+
+		if (!_armed.armed) {
+			if (pre_flt_fail_innov_heading || pre_flt_fail_innov_vel_horiz) {
+				xy_valid = false;
+			}
+
+			if (pre_flt_fail_innov_vel_horiz) {
+				v_xy_valid = false;
+			}
+		}
+
+		const vehicle_global_position_s &gpos = _global_position_sub.get();
+
+		_status_flags.condition_global_position_valid =
+			check_posvel_validity(xy_valid, gpos.eph, _param_com_pos_fs_eph.get(), gpos.timestamp,
+					      &_last_gpos_fail_time_us, &_gpos_probation_time_us, _status_flags.condition_global_position_valid);
+
+		_status_flags.condition_local_position_valid =
+			check_posvel_validity(xy_valid, lpos.eph, lpos_eph_threshold_adj, lpos.timestamp,
+					      &_last_lpos_fail_time_us, &_lpos_probation_time_us, _status_flags.condition_local_position_valid);
+
+		_status_flags.condition_local_velocity_valid =
+			check_posvel_validity(v_xy_valid, lpos.evh, _param_com_vel_fs_evh.get(), lpos.timestamp,
+					      &_last_lvel_fail_time_us, &_lvel_probation_time_us, _status_flags.condition_local_velocity_valid);
 	}

+
+	// altitude
 	_status_flags.condition_local_altitude_valid = lpos.z_valid
 			&& (hrt_elapsed_time(&lpos.timestamp) < (_param_com_pos_fs_delay.get() * 1_s));

@@ -4122,43 +4194,41 @@ void Commander::estimator_check()
 	}

 	_status_flags.condition_angular_velocity_valid = condition_angular_velocity_valid;
-}

-void Commander::UpdateEstimateValidity()
-{
-	const vehicle_local_position_s &lpos = _local_position_sub.get();
-	const vehicle_global_position_s &gpos = _global_position_sub.get();
-	const estimator_status_s &status = _estimator_status_sub.get();

-	float lpos_eph_threshold_adj = _param_com_pos_fs_eph.get();
+	// gps
+	const bool condition_gps_position_was_valid = _status_flags.condition_gps_position_valid;

-	// relax local position eph threshold in operator controlled position mode
-	if (_internal_state.main_state == commander_state_s::MAIN_STATE_POSCTL &&
-	    ((_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ALTCTL)
-	     || (_status.nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL))) {
+	if (_vehicle_gps_position_sub.updated() || force) {
+		vehicle_gps_position_s vehicle_gps_position;

-		// Set the allowable position uncertainty based on combination of flight and estimator state
-		// When we are in a operator demanded position control mode and are solely reliant on optical flow, do not check position error because it will gradually increase throughout flight and the operator will compensate for the drift
-		const bool reliant_on_opt_flow = ((status.control_mode_flags & (1 << estimator_status_s::CS_OPT_FLOW))
-						  && !(status.control_mode_flags & (1 << estimator_status_s::CS_GPS))
-						  && !(status.control_mode_flags & (1 << estimator_status_s::CS_EV_POS)));
+		if (_vehicle_gps_position_sub.copy(&vehicle_gps_position)) {

-		if (reliant_on_opt_flow) {
-			lpos_eph_threshold_adj = INFINITY;
+			bool time = (vehicle_gps_position.timestamp != 0) && (hrt_elapsed_time(&vehicle_gps_position.timestamp) < 1_s);
+
+			bool fix = vehicle_gps_position.fix_type >= 2;
+			bool eph = vehicle_gps_position.eph < _param_com_pos_fs_eph.get();
+			bool epv = vehicle_gps_position.epv < _param_com_pos_fs_epv.get();
+			bool evh = vehicle_gps_position.s_variance_m_s < _param_com_vel_fs_evh.get();
+
+			_vehicle_gps_position_valid.set_state_and_update(time && fix && eph && epv && evh, hrt_absolute_time());
+			_status_flags.condition_gps_position_valid = _vehicle_gps_position_valid.get_state();
+
+			_vehicle_gps_position_timestamp_last = vehicle_gps_position.timestamp;
+		}
+
+	} else {
+		const hrt_abstime now_us = hrt_absolute_time();
+
+		if (now_us > _vehicle_gps_position_timestamp_last + GPS_VALID_TIME) {
+			_vehicle_gps_position_valid.set_state_and_update(false, now_us);
+			_status_flags.condition_gps_position_valid = false;
 		}
 	}

-	_status_flags.condition_global_position_valid =
-		check_posvel_validity(lpos.xy_valid && !_nav_test_failed, gpos.eph, _param_com_pos_fs_eph.get(), gpos.timestamp,
-				      &_last_gpos_fail_time_us, &_gpos_probation_time_us, _status_flags.condition_global_position_valid);
-
-	_status_flags.condition_local_position_valid =
-		check_posvel_validity(lpos.xy_valid && !_nav_test_failed, lpos.eph, lpos_eph_threshold_adj, lpos.timestamp,
-				      &_last_lpos_fail_time_us, &_lpos_probation_time_us, _status_flags.condition_local_position_valid);
-
-	_status_flags.condition_local_velocity_valid =
-		check_posvel_validity(lpos.v_xy_valid && !_nav_test_failed, lpos.evh, _param_com_vel_fs_evh.get(), lpos.timestamp,
-				      &_last_lvel_fail_time_us, &_lvel_probation_time_us, _status_flags.condition_local_velocity_valid);
+	if (condition_gps_position_was_valid && !_status_flags.condition_gps_position_valid) {
+		PX4_WARN("GPS no longer valid");
+	}
 }

 void
@@ -69,6 +69,7 @@
 #include <uORB/topics/esc_status.h>
 #include <uORB/topics/estimator_selector_status.h>
 #include <uORB/topics/estimator_status.h>
+#include <uORB/topics/estimator_status_flags.h>
 #include <uORB/topics/geofence_result.h>
 #include <uORB/topics/iridiumsbd_status.h>
 #include <uORB/topics/manual_control_setpoint.h>
@@ -84,6 +85,7 @@
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vtol_vehicle_status.h>
@@ -144,7 +146,7 @@ private:

 	void esc_status_check();

-	void estimator_check();
+	void estimator_check(bool force = false);

 	bool handle_command(const vehicle_command_s &cmd);

@@ -172,8 +174,6 @@ private:

 	void update_control_mode();

-	void UpdateEstimateValidity();
-
 	bool shutdown_if_allowed();

 	bool stabilization_required();
@@ -247,7 +247,6 @@ private:
 		(ParamBool<px4::params::COM_ARM_MIS_REQ>) _param_arm_mission_required,
 		(ParamBool<px4::params::COM_ARM_AUTH_REQ>) _param_arm_auth_required,
 		(ParamBool<px4::params::COM_ARM_CHK_ESCS>) _param_escs_checks_required,
-		(ParamBool<px4::params::COM_REARM_GRACE>) _param_com_rearm_grace,

 		(ParamInt<px4::params::COM_FLIGHT_UUID>) _param_flight_uuid,
 		(ParamInt<px4::params::COM_TAKEOFF_ACT>) _param_takeoff_finished_action,
@@ -314,6 +313,10 @@ private:
 	bool		_nav_test_passed{false};	/**< true if the post takeoff navigation test has passed */
 	bool		_nav_test_failed{false};	/**< true if the post takeoff navigation test has failed */

+	static constexpr hrt_abstime GPS_VALID_TIME{3_s};
+	Hysteresis _vehicle_gps_position_valid{false};
+	hrt_abstime _vehicle_gps_position_timestamp_last{0};
+
 	bool		_geofence_loiter_on{false};
 	bool		_geofence_rtl_on{false};
 	bool		_geofence_land_on{false};
@@ -411,6 +414,7 @@ private:
 	uORB::Subscription					_cpuload_sub{ORB_ID(cpuload)};
 	uORB::Subscription					_esc_status_sub{ORB_ID(esc_status)};
 	uORB::Subscription					_estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
+	uORB::Subscription					_estimator_status_sub{ORB_ID(estimator_status)};
 	uORB::Subscription					_geofence_result_sub{ORB_ID(geofence_result)};
 	uORB::Subscription					_iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};
 	uORB::Subscription					_land_detector_sub{ORB_ID(vehicle_land_detected)};
@@ -420,6 +424,7 @@ private:
 	uORB::Subscription					_system_power_sub{ORB_ID(system_power)};
 	uORB::Subscription					_vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
 	uORB::Subscription					_vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
+	uORB::Subscription					_vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
 	uORB::Subscription					_vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
 	uORB::Subscription					_wind_sub{ORB_ID(wind)};

@@ -433,8 +438,7 @@ private:
 	uORB::Subscription					_power_button_state_sub {ORB_ID(power_button_state)};
 #endif // BOARD_HAS_POWER_CONTROL

-	uORB::SubscriptionData<airspeed_s>			_airspeed_sub{ORB_ID(airspeed)};
-	uORB::SubscriptionData<estimator_status_s>		_estimator_status_sub{ORB_ID(estimator_status)};
+	uORB::SubscriptionData<estimator_status_flags_s>	_estimator_status_flags_sub{ORB_ID(estimator_status_flags)};
 	uORB::SubscriptionData<mission_result_s>		_mission_result_sub{ORB_ID(mission_result)};
 	uORB::SubscriptionData<offboard_control_mode_s>		_offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
 	uORB::SubscriptionData<vehicle_global_position_s>	_global_position_sub{ORB_ID(vehicle_global_position)};
@@ -157,15 +157,13 @@ struct accel_worker_data_s {
 	orb_advert_t	*mavlink_log_pub{nullptr};
 	unsigned	done_count{0};
 	float		accel_ref[MAX_ACCEL_SENS][detect_orientation_side_count][3] {};
-	float		accel_temperature_ref[MAX_ACCEL_SENS] {NAN, NAN, NAN, NAN};
 };

 // Read specified number of accelerometer samples, calculate average and dispersion.
 static calibrate_return read_accelerometer_avg(float (&accel_avg)[MAX_ACCEL_SENS][detect_orientation_side_count][3],
-		float (&accel_temperature_avg)[MAX_ACCEL_SENS],	unsigned orient, unsigned samples_num)
+		unsigned orient, unsigned samples_num)
 {
 	Vector3f accel_sum[MAX_ACCEL_SENS] {};
-	float temperature_sum[MAX_ACCEL_SENS] {NAN, NAN, NAN, NAN};
 	unsigned counts[MAX_ACCEL_SENS] {};

 	unsigned errcount = 0;
@@ -217,14 +215,6 @@ static calibrate_return read_accelerometer_avg(float (&accel_avg)[MAX_ACCEL_SENS
 					accel_sum[accel_index] += Vector3f{arp.x, arp.y, arp.z} - offset;

 					counts[accel_index]++;
-
-					if (!PX4_ISFINITE(temperature_sum[accel_index])) {
-						// set first valid value
-						temperature_sum[accel_index] = (arp.temperature * counts[accel_index]);
-
-					} else {
-						temperature_sum[accel_index] += arp.temperature;
-					}
 				}
 			}

@@ -248,8 +238,6 @@ static calibrate_return read_accelerometer_avg(float (&accel_avg)[MAX_ACCEL_SENS
 	for (unsigned s = 0; s < MAX_ACCEL_SENS; s++) {
 		const Vector3f avg{accel_sum[s] / counts[s]};
 		avg.copyTo(accel_avg[s][orient]);
-
-		accel_temperature_avg[s] = temperature_sum[s] /  counts[s];
 	}

 	return calibrate_return_ok;
@@ -263,7 +251,7 @@ static calibrate_return accel_calibration_worker(detect_orientation_return orien
 	calibration_log_info(worker_data->mavlink_log_pub, "[cal] Hold still, measuring %s side",
 			     detect_orientation_str(orientation));

-	read_accelerometer_avg(worker_data->accel_ref, worker_data->accel_temperature_ref, orientation, samples_num);
+	read_accelerometer_avg(worker_data->accel_ref, orientation, samples_num);

 	// check accel
 	for (unsigned accel_index = 0; accel_index < MAX_ACCEL_SENS; accel_index++) {
@@ -414,8 +402,6 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
 				const Matrix3f accel_T_rotated{board_rotation_t *accel_T * board_rotation};
 				calibrations[i].set_scale(accel_T_rotated.diag());

-				calibrations[i].set_temperature(worker_data.accel_temperature_ref[i]);
-
 #if defined(DEBUD_BUILD)
 				PX4_INFO("accel %d: offset", i);
 				offset.print();
@@ -490,7 +476,6 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub)
 	for (unsigned accel_index = 0; accel_index < MAX_ACCEL_SENS; accel_index++) {
 		sensor_accel_s arp{};
 		Vector3f accel_sum{};
-		float temperature_sum{NAN};
 		unsigned count = 0;

 		while (accel_subs[accel_index].update(&arp)) {
@@ -526,21 +511,11 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub)

 					if (diff.norm() < 1.f) {
 						accel_sum += Vector3f{arp.x, arp.y, arp.z} - offset;
-
 						count++;
-
-						if (!PX4_ISFINITE(temperature_sum)) {
-							// set first valid value
-							temperature_sum = (arp.temperature * count);
-
-						} else {
-							temperature_sum += arp.temperature;
-						}
 					}

 				} else {
 					accel_sum = accel;
-					temperature_sum = arp.temperature;
 					count = 1;
 				}
 			}
@@ -550,7 +525,6 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub)

 			bool calibrated = false;
 			const Vector3f accel_avg = accel_sum / count;
-			const float temperature_avg = temperature_sum / count;

 			Vector3f offset{0.f, 0.f, 0.f};

@@ -561,7 +535,7 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub)
 				// use vehicle_attitude if available
 				const vehicle_attitude_s &att = attitude_sub.get();
 				const matrix::Quatf q{att.q};
-				const Vector3f accel_ref = q.conjugate_inversed(Vector3f{0.f, 0.f, -CONSTANTS_ONE_G});
+				const Vector3f accel_ref = q.rotateVectorInverse(Vector3f{0.f, 0.f, -CONSTANTS_ONE_G});

 				// sanity check angle between acceleration vectors
 				const float angle = AxisAnglef(Quatf(accel_avg, accel_ref)).angle();
@@ -593,7 +567,6 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub)

 			} else {
 				calibration.set_offset(offset);
-				calibration.set_temperature(temperature_avg);

 				if (calibration.ParametersSave(accel_index)) {
 					calibration.PrintStatus();
@@ -678,16 +678,6 @@ PARAM_DEFINE_INT32(COM_ARM_MAG_ANG, 45);
 */
 PARAM_DEFINE_INT32(COM_ARM_MAG_STR, 2);

-/**
- * Rearming grace period
- *
- * Re-arming grace allows to rearm the drone with manual command without running prearmcheck during 5 s after disarming.
- *
- * @group Commander
- * @boolean
- */
-PARAM_DEFINE_INT32(COM_REARM_GRACE, 1);
-
 /**
 * Enable RC stick override of auto and/or offboard modes
 *
@@ -71,7 +71,6 @@ struct gyro_worker_data_t {
 	calibration::Gyroscope calibrations[MAX_GYROS] {};

 	Vector3f offset[MAX_GYROS] {};
-	float temperature[MAX_GYROS] {NAN, NAN, NAN, NAN};

 	math::MedianFilter<float, 9> filter[3] {};
 };
@@ -119,14 +118,6 @@ static calibrate_return gyro_calibration_worker(gyro_worker_data_t &worker_data)

 						calibration_counter[gyro_index]++;

-						if (!PX4_ISFINITE(worker_data.temperature[gyro_index])) {
-							// set first valid value
-							worker_data.temperature[gyro_index] = gyro_report.temperature * calibration_counter[gyro_index];
-
-						} else {
-							worker_data.temperature[gyro_index] += gyro_report.temperature;
-						}
-
 						if (gyro_index == 0) {
 							worker_data.filter[0].insert(gyro_report.x - thermal_offset(0));
 							worker_data.filter[1].insert(gyro_report.y - thermal_offset(1));
@@ -169,7 +160,6 @@ static calibrate_return gyro_calibration_worker(gyro_worker_data_t &worker_data)
 		}

 		worker_data.offset[s] /= calibration_counter[s];
-		worker_data.temperature[s] /= calibration_counter[s];
 	}

 	return calibrate_return_ok;
@@ -269,8 +259,6 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)

 			if (calibration.device_id() != 0) {
 				calibration.set_offset(worker_data.offset[uorb_index]);
-				calibration.set_temperature(worker_data.temperature[uorb_index]);
-
 				calibration.PrintStatus();

 				if (calibration.ParametersSave(uorb_index, true)) {
@@ -94,8 +94,6 @@ struct mag_worker_data_t {
 	float		*y[MAX_MAGS];
 	float		*z[MAX_MAGS];

-	float		temperature[MAX_MAGS] {NAN, NAN, NAN, NAN};
-
 	calibration::Magnetometer calibration[MAX_MAGS] {};
 };

@@ -342,7 +340,6 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
 		if (mag_sub[0].updatedBlocking(1000_ms)) {
 			bool rejected = false;
 			Vector3f new_samples[MAX_MAGS] {};
-			float new_temperature[MAX_MAGS] {NAN, NAN, NAN, NAN};

 			for (uint8_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) {
 				if (worker_data->calibration[cur_mag].device_id() != 0) {
@@ -371,7 +368,6 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta

 						if (!reject) {
 							new_samples[cur_mag] = Vector3f{mag.x, mag.y, mag.z};
-							new_temperature[cur_mag] = mag.temperature;
 							updated = true;
 							break;
 						}
@@ -392,14 +388,6 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
 						worker_data->y[cur_mag][worker_data->calibration_counter_total[cur_mag]] = new_samples[cur_mag](1);
 						worker_data->z[cur_mag][worker_data->calibration_counter_total[cur_mag]] = new_samples[cur_mag](2);

-						if (!PX4_ISFINITE(worker_data->temperature[cur_mag])) {
-							// set first valid value
-							worker_data->temperature[cur_mag] = new_temperature[cur_mag];
-
-						} else {
-							worker_data->temperature[cur_mag] = 0.5f * (worker_data->temperature[cur_mag] + new_temperature[cur_mag]);
-						}
-
 						worker_data->calibration_counter_total[cur_mag]++;
 					}
 				}
@@ -912,8 +900,6 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
 					current_cal.set_offdiagonal(offdiag[cur_mag]);
 				}

-				current_cal.set_temperature(worker_data.temperature[cur_mag]);
-
 				current_cal.PrintStatus();

 				if (current_cal.ParametersSave(cur_mag, true)) {
@@ -1019,7 +1005,6 @@ int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radian
 				// use any existing scale and store the offset to the expected earth field
 				const Vector3f offset = Vector3f{mag.x, mag.y, mag.z} - (cal.scale().I() * cal.rotation().transpose() * expected_field);
 				cal.set_offset(offset);
-				cal.set_temperature(mag.temperature);

 				// save new calibration
 				if (cal.ParametersSave(cur_mag)) {
@@ -59,6 +59,7 @@ static constexpr const char reason_no_local_position[] = "no local position";
 static constexpr const char reason_no_global_position[] = "no global position";
 static constexpr const char reason_no_datalink[] = "no datalink";
 static constexpr const char reason_no_rc_and_no_datalink[] = "no RC and no datalink";
+static constexpr const char reason_no_gps[] = "no GPS";

 // This array defines the arming state transitions. The rows are the new state, and the columns
 // are the current state. Using new state and current state you can index into the array which
@@ -476,6 +477,8 @@ static void enable_failsafe(vehicle_status_s &status, bool old_failsafe, orb_adv
 		case event_failsafe_reason_t::no_datalink: reason = reason_no_datalink; break;

 		case event_failsafe_reason_t::no_rc_and_no_datalink: reason = reason_no_rc_and_no_datalink; break;
+
+		case event_failsafe_reason_t::no_gps: reason = reason_no_gps; break;
 		}

 		mavlink_log_critical(mavlink_log_pub, "Failsafe enabled: %s\t", reason);
@@ -49,7 +49,7 @@ ActuatorEffectivenessCustom::getEffectivenessMatrix(Configuration &configuration
 	}

 	// motors
-	_motors.enableYawControl(false);
+	_motors.enablePropellerTorque(false);
 	const bool motors_added_successfully = _motors.addActuators(configuration);

 	// Torque
@@ -51,7 +51,7 @@ ActuatorEffectivenessFixedWing::getEffectivenessMatrix(Configuration &configurat
 	}

 	// Motors
-	_rotors.enableYawControl(false);
+	_rotors.enablePropellerTorque(false);
 	const bool rotors_added_successfully = _rotors.addActuators(configuration);

 	// Control Surfaces
@@ -51,7 +51,7 @@ ActuatorEffectivenessMCTilt::getEffectivenessMatrix(Configuration &configuration
 	}

 	// MC motors
-	_mc_rotors.enableYawControl(!_tilts.hasYawControl());
+	_mc_rotors.enablePropellerTorque(!_tilts.hasYawControl());
 	const bool rotors_added_successfully = _mc_rotors.addActuators(configuration);

 	// Tilts
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
JacobCrabill	8e01fe661a	fixup! Update submodule nuttx	2022-03-18 10:56:44 -07:00
JacobCrabill	64863fe54e	uavcan_v1: Add Publishers dir to includes	2022-03-18 09:03:48 -07:00
JacobCrabill	73b1de85cb	uavcan_v1: SocketCAN set 'can_fd' based on CONFIG	2022-03-18 09:03:06 -07:00
JacobCrabill	fd3662097f	fixup! Update submodule nuttx	2022-03-18 09:02:00 -07:00
JacobCrabill	76da12fce3	uavcan_v1: Immediate transmit() from Publishers Add 'do_transmit()' to UavcanNode Add 'transmit()' to Publisher base class to call do_transmit() Call transmit() after canardTxPush from Publishers	2022-03-17 17:01:54 -07:00
JacobCrabill	9531900afa	mro/ctrl-zero-h7-oem: Start adding socketcan support	2022-03-16 10:21:10 -07:00
JacobCrabill	25271a3f04	fixup! Update submodule nuttx	2022-03-16 10:20:29 -07:00
JacobCrabill	4048ac57bf	fixup! Update submodule nuttx	2022-03-15 18:53:13 -07:00
JacobCrabill	399ab35a4f	fixup! HACK: Disable SO_TIMESTAMP in uavcan_v1 temporarily while bringing up H7 SocketCAN driver	2022-03-15 18:52:00 -07:00
JacobCrabill	f4f8eee02d	cubeorange: Add TX_DEADLINE support	2022-03-15 18:24:03 -07:00
JacobCrabill	dec15b4fa9	Update submodule nuttx	2022-03-15 18:23:22 -07:00
JacobCrabill	7db8d3ad78	HACK: Disable SO_TIMESTAMP in uavcan_v1 temporarily while bringing up H7 SocketCAN driver	2022-03-15 17:45:34 -07:00
JacobCrabill	7e34806046	mavlink: Replace CONFIG_NET with CONFIG_NET_UDP Allows use of SocketCAN w/o also enabling UDP support in NuttX	2022-03-15 17:22:27 -07:00
JacobCrabill	9f18ef6688	Orange Cube: Add socketcan target	2022-03-15 17:11:42 -07:00
Daniel Agar	5717434e93	ekf2: initialize GPS drift metrics to NAN rather than 0	2022-03-14 10:09:24 -04:00
alessandro	62d1058cc2	log irlock_report and landing_target_pose messages Need to log both, because on some systems the information will come in directly as a landing_target_pose message, and on others it's coming in as irlock_report and then filtered in PX4 to produce the landing_target_pose message.	2022-03-14 10:13:08 +01:00
Matthias Grob	f1e44c6e2a	PreFlightCheck: only allow modes suitable for takeoff	2022-03-14 09:03:39 +01:00
Daniel Agar	a430f0ccae	ekf2: add simple zero velocity update when vehicle at rest (#19149 ) - further decreases initial tilt alignment time (now down to 2.5 seconds if still) and improves initial bias estimates	2022-03-12 12:56:31 -05:00
Daniel Agar	3d54d25867	sensor calibration delete temperature (CAL_ACCx_TEMP, CAL_GYROx_TEMP, CAL_MAGx_TEMP) - this was an experiment to casually monitor sensor offsets relative to temperature, but now that all calibration offsets can be adjusted post-flight the stored temperature can be misleading - deleting to save a little bit of flash (and storing the temperature wasn't useful)	2022-03-12 11:38:49 -05:00
wangwwno1	aa64789792	sensors/vehicle_imu: Fix Integration Rate greater than Gyro Rate (#19318 )	2022-03-12 11:29:23 -05:00
RomanBapst	58a4c38519	rtl: don't fly mission landing if we trigger rtl in hover Signed-off-by: RomanBapst <bapstroman@gmail.com>	2022-03-11 15:23:31 +03:00
bresch	58bd3d0c60	cmake: use elif -> elseif	2022-03-11 11:59:20 +01:00
JunwooHWANG	04f8453f4a	Reduce Beeepr Default volume : 40 -> 20, since it's too loud for TAP_ESC devices from Yuneec (#19311 )	2022-03-11 10:27:57 +01:00
Silvan Fuhrer	f4c300af25	FlightTaskAuto: Nudging: only set yawrate_sp if WV is disabled or stick out of dead-zone Otherwise the setpoint from weather vane is constantly overwritten by it, even if the yaw stick is not moved. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-03-11 09:18:31 +01:00
Alex Klimaj	71850eeda6	mavlink: Add flow control parameters (#19254 )	2022-03-10 08:22:00 -08:00
Daniel Agar	b66dd5ffa6	adis16470: fix gyro scaling	2022-03-09 23:00:54 -05:00
Roman Bapst	69560bd4f4	Compensate VTOL transition time for air density (#19293 ) * vtol: compensate front transition minimum time and front transiton openloop time for air density Signed-off-by: RomanBapst <bapstroman@gmail.com>	2022-03-09 08:41:30 -05:00
alessandro	0617fd2b6f	fmu-v6x: increase UART5 buffer size The same fix had to be done for the fmu-v5x: PX4/PX4-Autopilot#14932	2022-03-09 08:39:34 -05:00
bresch	182980526f	commander: allow rearming grace period for arming switch only	2022-03-09 08:42:21 +01:00
wangwwno1	cd5a1e510a	ekf2: typo Fix: pub.advertised() -> pub.advertise() (#19302 )	2022-03-09 08:39:08 +01:00
Jacob Dahl	3e21efb721	ina228: fix sign error on CURRENT reading (#19296 )	2022-03-08 19:47:11 -05:00
Daniel Agar	b7e0f17c6a	ekf2: minor position/velocity reset cleanup - try to use avoid resetVelocity() call where possible - reset timeouts centrally	2022-03-08 11:16:35 -05:00
Daniel Agar	c10ea97967	ekf2: fusion helpers return success/fail and set pos/vel update timestamps centrally (if healthy)	2022-03-08 11:16:35 -05:00
Matthias Grob	c4bc062714	helper_functions: generalize unwrapping function	2022-03-08 10:56:32 +01:00
Matthias Grob	c86c2db07f	helper_functions: simplify unwrap function	2022-03-08 10:56:32 +01:00
Thomas Stastny	5a3aba9c21	matrix: add angle unwrapping method	2022-03-08 10:56:32 +01:00
Matthias Grob	68a0414622	Quaternion: rename function to rotate vectors	2022-03-07 20:03:54 -05:00
Daniel Agar	5affa693f2	uavcan: increase ESC max rate 200->400 Hz - this should run synchronized with the rate controller and can be limited by IMU_GYRO_RATEMAX	2022-03-07 10:28:51 -05:00
Beat Küng	601c588294	holybro/kakuteh7: disable bluetooth	2022-03-04 08:02:11 -05:00
Beat Küng	047352d049	holybro/kakuteh7: update bootloader binary	2022-03-04 08:02:11 -05:00
Beat Küng	591c95ce2f	mixer_module: print actual failsafe value	2022-03-04 08:02:11 -05:00
Bulut Gözübüyük	80c6ab7106	Add support for Omnibus F4 boards with ICM20608G IMUs One can use following command to compile: make omnibus_f4sd_icm20608g Co-Authored-By: berkercanatar <19846944+berkercanatar@users.noreply.github.com>	2022-03-04 08:27:48 +01:00
Matthias Grob	666cf2326d	mission_block: handle SET_ROI_LOCATION with absolute altitude correctly (#19258 )	2022-03-03 13:25:00 +01:00
Daniel Agar	49df00c319	lib/mixer_module: check thrust factor range valid and minor optimization (#19272 )	2022-03-03 08:10:09 +01:00
JunwooHWANG	35af604a82	Added RTL Switch Setting for ATL Mantis Edu (#19267 ) Co-authored-by: Junwoo Hwang <junwoo@auterion.com>	2022-03-02 16:57:20 +01:00
Daniel Agar	52221b0bb7	vscode: add stlink debug config (#19269 ) Co-authored-by: Jacob Crabill <jacob.crabill@gmail.com>	2022-03-01 09:47:22 -05:00
PX4 BuildBot	c2c455be0d	Update submodule flightgear_bridge to latest Sat Feb 26 16:47:29 UTC 2022 - flightgear_bridge in PX4/Firmware (`8549fadb6c`): https://github.com/PX4/PX4-FlightGear-Bridge/commit/ea9b6cb5b93365928190864a6592c0a280e101ea - flightgear_bridge current upstream: https://github.com/PX4/PX4-FlightGear-Bridge/commit/f47ce7b5fbbb3aa43d33d2be1f6cd3746b13d5bf - Changes: https://github.com/PX4/PX4-FlightGear-Bridge/compare/ea9b6cb5b93365928190864a6592c0a280e101ea...f47ce7b5fbbb3aa43d33d2be1f6cd3746b13d5bf f47ce7b 2022-02-07 Roman Dvořák - Add support of TF-G1 and TF-G2 to FG bridge, update readme, update mavlink communication Add support of TF-G1 and TF-G2 to FG bridge, update readme, update mavlink communication	2022-02-26 16:57:20 -05:00
Daniel Agar	6224e11463	Update world_magnetic_model to latest Sat Feb 26 16:47:08 UTC 2022 (#19262 ) * Update world_magnetic_model to latest Sat Feb 26 16:47:08 UTC 2022 * [AUTO COMMIT] update change indication Co-authored-by: PX4 BuildBot <bot@px4.io>	2022-02-26 15:39:37 -05:00
Daniel Agar	44f0278d97	Update submodule GPSDrivers to latest Sat Feb 26 16:47:30 UTC 2022 (#19264 ) - GPSDrivers in PX4/Firmware (be9dbf6a077309c4c6bcf8d2de91b82502bf5d01): https://github.com/PX4/PX4-GPSDrivers/commit/d6940d9c8ccb8ab3273c677097a29d46903021ff - GPSDrivers current upstream: https://github.com/PX4/PX4-GPSDrivers/commit/fa275c39935e00906d7a691770d2c10f1ea95d3c - Changes: https://github.com/PX4/PX4-GPSDrivers/compare/d6940d9c8ccb8ab3273c677097a29d46903021ff...fa275c39935e00906d7a691770d2c10f1ea95d3c fa275c3 2022-02-17 chalkytoast - ubx: ensure payloadRxDone does not return -1 Co-authored-by: PX4 BuildBot <bot@px4.io>	2022-02-26 15:38:43 -05:00
PX4 BuildBot	d94ad5bd6d	Update submodule mavlink to latest Sat Feb 26 16:47:38 UTC 2022 - mavlink in PX4/Firmware (95358ae501e92687b00d2786bc8eadf28ff93930): https://github.com/mavlink/mavlink/commit/4ee1eebbd173a3a483b79088b58fef6ec0a4dfef - mavlink current upstream: https://github.com/mavlink/mavlink/commit/b568a60fca42599d9998434e606f6e38e0b5e298 - Changes: https://github.com/mavlink/mavlink/compare/4ee1eebbd173a3a483b79088b58fef6ec0a4dfef...b568a60fca42599d9998434e606f6e38e0b5e298 b568a60f 2022-02-24 Hamish Willee - Update pymavlink to latest (#1805) b86834e0 2022-02-23 Hamish Willee - Component info - tidy descriptiosn (#1803) 79e9545a 2022-02-23 Hamish Willee - Remove external dialect doc and improve dialect pages (#1802) fd7ccfbe 2022-02-23 Hamish Willee - MAV_PROTOCOL_CAPABILITY_FLIGHT_TERMINATION - improve linking to command (#1774) 2b7de26c 2022-02-23 Hamish Willee - [Discuss.] MAV_PROTOCOL_CAPABILITY_FTP - supports FTP protocol not specific message (#1772) a7541658 2022-02-18 Alessandro Ros - fix common.xml include in AVSSUAS (#1800) 5b4482fe 2022-02-17 Hamish Willee - RFC 0016 - Mavlink Standard Modes - Add to development.xml	2022-02-26 15:07:42 -05:00
ShiauweiZhao	8549fadb6c	add icm42670p driver kconfig	2022-02-25 12:21:06 -05:00
Jukka Laitinen	aae0876d82	platforms/rpi: Clean away the removed hrt_elapsed_time_atomic Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2022-02-25 13:35:14 +01:00
Jukka Laitinen	77f71e61d2	Add a generic hrt driver userspace interface This adds a nuttx userspace interface for hrt driver, communicating with the actual px4 hrt driver via BOARDCTL IOCTLs This is be used when running PX4 in NuttX protected or kernel builds Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2022-02-25 13:35:14 +01:00
Jukka Laitinen	9f049b4dca	Inline ts_to_abstime and abstime_to_ts Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2022-02-25 13:35:14 +01:00
Beat Küng	3e6a35fe8a	px4/fmu-v2/rover: disable module to reduce flash	2022-02-25 08:30:58 +01:00
Beat Küng	29d5dd9b8f	omnibus/f4sd: disable module to reduce flash	2022-02-25 08:30:58 +01:00
Beat Küng	bc9dfe8599	holybro/kakutef7: disable module to reduce flash	2022-02-25 08:30:58 +01:00
Beat Küng	ddad4c31c9	control_allocator: compute thrust scaling individually per axis Before, adding the pusher to the same matrix as the upwards motors affected the scaling for the upwards motors, resulting in values not equal to -1 anymore.	2022-02-25 08:30:58 +01:00
Beat Küng	76d8d8cae6	control_allocator: generic motor configuration for standard vtols This adds the pusher/puller to the standard motors and makes the axis configurable.	2022-02-25 08:30:58 +01:00
Beat Küng	b2dc9ee710	control_allocator: add title & help url to geometry UI	2022-02-25 08:30:58 +01:00
Beat Küng	cbcae260e4	mavlink: update submodule	2022-02-25 08:30:58 +01:00
Beat Küng	e10ff59340	px4/fmu-v5/stackcheck: disable module to reduce flash	2022-02-25 08:30:58 +01:00
Beat Küng	dce2968470	ROMFS: set CA_* + HIL_ACT_* params for hitl+sih airframes	2022-02-25 08:30:58 +01:00
Beat Küng	9b629a9e95	hitl,sitl,sih: use separate actuator_outputs_sim for SYS_CTRL_ALLOC==1 - removes the need to do type-specific rescaling of pwm to normalized values - allows to run physical output drivers alongside HIL/SIH	2022-02-25 08:30:58 +01:00
Charles Cross	1e32398217	Adds scheduling call to ControlAllocator initialization	2022-02-25 08:16:08 +01:00
Charles Cross	aecfbef128	Enables 20Hz backup scheduling for ControlAllocator	2022-02-25 08:16:08 +01:00
Charles Cross	b0352135bb	Restores Dshot trigger condition and adds dynamic mixing condition	2022-02-25 08:16:08 +01:00
Charles Cross	51c055832f	Changes ControlAllocator to always publish actuator controls and status	2022-02-25 08:16:08 +01:00
Charles Cross	59f9a40584	Changes Dshot commands to always trigger from updateOutputs(), unless outputs are completely off	2022-02-25 08:16:08 +01:00
Charles Cross	46f8de3a17	Changes actuator_test syscmd value arg to be a float from -1 to 1	2022-02-25 08:16:08 +01:00
Daniel Agar	32a91377bf	Tools/process_sensor_caldata.py fix scipy import	2022-02-24 20:27:26 -05:00
Jukka Laitinen	caaa13ddc0	uORB performance updates Move some logic from Subscriber into uORBManager. This reduces calls from the modules to the uORB manager, improving performance in protected build. Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2022-02-24 11:50:05 -05:00
Daniel Agar	611d50edf3	boards: holybro kakutef7 disable tone_alarm to save flash	2022-02-24 11:33:25 -05:00
Daniel Agar	8de2c80b34	mavlink: SCALED_IMU streams add temperature - preference for accel or gyro temperature before mag	2022-02-23 14:05:31 -05:00
Daniel Agar	5b5d428189	boards: px4_fmu-v5_stackcheck disable fake_gps to save flash	2022-02-23 10:44:09 -05:00
Daniel Agar	01daf8d6d6	boards: omnibus_f4sd disable events module to save flash	2022-02-23 10:42:48 -05:00
Daniel Agar	11f617ca9b	ekf2: default to in air and not at rest - this is a more conservative default if a vehicle isn't set (no land detector running) - handled horizontal preflight failures in commander when disarmed rather than overloading xy_valid and v_xy_valid flags - ekf2 no longer depend on arming or standby status	2022-02-22 16:59:10 -05:00
Daniel Agar	d6d529539d	ekf2: consolidate and simplify gnd effect logic	2022-02-22 16:59:10 -05:00
Daniel Agar	1d7791dad6	commander: monitor GPS validity and EKF2 dead reckoning - ekf2: expose dead reckoning as control status flags - commander: - add GPS validity check - in AUTO MISSION if dependent on GPS then a loss of GPS will	2022-02-22 09:46:21 -05:00
Julian Oes	6021b8efb3	navigator: only apply breaking for multirotors	2022-02-22 09:41:22 -05:00
Julian Oes	fa6c051ae5	navigator: break smoothly when entering hold mode This uses the existing breaking functionality when hold/loiter mode is activated.	2022-02-22 09:41:22 -05:00
Julian Oes	f88dd28e85	navigator: extract breaking functionality out This only moves the breaking functionality out from the reposition command handling to a function, so that it can get re-used in other places.	2022-02-22 09:41:22 -05:00
bresch	0e0e0d8be7	[AUTO COMMIT] update change indication	2022-02-22 11:49:25 +01:00
bresch	4f0a959244	gps_hgt: add and improve unit tests	2022-02-22 11:49:25 +01:00
bresch	edabfd2f0e	ekf_hgt: call specific height reset function instead of generic one - Also use the delayed current data instead of newest that might not be available (gps buffer is sometimes empty if the dt between samples is larger than the delayed horizon). - Separate "baro fault" from "baro intermittent": intermittent is a temporary failure and should prevent from switching to baro right now, but "fault" means that it should never be used anymore - In case of height timeout, check for metrics but not for consistency as the filter is likely to have diverged already.	2022-02-22 11:49:25 +01:00
bresch	cba73585e1	use recorded last sensor timestamp for intermittent check	2022-02-22 11:49:25 +01:00
Daniel Agar	f431b233f3	ekf2: preflight checker fix flow y innovation filter	2022-02-22 09:39:22 +01:00
Daniel Agar	4f62b01dc7	Tools/px4moduledoc: add new rpm_sensor subcategory	2022-02-22 07:57:30 +01:00