drivers/rc/ghst_rc: create new standalone ghst_rc driver

2026-06-06 17:00:04 +08:00 · 2022-02-21 21:49:55 -05:00
195 changed files with 7603 additions and 8828 deletions
@@ -24,8 +24,8 @@
 	branch = master
 [submodule "platforms/nuttx/NuttX/nuttx"]
 	path = platforms/nuttx/NuttX/nuttx
-	url = https://github.com/JacobCrabill/incubator-nuttx.git
-	branch = dev-h7-socketcan
+	url = https://github.com/PX4/NuttX.git
+	branch = px4_firmware_nuttx-10.1.0+
 [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 5
+param set-default CA_ROTOR_COUNT 4
 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,9 +23,6 @@ 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,26 +39,6 @@ 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,23 +15,6 @@ 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,37 +61,6 @@ 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,23 +17,6 @@ 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,26 +17,6 @@ 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,26 +26,6 @@ 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 5
+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
@@ -36,9 +36,6 @@ 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,8 +164,6 @@ 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,11 +330,9 @@ 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',
            }
-        for optional in ['type', 'title']:
-            if optional in current_type:
-                mixer_config[optional] = current_type[optional]
+        if 'type' in current_type:
+            mixer_config['type'] = current_type['type']
        actuators = []
        for actuator_conf in current_type['actuators']:
            actuator = {
@@ -7,8 +7,7 @@ import os
 import math
 import matplotlib.pyplot as plt
 import numpy as np
-
-from scipy.signal import medfilt
+import scipy as sp

 from pyulog import *

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

 def median_filter(data):
-    return medfilt(data, 31)
+    return sp.signal.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', 'rpm_sensor']
+                           'magnetometer', 'baro', 'optical_flow']

    max_line_length = 80 # wrap lines that are longer than this

@@ -1,245 +0,0 @@
-#
-# 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
@@ -1,22 +0,0 @@
-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,6 +15,7 @@ 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
@@ -26,6 +27,7 @@ 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,8 +133,6 @@

 #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 */
@@ -177,7 +175,6 @@
 		PX4_ADC_GPIO,                     \
 		GPIO_TONE_ALARM_IDLE,             \
 		GPIO_RSSI_IN,                     \
-		GPIO_RF_SWITCH,                   \
 	}

 #define BOARD_ENABLE_CONSOLE_BUFFER
@@ -160,9 +160,6 @@ 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();
@@ -1,249 +0,0 @@
-#
-# 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"
@@ -1,13 +0,0 @@
-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,6 +22,7 @@ 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
@@ -33,7 +34,7 @@ CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
-CONFIG_SYSTEMCMDS_DMESG=n
+CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
 CONFIG_SYSTEMCMDS_MIXER=y
 CONFIG_SYSTEMCMDS_PARAM=y
@@ -1,3 +0,0 @@
-CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=n
-CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=n
-CONFIG_DRIVERS_IMU_INVENSENSE_ICM20608G=y
@@ -8,11 +8,7 @@ board_adc start
 if ! mpu6000 -R 6 -s start
 then
 	# some boards such as the Hobbywing XRotor F4 G2 use the ICM-20602
-	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
+	icm20602 -s -R 6 start
 fi

 bmp280 -s start
@@ -39,7 +39,6 @@ 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,6 +12,7 @@ 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,12 +14,10 @@ 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
@@ -38,6 +38,7 @@ CONFIG_DRIVERS_PWM_INPUT=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_PWM_OUT_SIM=y
 CONFIG_DRIVERS_PX4IO=y
+CONFIG_COMMON_RC=y
 CONFIG_DRIVERS_RC_INPUT=y
 CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_RPM=y
@@ -60,6 +61,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
 CONFIG_MODULES_LAND_DETECTOR=y
@@ -81,7 +83,6 @@ CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
@@ -258,7 +258,6 @@ 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)
-elseif(CMAKE_BUILD_TYPE STREQUAL FuzzTesting)
+elif(CMAKE_BUILD_TYPE STREQUAL FuzzTesting)
 	set(ENABLE_LOCKSTEP_SCHEDULER no)
 else()
 	set(ENABLE_LOCKSTEP_SCHEDULER yes)
@@ -3,6 +3,3 @@ 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,8 +33,6 @@ 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 = 20
+uint8 VOLUME_LEVEL_DEFAULT = 40
 uint8 VOLUME_LEVEL_MAX = 100

 uint8 ORB_QUEUE_LENGTH = 4
@@ -13,19 +13,12 @@ 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,40 +136,19 @@ public:
 	/**
 	 * Check if there is a new update.
 	 */
-	bool updated()
-	{
-		if (!valid()) {
-			subscribe();
-		}
-
-		return valid() ? Manager::updates_available(_node, _last_generation) : false;
-	}
+	bool updated() { return advertised() && Manager::updates_available(_node, _last_generation); }

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

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

 	/**
 	 * 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->only_if_updated);
+			data->ret = uORB::Manager::orb_data_copy(data->handle, data->dst, data->generation);
 		}
 		break;

@@ -479,16 +479,8 @@ 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 only_if_updated)
+bool uORB::Manager::orb_data_copy(void *node_handle, void *dst, unsigned &generation)
 {
-	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);
 }

@@ -517,8 +509,7 @@ 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 is_advertised(node_handle) ? static_cast<const uORB::DeviceNode *>(node_handle)->updates_available(
-		       last_generation) : 0;
+	return static_cast<const uORB::DeviceNode *>(node_handle)->updates_available(last_generation);
 }

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

@@ -451,7 +450,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, bool only_if_updated);
+	static bool orb_data_copy(void *node_handle, void *dst, unsigned &generation);

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

@@ -461,10 +460,9 @@ 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 is_advertised(node_handle) ? static_cast<const DeviceNode *>(node_handle)->updates_available(last_generation) : 0; }
+	static unsigned updates_available(const void *node_handle, unsigned last_generation) { return static_cast<const DeviceNode *>(node_handle)->updates_available(last_generation); }

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

@@ -21,24 +21,6 @@
                "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,7 +8,6 @@ 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
@@ -1,221 +0,0 @@
-/****************************************************************************
- *
- *   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,6 +561,31 @@ 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,6 +573,31 @@ 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,6 +609,31 @@ 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,6 +497,43 @@ 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,33 +72,6 @@
 #  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 */
@@ -302,8 +275,6 @@ 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
 */
@@ -723,6 +694,31 @@ 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
 */
@@ -747,16 +743,6 @@ 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
 }

 /**
@@ -1017,81 +1003,6 @@ 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,6 +144,19 @@ 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.
 *
@@ -471,6 +484,13 @@ 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,10 +47,6 @@
 #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

 /**
@@ -80,10 +76,6 @@ 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;


@@ -97,35 +89,6 @@ 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).
 */
@@ -134,25 +97,12 @@ __EXPORT extern hrt_abstime hrt_absolute_time(void);
 /**
 * Convert a timespec to absolute time.
 */
-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;
-}
+__EXPORT extern hrt_abstime ts_to_abstime(const struct timespec *ts);

 /**
 * Convert absolute time to a timespec.
 */
-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;
-}
+__EXPORT extern void	abstime_to_ts(struct timespec *ts, hrt_abstime abstime);

 /**
 * 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 || _mixing_output.useDynamicMixing()) {
+	if (stop_motors || num_control_groups_updated > 0) {
 		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(1.f / 0.1f)); // 1 LSB = 0.1°/sec
+	_px4_gyro.set_scale(math::radians(2000.f / 32768.f));
 	_px4_gyro.set_range(math::radians(2000.f));

 	_px4_accel.set_scale(1.25f * CONSTANTS_ONE_G / 1000.0f); // accel 1.25 mg/LSB
@@ -1,5 +0,0 @@
-menuconfig DRIVERS_IMU_INVENSENSE_ICM42670P
-	bool "icm42670p"
-	default n
-	---help---
-		Enable support for icm42670p
@@ -124,12 +124,7 @@ 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); // Convert to 20bit value
-
-		// Handle negative 20bit twos complement
-		if (data & 0x80000) {
-			data = -((0x000FFFFF & ~data) + 1);
-		}
+		data = swap32(received_bytes) >> ((32 - 24) + 4);

 	} else {
 		perf_count(_comms_errors);
@@ -81,40 +81,9 @@ bool
 PWMSim::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
 		      unsigned num_control_groups_updated)
 {
-	// 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;
+	// 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;
 }

 int
@@ -43,9 +43,6 @@
 #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"
@@ -89,7 +86,5 @@ 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)};
 };

@@ -0,0 +1,9 @@
+menu "RC"
+    menuconfig COMMON_RC
+        bool "Common RC"
+        default n
+        select DRIVERS_RC_GHST_RC
+        ---help---
+            Enable default set of magnetometer drivers
+    rsource "*/Kconfig"
+endmenu
@@ -0,0 +1,47 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+px4_add_module(
+	MODULE drivers__rc__ghst_rc
+	MAIN ghst_rc
+	COMPILE_FLAGS
+	SRCS
+		ghst_telemetry.cpp
+		ghst_telemetry.hpp
+		GhstRc.cpp
+		GhstRc.hpp
+
+	MODULE_CONFIG
+		module.yaml
+	DEPENDS
+		rc
+	)
@@ -0,0 +1,241 @@
+/****************************************************************************
+ *
+ *   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 "GhstRc.hpp"
+
+#include <poll.h>
+#include <termios.h>
+
+using namespace time_literals;
+
+GhstRc::GhstRc(const char *device) :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(device))
+{
+	if (device) {
+		strncpy(_device, device, sizeof(_device) - 1);
+		_device[sizeof(_device) - 1] = '\0';
+	}
+}
+
+GhstRc::~GhstRc()
+{
+	delete _ghst_telemetry;
+
+	perf_free(_cycle_interval_perf);
+	perf_free(_publish_interval_perf);
+}
+
+int GhstRc::task_spawn(int argc, char *argv[])
+{
+	bool error_flag = false;
+
+	int myoptind = 1;
+	int ch;
+	const char *myoptarg = nullptr;
+	const char *device = nullptr;
+
+	while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'd':
+			device = myoptarg;
+			break;
+
+		case '?':
+			error_flag = true;
+			break;
+
+		default:
+			PX4_WARN("unrecognized flag");
+			error_flag = true;
+			break;
+		}
+	}
+
+	if (error_flag) {
+		return -1;
+	}
+
+	if (device == nullptr) {
+		PX4_ERR("valid device required");
+		return PX4_ERROR;
+	}
+
+	GhstRc *instance = new GhstRc(device);
+
+	if (instance == nullptr) {
+		PX4_ERR("alloc failed");
+		return PX4_ERROR;
+	}
+
+	_object.store(instance);
+	_task_id = task_id_is_work_queue;
+
+	instance->ScheduleNow();
+
+	return PX4_OK;
+}
+
+void GhstRc::Run()
+{
+	if (should_exit()) {
+		ScheduleClear();
+		::close(_rc_fd);
+		exit_and_cleanup();
+		return;
+	}
+
+	if (_rc_fd < 0) {
+		_rc_fd = ::open(_device, O_RDWR | O_NONBLOCK);
+	}
+
+	// poll with 3 second timeout
+	pollfd fds[1];
+	fds[0].fd = _rc_fd;
+	fds[0].events = POLLIN;
+	::poll(fds, 1, 3000);
+
+	perf_count(_cycle_interval_perf);
+
+	const hrt_abstime cycle_timestamp = hrt_absolute_time();
+
+	// read all available data from the serial RC input UART
+	int new_bytes = ::read(_rc_fd, &_rcs_buf[0], RC_MAX_BUFFER_SIZE);
+
+	if (new_bytes > 0) {
+		_bytes_rx += new_bytes;
+	}
+
+	if (_rc_scan_begin == 0) {
+		_rc_scan_begin = cycle_timestamp;
+		// Configure serial port for GHST
+		ghst_config(_rc_fd);
+	}
+
+	if (_rc_locked || (cycle_timestamp - _rc_scan_begin < 300_ms)) {
+
+	}
+
+	// parse new data
+	if (new_bytes > 0) {
+		uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS] {};
+		uint16_t raw_rc_count = 0;
+		int8_t ghst_rssi = -1;
+		bool rc_updated = ghst_parse(cycle_timestamp, &_rcs_buf[0], new_bytes, &raw_rc_values[0], &ghst_rssi, &raw_rc_count,
+					     input_rc_s::RC_INPUT_MAX_CHANNELS);
+
+		if (rc_updated) {
+			if (!_rc_locked) {
+				_rc_locked = true;
+				PX4_INFO("RC input locked");
+			}
+
+			// we have a new GHST frame. Publish it.
+			input_rc_s input_rc{};
+			input_rc.timestamp_last_signal = cycle_timestamp;
+			input_rc.channel_count = math::max(raw_rc_count, (uint16_t)input_rc_s::RC_INPUT_MAX_CHANNELS);
+			input_rc.rssi = ghst_rssi;
+			input_rc.rc_lost = (raw_rc_count == 0);
+			input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_GHST;
+
+			for (int i = 0; i < input_rc.channel_count; i++) {
+				input_rc.values[i] = raw_rc_values[i];
+			}
+
+			input_rc.timestamp = hrt_absolute_time();
+			_input_rc_pub.publish(input_rc);
+			perf_count(_publish_interval_perf);
+
+			if (!_ghst_telemetry) {
+				_ghst_telemetry = new GHSTTelemetry(_rc_fd);
+			}
+
+			if (_ghst_telemetry) {
+				_ghst_telemetry->update(cycle_timestamp);
+			}
+		}
+	}
+
+	ScheduleDelayed(4_ms);
+}
+
+int GhstRc::print_status()
+{
+	if (_device[0] != '\0') {
+		PX4_INFO("UART device: %s", _device);
+		PX4_INFO("UART RX bytes: %"  PRIu32, _bytes_rx);
+	}
+
+	PX4_INFO("RC state: %s", _rc_locked ? "found" : "searching for signal");
+
+	if (_rc_locked) {
+		PX4_INFO("Telemetry: %s", _ghst_telemetry ? "yes" : "no");
+	}
+
+	perf_print_counter(_cycle_interval_perf);
+	perf_print_counter(_publish_interval_perf);
+
+	return 0;
+}
+
+int GhstRc::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int GhstRc::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+This module does the GHST RC input parsing.
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("ghst_rc", "driver");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/ttyS3", "<file:dev>", "RC device", true);
+
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+extern "C" __EXPORT int ghst_rc_main(int argc, char *argv[])
+{
+	return GhstRc::main(argc, argv);
+}
@@ -31,39 +31,62 @@
 *
 ****************************************************************************/

-#include "../PreFlightCheck.hpp"
+#pragma once

-#include <systemlib/mavlink_log.h>
+#include <float.h>

-using namespace time_literals;
+#include <drivers/drv_hrt.h>
+#include <lib/perf/perf_counter.h>
+#include <lib/rc/ghst.hpp>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/log.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionInterval.hpp>
+#include <uORB/topics/input_rc.h>

-bool PreFlightCheck::modeCheck(orb_advert_t *mavlink_log_pub, const bool report_fail,
-			       const vehicle_status_s &vehicle_status)
+#include "ghst_telemetry.hpp"
+
+class GhstRc : public ModuleBase<GhstRc>, public ModuleParams, public px4::ScheduledWorkItem
 {
-	bool success = true;
+public:
+	GhstRc(const char *device);
+	~GhstRc() override;

-	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;
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);

-	default:
-		success = false;
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);

-		if (report_fail) {
-			mavlink_log_critical(mavlink_log_pub, "Mode not suitable for takeoff");
-		}
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);

-		break;
-	}
+	/** @see ModuleBase::print_status() */
+	int print_status() override;

-	return success;
-}
+private:
+	void Run() override;
+
+	hrt_abstime _rc_scan_begin{0};
+
+	bool _rc_locked{false};
+
+	uORB::PublicationMulti<input_rc_s> _input_rc_pub{ORB_ID(input_rc)};
+
+	int _rc_fd{-1};
+	char _device[20] {}; ///< device / serial port path
+
+	static constexpr size_t RC_MAX_BUFFER_SIZE{64};
+	uint8_t _rcs_buf[RC_MAX_BUFFER_SIZE] {};
+	uint32_t _bytes_rx{0};
+
+	GHSTTelemetry *_ghst_telemetry{nullptr};
+
+	perf_counter_t	_cycle_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")};
+	perf_counter_t	_publish_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": publish interval")};
+};
@@ -0,0 +1,5 @@
+menuconfig DRIVERS_RC_GHST_RC
+	bool "ghst_rc"
+	default n
+	---help---
+		Enable support for ghst rc
@@ -41,6 +41,7 @@
 */

 #include "ghst_telemetry.hpp"
+
 #include <lib/rc/ghst.hpp>

 using time_literals::operator ""_s;
@@ -0,0 +1,11 @@
+module_name: GHST RC Input Driver
+serial_config:
+    - command: "ghst_rc start -d ${SERIAL_DEV}"
+      port_config_param:
+        name: GHST_RC_PRT_CFG
+        group: Serial
+        #default: RC
+        #depends_on_port: RC
+        description_extended: |
+            Ghost RC input driver.
+
@@ -37,7 +37,6 @@ px4_add_module(
 	SRCS
 		RCInput.cpp
 		crsf_telemetry.cpp
-		ghst_telemetry.cpp
 	MODULE_CONFIG
 		module.yaml
 	DEPENDS
@@ -74,7 +74,6 @@ RCInput::~RCInput()
 	dsm_deinit();

 	delete _crsf_telemetry;
-	delete _ghst_telemetry;

 	perf_free(_cycle_perf);
 	perf_free(_publish_interval_perf);
@@ -665,56 +664,6 @@ void RCInput::Run()
 					}
 				}

-			} else {
-				// Scan the next protocol
-				set_rc_scan_state(RC_SCAN_GHST);
-			}
-
-			break;
-
-		case RC_SCAN_GHST:
-			if (_rc_scan_begin == 0) {
-				_rc_scan_begin = cycle_timestamp;
-				// Configure serial port for GHST
-				ghst_config(_rcs_fd);
-
-				// flush serial buffer and any existing buffered data
-				tcflush(_rcs_fd, TCIOFLUSH);
-				memset(_rcs_buf, 0, sizeof(_rcs_buf));
-
-			} else if (_rc_scan_locked
-				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
-
-				// parse new data
-				if (newBytes > 0) {
-					int8_t ghst_rssi = -1;
-					rc_updated = ghst_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &ghst_rssi,
-								&_raw_rc_count, input_rc_s::RC_INPUT_MAX_CHANNELS);
-
-					if (rc_updated) {
-						// we have a new GHST frame. Publish it.
-						_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_GHST;
-						fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, false, 0, ghst_rssi);
-
-						// ghst telemetry works on fmu-v5
-						// on other Pixhawk (-related) boards we cannot write to the RC UART
-						// another option is to use a different UART port
-#ifdef BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT
-
-						if (!_rc_scan_locked && !_ghst_telemetry) {
-							_ghst_telemetry = new GHSTTelemetry(_rcs_fd);
-						}
-
-#endif /* BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT */
-
-						_rc_scan_locked = true;
-
-						if (_ghst_telemetry) {
-							_ghst_telemetry->update(cycle_timestamp);
-						}
-					}
-				}
-
 			} else {
 				// Scan the next protocol
 				set_rc_scan_state(RC_SCAN_SBUS);
@@ -827,10 +776,6 @@ int RCInput::print_status()
 			PX4_INFO("CRSF Telemetry: %s", _crsf_telemetry ? "yes" : "no");
 			break;

-		case RC_SCAN_GHST:
-			PX4_INFO("GHST Telemetry: %s", _ghst_telemetry ? "yes" : "no");
-			break;
-
 		case RC_SCAN_SBUS:
 			PX4_INFO("SBUS frame drops: %u", sbus_dropped_frames());
 			break;
@@ -40,7 +40,6 @@
 #include <drivers/drv_hrt.h>
 #include <lib/perf/perf_counter.h>
 #include <lib/rc/crsf.h>
-#include <lib/rc/ghst.hpp>
 #include <lib/rc/dsm.h>
 #include <lib/rc/sbus.h>
 #include <lib/rc/st24.h>
@@ -61,7 +60,6 @@
 #include <uORB/topics/vehicle_status.h>

 #include "crsf_telemetry.h"
-#include "ghst_telemetry.hpp"

 #ifdef HRT_PPM_CHANNEL
 # include <systemlib/ppm_decode.h>
@@ -97,7 +95,6 @@ private:
 		RC_SCAN_SUMD,
 		RC_SCAN_ST24,
 		RC_SCAN_CRSF,
-		RC_SCAN_GHST
 	} _rc_scan_state{RC_SCAN_SBUS};

 	static constexpr char const *RC_SCAN_STRING[7] {
@@ -107,7 +104,6 @@ private:
 		"SUMD",
 		"ST24",
 		"CRSF",
-		"GHST"
 	};

 	void Run() override;
@@ -159,7 +155,6 @@ private:
 	uint16_t _raw_rc_count{};

 	CRSFTelemetry *_crsf_telemetry{nullptr};
-	GHSTTelemetry *_ghst_telemetry{nullptr};

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

 	static_assert(uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize >= MAX_ACTUATORS, "Too many actuators");
@@ -112,7 +112,6 @@ px4_add_module(
 	INCLUDES
 		${LIBCANARD_DIR}/libcanard/
 		${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated
-		${CMAKE_CURRENT_SOURCE_DIR}/Publishers
 	SRCS
 		PublicationManager.cpp
 		PublicationManager.hpp
@@ -125,7 +124,6 @@ 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,11 +56,8 @@ 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,9 +45,7 @@
 #include <reg/drone/physics/kinematics/geodetic/Point_0_1.h>
 #include <reg/drone/service/gnss/DilutionOfPrecision_0_1.h>

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

 class UavcanGnssPublisher : public UavcanPublisher
 {
@@ -126,8 +124,6 @@ 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,8 +95,6 @@ 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();
 			}
 		}
 	};
@@ -1,49 +0,0 @@
-/****************************************************************************
- *
- *   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,6 +142,4 @@ protected:
 	CanardTransferID _transfer_id {0};

 	UavcanPublisher *_next_pub {nullptr};
-
-	void transmit();
 };
@@ -81,7 +81,6 @@ 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,8 +140,6 @@ 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,10 +110,6 @@
                        "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> rotateVector(const Vector3<Type> &vec) const
+	Vector3<Type> conjugate(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> rotateVectorInverse(const Vector3<Type> &vec) const
+	Vector3<Type> conjugate_inversed(const Vector3<Type> &vec) const
 	{
 		const Quaternion &q = *this;
 		Quaternion v(Type(0), vec(0), vec(1), vec(2));
@@ -123,35 +123,6 @@ 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,4 +44,3 @@ 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.rotateVector(v), v));
+	TEST(isEqual(quat_v.conjugate(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.rotateVector(v1).normalized() * v.norm(), v));
+	TEST(isEqual(quat_v.conjugate(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.rotateVector(v1), -v1));
+	TEST(isEqual(quat_v.conjugate(v1), -v1));
 	// special 180 degree case 2
 	v1 = Vector3f(1.f, 2.f, 0.f);
 	quat_v = Quatf(v1, -v1);
-	TEST(isEqual(quat_v.rotateVector(v1), -v1));
+	TEST(isEqual(quat_v.conjugate(v1), -v1));
 	// special 180 degree case 3
 	v1 = Vector3f(0.f, 0.f, 1.f);
 	quat_v = Quatf(v1, -v1);
-	TEST(isEqual(quat_v.rotateVector(v1), -v1));
+	TEST(isEqual(quat_v.conjugate(v1), -v1));
 	// special 180 degree case 4
 	v1 = Vector3f(1.f, 1.f, 1.f);
 	quat_v = Quatf(v1, -v1);
-	TEST(isEqual(quat_v.rotateVector(v1), -v1));
+	TEST(isEqual(quat_v.conjugate(v1), -v1));

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

 	// conjugate
 	v = Vector3f(1.5f, 2.2f, 3.2f);
-	TEST(isEqual(q.rotateVectorInverse(v1), Dcmf(q).T()*v1));
-	TEST(isEqual(q.rotateVector(v1), Dcmf(q)*v1));
+	TEST(isEqual(q.conjugate_inversed(v1), Dcmf(q).T()*v1));
+	TEST(isEqual(q.conjugate(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.rotateVector(velocityStateBody) * dt);
+	return positionMeasurement - (positionState +  bodyOrientation.conjugate(velocityStateBody) * dt);
 }

 int main()
@@ -1,62 +0,0 @@
-#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,28 +140,12 @@ private:

 void FunctionMotors::updateValues(uint32_t reversible, float thrust_factor, float *values, int num_values)
 {
-	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);
-
+	if (thrust_factor > FLT_EPSILON) {
 		for (int i = 0; i < num_values; ++i) {
 			float control = values[i];
-
-			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;
-			}
+			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;
 		}
 	}

@@ -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: %3i, value: %i, failsafe: %d, disarmed: %d, min: %d, max: %d\n", i,
+			PX4_INFO_RAW("Channel %i: func: %i, value: %i, failsafe: %d, disarmed: %d, min: %d, max: %d\n", i,
 				     (int)_function_assignment[i], _current_output_value[i],
-				     actualFailsafeValue(i), _disarmed_value[i], _min_value[i], _max_value[i]);
+				     _failsafe_value[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) const
+MixingOutput::actualFailsafeValue(int index)
 {
 	if (!_use_dynamic_mixing) {
-		return _failsafe_value[index];
+		return failsafeValue(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) const;
+	uint16_t actualFailsafeValue(int index);

 	/**
 	 * Get the motor index that maps from PX4 convention to the configured one
@@ -207,6 +207,16 @@ 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));

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

 	_thermal_offset.zero();
+	_temperature = NAN;

 	_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;

@@ -274,6 +285,13 @@ 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;
 	}

@@ -284,18 +302,20 @@ 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], Ext ROT: %d\n",
+			     " EN: %d, offset: [%05.3f %05.3f %05.3f], scale: [%05.3f %05.3f %05.3f], %.1f degC, 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], Internal\n",
+			     " EN: %d, offset: [%05.3f %05.3f %05.3f], scale: [%05.3f %05.3f %05.3f], %.1f degC, 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)_scale(0), (double)_scale(1), (double)_scale(2),
+			     (double)_temperature);
 	}

 	if (_thermal_offset.norm() > 0.f) {
@@ -64,6 +64,7 @@ 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; }
@@ -99,6 +100,8 @@ 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};
@@ -107,6 +110,7 @@ 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,6 +192,16 @@ 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));

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

 	_thermal_offset.zero();
+	_temperature = NAN;

 	_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;

@@ -254,6 +265,13 @@ 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;
 	}

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

 	if (_thermal_offset.norm() > 0.f) {
@@ -63,6 +63,7 @@ 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; }
@@ -103,6 +104,8 @@ 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,6 +113,7 @@ 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,6 +192,16 @@ 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));

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

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

 	_calibration_index = -1;
@@ -273,6 +285,13 @@ 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;
 	}

@@ -283,18 +302,20 @@ 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], Ext ROT: %d\n",
+			     " EN: %d, offset: [%05.3f %05.3f %05.3f], scale: [%05.3f %05.3f %05.3f], %.1f degC, 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], Internal\n",
+			     " EN: %d, offset: [%05.3f %05.3f %05.3f], scale: [%05.3f %05.3f %05.3f], %.1f degC, 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)_scale(0, 0), (double)_scale(1, 1), (double)_scale(2, 2),
+			     (double)_temperature);
 	}

 #if defined(DEBUG_BUILD)
@@ -66,6 +66,7 @@ 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; }
@@ -101,6 +102,8 @@ public:
 	void UpdatePower(float power) { _power = power; }

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

 	matrix::Dcmf _rotation;
@@ -108,6 +111,7 @@ 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,6 +149,21 @@ 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,6 +71,7 @@ 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.1534,
+// Date: 2022.0082,
 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 */ {  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, },
+	/* 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, },
 };

 // Magnetic inclination data in radians * 10^-4
 // Model: WMM-2020,
 // Version: 0.5.1.11,
-// Date: 2022.1534,
+// Date: 2022.0082,
 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 */ { -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 */ { -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 */ {  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.1534,
+// Date: 2022.0082,
 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 */ {  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, },
+	/* 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, },
 };
@@ -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.rotateVectorInverse((vel - _vel_prev) / vel_dt);
+						_pos_acc = _q.conjugate_inversed((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.rotateVector(_vision_hdg);
+			Vector3f vision_hdg_earth = _q.conjugate(_vision_hdg);
 			float vision_hdg_err = wrap_pi(atan2f(vision_hdg_earth(1), vision_hdg_earth(0)));
 			// Project correction to body frame
-			corr += _q.rotateVectorInverse(Vector3f(0.0f, 0.0f, -vision_hdg_err)) * _param_att_w_ext_hdg.get();
+			corr += _q.conjugate_inversed(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.rotateVector(_mocap_hdg);
+			Vector3f mocap_hdg_earth = _q.conjugate(_mocap_hdg);
 			float mocap_hdg_err = wrap_pi(atan2f(mocap_hdg_earth(1), mocap_hdg_earth(0)));
 			// Project correction to body frame
-			corr += _q.rotateVectorInverse(Vector3f(0.0f, 0.0f, -mocap_hdg_err)) * _param_att_w_ext_hdg.get();
+			corr += _q.conjugate_inversed(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.rotateVector(_mag);
+		Vector3f mag_earth = _q.conjugate(_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.rotateVectorInverse(Vector3f(0.0f, 0.0f, -mag_err)) * _param_att_w_mag.get() * gainMult;
+		corr += _q.conjugate_inversed(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.rotateVectorInverse(Vector3f(0.0f, 0.0f, 1.0f));
+	// Vector3f k = _q.conjugate_inversed(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,7 +47,6 @@ 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,7 +271,6 @@ 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);

--- a/Show More
+++ b/Show More