[RFC] control_allocator: custom configuration parameters

sensors/vehicle_angular_velocity: gyro RPM dynamic notch filter handle negative RPM
- some UAVCAN ESCs report negative RPM for reverse rotation - lower hard coded safety limit RPM limit to 10 Hz (600 RPM) - avoid disabling notch filters that weren't configured
2026-06-05 10:30:06 +08:00 · 2021-11-02 22:08:17 -04:00 · 2021-11-02 09:36:02 -04:00 · 2021-11-02 09:34:21 -04:00 · 2021-11-02 10:21:54 +01:00 · 2021-11-01 13:59:34 +01:00
162 changed files with 9804 additions and 6789 deletions
@@ -10,6 +10,7 @@ on:
 jobs:
  enumerate_targets:
    runs-on: ubuntu-latest
+    container: px4io/px4-dev-base-focal:2021-09-08
    outputs:
      matrix: ${{ steps.set-matrix.outputs.matrix }}
    steps:
@@ -128,6 +128,9 @@ config BOARD_KEYSTORE

 menu "Serial ports"

+    config BOARD_SERIAL_URT6
+        string "URT6 tty port"
+
    config BOARD_SERIAL_GPS1
        string "GPS1 tty port"

@@ -158,8 +161,14 @@ menu "Serial ports"
    config BOARD_SERIAL_TEL5
        string "TEL5 tty port"

+    config BOARD_SERIAL_RC
+        string "RC tty port"
+
    config BOARD_SERIAL_WIFI
        string "WIFI tty port"
+
+    config BOARD_SERIAL_PPB
+        string "PPB (Pixhawk Payload Bus) tty port"
 endmenu

 menu "drivers"
@@ -84,6 +84,8 @@ param set-default MPC_Z_VEL_P_ACC 5
 param set-default MPC_Z_VEL_I_ACC 3
 param set-default MPC_LAND_ALT1 3
 param set-default MPC_LAND_ALT2 1
+param set-default MPC_POS_MODE 3
+param set-default CP_GO_NO_DATA 1

 # Navigator Parameters
 param set-default NAV_ACC_RAD 2
@@ -16,6 +16,12 @@ ekf2 start &
 fw_att_control start
 fw_pos_control_l1 start
 airspeed_selector start
+
+#
+# Start attitude control auto-tuner
+#
+fw_autotune_attitude_control start
+
 #
 # Start Land Detector.
 #
@@ -24,9 +24,11 @@ mc_att_control start vtol
 flight_mode_manager start
 mc_pos_control start vtol
 mc_hover_thrust_estimator start
+mc_autotune_attitude_control start

 fw_att_control start vtol
 fw_pos_control_l1 start vtol
+fw_autotune_attitude_control start vtol

 # Start Land Detector
 # Multicopter for now until we have something for VTOL
@@ -1,6 +1,12 @@
 #!/usr/bin/env bash
 set -eu

+PATTERN="-e ."
+
+if [ $# -gt 0 ]; then
+    PATTERN="$1"
+fi
+
 exec find boards msg src platforms test \
    -path msg/templates/urtps -prune -o \
    -path platforms/nuttx/NuttX -prune -o \
@@ -21,4 +27,4 @@ exec find boards msg src platforms test \
    -path src/lib/crypto/monocypher -prune -o \
    -path src/lib/crypto/libtomcrypt -prune -o \
    -path src/lib/crypto/libtommath -prune -o \
-    -type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \)
+    -type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \) | grep $PATTERN
@@ -6,6 +6,14 @@ import os
 import sys
 import json
 import re
+from kconfiglib import Kconfig
+
+kconf = Kconfig()
+
+# Supress warning output
+kconf.warn_assign_undef = False
+kconf.warn_assign_override = False
+kconf.warn_assign_redun = False

 source_dir = os.path.join(os.path.dirname(os.path.abspath(__file__)), '..')

@@ -28,42 +36,43 @@ excluded_labels = [
    'uavcanv1' # TODO: fix and enable
    ]

-def process_target(cmake_file, target_name):
+def process_target(px4board_file, target_name):
    ret = None
-    is_board_def = False
    platform = None
    toolchain = None
-    for line in open(cmake_file, 'r'):
-        if 'px4_add_board' in line:
-            is_board_def = True
-        if not is_board_def:
-            continue

-        re_platform = re.search('PLATFORM\s+([^\s]+)', line)
-        if re_platform: platform = re_platform.group(1)
+    if px4board_file.endswith("default.px4board"):
+        kconf.load_config(px4board_file, replace=True)
+    else: # Merge config with default.px4board
+        default_kconfig = re.sub(r'[a-zA-Z\d_]+\.px4board', 'default.px4board', px4board_file)
+        kconf.load_config(default_kconfig, replace=True)
+        kconf.load_config(px4board_file, replace=False)

-        re_toolchain = re.search('TOOLCHAIN\s+([^\s]+)', line)
-        if re_toolchain: toolchain = re_toolchain.group(1)
+    if "BOARD_TOOLCHAIN" in kconf.syms:
+        toolchain = kconf.syms["BOARD_TOOLCHAIN"].str_value

-    if is_board_def:
-        assert platform, f"PLATFORM not found in {cmake_file}"
+    if "BOARD_PLATFORM" in kconf.syms:
+        platform = kconf.syms["BOARD_PLATFORM"].str_value

-        if platform not in excluded_platforms:
-            # get the container based on the platform and toolchain
-            container = platform
-            if platform == 'posix':
-                container = 'base-focal'
-                if toolchain:
-                    if toolchain.startswith('aarch64'):
-                        container = 'aarch64'
-                    elif toolchain == 'arm-linux-gnueabihf':
-                        container = 'armhf'
-                    else:
-                        if verbose: print(f'possibly unmatched toolchain: {toolchain}')
-            elif platform == 'nuttx':
-                container = 'nuttx-focal'
+    assert platform, f"PLATFORM not found in {px4board_file}"
+
+    if platform not in excluded_platforms:
+        # get the container based on the platform and toolchain
+        container = platform
+        if platform == 'posix':
+            container = 'base-focal'
+            if toolchain:
+                if toolchain.startswith('aarch64'):
+                    container = 'aarch64'
+                elif toolchain == 'arm-linux-gnueabihf':
+                    container = 'armhf'
+                else:
+                    if verbose: print(f'possibly unmatched toolchain: {toolchain}')
+        elif platform == 'nuttx':
+            container = 'nuttx-focal'
+
+        ret = {'target': target_name, 'container': container}

-            ret = {'target': target_name, 'container': container}
    return ret

 for manufacturer in os.scandir(os.path.join(source_dir, 'boards')):
@@ -77,8 +86,8 @@ for manufacturer in os.scandir(os.path.join(source_dir, 'boards')):
        if not board.is_dir():
            continue
        for files in os.scandir(board.path):
-            if files.is_file() and files.name.endswith('.cmake'):
-                label = files.name[:-6]
+            if files.is_file() and files.name.endswith('.px4board'):
+                label = files.name[:-9]
                target_name = manufacturer.name + '_' + board.name + '_' + label
                if label in excluded_labels:
                    if verbose: print(f'excluding label {label} ({target_name})')
@@ -110,10 +110,17 @@ def main(kconfig_file, config1, config2):

    for line in f:
        match = unset_match(line)
-        #pprint.pprint(match)
+        #pprint.pprint(line)
        if match is not None:
            sym_name = match.group(1)
            kconf.syms[sym_name].unset_value()
+
+            if kconf.syms[sym_name].type is BOOL:
+                for default, cond in kconf.syms[sym_name].orig_defaults:
+                    if(cond.str_value == 'y'):
+                        # Default is y, our diff is unset thus we've set it to no
+                        kconf.syms[sym_name].set_value(0)
+
    f.close()

    # Print warnings for symbols whose actual value doesn't match the assigned
@@ -48,6 +48,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 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_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
@@ -1,6 +1,5 @@
 CONFIG_BOARD_CONSTRAINED_FLASH=n
 CONFIG_BOARD_NO_HELP=n
-CONFIG_BOARD_CONSTRAINED_MEMORY=y
-CONFIG_SYSTEMCMDS_TOP=y
 CONFIG_SYSTEMCMDS_REBOOT=y
+CONFIG_SYSTEMCMDS_TOP=y
 CONFIG_SYSTEMCMDS_UORB=y
@@ -45,6 +45,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 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_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
@@ -29,6 +29,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 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_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
@@ -53,6 +53,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 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_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
@@ -38,6 +38,7 @@ 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
@@ -71,6 +72,7 @@ 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
@@ -83,7 +85,6 @@ CONFIG_IDLETHREAD_STACKSIZE=750
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_STRERROR=y
-CONFIG_FS_PROCFS_MAX_TASKS=64
 CONFIG_MEMSET_64BIT=y
 CONFIG_MEMSET_OPTSPEED=y
 CONFIG_MMCSD=y
@@ -129,16 +129,6 @@ __EXPORT void board_on_reset(int status)
 ************************************************************************************/
 __EXPORT void stm32_boardinitialize(void)
 {
-	// clear all existing MPU configuration from bootloader
-	for (int region = 0; region < CONFIG_ARM_MPU_NREGIONS; region++) {
-		putreg32(region, MPU_RNR);
-		putreg32(0, MPU_RBAR);
-		putreg32(0, MPU_RASR);
-
-		// save
-		putreg32(0, MPU_CTRL);
-	}
-
 	/* Reset PWM first thing */
 	board_on_reset(-1);

@@ -54,6 +54,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 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_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
@@ -38,6 +38,7 @@ 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
@@ -71,6 +72,7 @@ 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
@@ -83,7 +85,6 @@ CONFIG_IDLETHREAD_STACKSIZE=750
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_STRERROR=y
-CONFIG_FS_PROCFS_MAX_TASKS=64
 CONFIG_MEMSET_64BIT=y
 CONFIG_MEMSET_OPTSPEED=y
 CONFIG_MMCSD=y
@@ -129,16 +129,6 @@ __EXPORT void board_on_reset(int status)
 ************************************************************************************/
 __EXPORT void stm32_boardinitialize(void)
 {
-	// clear all existing MPU configuration from bootloader
-	for (int region = 0; region < CONFIG_ARM_MPU_NREGIONS; region++) {
-		putreg32(region, MPU_RNR);
-		putreg32(0, MPU_RBAR);
-		putreg32(0, MPU_RASR);
-
-		// save
-		putreg32(0, MPU_CTRL);
-	}
-
 	/* Reset PWM first thing */
 	board_on_reset(-1);

@@ -49,6 +49,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 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_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
@@ -39,6 +39,7 @@ 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
@@ -38,6 +38,7 @@ 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
@@ -120,16 +120,6 @@ __EXPORT void board_on_reset(int status)
 ************************************************************************************/
 __EXPORT void stm32_boardinitialize(void)
 {
-	// clear all existing MPU configuration from bootloader
-	for (int region = 0; region < CONFIG_ARM_MPU_NREGIONS; region++) {
-		putreg32(region, MPU_RNR);
-		putreg32(0, MPU_RBAR);
-		putreg32(0, MPU_RASR);
-
-		// save
-		putreg32(0, MPU_CTRL);
-	}
-
 	/* Reset PWM first thing */
 	board_on_reset(-1);

@@ -48,6 +48,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 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_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
@@ -30,6 +30,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 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_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
@@ -38,7 +38,6 @@ CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_DRIVERS_UAVCAN=y
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
-CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
@@ -48,13 +47,13 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 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_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
-CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
 CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MAVLINK=y
@@ -99,4 +98,3 @@ CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_SYSTEMCMDS_USB_CONNECTED=y
 CONFIG_SYSTEMCMDS_VER=y
 CONFIG_SYSTEMCMDS_WORK_QUEUE=y
-CONFIG_EXAMPLES_FAKE_GPS=y
@@ -38,6 +38,7 @@ 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
@@ -72,6 +73,7 @@ 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
@@ -84,7 +86,6 @@ CONFIG_IDLETHREAD_STACKSIZE=750
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_STRERROR=y
-CONFIG_FS_PROCFS_MAX_TASKS=64
 CONFIG_MEMSET_64BIT=y
 CONFIG_MEMSET_OPTSPEED=y
 CONFIG_MMCSD=y
@@ -162,16 +162,6 @@ __EXPORT void board_on_reset(int status)
 __EXPORT void
 stm32_boardinitialize(void)
 {
-	// clear all existing MPU configuration from bootloader
-	for (int region = 0; region < CONFIG_ARM_MPU_NREGIONS; region++) {
-		putreg32(region, MPU_RNR);
-		putreg32(0, MPU_RBAR);
-		putreg32(0, MPU_RASR);
-
-		// save
-		putreg32(0, MPU_CTRL);
-	}
-
 	board_on_reset(-1); /* Reset PWM first thing */

 	/* configure LEDs */
@@ -5,6 +5,7 @@ CONFIG_BOARD_EXTERNAL_METADATA=y
 CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS3"
 CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS0"
 CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1"
+CONFIG_BOARD_SERIAL_RC="/dev/ttyS4"
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_BMP280=y
 CONFIG_DRIVERS_DSHOT=y
@@ -53,6 +53,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 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_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
@@ -48,6 +48,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 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_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
@@ -47,6 +47,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 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_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
@@ -46,6 +46,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 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_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
@@ -46,6 +46,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 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_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
@@ -46,6 +46,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 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_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
@@ -38,6 +38,7 @@ 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
@@ -73,6 +74,7 @@ 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
@@ -85,7 +87,6 @@ CONFIG_IDLETHREAD_STACKSIZE=750
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_STRERROR=y
-CONFIG_FS_PROCFS_MAX_TASKS=64
 CONFIG_MEMSET_64BIT=y
 CONFIG_MEMSET_OPTSPEED=y
 CONFIG_MMCSD=y
@@ -123,16 +123,6 @@ __EXPORT void board_on_reset(int status)
 ************************************************************************************/
 __EXPORT void stm32_boardinitialize(void)
 {
-	// clear all existing MPU configuration from bootloader
-	for (int region = 0; region < CONFIG_ARM_MPU_NREGIONS; region++) {
-		putreg32(region, MPU_RNR);
-		putreg32(0, MPU_RBAR);
-		putreg32(0, MPU_RASR);
-
-		// save
-		putreg32(0, MPU_CTRL);
-	}
-
 	/* Reset PWM first thing */
 	board_on_reset(-1);

@@ -47,6 +47,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 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_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
@@ -38,6 +38,7 @@ 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
@@ -73,6 +74,7 @@ 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
@@ -85,7 +87,6 @@ CONFIG_IDLETHREAD_STACKSIZE=750
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_STRERROR=y
-CONFIG_FS_PROCFS_MAX_TASKS=64
 CONFIG_MEMSET_64BIT=y
 CONFIG_MEMSET_OPTSPEED=y
 CONFIG_MMCSD=y
@@ -123,16 +123,6 @@ __EXPORT void board_on_reset(int status)
 ************************************************************************************/
 __EXPORT void stm32_boardinitialize(void)
 {
-	// clear all existing MPU configuration from bootloader
-	for (int region = 0; region < CONFIG_ARM_MPU_NREGIONS; region++) {
-		putreg32(region, MPU_RNR);
-		putreg32(0, MPU_RBAR);
-		putreg32(0, MPU_RASR);
-
-		// save
-		putreg32(0, MPU_CTRL);
-	}
-
 	/* Reset PWM first thing */
 	board_on_reset(-1);

@@ -45,6 +45,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 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_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
@@ -38,6 +38,7 @@ 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
@@ -71,6 +72,7 @@ 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
@@ -83,7 +85,6 @@ CONFIG_IDLETHREAD_STACKSIZE=750
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_STRERROR=y
-CONFIG_FS_PROCFS_MAX_TASKS=64
 CONFIG_MEMSET_64BIT=y
 CONFIG_MEMSET_OPTSPEED=y
 CONFIG_MMCSD=y
@@ -123,16 +123,6 @@ __EXPORT void board_on_reset(int status)
 ************************************************************************************/
 __EXPORT void stm32_boardinitialize(void)
 {
-	// clear all existing MPU configuration from bootloader
-	for (int region = 0; region < CONFIG_ARM_MPU_NREGIONS; region++) {
-		putreg32(region, MPU_RNR);
-		putreg32(0, MPU_RBAR);
-		putreg32(0, MPU_RASR);
-
-		// save
-		putreg32(0, MPU_CTRL);
-	}
-
 	/* Reset PWM first thing */
 	board_on_reset(-1);

@@ -48,6 +48,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 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_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
@@ -49,6 +49,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 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_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
@@ -48,6 +48,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 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_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
@@ -53,6 +53,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 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_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
@@ -103,8 +103,7 @@ CONFIG_SCHED_WAITPID=y
 CONFIG_SDCLONE_DISABLE=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
-CONFIG_SPITOOL_DEFFREQ=400000
-CONFIG_SPITOOL_MAXBUS=0
+CONFIG_SPITOOL_MAXBUS=2
 CONFIG_STACK_COLORATION=y
 CONFIG_START_DAY=18
 CONFIG_START_MONTH=8
@@ -62,7 +62,7 @@ else()
 		init.c
 		mtd.cpp
 		periphclocks.c
-		spi.c
+		spi.cpp
 		timer_config.cpp
 		userleds.c
 	)
@@ -160,8 +160,9 @@ int s32k1xx_bringup(void);
 *
 ****************************************************************************/

-#ifdef CONFIG_S32K1XX_LPSPI
+#ifdef CONFIG_S32K1XX_LPSPI0
 void s32k1xx_spidev_initialize(void);
+int s32k1xx_spi_bus_initialize(void);
 #endif

 /****************************************************************************************************
@@ -124,12 +124,13 @@ int s32k1xx_bringup(void)
 	s32k1xx_eeeprom_register(0, 4096);
 #endif

-#ifdef CONFIG_S32K1XX_LPSPI
+#ifdef CONFIG_S32K1XX_LPSPI0
 	/* Configure SPI chip selects if 1) SPI is not disabled, and 2) the weak
 	 * function s32k1xx_spidev_initialize() has been brought into the link.
 	 */

 	s32k1xx_spidev_initialize();
+	s32k1xx_spi_bus_initialize();
 #endif

 #if defined(CONFIG_S32K1XX_LPI2C0)
@@ -34,6 +34,5 @@
 #include <px4_arch/i2c_hw_description.h>

 constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = {
-	initI2CBusInternal(PX4_BUS_NUMBER_TO_PX4(1)),
 	initI2CBusExternal(PX4_BUS_NUMBER_TO_PX4(0)),
 };
@@ -0,0 +1,230 @@
+/************************************************************************************
+ *
+ *   Copyright (C) 2016, 2018, 2021 Gregory Nutt. All rights reserved.
+ *   Authors: Gregory Nutt <gnutt@nuttx.org>
+ *            David Sidrane <david_s5@nscdg.com>
+ *            Landon Haugh <landon.haugh@nxp.com>
+ *
+ * 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 NuttX 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 <nuttx/config.h>
+
+#include <px4_arch/spi_hw_description.h>
+#include <px4_platform_common/px4_config.h>
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <nuttx/spi/spi.h>
+#include <arch/board/board.h>
+
+#include "arm_arch.h"
+#include "chip.h"
+#include <systemlib/px4_macros.h>
+
+#include "s32k1xx_config.h"
+#include "s32k1xx_lpspi.h"
+#include "s32k1xx_pin.h"
+#include "board_config.h"
+
+#if defined(CONFIG_S32K1XX_LPSPI0)
+// Only LPSPI0 is defined in defconfig. cont. || defined(CONFIG_S32K1XX_LPSPI1) || defined(CONFIG_S32K1XX_LPSPI2)
+
+// UCANS32K146 has only one external SPI @ PTB5
+constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
+	initSPIBusExternal(SPI::Bus::SPI0, {
+		// Going to assume PTB5 means PortB, Pin5
+		initSPIConfigExternal(SPI::CS{GPIO::PortB, GPIO::Pin5})
+	}),
+};
+
+static constexpr bool unused = validateSPIConfig(px4_spi_buses);
+
+#define PX4_MK_GPIO(pin_ftmx, io)    ((((uint32_t)(pin_ftmx)) & ~(_PIN_MODE_MASK | _PIN_OPTIONS_MASK)) |(io))
+
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+__EXPORT void board_spi_reset(int ms, int bus_mask)
+{
+	/* Goal not to back feed the chips on the bus via IO lines */
+
+	/* Next Change CS to inputs with pull downs */
+	for (int bus = 0; bus < SPI_BUS_MAX_BUS_ITEMS; ++bus) {
+		if (px4_spi_buses[bus].bus == PX4_BUS_NUMBER_TO_PX4(1)) {
+			for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) {
+				if (px4_spi_buses[bus].devices[i].cs_gpio != 0) {
+					// s32k1xx_pinconfig is defined
+					// Only one argument, (uint32_t cfgset)
+					s32k1xx_pinconfig(PX4_MK_GPIO(px4_spi_buses[bus].devices[i].cs_gpio, GPIO_PULLDOWN));
+				}
+			}
+		}
+	}
+
+	/* Restore all the CS to ouputs inactive */
+	for (int bus = 0; bus < SPI_BUS_MAX_BUS_ITEMS; ++bus) {
+		if (px4_spi_buses[bus].bus == PX4_BUS_NUMBER_TO_PX4(1)) {
+			for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) {
+				if (px4_spi_buses[bus].devices[i].cs_gpio != 0) {
+					s32k1xx_pinconfig(px4_spi_buses[bus].devices[i].cs_gpio);
+				}
+			}
+		}
+	}
+}
+
+/************************************************************************************
+ * Name: s32k1xx_spidev_initialize
+ *
+ * Description:
+ *   Called to configure SPI chip select GPIO pins for the NXP UCANS32K146 board.
+ *
+ ************************************************************************************/
+
+void s32k1xx_spidev_initialize(void)
+{
+	board_spi_reset(10, 0xffff);
+
+	for (int bus = 0; bus < SPI_BUS_MAX_BUS_ITEMS; ++bus) {
+		for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) {
+			if (px4_spi_buses[bus].devices[i].cs_gpio != 0) {
+				s32k1xx_pinconfig(px4_spi_buses[bus].devices[i].cs_gpio);
+			}
+		}
+	}
+}
+
+/************************************************************************************
+ * Name: s32k1xx_spi_bus_initialize
+ *
+ * Description:
+ *   Called to configure SPI chip select GPIO pins for the NXP UCANS32K146 board.
+ *
+ ************************************************************************************/
+
+static const px4_spi_bus_t *_spi_bus0;
+
+__EXPORT int s32k1xx_spi_bus_initialize(void)
+{
+
+	for (int i = 0; i < SPI_BUS_MAX_BUS_ITEMS; ++i) {
+		switch (px4_spi_buses[i].bus) {
+		case 1: _spi_bus0 = &px4_spi_buses[i]; break;
+		}
+	}
+
+	struct spi_dev_s *spi_ext = px4_spibus_initialize(1);
+
+	if (!spi_ext) {
+		syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", 2);
+		return -ENODEV;
+	}
+
+	/* Default external bus to 1MHz and de-assert the known chip selects.
+	 */
+
+	SPI_SETFREQUENCY(spi_ext, 8 * 1000 * 1000);
+	SPI_SETBITS(spi_ext, 8);
+	SPI_SETMODE(spi_ext, SPIDEV_MODE3);
+
+	/* deselect all */
+	for (int bus = 0; bus < SPI_BUS_MAX_BUS_ITEMS; ++bus) {
+		for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) {
+			if (px4_spi_buses[bus].devices[i].cs_gpio != 0) {
+				SPI_SELECT(spi_ext, px4_spi_buses[bus].devices[i].devid, false);
+			}
+		}
+	}
+
+	return OK;
+
+}
+
+/************************************************************************************
+ * Name: s32k1xx_spi[n]select, s32k1xx_spi[n]status, and s32k1xx_spi[n]cmddata
+ *
+ * Description:
+ *   These external functions must be provided by board-specific logic.  They are
+ *   implementations of the select, status, and cmddata methods of the SPI interface
+ *   defined by struct spi_ops_s (see include/nuttx/spi/spi.h). All other methods
+ *   including s32k1xx_spibus_initialize()) are provided by common s32k1xx logic.
+ *   To use this common SPI logic on your board:
+ *
+ *   1. Provide logic in s32k1xx_boardinitialize() to configure SPI chip select
+ *      pins.
+ *   2. Provide s32k1xx_spi[n]select() and s32k1xx_spi[n]status() functions
+ *      in your board-specific logic.  These functions will perform chip selection
+ *      and status operations using GPIOs in the way your board is configured.
+ *   2. If CONFIG_SPI_CMDDATA is defined in the NuttX configuration, provide
+ *      s32k1xx_spi[n]cmddata() functions in your board-specific logic.  These
+ *      functions will perform cmd/data selection operations using GPIOs in the way
+ *      your board is configured.
+ *   3. Add a call to s32k1xx_spibus_initialize() in your low level application
+ *      initialization logic
+ *   4. The handle returned by s32k1xx_spibus_initialize() may then be used to bind the
+ *      SPI driver to higher level logic (e.g., calling
+ *      mmcsd_spislotinitialize(), for example, will bind the SPI driver to
+ *      the SPI MMC/SD driver).
+ *
+ ************************************************************************************/
+
+static inline void s32k1xx_spixselect(const px4_spi_bus_t *bus, struct spi_dev_s *dev, uint32_t devid, bool selected)
+{
+	for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) {
+		if (bus->devices[i].cs_gpio == 0) {
+			break;
+		}
+
+		if (devid == bus->devices[i].devid) {
+			// SPI select is active low, so write !selected to select the device
+			// s32k1xx_gpiowrite is defined (uint32_t pinset, bool value)
+			s32k1xx_gpiowrite(bus->devices[i].cs_gpio, !selected);
+		}
+	}
+}
+
+
+void s32k1xx_lpspi0select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected)
+{
+	spiinfo("devid: %d CS: %s\n", (int)devid, selected ? "assert" : "de-assert");
+	s32k1xx_spixselect(_spi_bus0, dev, devid, selected);
+}
+
+uint8_t s32k1xx_lpspi0status(FAR struct spi_dev_s *dev, uint32_t devid)
+{
+	return SPI_STATUS_PRESENT;
+}
+
+#endif /* CONFIG_S32K1XX_LPSPI0 */
@@ -2,6 +2,7 @@ CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
 CONFIG_BOARD_ARCHITECTURE="cortex-m4"
 CONFIG_BOARD_CONSTRAINED_FLASH=y
 CONFIG_BOARD_CONSTRAINED_MEMORY=y
+CONFIG_BOARD_SERIAL_URT6="/dev/ttyS2"
 CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1"
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_BMP280=y
@@ -52,6 +53,7 @@ CONFIG_SYSTEMCMDS_REFLECT=y
 CONFIG_SYSTEMCMDS_SD_BENCH=y
 CONFIG_SYSTEMCMDS_SD_STRESS=y
 CONFIG_SYSTEMCMDS_TOP=y
+CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
 CONFIG_SYSTEMCMDS_TUNE_CONTROL=y
 CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_SYSTEMCMDS_USB_CONNECTED=y
@@ -34,8 +34,8 @@
 #include <px4_arch/io_timer_hw_description.h>

 constexpr io_timers_t io_timers[MAX_IO_TIMERS] = {
-	initIOTimer(Timer::Timer2, DMA{DMA::Index1, DMA::Stream1, DMA::Channel3}),
 	initIOTimer(Timer::Timer3, DMA{DMA::Index1, DMA::Stream2, DMA::Channel5}),
+	initIOTimer(Timer::Timer2, DMA{DMA::Index1, DMA::Stream1, DMA::Channel3}),
 };

 constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
@@ -5,9 +5,11 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n
 CONFIG_MODULES_MC_POS_CONTROL=n
 CONFIG_MODULES_MC_RATE_CONTROL=n
 CONFIG_SYSTEMCMDS_BL_UPDATE=n
-CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
 CONFIG_DRIVERS_CAMERA_CAPTURE=y
 CONFIG_DRIVERS_CAMERA_TRIGGER=y
+CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_ETS=y
+CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_MS5525=y
+CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_SDP3X=y
 CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_SERIAL=y
 CONFIG_DRIVERS_DISTANCE_SENSOR_LL40LS=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
@@ -52,6 +52,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 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_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
@@ -52,6 +52,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 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_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
@@ -50,6 +50,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 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_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
@@ -1,14 +1,11 @@
+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_MODULES_ATTITUDE_ESTIMATOR_Q=n
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n
 CONFIG_BOARD_TESTING=y
-CONFIG_DRIVERS_ADC_ADS1115=n
 CONFIG_DRIVERS_TEST_PPM=y
-CONFIG_DRIVERS_PCA9685=n
-CONFIG_DRIVERS_PCA9685_PWM_OUT=n
 CONFIG_SYSTEMCMDS_MICROBENCH=y
-CONFIG_DRIVERS_RPM=n
-CONFIG_DRIVERS_PCA9685=n
-CONFIG_DRIVERS_PCA9685_PWM_OUT=n
-CONFIG_DRIVERS_IRLOCK=n
-CONFIG_DRIVERS_ADC_ADS1115=n
-CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n
@@ -55,6 +55,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 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_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
@@ -6,7 +6,6 @@ CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=n
 CONFIG_DRIVERS_IRLOCK=n
 CONFIG_DRIVERS_OSD=n
 CONFIG_DRIVERS_PCA9685_PWM_OUT=n
-CONFIG_DRIVERS_PCA9685=y
 CONFIG_DRIVERS_PWM_INPUT=n
 CONFIG_DRIVERS_ROBOCLAW=n
 CONFIG_DRIVERS_RPM=n
@@ -7,6 +7,7 @@ CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS7"
 CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6"
 CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4"
 CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1"
+CONFIG_BOARD_SERIAL_PPB="/dev/ttyS3"
 CONFIG_DRIVERS_ADC_ADS1115=y
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_COMMON_BAROMETERS=y
@@ -56,6 +57,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 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_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
@@ -50,6 +50,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 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_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
@@ -47,6 +47,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 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_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
@@ -29,6 +29,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 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_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
@@ -17,6 +17,7 @@ CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_EVENTS=y
 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_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
@@ -1,3 +1,2 @@
 CONFIG_BOARD_NOLOCKSTEP=y
 CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_SERIAL=y
-CONFIG_MODULES_CONTROL_ALLOCATOR=y
@@ -29,6 +29,7 @@ CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_EVENTS=y
 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_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
@@ -210,6 +210,9 @@ if(EXISTS ${BOARD_DEFCONFIG})

 	# Serial ports
 	set(board_serial_ports)
+	if(SERIAL_URT6)
+		list(APPEND board_serial_ports URT6:${SERIAL_URT6})
+	endif()
 	if(SERIAL_GPS1)
        list(APPEND board_serial_ports GPS1:${SERIAL_GPS1})
 	endif()
@@ -240,9 +243,15 @@ if(EXISTS ${BOARD_DEFCONFIG})
 	if(SERIAL_TEL5)
        list(APPEND board_serial_ports TEL5:${SERIAL_TEL5})
 	endif()
+	if(SERIAL_RC)
+		list(APPEND board_serial_ports RC:${SERIAL_RC})
+	endif()
 	if(SERIAL_WIFI)
        list(APPEND board_serial_ports WIFI:${SERIAL_WIFI})
 	endif()
+	if(SERIAL_PPB)
+        list(APPEND board_serial_ports PPB:${SERIAL_PPB})
+	endif()


 	# ROMFS
@@ -156,11 +156,12 @@ function(px4_add_module)
 	if(NOT DYNAMIC)
 		target_link_libraries(${MODULE} PRIVATE prebuild_targets parameters_interface px4_layer px4_platform systemlib)
 		set_property(GLOBAL APPEND PROPERTY PX4_MODULE_LIBRARIES ${MODULE})
-		set_property(GLOBAL APPEND PROPERTY PX4_MODULE_PATHS ${CMAKE_CURRENT_SOURCE_DIR})
-		px4_list_make_absolute(ABS_SRCS ${CMAKE_CURRENT_SOURCE_DIR} ${SRCS})
-		set_property(GLOBAL APPEND PROPERTY PX4_SRC_FILES ${ABS_SRCS})
 	endif()

+	set_property(GLOBAL APPEND PROPERTY PX4_MODULE_PATHS ${CMAKE_CURRENT_SOURCE_DIR})
+	px4_list_make_absolute(ABS_SRCS ${CMAKE_CURRENT_SOURCE_DIR} ${SRCS})
+	set_property(GLOBAL APPEND PROPERTY PX4_SRC_FILES ${ABS_SRCS})
+
 	# set defaults if not set
 	set(MAIN_DEFAULT MAIN-NOTFOUND)
 	set(STACK_MAIN_DEFAULT 2048)
@@ -9,7 +9,11 @@ float32 variance_east		# Wind estimate error variance in east / Y direction (m/s

 float32 tas_innov 		# True airspeed innovation
 float32 tas_innov_var 		# True airspeed innovation variance
-float32 tas_scale 		# Estimated true airspeed scale factor
+
+float32 tas_scale_raw 		# Estimated true airspeed scale factor (not validated)
+float32 tas_scale_raw_var 	# True airspeed scale factor variance
+
+float32 tas_scale_validated	# Estimated true airspeed scale factor after validation

 float32 beta_innov 		# Sideslip measurement innovation
 float32 beta_innov_var 		# Sideslip measurement innovation variance
@@ -0,0 +1,71 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+#pragma once
+
+#include "../../../s32k1xx/include/px4_arch/spi_hw_description.h"
+
+constexpr bool validateSPIConfig(const px4_spi_bus_t spi_busses_conf[SPI_BUS_MAX_BUS_ITEMS])
+{
+	const bool nuttx_enabled_spi_buses[] = {
+#ifdef CONFIG_S32K1XX_LPSPI0
+		true,
+#else
+		false,
+#endif
+#ifdef CONFIG_S32K1XX_LPSPI1
+		true,
+#else
+		false,
+#endif
+#ifdef CONFIG_S32K1XX_LPSPI2
+		true,
+#else
+		false,
+#endif
+	};
+
+	for (unsigned i = 0; i < sizeof(nuttx_enabled_spi_buses) / sizeof(nuttx_enabled_spi_buses[0]); ++i) {
+		bool found_bus = false;
+
+		for (int j = 0; j < SPI_BUS_MAX_BUS_ITEMS; ++j) {
+			if (spi_busses_conf[j].bus == (int)i + 1) {
+				found_bus = true;
+			}
+		}
+
+		// Either the bus is enabled in NuttX and configured in spi_busses_conf, or disabled and not configured
+		constexpr_assert(found_bus == nuttx_enabled_spi_buses[i], "SPI bus config mismatch (CONFIG_S32K1XX_LPSPIn)");
+	}
+
+	return false;
+}
@@ -240,3 +240,22 @@ static inline constexpr uint32_t getGPIOPin(GPIO::Pin pin)

 	return 0;
 }
+
+namespace SPI
+{
+
+enum class Bus {
+	SPI0 = 1,
+	SPI1,
+	SPI2,
+};
+
+using CS = GPIO::GPIOPin;
+using DRDY = GPIO::GPIOPin;
+
+struct bus_device_external_cfg_t {
+	CS cs_gpio;
+	DRDY drdy_gpio;
+};
+
+} // namespace SPI
@@ -0,0 +1,133 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <px4_arch/hw_description.h>
+#include <px4_platform_common/spi.h>
+
+#include "s32k1xx_config.h"
+#include "s32k1xx_lpspi.h"
+#include "s32k1xx_pin.h"
+
+static inline constexpr px4_spi_bus_device_t initSPIDevice(uint32_t devid, SPI::CS cs_gpio, SPI::DRDY drdy_gpio = {})
+{
+	px4_spi_bus_device_t ret{};
+	ret.cs_gpio = getGPIOPort(cs_gpio.port) | getGPIOPin(cs_gpio.pin) | (GPIO_LOWDRIVE | GPIO_OUTPUT_ONE);
+
+	if (drdy_gpio.port != GPIO::PortInvalid) {
+		ret.drdy_gpio = getGPIOPort(drdy_gpio.port) | getGPIOPin(drdy_gpio.pin) | (GPIO_PULLUP | PIN_INT_BOTH);
+	}
+
+	if (PX4_SPIDEVID_TYPE(devid) == 0) { // it's a PX4 device (internal or external)
+		ret.devid = PX4_SPIDEV_ID(PX4_SPI_DEVICE_ID, devid);
+
+	} else { // it's a NuttX device (e.g. SPIDEV_FLASH(0))
+		ret.devid = devid;
+	}
+
+	ret.devtype_driver = PX4_SPI_DEV_ID(devid);
+	return ret;
+}
+
+static inline constexpr px4_spi_bus_t initSPIBus(SPI::Bus bus, const px4_spi_bus_devices_t &devices,
+		GPIO::GPIOPin power_enable = {})
+{
+	px4_spi_bus_t ret{};
+	ret.requires_locking = false;
+
+	for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) {
+		ret.devices[i] = devices.devices[i];
+
+
+		if (ret.devices[i].cs_gpio != 0) {
+			if (PX4_SPI_DEVICE_ID == PX4_SPIDEVID_TYPE(ret.devices[i].devid)) {
+				int same_devices_count = 0;
+
+				for (int j = 0; j < i; ++j) {
+					if (ret.devices[j].cs_gpio != 0) {
+						same_devices_count += (ret.devices[i].devid & 0xff) == (ret.devices[j].devid & 0xff);
+					}
+				}
+
+				// increment the 2. LSB byte to allow multiple devices of the same type
+				ret.devices[i].devid |= same_devices_count << 8;
+
+			} else {
+				// A bus potentially requires locking if it is accessed by non-PX4 devices (i.e. NuttX drivers)
+				ret.requires_locking = true;
+			}
+		}
+	}
+
+	ret.bus = (int)bus;
+	ret.is_external = false;
+
+	if (power_enable.port != GPIO::PortInvalid) {
+		ret.power_enable_gpio = getGPIOPort(power_enable.port) | getGPIOPin(power_enable.pin) |
+					(GPIO_HIGHDRIVE | GPIO_OUTPUT_ONE);
+	}
+
+	return ret;
+}
+
+// just a wrapper since we cannot pass brace-enclosed initialized arrays directly as arguments
+struct bus_device_external_cfg_array_t {
+	SPI::bus_device_external_cfg_t devices[SPI_BUS_MAX_DEVICES];
+};
+
+static inline constexpr px4_spi_bus_t initSPIBusExternal(SPI::Bus bus, const bus_device_external_cfg_array_t &devices)
+{
+	px4_spi_bus_t ret{};
+
+	for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) {
+		if (devices.devices[i].cs_gpio.port == GPIO::PortInvalid) {
+			break;
+		}
+
+		ret.devices[i] = initSPIDevice(i, devices.devices[i].cs_gpio, devices.devices[i].drdy_gpio);
+	}
+
+	ret.bus = (int)bus;
+	ret.is_external = true;
+	ret.requires_locking = false; // external buses are never accessed by NuttX drivers
+	return ret;
+}
+
+static inline constexpr SPI::bus_device_external_cfg_t initSPIConfigExternal(SPI::CS cs_gpio, SPI::DRDY drdy_gpio = {})
+{
+	SPI::bus_device_external_cfg_t ret{};
+	ret.cs_gpio = cs_gpio;
+	ret.drdy_gpio = drdy_gpio;
+	return ret;
+}
@@ -1,6 +1,6 @@
 module_name: uLanding Radar
 serial_config:
-    - command: ulanding_radar start ${SERIAL_DEV}
+    - command: ulanding_radar start -d ${SERIAL_DEV}
      port_config_param:
        name: SENS_ULAND_CFG
        group: Sensors
@@ -1205,7 +1205,7 @@ int uavcan_main(int argc, char *argv[])
 	}

 	if (!std::strcmp(argv[1], "hardpoint")) {
-		if (!std::strcmp(argv[2], "set") && argc > 4) {
+		if (argc > 4 && !std::strcmp(argv[2], "set")) {
 			const int hardpoint_id = atoi(argv[3]);
 			const int command = atoi(argv[4]);

@@ -73,7 +73,7 @@ if DRIVERS_UAVCAN_V1
            default n

        config UAVCAN_V1_GNSS_SUBSCRIBER_0
-            bool "GNSS Subscriber 0 "
+            bool "GNSS Subscriber 0"
            default n

        config UAVCAN_V1_GNSS_SUBSCRIBER_1
@@ -0,0 +1,92 @@
+/****************************************************************************
+ *
+ *   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 NodeClient.hpp
+ *
+ * Defines basic implementation of UAVCAN PNP for dynamic Node ID
+ *
+ * @author Peter van der Perk <peter.vanderperk@nxp.com>
+ */
+
+#pragma once
+
+
+#include <px4_platform_common/defines.h>
+#include <drivers/drv_hrt.h>
+
+#include "CanardInterface.hpp"
+
+#include <uavcan/node/ID_1_0.h>
+#include <uavcan/pnp/NodeIDAllocationData_1_0.h>
+#include <uavcan/pnp/NodeIDAllocationData_2_0.h>
+
+#include "Services/AccessRequest.hpp"
+#include "Services/ListRequest.hpp"
+
+#define PNP1_PORT_ID                                 uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_
+#define PNP1_PAYLOAD_SIZE                            uavcan_pnp_NodeIDAllocationData_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_
+#define PNP2_PORT_ID                                 uavcan_pnp_NodeIDAllocationData_2_0_FIXED_PORT_ID_
+#define PNP2_PAYLOAD_SIZE                            uavcan_pnp_NodeIDAllocationData_2_0_SERIALIZATION_BUFFER_SIZE_BYTES_
+
+class NodeClient : public UavcanBaseSubscriber
+{
+public:
+	NodeClient(CanardInstance &ins, UavcanParamManager &pmgr) : UavcanBaseSubscriber(ins, "NodeIDAllocationData", 0),
+		_canard_instance(ins) { };
+
+	void subscribe() override
+	{
+
+		canardRxSubscribe(&_canard_instance,
+				  CanardTransferKindMessage,
+				  (_canard_instance.mtu_bytes == CANARD_MTU_CAN_FD ? PNP2_PORT_ID : PNP1_PORT_ID),  // The fixed Subject-ID
+				  (_canard_instance.mtu_bytes == CANARD_MTU_CAN_FD ? PNP2_PAYLOAD_SIZE : PNP1_PAYLOAD_SIZE),
+				  CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
+				  &_subj_sub._canard_sub);
+	}
+
+	bool HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg);
+	bool HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_2_0 &msg);
+
+	void callback(const CanardTransfer &receive); // NodeIDAllocation callback
+
+	void update();
+
+private:
+
+	CanardInstance &_canard_instance;
+	CanardTransferID _node_id_alloc_transfer_id{0};
+
+	hrt_abstime _nodealloc_request_last{0};
+};
@@ -36,6 +36,7 @@
 #include "UavcanPublisherBase.hpp"

 #include <com/hex/equipment/flow/Measurement.hpp>
+#include <conversion/rotation.h>

 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/optical_flow.h>
@@ -53,7 +54,16 @@ public:
 		UavcanPublisherBase(com::hex::equipment::flow::Measurement::DefaultDataTypeID),
 		uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(optical_flow)),
 		uavcan::Publisher<com::hex::equipment::flow::Measurement>(node)
-	{}
+	{
+		_rotation = matrix::Dcmf{matrix::Eulerf{0.f, 0.f, 0.f}};
+
+		param_t rot = param_find("CANNODE_FLOW_ROT");
+		int32_t val = 0;
+
+		if (param_get(rot, &val) == PX4_OK) {
+			_rotation = get_rot_matrix((enum Rotation)val);
+		}
+	}

 	void PrintInfo() override
 	{
@@ -73,10 +83,16 @@ public:
 		if (uORB::SubscriptionCallbackWorkItem::update(&optical_flow)) {
 			com::hex::equipment::flow::Measurement measurement{};
 			measurement.integration_interval  = optical_flow.integration_timespan * 1e-6f; // us -> s
-			measurement.rate_gyro_integral[0] = optical_flow.gyro_x_rate_integral;
-			measurement.rate_gyro_integral[1] = optical_flow.gyro_y_rate_integral;
-			measurement.flow_integral[0] = optical_flow.pixel_flow_x_integral;
-			measurement.flow_integral[1] = optical_flow.pixel_flow_y_integral;
+
+			// rotate measurements in yaw from sensor frame to body frame
+			const matrix::Vector3f gyro_flow_rotated = _rotation * matrix::Vector3f{optical_flow.gyro_x_rate_integral, optical_flow.gyro_y_rate_integral, 0.f};
+			const matrix::Vector3f pixel_flow_rotated = _rotation * matrix::Vector3f{optical_flow.pixel_flow_x_integral, optical_flow.pixel_flow_y_integral, 0.f};
+
+			measurement.rate_gyro_integral[0] = gyro_flow_rotated(0);
+			measurement.rate_gyro_integral[1] = gyro_flow_rotated(1);
+			measurement.flow_integral[0] = pixel_flow_rotated(0);
+			measurement.flow_integral[1] = pixel_flow_rotated(1);
+
 			measurement.quality = optical_flow.quality;

 			uavcan::Publisher<com::hex::equipment::flow::Measurement>::broadcast(measurement);
@@ -85,5 +101,7 @@ public:
 			uORB::SubscriptionCallbackWorkItem::registerCallback();
 		}
 	}
+private:
+	matrix::Dcmf _rotation;
 };
 } // namespace uavcannode
@@ -59,3 +59,24 @@ PARAM_DEFINE_INT32(CANNODE_BITRATE, 1000000);
 * @group UAVCAN
 */
 PARAM_DEFINE_INT32(CANNODE_TERM, 0);
+
+/**
+ * Cannode flow board rotation
+ *
+ * This parameter defines the yaw rotation of the Cannode flow board relative to the vehicle body frame.
+ * Zero rotation is defined as X on flow board pointing towards front of vehicle.
+ *
+ * @value 0 No rotation
+ * @value 1 Yaw 45°
+ * @value 2 Yaw 90°
+ * @value 3 Yaw 135°
+ * @value 4 Yaw 180°
+ * @value 5 Yaw 225°
+ * @value 6 Yaw 270°
+ * @value 7 Yaw 315°
+ *
+ * @reboot_required true
+ *
+ * @group UAVCAN
+ */
+PARAM_DEFINE_INT32(CANNODE_FLOW_ROT, 0);
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2013-2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-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
@@ -276,7 +276,9 @@ ECL_L1_Pos_Controller::navigate_loiter(const Vector2d &vector_A, const Vector2d
 	float tangent_vel = xtrack_vel_center * loiter_direction;

 	/* prevent PD output from turning the wrong way when in circle mode */
-	if (tangent_vel < 0.0f && _circle_mode) {
+	const float l1_op_tan_vel = 2.f; // hard coded max tangential velocity in the opposite direction
+
+	if (tangent_vel < -l1_op_tan_vel && _circle_mode) {
 		lateral_accel_sp_circle_pd = math::max(lateral_accel_sp_circle_pd, 0.0f);
 	}

@@ -120,16 +120,13 @@ float VelocitySmoothing::computeT1(float T123, float a0, float v3, float j_max,
 	float T3_plus = a0 / j_max + T1_plus;
 	float T3_minus = a0 / j_max + T1_minus;

-	float T13_plus = T1_plus + T3_plus;
-	float T13_minus = T1_minus + T3_minus;
-
 	float T1 = 0.f;

-	if (T13_plus > T123) {
-		T1 = T1_minus;
-
-	} else if (T13_minus > T123) {
+	if ((T1_plus >= 0.f && T3_plus >= 0.f) && ((T1_plus + T3_plus) <= T123)) {
 		T1 = T1_plus;
+
+	} else if ((T1_minus >= 0.f && T3_minus >= 0.f) && ((T1_minus + T3_minus) <= T123)) {
+		T1 = T1_minus;
 	}

 	T1 = saturateT1ForAccel(a0, j_max, T1, a_max);
@@ -282,7 +279,8 @@ void VelocitySmoothing::timeSynchronization(VelocitySmoothing *traj, int n_traj)

 	if (desired_time > FLT_EPSILON) {
 		for (int i = 0; i < n_traj; i++) {
-			if (i != longest_traj_index) {
+			if ((i != longest_traj_index)
+			    && (traj[i].getTotalTime() < desired_time)) {
 				traj[i].updateDurationsGivenTotalTime(desired_time);
 			}
 		}
@@ -112,6 +112,38 @@ TEST_F(VelocitySmoothingTest, testTimeSynchronization)
 	EXPECT_LE(fabsf(_trajectories[0].getTotalTime() - _trajectories[1].getTotalTime()), 0.0001);
 }

+TEST_F(VelocitySmoothingTest, testTimeSynchronizationSameDelta)
+{
+	// GIVEN: a set of initial conditions
+	Vector3f a0(0.f, 0.f, 0.f);
+	Vector3f v0(0.5f, -0.2f, 0.f);
+	Vector3f x0(0.f, 0.f, 0.f);
+
+	setInitialConditions(a0, v0, x0);
+
+	// WHEN: the same delta velocity is set to the XY-axes
+	const float delta_v = 0.3f;
+	Vector3f velocity_setpoints{v0(0) + delta_v, v0(1) + delta_v, 0.f};
+
+	for (int i = 0; i < 3; i++) {
+		_trajectories[i].updateDurations(velocity_setpoints(i));
+	}
+
+	VelocitySmoothing::timeSynchronization(_trajectories, 3);
+
+	// THEN: they should have the same T1, T2 and T3 dirations
+	EXPECT_FLOAT_EQ(_trajectories[0].getTotalTime(), _trajectories[1].getTotalTime());
+	EXPECT_FLOAT_EQ(_trajectories[0].getT1(), _trajectories[1].getT1());
+	EXPECT_FLOAT_EQ(_trajectories[0].getT2(), _trajectories[1].getT2());
+	EXPECT_FLOAT_EQ(_trajectories[0].getT3(), _trajectories[1].getT3());
+
+	// AND: the Z axis should have the same duration but spend all its time in T2 (constant phase)
+	EXPECT_FLOAT_EQ(_trajectories[2].getTotalTime(), _trajectories[0].getTotalTime());
+	EXPECT_FLOAT_EQ(_trajectories[2].getT1(), 0.f);
+	EXPECT_FLOAT_EQ(_trajectories[2].getT2(), _trajectories[0].getTotalTime());
+	EXPECT_FLOAT_EQ(_trajectories[2].getT3(), 0.f);
+}
+
 TEST_F(VelocitySmoothingTest, testConstantSetpoint)
 {
 	// GIVEN: A set of constraints
@@ -56,7 +56,7 @@ WindEstimator::initialise(const matrix::Vector3f &velI, const matrix::Vector2f &
 	// initilaise wind states assuming zero side slip and horizontal flight
 	_state(INDEX_W_N) = velI(INDEX_W_N) - tas_meas * cosf(heading_est);
 	_state(INDEX_W_E) = velI(INDEX_W_E) - tas_meas * sinf(heading_est);
-	_state(INDEX_TAS_SCALE) = 1.0f;
+	_state(INDEX_TAS_SCALE) = _scale_init;

 	// compute jacobian of states wrt north/each earth velocity states and true airspeed measurement
 	float L0 = v_e * v_e;
@@ -104,7 +104,7 @@ WindEstimator::update(uint64_t time_now)
 	_wind_estimator_reset = false;

 	// run covariance prediction at 1Hz
-	if (time_now - _time_last_update < 1000 * 1000 || _time_last_update == 0) {
+	if (time_now - _time_last_update < 1_s || _time_last_update == 0) {
 		if (_time_last_update == 0) {
 			_time_last_update = time_now;
 		}
@@ -149,7 +149,7 @@ WindEstimator::fuse_airspeed(uint64_t time_now, const float true_airspeed, const
 	}

 	// don't fuse faster than 10Hz
-	if (time_now - _time_last_airspeed_fuse < 100 * 1000) {
+	if (time_now - _time_last_airspeed_fuse < 100_ms) {
 		return;
 	}

@@ -160,28 +160,28 @@ WindEstimator::fuse_airspeed(uint64_t time_now, const float true_airspeed, const
 	const float v_e = velI(1);
 	const float v_d = velI(2);

-	const float k_tas = _state(INDEX_TAS_SCALE);
+	// calculate airspeed from ground speed and wind states (without scale)
+	const float airspeed_predicted_raw = sqrtf((v_n - _state(INDEX_W_N)) * (v_n - _state(INDEX_W_N)) +
+					     (v_e - _state(INDEX_W_E)) * (v_e - _state(INDEX_W_E)) + v_d * v_d);

-	// compute kalman gain K
-	const float HH0 = sqrtf(v_d * v_d + (v_e - _state(INDEX_W_E)) * (v_e - _state(INDEX_W_E)) + (v_n - _state(
-					INDEX_W_N)) * (v_n - _state(INDEX_W_N)));
-	const float HH1 = k_tas / HH0;
+	// compute state observation matrix H
+	const float HH0 = airspeed_predicted_raw;
+	const float HH1 = _state(INDEX_TAS_SCALE) / math::max(HH0, 0.1f);

 	matrix::Matrix<float, 1, 3> H_tas;
 	H_tas(0, 0) = HH1 * (-v_n + _state(INDEX_W_N));
 	H_tas(0, 1) = HH1 * (-v_e + _state(INDEX_W_E));
 	H_tas(0, 2) = HH0;

-	matrix::Matrix<float, 3, 1> K = _P * H_tas.transpose();
-
+	// compute innovation covariance S
 	const matrix::Matrix<float, 1, 1> S = H_tas * _P * H_tas.transpose() + _tas_var;

+	// compute Kalman gain
+	matrix::Matrix<float, 3, 1> K = _P * H_tas.transpose();
 	K /= S(0, 0);
-	// compute innovation
-	const float airspeed_pred = k_tas * sqrtf((v_n - _state(INDEX_W_N)) * (v_n - _state(INDEX_W_N)) + (v_e - _state(
-					    INDEX_W_E)) *
-				    (v_e - _state(INDEX_W_E)) + v_d * v_d);

+	// compute innovation
+	const float airspeed_pred = _state(INDEX_TAS_SCALE) * airspeed_predicted_raw;
 	_tas_innov = true_airspeed - airspeed_pred;

 	// innovation variance
@@ -190,13 +190,13 @@ WindEstimator::fuse_airspeed(uint64_t time_now, const float true_airspeed, const
 	bool reinit_filter = false;
 	bool meas_is_rejected = false;

+	// note: _time_rejected_tas and reinit_filter are not used anymore as filter can't get reset due to tas rejection
 	meas_is_rejected = check_if_meas_is_rejected(time_now, _tas_innov, _tas_innov_var, _tas_gate, _time_rejected_tas,
 			   reinit_filter);

-	reinit_filter |= _tas_innov_var < 0.0f;
-
-	if (meas_is_rejected || reinit_filter) {
-		if (reinit_filter) {
+	if (meas_is_rejected || _tas_innov_var < 0.f) {
+		// only reset filter if _tas_innov_var gets unfeasible, but not never if tas measurement is rejected
+		if (_tas_innov_var < 0.0f) {
 			_initialised =	initialise(velI, matrix::Vector2f(0.1f, 0.1f), true_airspeed);
 		}

@@ -224,7 +224,7 @@ WindEstimator::fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const
 	}

 	// don't fuse faster than 10Hz
-	if (time_now - _time_last_beta_fuse < 100 * 1000) {
+	if (time_now - _time_last_beta_fuse < 100_ms) {
 		return;
 	}

@@ -259,11 +259,11 @@ WindEstimator::fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const
 	H_beta(0, 1) = HB15 * (HB12 - HB7 + HB8) + HB16 * HB5;
 	H_beta(0, 2) = 0;

-	// compute kalman gain
-	matrix::Matrix<float, 3, 1> K = _P * H_beta.transpose();
-
+	// compute innovation covariance S
 	const matrix::Matrix<float, 1, 1> S = H_beta * _P * H_beta.transpose() + _beta_var;

+	// compute Kalman gain
+	matrix::Matrix<float, 3, 1> K = _P * H_beta.transpose();
 	K /= S(0, 0);

 	// compute predicted side slip angle
@@ -333,16 +333,6 @@ WindEstimator::run_sanity_checks()
 		return;
 	}

-	// check if we should inhibit learning of airspeed scale factor and rather use a pre-set value.
-	// airspeed scale factor errors arise from sensor installation which does not change and only needs
-	// to be computed once for a perticular installation.
-	if (_enforced_airspeed_scale < 0) {
-		_state(INDEX_TAS_SCALE) = math::max(0.0f, _state(INDEX_TAS_SCALE));
-
-	} else {
-		_state(INDEX_TAS_SCALE) = _enforced_airspeed_scale;
-	}
-
 	// attain symmetry
 	for (unsigned row = 0; row < 3; row++) {
 		for (unsigned column = 0; column < row; column++) {
@@ -364,7 +354,7 @@ WindEstimator::check_if_meas_is_rejected(uint64_t time_now, float innov, float i
 		time_meas_rejected = 0;
 	}

-	reinit_filter = time_now - time_meas_rejected > 5 * 1000 * 1000 && time_meas_rejected != 0;
+	reinit_filter = time_now - time_meas_rejected > 5_s && time_meas_rejected != 0;

 	return time_meas_rejected != 0;
 }
@@ -41,6 +41,8 @@
 #include <mathlib/mathlib.h>
 #include <matrix/math.hpp>

+using namespace time_literals;
+
 class WindEstimator
 {
 public:
@@ -59,27 +61,21 @@ public:
 			   const matrix::Vector2f &velIvar);
 	void fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const matrix::Quatf &q_att);

-	void get_wind(float wind[2])
-	{
-		wind[0] = _state(INDEX_W_N);
-		wind[1] = _state(INDEX_W_E);
-	}
-
 	bool is_estimate_valid() { return _initialised; }

 	bool check_if_meas_is_rejected(uint64_t time_now, float innov, float innov_var, uint8_t gate_size,
 				       uint64_t &time_meas_rejected, bool &reinit_filter);

-	float get_tas_scale() { return _state(INDEX_TAS_SCALE); }
+	matrix::Vector2f get_wind() { return matrix::Vector2f{_state(INDEX_W_N), _state(INDEX_W_E)}; }
+
+	// invert scale (CAS = IAS * scale), protect agains division by 0, constrain to [0.1, 10]
+	float get_tas_scale() { return 1.f / math::constrain(_state(INDEX_TAS_SCALE), 0.1f, 10.0f); }
+	float get_tas_scale_var() { return _P(2, 2); }
 	float get_tas_innov() { return _tas_innov; }
 	float get_tas_innov_var() { return _tas_innov_var; }
 	float get_beta_innov() { return _beta_innov; }
 	float get_beta_innov_var() { return _beta_innov_var; }
-	void get_wind_var(float wind_var[2])
-	{
-		wind_var[0] = _P(0, 0);
-		wind_var[1] = _P(1, 1);
-	}
+	matrix::Vector2f  get_wind_var() { return matrix::Vector2f{_P(0, 0), _P(1, 1)}; }
 	bool get_wind_estimator_reset() { return _wind_estimator_reset; }

 	void set_wind_p_noise(float wind_sigma) { _wind_p_var = wind_sigma * wind_sigma; }
@@ -88,10 +84,7 @@ public:
 	void set_beta_noise(float beta_var) { _beta_var = beta_var * beta_var; }
 	void set_tas_gate(uint8_t gate_size) {_tas_gate = gate_size; }
 	void set_beta_gate(uint8_t gate_size) {_beta_gate = gate_size; }
-
-	// Inhibit learning of the airspeed scale factor and force the estimator to use _enforced_airspeed_scale as scale factor.
-	// Negative input values enable learning of the airspeed scale factor.
-	void enforce_airspeed_scale(float scale) {_enforced_airspeed_scale = scale; }
+	void set_scale_init(float scale_init) {_scale_init = 1.f / math::constrain(scale_init, 0.1f, 10.f); }

 private:
 	enum {
@@ -100,7 +93,7 @@ private:
 		INDEX_TAS_SCALE
 	};	///< enum which can be used to access state.

-	matrix::Vector3f _state;		///< state vector
+	matrix::Vector3f _state{0.f, 0.f, 1.f};
 	matrix::Matrix3f _P;		///< state covariance matrix

 	float _tas_innov{0.0f};	///< true airspeed innovation
@@ -118,15 +111,16 @@ private:
 	uint8_t _tas_gate{3};	///< airspeed fusion gate size
 	uint8_t _beta_gate{1};	///< sideslip fusion gate size

+	float _scale_init{1.f};
+
 	uint64_t _time_last_airspeed_fuse = 0;	///< timestamp of last airspeed fusion
-	uint64_t _time_last_beta_fuse = 0;		///< timestamp of last sideslip fusion
-	uint64_t _time_last_update = 0;			///< timestamp of last covariance prediction
-	uint64_t _time_rejected_beta = 0;		///< timestamp of when sideslip measurements have consistently started to be rejected
+	uint64_t _time_last_beta_fuse = 0;	///< timestamp of last sideslip fusion
+	uint64_t _time_last_update = 0;		///< timestamp of last covariance prediction
+	uint64_t _time_rejected_beta = 0;	///< timestamp of when sideslip measurements have consistently started to be rejected
 	uint64_t _time_rejected_tas =
-		0;		///<timestamp of when true airspeed measurements have consistently started to be rejected
+		0;	///< timestamp of when true airspeed measurements have consistently started to be rejected

 	bool _wind_estimator_reset = false; ///< wind estimator was reset in this cycle
-	float _enforced_airspeed_scale{-1.0f}; ///< by default we want to estimate the true airspeed scale factor (see enforce_airspeed_scale(...) )

 	// initialise state and state covariance matrix
 	bool initialise(const matrix::Vector3f &velI, const matrix::Vector2f &velIvar, const float tas_meas);
@@ -31,6 +31,22 @@
 #
 ############################################################################

+add_custom_target(world_magnetic_model_update
+	COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/fetch_noaa_table.py > ${CMAKE_CURRENT_SOURCE_DIR}/geo_magnetic_tables.hpp
+	DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/fetch_noaa_table.py
+	WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
+	USES_TERMINAL
+	COMMENT "Updating world magnetic model from NOAA (${CMAKE_CURRENT_SOURCE_DIR}/geo_magnetic_tables.hpp)"
+)
+
+add_custom_target(world_magnetic_model_tests_update
+	COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/generate_gtest.py > ${CMAKE_CURRENT_SOURCE_DIR}/test_geo_lookup.cpp
+	DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/generate_gtest.py
+	WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
+	USES_TERMINAL
+	COMMENT "Updating world magnetic model unit tests from NOAA (${CMAKE_CURRENT_SOURCE_DIR}/test_geo_lookup.cpp)"
+)
+
 add_library(world_magnetic_model
 	geo_mag_declination.cpp
 	geo_mag_declination.h
@@ -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: 2021.6383,
+// Date: 2021.8164,
 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 */ {  26010, 24264, 22519, 20774, 19028, 17283, 15538, 13792, 12047, 10302,  8556,  6811,  5066,  3321,  1575,  -170, -1915, -3661, -5406, -7151, -8896,-10642,-12387,-14132,-15878,-17623,-19368,-21114,-22859,-24605,-26350,-28095,-29841, 31246, 29500, 27755, 26010, },
-	/* LAT: -80 */ {  22579, 20444, 18501, 16725, 15083, 13543, 12079, 10667,  9291,  7941,  6608,  5289,  3980,  2675,  1368,    49, -1291, -2661, -4069, -5519, -7012, -8546,-10122,-11739,-13402,-15118,-16902,-18772,-20753,-22870,-25142,-27576,-30145, 30045, 27423, 24913, 22579, },
-	/* LAT: -70 */ {  14969, 13573, 12449, 11490, 10625,  9796,  8956,  8071,  7119,  6101,  5031,  3937,  2850,  1793,   770,  -240, -1276, -2378, -3575, -4870, -6247, -7673, -9118,-10558,-11983,-13397,-14824,-16307,-17925,-19830,-22335,-26111, 30802, 24183, 19627, 16842, 14969, },
-	/* LAT: -60 */ {   8388,  8149,  7874,  7606,  7358,  7109,  6805,  6376,  5765,  4948,  3949,  2838,  1715,   683,  -204,  -967, -1696, -2515, -3510, -4698, -6021, -7386, -8704, -9914,-10980,-11881,-12599,-13095,-13266,-12817,-10726, -3581,  4802,  7588,  8378,  8515,  8388, },
-	/* LAT: -50 */ {   5459,  5499,  5448,  5363,  5294,  5261,  5230,  5107,  4768,  4109,  3104,  1834,   490,  -697, -1579, -2155, -2557, -2994, -3667, -4664, -5894, -7165, -8305, -9211, -9816,-10064, -9879, -9130, -7629, -5275, -2374,   372,  2481,  3904,  4779,  5254,  5459, },
-	/* LAT: -40 */ {   3936,  4030,  4042,  4002,  3946,  3916,  3923,  3915,  3746,  3218,  2203,   766,  -800, -2124, -3002, -3467, -3661, -3726, -3886, -4431, -5386, -6454, -7342, -7890, -8010, -7646, -6769, -5390, -3676, -1968,  -504,   718,  1760,  2623,  3276,  3705,  3936, },
-	/* LAT: -30 */ {   2967,  3055,  3089,  3080,  3027,  2952,  2895,  2863,  2739,  2268,  1243,  -268, -1877, -3142, -3897, -4256, -4347, -4152, -3721, -3476, -3808, -4544, -5248, -5611, -5500, -4926, -3974, -2766, -1549,  -597,    84,   686,  1307,  1905,  2411,  2770,  2967, },
-	/* LAT: -20 */ {   2325,  2372,  2395,  2403,  2366,  2276,  2172,  2098,  1954,  1465,   420, -1060, -2542, -3616, -4166, -4297, -4108, -3564, -2709, -1896, -1594, -1931, -2579, -3057, -3106, -2756, -2125, -1306,  -522,   -26,   241,   544,   982,  1454,  1871,  2175,  2325, },
-	/* LAT: -10 */ {   1931,  1927,  1911,  1914,  1891,  1812,  1707,  1619,  1436,   891,  -161, -1533, -2812, -3656, -3943, -3732, -3168, -2392, -1546,  -783,  -305,  -322,  -783, -1292, -1513, -1430, -1119,  -618,  -115,   132,   186,   348,   715,  1146,  1533,  1815,  1931, },
-	/* LAT:   0 */ {   1719,  1687,  1636,  1633,  1627,  1564,  1465,  1356,  1104,   487,  -547, -1773, -2835, -3437, -3456, -2970, -2208, -1425,  -768,  -228,   190,   313,    43,  -374,  -634,  -693,  -594,  -330,   -36,    58,     3,    93,   431,   865,  1278,  1593,  1719, },
-	/* LAT:  10 */ {   1586,  1595,  1557,  1578,  1608,  1565,  1450,  1269,   895,   179,  -839, -1918, -2754, -3110, -2922, -2326, -1552,  -836,  -315,    69,   396,   551,   397,    76,  -162,  -270,  -292,  -208,  -100,  -138,  -272,  -244,    54,   501,   976,  1378,  1586, },
-	/* LAT:  20 */ {   1407,  1556,  1620,  1716,  1806,  1790,  1641,  1344,   802,   -54, -1098, -2055, -2664, -2787, -2470, -1875, -1165,  -512,   -46,   267,   527,   678,   592,   350,   148,    31,   -57,  -117,  -194,  -380,  -614,  -672,  -442,     0,   537,  1052,  1407, },
-	/* LAT:  30 */ {   1113,  1481,  1742,  1968,  2130,  2143,  1960,  1542,   816,  -213, -1336, -2222, -2649, -2598, -2206, -1631,  -976,  -355,   112,   421,   655,   805,   790,   642,   491,   367,   215,    10,  -268,  -640, -1008, -1169, -1012,  -590,   -17,   592,  1113, },
-	/* LAT:  40 */ {    765,  1352,  1846,  2238,  2488,  2533,  2320,  1789,   879,  -352, -1600, -2478, -2810, -2666, -2224, -1633,  -980,  -348,   166,   534,   808,  1010,  1109,  1100,  1024,   877,   614,   211,  -322,  -926, -1449, -1696, -1584, -1170,  -571,   103,   765, },
-	/* LAT:  50 */ {    485,  1233,  1914,  2469,  2834,  2940,  2711,  2056,   912,  -594, -2029, -2957, -3259, -3072, -2584, -1938, -1227,  -527,    92,   602,  1024,  1383,  1668,  1842,  1864,  1681,  1244,   545,  -342, -1244, -1928, -2221, -2100, -1659, -1021,  -284,   485, },
-	/* LAT:  60 */ {    303,  1159,  1965,  2657,  3158,  3367,  3149,  2331,   803, -1176, -2910, -3900, -4155, -3895, -3321, -2573, -1744,  -900,   -89,   666,  1362,  1996,  2543,  2944,  3110,  2932,  2306,  1204,  -203, -1527, -2404, -2721, -2558, -2061, -1361,  -552,   303, },
-	/* LAT:  70 */ {     96,  1037,  1936,  2734,  3345,  3633,  3369,  2208,   -78, -2838, -4805, -5625, -5623, -5130, -4353, -3412, -2381, -1310,  -231,   832,  1861,  2831,  3703,  4415,  4870,  4913,  4322,  2873,   698, -1410, -2719, -3169, -3010, -2473, -1716,  -838,    96, },
-	/* LAT:  80 */ {   -519,   407,  1264,  1958,  2340,  2137,   872, -1890, -5234, -7363, -8073, -7905, -7248, -6311, -5204, -3994, -2719, -1406,   -76,  1256,  2573,  3858,  5087,  6226,  7223,  7984,  8332,  7907,  6044,  2395, -1147, -2884, -3273, -2960, -2291, -1442,  -519, },
-	/* LAT:  90 */ { -30180,-28435,-26689,-24944,-23198,-21453,-19708,-17962,-16217,-14472,-12726,-10981, -9236, -7491, -5746, -4001, -2255,  -510,  1235,  2980,  4725,  6471,  8216,  9961, 11706, 13452, 15197, 16943, 18688, 20433, 22179, 23924, 25670, 27415, 29161, 30906,-30180, },
+	/* LAT: -90 */ {  26005, 24260, 22514, 20769, 19024, 17278, 15533, 13788, 12042, 10297,  8552,  6807,  5061,  3316,  1571,  -175, -1920, -3665, -5410, -7156, -8901,-10646,-12392,-14137,-15882,-17628,-19373,-21118,-22864,-24609,-26354,-28100,-29845, 31241, 29496, 27751, 26005, },
+	/* LAT: -80 */ {  22574, 20439, 18497, 16721, 15080, 13540, 12076, 10664,  9288,  7938,  6606,  5287,  3978,  2673,  1365,    47, -1294, -2664, -4073, -5523, -7016, -8551,-10127,-11744,-13407,-15124,-16908,-18779,-20760,-22877,-25150,-27583,-30153, 30038, 27417, 24908, 22574, },
+	/* LAT: -70 */ {  14970, 13574, 12450, 11491, 10624,  9795,  8955,  8069,  7117,  6099,  5029,  3935,  2849,  1792,   769,  -241, -1277, -2380, -3577, -4874, -6251, -7678, -9123,-10564,-11989,-13404,-14830,-16314,-17933,-19839,-22347,-26128, 30786, 24176, 19626, 16843, 14970, },
+	/* LAT: -60 */ {   8393,  8153,  7878,  7608,  7360,  7110,  6805,  6376,  5764,  4946,  3947,  2836,  1714,   682,  -204,  -966, -1695, -2514, -3511, -4701, -6025, -7391, -8710, -9920,-10986,-11886,-12604,-13099,-13271,-12822,-10729, -3567,  4821,  7600,  8386,  8521,  8393, },
+	/* LAT: -50 */ {   5464,  5503,  5451,  5365,  5295,  5262,  5230,  5106,  4766,  4107,  3101,  1830,   488,  -699, -1579, -2152, -2553, -2991, -3666, -4666, -5899, -7171, -8311, -9216, -9820,-10066, -9880, -9130, -7628, -5271, -2370,   376,  2485,  3909,  4783,  5258,  5464, },
+	/* LAT: -40 */ {   3939,  4033,  4044,  4003,  3947,  3916,  3923,  3915,  3744,  3215,  2199,   762,  -804, -2126, -3001, -3464, -3656, -3720, -3881, -4431, -5391, -6460, -7348, -7895, -8012, -7646, -6766, -5386, -3672, -1966,  -503,   719,  1762,  2625,  3279,  3708,  3939, },
+	/* LAT: -30 */ {   2970,  3057,  3091,  3081,  3027,  2951,  2894,  2862,  2737,  2265,  1238,  -274, -1883, -3146, -3898, -4254, -4343, -4145, -3714, -3473, -3812, -4550, -5253, -5613, -5499, -4923, -3970, -2762, -1547,  -597,    84,   685,  1307,  1906,  2413,  2772,  2970, },
+	/* LAT: -20 */ {   2328,  2375,  2397,  2404,  2365,  2275,  2170,  2096,  1951,  1461,   415, -1067, -2548, -3620, -4167, -4295, -4103, -3557, -2700, -1891, -1594, -1934, -2582, -3059, -3105, -2754, -2122, -1303,  -521,   -27,   239,   542,   980,  1454,  1872,  2177,  2328, },
+	/* LAT: -10 */ {   1934,  1929,  1912,  1914,  1891,  1810,  1705,  1616,  1433,   887,  -166, -1539, -2817, -3659, -3942, -3728, -3162, -2385, -1539,  -778,  -302,  -322,  -785, -1293, -1513, -1428, -1117,  -617,  -115,   131,   184,   345,   713,  1145,  1534,  1817,  1934, },
+	/* LAT:   0 */ {   1721,  1689,  1638,  1634,  1627,  1562,  1463,  1353,  1101,   483,  -552, -1778, -2839, -3438, -3453, -2965, -2202, -1419,  -763,  -224,   194,   314,    43,  -374,  -633,  -692,  -593,  -330,   -37,    57,     0,    90,   428,   864,  1279,  1595,  1721, },
+	/* LAT:  10 */ {   1587,  1596,  1558,  1578,  1608,  1563,  1447,  1265,   891,   175,  -844, -1922, -2756, -3109, -2918, -2321, -1546,  -831,  -311,    72,   399,   553,   398,    76,  -161,  -269,  -291,  -208,  -101,  -139,  -275,  -247,    51,   500,   976,  1380,  1587, },
+	/* LAT:  20 */ {   1408,  1557,  1621,  1716,  1805,  1788,  1639,  1341,   798,   -58, -1101, -2057, -2664, -2784, -2466, -1870, -1160,  -507,   -43,   270,   530,   680,   593,   351,   150,    32,   -57,  -118,  -195,  -382,  -617,  -675,  -444,    -1,   536,  1052,  1408, },
+	/* LAT:  30 */ {   1113,  1480,  1741,  1967,  2128,  2141,  1958,  1539,   813,  -216, -1338, -2223, -2648, -2595, -2202, -1625,  -970,  -351,   116,   425,   658,   808,   791,   643,   493,   369,   215,     9,  -270,  -643, -1011, -1171, -1014,  -592,   -17,   592,  1113, },
+	/* LAT:  40 */ {    762,  1349,  1844,  2236,  2486,  2531,  2317,  1786,   876,  -354, -1600, -2476, -2807, -2661, -2218, -1628,  -974,  -343,   170,   538,   811,  1013,  1111,  1102,  1026,   878,   614,   209,  -325,  -930, -1452, -1699, -1586, -1171,  -573,   101,   762, },
+	/* LAT:  50 */ {    480,  1229,  1909,  2465,  2831,  2937,  2708,  2054,   910,  -594, -2027, -2952, -3253, -3065, -2577, -1931, -1221,  -521,    98,   607,  1029,  1387,  1671,  1844,  1866,  1682,  1243,   542,  -346, -1249, -1931, -2223, -2102, -1661, -1023,  -287,   480, },
+	/* LAT:  60 */ {    296,  1151,  1957,  2650,  3152,  3362,  3145,  2328,   804, -1171, -2902, -3890, -4145, -3885, -3311, -2564, -1736,  -892,   -81,   673,  1368,  2002,  2548,  2948,  3113,  2933,  2305,  1199,  -210, -1532, -2409, -2724, -2560, -2064, -1365,  -558,   296, },
+	/* LAT:  70 */ {     83,  1023,  1921,  2720,  3332,  3621,  3360,  2206,   -69, -2819, -4783, -5605, -5606, -5115, -4339, -3399, -2370, -1300,  -222,   842,  1870,  2839,  3710,  4421,  4875,  4916,  4321,  2868,   689, -1420, -2727, -3176, -3017, -2481, -1726,  -849,    83, },
+	/* LAT:  80 */ {   -550,   375,  1231,  1924,  2307,  2109,   859, -1866, -5178, -7306, -8026, -7868, -7217, -6285, -5182, -3974, -2701, -1389,   -60,  1271,  2588,  3872,  5101,  6240,  7237,  7998,  8346,  7917,  6042,  2367, -1189, -2923, -3308, -2992, -2322, -1474,  -550, },
+	/* LAT:  90 */ { -30114,-28368,-26623,-24877,-23132,-21387,-19641,-17896,-16151,-14405,-12660,-10915, -9170, -7425, -5679, -3934, -2189,  -444,  1301,  3046,  4792,  6537,  8282, 10027, 11773, 13518, 15263, 17009, 18754, 20500, 22245, 23991, 25736, 27482, 29227, 30973,-30114, },
 };

 // Magnetic inclination data in radians * 10^-4
 // Model: WMM-2020,
 // Version: 0.5.1.11,
-// Date: 2021.6383,
+// Date: 2021.8164,
 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 */ { -12577,-12577,-12577,-12577,-12577,-12577,-12577,-12577,-12577,-12577,-12577,-12577,-12577,-12577,-12577,-12577,-12577,-12577,-12577,-12577,-12577,-12577,-12577,-12577,-12577,-12577,-12577,-12577,-12577,-12577,-12577,-12577,-12577,-12577,-12577,-12577,-12577, },
-	/* LAT: -80 */ { -13664,-13530,-13369,-13189,-12995,-12793,-12589,-12388,-12197,-12020,-11863,-11728,-11618,-11532,-11468,-11427,-11407,-11410,-11437,-11491,-11575,-11689,-11833,-12006,-12204,-12421,-12650,-12883,-13111,-13324,-13511,-13661,-13766,-13818,-13815,-13761,-13664, },
-	/* LAT: -70 */ { -14113,-13795,-13475,-13152,-12820,-12475,-12120,-11762,-11417,-11108,-10854,-10669,-10556,-10503,-10489,-10494,-10502,-10513,-10538,-10594,-10702,-10875,-11119,-11432,-11804,-12220,-12668,-13131,-13598,-14050,-14469,-14816,-15008,-14957,-14726,-14429,-14113, },
-	/* LAT: -60 */ { -13523,-13170,-12833,-12500,-12157,-11784,-11369,-10913,-10444,-10012, -9679, -9500, -9494, -9629, -9834,-10030,-10160,-10208,-10199,-10186,-10232,-10388,-10673,-11078,-11574,-12128,-12713,-13309,-13898,-14459,-14956,-15246,-15075,-14692,-14287,-13895,-13523, },
-	/* LAT: -50 */ { -12498,-12157,-11827,-11505,-11182,-10835,-10436, -9964, -9433, -8909, -8515, -8383, -8577, -9035, -9599,-10115,-10482,-10654,-10632,-10486,-10344,-10344,-10557,-10967,-11505,-12096,-12683,-13223,-13671,-13970,-14079,-14007,-13804,-13521,-13193,-12846,-12498, },
-	/* LAT: -40 */ { -11240,-10894,-10548,-10204, -9864, -9525, -9164, -8737, -8215, -7647, -7215, -7163, -7612, -8430, -9353,-10187,-10853,-11297,-11450,-11295,-10957,-10680,-10663,-10936,-11393,-11897,-12345,-12671,-12833,-12839,-12749,-12611,-12430,-12198,-11912,-11585,-11240, },
-	/* LAT: -30 */ {  -9601, -9226, -8850, -8461, -8068, -7692, -7333, -6940, -6426, -5813, -5353, -5421, -6174, -7368, -8620, -9722,-10642,-11362,-11775,-11782,-11420,-10913,-10571,-10562,-10809,-11130,-11390,-11507,-11448,-11272,-11096,-10958,-10810,-10602,-10321, -9976, -9601, },
-	/* LAT: -20 */ {  -7370, -6936, -6524, -6098, -5653, -5222, -4828, -4411, -3842, -3154, -2697, -2937, -4026, -5629, -7260, -8639, -9715,-10502,-10954,-11003,-10651,-10043, -9482, -9230, -9274, -9435, -9579, -9601, -9431, -9158, -8961, -8869, -8760, -8550, -8233, -7823, -7370, },
-	/* LAT: -10 */ {  -4413, -3886, -3439, -3006, -2549, -2097, -1680, -1219,  -587,   121,   495,    90, -1217, -3122, -5101, -6725, -7838, -8485, -8762, -8710, -8312, -7635, -6971, -6620, -6576, -6669, -6788, -6815, -6629, -6332, -6173, -6174, -6126, -5906, -5523, -4998, -4413, },
-	/* LAT:   0 */ {   -903,  -291,   166,   565,   982,  1400,  1794,  2245,  2837,  3422,  3636,  3158,  1883,   -14, -2061, -3728, -4756, -5200, -5268, -5105, -4674, -3967, -3262, -2886, -2824, -2895, -3022, -3097, -2968, -2731, -2671, -2804, -2856, -2665, -2241, -1616,  -903, },
-	/* LAT:  10 */ {   2564,  3180,  3607,  3945,  4299,  4666,  5022,  5416,  5876,  6254,  6299,  5824,  4757,  3202,  1510,   122,  -700,  -961,  -878,  -648,  -244,   388,  1020,  1361,  1423,  1375,  1272,  1181,  1224,  1330,  1260,  1004,   823,   900,  1251,  1849,  2564, },
-	/* LAT:  20 */ {   5418,  5939,  6314,  6606,  6919,  7263,  7609,  7962,  8302,  8509,  8428,  7975,  7149,  6059,  4932,  4015,  3475,  3346,  3489,  3728,  4055,  4515,  4975,  5231,  5286,  5265,  5214,  5158,  5148,  5133,  4964,  4639,  4349,  4263,  4435,  4854,  5418, },
-	/* LAT:  30 */ {   7569,  7939,  8254,  8535,  8844,  9192,  9552,  9897, 10179, 10297, 10158,  9742,  9118,  8408,  7745,  7229,  6931,  6883,  7020,  7228,  7472,  7769,  8056,  8230,  8285,  8295,  8296,  8286,  8266,  8187,  7969,  7621,  7270,  7052,  7036,  7228,  7569, },
-	/* LAT:  40 */ {   9266,  9486,  9741, 10026, 10353, 10715, 11085, 11427, 11682, 11768, 11622, 11265, 10793, 10319,  9921,  9634,  9481,  9473,  9580,  9739,  9912, 10094, 10265, 10390, 10467, 10526, 10578, 10612, 10597, 10490, 10251,  9904,  9539,  9255,  9112,  9124,  9266, },
-	/* LAT:  50 */ {  10801, 10923, 11125, 11395, 11719, 12075, 12431, 12749, 12974, 13035, 12898, 12600, 12235, 11889, 11612, 11423, 11327, 11321, 11385, 11485, 11597, 11712, 11828, 11943, 12060, 12181, 12293, 12366, 12359, 12237, 11994, 11668, 11329, 11044, 10854, 10774, 10801, },
-	/* LAT:  60 */ {  12318, 12393, 12544, 12764, 13036, 13338, 13641, 13909, 14086, 14114, 13978, 13728, 13438, 13166, 12944, 12786, 12693, 12660, 12674, 12720, 12787, 12871, 12976, 13107, 13265, 13440, 13604, 13714, 13723, 13606, 13382, 13104, 12825, 12588, 12417, 12326, 12318, },
-	/* LAT:  70 */ {  13759, 13803, 13900, 14044, 14225, 14430, 14640, 14822, 14926, 14905, 14765, 14562, 14343, 14138, 13964, 13828, 13733, 13677, 13657, 13669, 13710, 13781, 13884, 14019, 14185, 14371, 14555, 14697, 14746, 14676, 14516, 14317, 14122, 13958, 13837, 13770, 13759, },
-	/* LAT:  80 */ {  15001, 15015, 15054, 15115, 15193, 15280, 15360, 15405, 15383, 15301, 15186, 15060, 14936, 14822, 14722, 14639, 14577, 14536, 14517, 14520, 14546, 14595, 14666, 14758, 14869, 14995, 15130, 15262, 15369, 15414, 15378, 15294, 15200, 15116, 15053, 15014, 15001, },
-	/* LAT:  90 */ {  15392, 15392, 15392, 15392, 15392, 15392, 15392, 15392, 15392, 15392, 15392, 15392, 15392, 15392, 15392, 15392, 15392, 15392, 15392, 15392, 15392, 15392, 15392, 15392, 15392, 15392, 15392, 15392, 15392, 15392, 15392, 15392, 15392, 15392, 15392, 15392, 15392, },
+	/* LAT: -90 */ { -12576,-12576,-12576,-12576,-12576,-12576,-12576,-12576,-12576,-12576,-12576,-12576,-12576,-12576,-12576,-12576,-12576,-12576,-12576,-12576,-12576,-12576,-12576,-12576,-12576,-12576,-12576,-12576,-12576,-12576,-12576,-12576,-12576,-12576,-12576,-12576,-12576, },
+	/* LAT: -80 */ { -13662,-13529,-13368,-13187,-12993,-12792,-12587,-12387,-12195,-12019,-11862,-11727,-11617,-11531,-11467,-11426,-11406,-11409,-11436,-11490,-11574,-11688,-11832,-12006,-12203,-12420,-12649,-12882,-13110,-13323,-13510,-13660,-13765,-13817,-13814,-13760,-13662, },
+	/* LAT: -70 */ { -14112,-13793,-13474,-13151,-12818,-12474,-12118,-11760,-11416,-11107,-10853,-10668,-10555,-10503,-10489,-10493,-10502,-10512,-10536,-10593,-10700,-10874,-11119,-11432,-11804,-12220,-12668,-13132,-13598,-14051,-14469,-14816,-15007,-14955,-14725,-14427,-14112, },
+	/* LAT: -60 */ { -13522,-13169,-12831,-12499,-12155,-11783,-11367,-10912,-10444,-10012, -9679, -9501, -9496, -9631, -9836,-10031,-10160,-10207,-10197,-10184,-10230,-10387,-10673,-11078,-11574,-12129,-12714,-13310,-13899,-14460,-14957,-15247,-15075,-14692,-14287,-13894,-13522, },
+	/* LAT: -50 */ { -12497,-12156,-11826,-11504,-11181,-10834,-10435, -9963, -9432, -8909, -8515, -8385, -8580, -9038, -9603,-10118,-10484,-10653,-10630,-10483,-10340,-10342,-10557,-10968,-11506,-12097,-12684,-13224,-13672,-13971,-14079,-14007,-13804,-13521,-13193,-12846,-12497, },
+	/* LAT: -40 */ { -11240,-10893,-10547,-10203, -9863, -9525, -9163, -8737, -8215, -7648, -7217, -7166, -7618, -8437, -9359,-10192,-10857,-11299,-11450,-11292,-10953,-10677,-10662,-10936,-11394,-11899,-12347,-12672,-12833,-12839,-12749,-12611,-12431,-12198,-11912,-11585,-11240, },
+	/* LAT: -30 */ {  -9601, -9226, -8849, -8460, -8067, -7691, -7333, -6940, -6426, -5813, -5355, -5426, -6182, -7378, -8630, -9730,-10649,-11367,-11777,-11780,-11417,-10910,-10570,-10562,-10810,-11131,-11390,-11507,-11447,-11271,-11095,-10959,-10811,-10603,-10321, -9977, -9601, },
+	/* LAT: -20 */ {  -7370, -6935, -6522, -6095, -5651, -5220, -4827, -4410, -3842, -3155, -2699, -2943, -4037, -5641, -7272, -8649, -9723,-10508,-10957,-11003,-10649,-10040, -9479, -9229, -9273, -9435, -9578, -9599, -9429, -9156, -8960, -8869, -8761, -8552, -8235, -7825, -7370, },
+	/* LAT: -10 */ {  -4413, -3885, -3437, -3003, -2545, -2094, -1677, -1218,  -587,   120,   492,    82, -1229, -3138, -5116, -6736, -7845, -8490, -8764, -8710, -8309, -7631, -6967, -6617, -6575, -6667, -6786, -6812, -6626, -6329, -6171, -6174, -6128, -5909, -5525, -5000, -4413, },
+	/* LAT:   0 */ {   -904,  -289,   169,   569,   986,  1403,  1796,  2246,  2837,  3420,  3631,  3150,  1871,   -30, -2076, -3739, -4763, -5203, -5269, -5105, -4672, -3963, -3258, -2883, -2821, -2892, -3018, -3093, -2964, -2727, -2668, -2804, -2858, -2668, -2244, -1618,  -904, },
+	/* LAT:  10 */ {   2564,  3181,  3610,  3948,  4302,  4669,  5024,  5417,  5875,  6251,  6295,  5817,  4747,  3189,  1498,   113,  -705,  -964,  -879,  -647,  -241,   391,  1024,  1364,  1426,  1379,  1277,  1185,  1229,  1334,  1263,  1004,   820,   896,  1248,  1846,  2564, },
+	/* LAT:  20 */ {   5418,  5940,  6315,  6609,  6921,  7265,  7611,  7963,  8302,  8507,  8425,  7969,  7141,  6050,  4924,  4009,  3471,  3344,  3489,  3729,  4056,  4518,  4977,  5233,  5289,  5268,  5219,  5162,  5152,  5136,  4966,  4639,  4347,  4261,  4432,  4852,  5418, },
+	/* LAT:  30 */ {   7569,  7939,  8255,  8536,  8845,  9193,  9552,  9897, 10177, 10295, 10154,  9738,  9112,  8402,  7740,  7225,  6928,  6882,  7020,  7229,  7473,  7770,  8058,  8232,  8288,  8298,  8299,  8290,  8269,  8189,  7970,  7621,  7269,  7051,  7035,  7228,  7569, },
+	/* LAT:  40 */ {   9266,  9486,  9741, 10026, 10353, 10715, 11085, 11426, 11681, 11766, 11619, 11261, 10789, 10315,  9918,  9632,  9480,  9473,  9580,  9740,  9913, 10096, 10267, 10392, 10469, 10528, 10581, 10614, 10599, 10491, 10252,  9904,  9539,  9255,  9112,  9124,  9266, },
+	/* LAT:  50 */ {  10801, 10923, 11125, 11395, 11719, 12074, 12430, 12748, 12972, 13033, 12895, 12598, 12233, 11886, 11610, 11421, 11326, 11321, 11385, 11486, 11598, 11713, 11829, 11944, 12062, 12183, 12296, 12368, 12361, 12238, 11994, 11668, 11329, 11044, 10854, 10774, 10801, },
+	/* LAT:  60 */ {  12318, 12392, 12544, 12763, 13035, 13336, 13640, 13907, 14084, 14112, 13976, 13726, 13436, 13164, 12943, 12785, 12693, 12660, 12674, 12721, 12788, 12872, 12977, 13108, 13267, 13441, 13606, 13716, 13724, 13606, 13383, 13104, 12825, 12588, 12418, 12327, 12318, },
+	/* LAT:  70 */ {  13759, 13802, 13899, 14043, 14223, 14428, 14638, 14820, 14924, 14903, 14764, 14561, 14342, 14137, 13963, 13828, 13733, 13678, 13658, 13670, 13711, 13782, 13885, 14020, 14186, 14373, 14557, 14699, 14747, 14676, 14516, 14318, 14123, 13958, 13838, 13770, 13759, },
+	/* LAT:  80 */ {  15000, 15014, 15053, 15113, 15191, 15277, 15357, 15402, 15382, 15300, 15186, 15060, 14937, 14822, 14722, 14640, 14577, 14536, 14517, 14521, 14547, 14596, 14667, 14759, 14871, 14997, 15132, 15263, 15370, 15416, 15379, 15294, 15200, 15116, 15053, 15013, 15000, },
+	/* LAT:  90 */ {  15393, 15393, 15393, 15393, 15393, 15393, 15393, 15393, 15393, 15393, 15393, 15393, 15393, 15393, 15393, 15393, 15393, 15393, 15393, 15393, 15393, 15393, 15393, 15393, 15393, 15393, 15393, 15393, 15393, 15393, 15393, 15393, 15393, 15393, 15393, 15393, 15393, },
 };

 // Magnetic strength data in milli-Gauss * 10
 // Model: WMM-2020,
 // Version: 0.5.1.11,
-// Date: 2021.6383,
+// Date: 2021.8164,
 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 */ {  5457, 5457, 5457, 5457, 5457, 5457, 5457, 5457, 5457, 5457, 5457, 5457, 5457, 5457, 5457, 5457, 5457, 5457, 5457, 5457, 5457, 5457, 5457, 5457, 5457, 5457, 5457, 5457, 5457, 5457, 5457, 5457, 5457, 5457, 5457, 5457, 5457, },
-	/* LAT: -80 */ {  6063, 6000, 5922, 5830, 5728, 5616, 5498, 5377, 5254, 5134, 5020, 4915, 4821, 4741, 4678, 4632, 4608, 4605, 4626, 4672, 4742, 4835, 4949, 5081, 5224, 5374, 5523, 5666, 5797, 5911, 6004, 6073, 6118, 6138, 6134, 6108, 6063, },
-	/* LAT: -70 */ {  6308, 6175, 6026, 5862, 5684, 5492, 5288, 5073, 4853, 4638, 4435, 4252, 4094, 3963, 3858, 3781, 3734, 3721, 3748, 3824, 3950, 4130, 4357, 4624, 4918, 5225, 5529, 5813, 6062, 6267, 6418, 6515, 6557, 6550, 6501, 6418, 6308, },
-	/* LAT: -60 */ {  6192, 6001, 5801, 5593, 5375, 5141, 4885, 4607, 4314, 4024, 3756, 3528, 3348, 3213, 3113, 3040, 2989, 2969, 2994, 3083, 3250, 3500, 3825, 4209, 4631, 5065, 5486, 5870, 6196, 6445, 6609, 6689, 6692, 6630, 6517, 6367, 6192, },
-	/* LAT: -50 */ {  5848, 5620, 5389, 5159, 4927, 4683, 4413, 4110, 3781, 3449, 3146, 2905, 2742, 2650, 2602, 2568, 2535, 2509, 2514, 2585, 2756, 3043, 3436, 3905, 4411, 4917, 5393, 5812, 6152, 6395, 6534, 6576, 6534, 6425, 6264, 6067, 5848, },
-	/* LAT: -40 */ {  5396, 5152, 4908, 4669, 4435, 4199, 3945, 3661, 3347, 3021, 2721, 2497, 2378, 2352, 2371, 2393, 2400, 2389, 2377, 2405, 2531, 2802, 3215, 3726, 4273, 4798, 5266, 5656, 5949, 6137, 6227, 6230, 6162, 6032, 5851, 5634, 5396, },
-	/* LAT: -30 */ {  4880, 4641, 4403, 4169, 3944, 3725, 3507, 3276, 3020, 2745, 2487, 2303, 2231, 2254, 2321, 2393, 2460, 2512, 2536, 2548, 2613, 2806, 3162, 3646, 4177, 4675, 5096, 5416, 5620, 5721, 5748, 5721, 5642, 5511, 5332, 5116, 4880, },
-	/* LAT: -20 */ {  4322, 4111, 3903, 3699, 3503, 3321, 3153, 2990, 2813, 2615, 2425, 2289, 2246, 2286, 2375, 2486, 2614, 2744, 2836, 2874, 2898, 2989, 3223, 3603, 4051, 4479, 4830, 5068, 5175, 5183, 5153, 5106, 5024, 4897, 4731, 4534, 4322, },
-	/* LAT: -10 */ {  3791, 3631, 3479, 3333, 3198, 3078, 2975, 2884, 2788, 2674, 2553, 2452, 2404, 2425, 2509, 2637, 2793, 2953, 3080, 3145, 3160, 3184, 3303, 3551, 3874, 4196, 4463, 4629, 4666, 4614, 4547, 4483, 4394, 4269, 4120, 3957, 3791, },
-	/* LAT:   0 */ {  3412, 3320, 3237, 3165, 3110, 3072, 3047, 3029, 3007, 2960, 2881, 2786, 2704, 2669, 2708, 2809, 2941, 3077, 3194, 3271, 3303, 3324, 3396, 3550, 3757, 3970, 4151, 4259, 4267, 4200, 4112, 4019, 3907, 3776, 3642, 3519, 3412, },
-	/* LAT:  10 */ {  3283, 3253, 3233, 3230, 3255, 3303, 3360, 3415, 3452, 3443, 3374, 3260, 3131, 3034, 3004, 3043, 3123, 3221, 3321, 3407, 3471, 3533, 3619, 3736, 3870, 4007, 4127, 4198, 4202, 4141, 4032, 3889, 3728, 3569, 3434, 3338, 3283, },
-	/* LAT:  20 */ {  3400, 3404, 3431, 3486, 3579, 3701, 3831, 3950, 4032, 4045, 3972, 3830, 3662, 3519, 3440, 3426, 3459, 3531, 3626, 3723, 3814, 3911, 4021, 4132, 4240, 4350, 4451, 4518, 4530, 4473, 4337, 4136, 3909, 3700, 3538, 3438, 3400, },
-	/* LAT:  30 */ {  3723, 3732, 3788, 3889, 4033, 4205, 4382, 4539, 4648, 4675, 4603, 4447, 4256, 4089, 3980, 3932, 3934, 3983, 4069, 4166, 4262, 4367, 4483, 4601, 4720, 4845, 4965, 5051, 5079, 5023, 4868, 4630, 4356, 4102, 3904, 3776, 3723, },
-	/* LAT:  40 */ {  4222, 4223, 4290, 4416, 4584, 4772, 4957, 5114, 5219, 5244, 5176, 5026, 4838, 4661, 4531, 4453, 4425, 4445, 4504, 4583, 4668, 4764, 4879, 5012, 5163, 5324, 5474, 5585, 5626, 5573, 5419, 5182, 4908, 4649, 4439, 4294, 4222, },
-	/* LAT:  50 */ {  4832, 4827, 4885, 4998, 5145, 5306, 5456, 5576, 5650, 5658, 5594, 5466, 5302, 5136, 4997, 4898, 4842, 4828, 4852, 4901, 4969, 5059, 5178, 5327, 5502, 5686, 5853, 5972, 6018, 5976, 5846, 5650, 5425, 5208, 5028, 4900, 4832, },
-	/* LAT:  60 */ {  5392, 5382, 5413, 5478, 5565, 5660, 5746, 5812, 5844, 5834, 5779, 5683, 5562, 5432, 5312, 5215, 5147, 5112, 5109, 5136, 5192, 5278, 5395, 5542, 5708, 5877, 6025, 6130, 6176, 6155, 6072, 5946, 5799, 5655, 5533, 5443, 5392, },
-	/* LAT:  70 */ {  5726, 5708, 5707, 5720, 5743, 5770, 5793, 5807, 5806, 5785, 5745, 5687, 5615, 5538, 5464, 5399, 5350, 5322, 5317, 5336, 5380, 5449, 5541, 5649, 5766, 5881, 5981, 6055, 6095, 6098, 6070, 6016, 5948, 5878, 5813, 5762, 5726, },
-	/* LAT:  80 */ {  5789, 5772, 5758, 5747, 5737, 5728, 5718, 5706, 5691, 5672, 5649, 5623, 5596, 5568, 5543, 5522, 5508, 5502, 5507, 5522, 5547, 5582, 5625, 5673, 5723, 5771, 5814, 5848, 5873, 5886, 5888, 5882, 5868, 5849, 5829, 5808, 5789, },
+	/* LAT: -90 */ {  5456, 5456, 5456, 5456, 5456, 5456, 5456, 5456, 5456, 5456, 5456, 5456, 5456, 5456, 5456, 5456, 5456, 5456, 5456, 5456, 5456, 5456, 5456, 5456, 5456, 5456, 5456, 5456, 5456, 5456, 5456, 5456, 5456, 5456, 5456, 5456, 5456, },
+	/* LAT: -80 */ {  6062, 5999, 5920, 5829, 5726, 5615, 5497, 5375, 5253, 5133, 5019, 4913, 4819, 4740, 4676, 4631, 4607, 4604, 4625, 4671, 4741, 4834, 4949, 5080, 5223, 5373, 5523, 5666, 5797, 5911, 6003, 6073, 6117, 6137, 6133, 6108, 6062, },
+	/* LAT: -70 */ {  6306, 6174, 6025, 5861, 5683, 5491, 5286, 5071, 4852, 4636, 4433, 4251, 4092, 3961, 3857, 3780, 3732, 3720, 3747, 3823, 3950, 4129, 4357, 4624, 4918, 5225, 5529, 5813, 6063, 6267, 6418, 6514, 6557, 6550, 6501, 6417, 6306, },
+	/* LAT: -60 */ {  6191, 6000, 5800, 5592, 5374, 5139, 4883, 4605, 4312, 4022, 3755, 3527, 3346, 3212, 3112, 3039, 2988, 2968, 2993, 3083, 3250, 3500, 3825, 4210, 4632, 5065, 5487, 5871, 6196, 6445, 6610, 6689, 6691, 6629, 6517, 6367, 6191, },
+	/* LAT: -50 */ {  5848, 5619, 5387, 5158, 4926, 4681, 4411, 4108, 3779, 3447, 3145, 2904, 2741, 2649, 2601, 2567, 2534, 2508, 2513, 2584, 2755, 3043, 3437, 3906, 4412, 4918, 5394, 5813, 6153, 6395, 6534, 6576, 6534, 6425, 6264, 6067, 5848, },
+	/* LAT: -40 */ {  5396, 5151, 4907, 4668, 4434, 4197, 3943, 3659, 3346, 3019, 2720, 2496, 2378, 2351, 2370, 2393, 2398, 2388, 2376, 2404, 2531, 2802, 3216, 3728, 4274, 4799, 5267, 5657, 5950, 6138, 6227, 6231, 6162, 6032, 5851, 5634, 5396, },
+	/* LAT: -30 */ {  4880, 4640, 4402, 4168, 3943, 3724, 3505, 3274, 3019, 2744, 2486, 2302, 2230, 2254, 2321, 2393, 2459, 2511, 2534, 2547, 2612, 2806, 3163, 3648, 4179, 4677, 5098, 5417, 5621, 5721, 5748, 5721, 5642, 5511, 5332, 5116, 4880, },
+	/* LAT: -20 */ {  4322, 4110, 3903, 3698, 3503, 3320, 3152, 2989, 2812, 2614, 2423, 2288, 2245, 2286, 2375, 2486, 2614, 2744, 2835, 2872, 2897, 2989, 3224, 3604, 4053, 4480, 4831, 5069, 5175, 5184, 5153, 5106, 5024, 4897, 4731, 4534, 4322, },
+	/* LAT: -10 */ {  3790, 3631, 3479, 3333, 3197, 3077, 2974, 2883, 2787, 2673, 2552, 2451, 2403, 2425, 2509, 2637, 2793, 2953, 3079, 3144, 3159, 3184, 3303, 3552, 3876, 4198, 4464, 4629, 4666, 4614, 4547, 4483, 4394, 4269, 4120, 3957, 3790, },
+	/* LAT:   0 */ {  3412, 3320, 3237, 3165, 3109, 3072, 3046, 3028, 3006, 2958, 2880, 2785, 2703, 2669, 2708, 2809, 2942, 3078, 3194, 3270, 3302, 3324, 3396, 3551, 3758, 3971, 4152, 4260, 4267, 4200, 4112, 4020, 3908, 3776, 3643, 3519, 3412, },
+	/* LAT:  10 */ {  3283, 3252, 3233, 3230, 3255, 3303, 3359, 3414, 3450, 3441, 3373, 3258, 3130, 3033, 3004, 3043, 3123, 3221, 3322, 3407, 3471, 3533, 3620, 3737, 3871, 4008, 4128, 4199, 4203, 4142, 4033, 3890, 3728, 3569, 3434, 3338, 3283, },
+	/* LAT:  20 */ {  3400, 3404, 3431, 3486, 3578, 3700, 3830, 3948, 4030, 4043, 3970, 3828, 3660, 3518, 3440, 3425, 3459, 3531, 3627, 3724, 3814, 3912, 4022, 4133, 4241, 4351, 4453, 4519, 4531, 4474, 4338, 4136, 3910, 3701, 3538, 3438, 3400, },
+	/* LAT:  30 */ {  3723, 3731, 3787, 3888, 4032, 4204, 4380, 4537, 4646, 4673, 4601, 4445, 4254, 4087, 3980, 3932, 3934, 3983, 4069, 4166, 4263, 4368, 4484, 4602, 4721, 4847, 4966, 5053, 5080, 5024, 4869, 4630, 4357, 4103, 3904, 3777, 3723, },
+	/* LAT:  40 */ {  4222, 4222, 4289, 4414, 4583, 4771, 4955, 5112, 5217, 5243, 5174, 5025, 4836, 4660, 4530, 4453, 4425, 4446, 4505, 4584, 4669, 4766, 4880, 5013, 5164, 5325, 5476, 5586, 5627, 5574, 5420, 5183, 4909, 4649, 4439, 4295, 4222, },
+	/* LAT:  50 */ {  4832, 4826, 4884, 4996, 5144, 5304, 5454, 5575, 5648, 5657, 5593, 5465, 5301, 5136, 4997, 4898, 4842, 4829, 4852, 4902, 4970, 5060, 5179, 5329, 5504, 5688, 5854, 5973, 6019, 5976, 5846, 5651, 5425, 5209, 5029, 4901, 4832, },
+	/* LAT:  60 */ {  5392, 5382, 5412, 5477, 5564, 5658, 5745, 5810, 5843, 5833, 5778, 5683, 5561, 5432, 5312, 5215, 5148, 5113, 5110, 5137, 5193, 5279, 5396, 5543, 5709, 5878, 6026, 6131, 6176, 6155, 6073, 5946, 5799, 5656, 5533, 5443, 5392, },
+	/* LAT:  70 */ {  5726, 5708, 5706, 5719, 5742, 5769, 5792, 5806, 5805, 5785, 5745, 5687, 5615, 5539, 5464, 5400, 5351, 5323, 5317, 5337, 5381, 5450, 5542, 5650, 5768, 5882, 5982, 6055, 6095, 6099, 6070, 6017, 5949, 5879, 5814, 5762, 5726, },
+	/* LAT:  80 */ {  5789, 5772, 5758, 5747, 5737, 5728, 5718, 5706, 5690, 5672, 5649, 5623, 5596, 5568, 5543, 5522, 5508, 5503, 5507, 5522, 5548, 5583, 5626, 5674, 5723, 5772, 5814, 5849, 5873, 5887, 5889, 5882, 5868, 5850, 5829, 5808, 5789, },
 	/* LAT:  90 */ {  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, 5680, 5680, 5680, 5680, 5680, 5680, 5680, 5680, },
 };
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-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
@@ -39,6 +39,10 @@

 #include "AirspeedValidator.hpp"

+AirspeedValidator::AirspeedValidator()
+{
+	reset_CAS_scale_check(); //this resets all elements of the Vectors to NAN
+}

 void
 AirspeedValidator::update_airspeed_validator(const airspeed_validator_update_data &input_data)
@@ -46,14 +50,16 @@ AirspeedValidator::update_airspeed_validator(const airspeed_validator_update_dat
 	// get indicated airspeed from input data (raw airspeed)
 	_IAS = input_data.airspeed_indicated_raw;

-	update_CAS_scale();
+	update_CAS_scale_validated(input_data.lpos_valid, input_data.ground_velocity, input_data.airspeed_true_raw);
+	update_CAS_scale_applied();
 	update_CAS_TAS(input_data.air_pressure_pa, input_data.air_temperature_celsius);
-	update_wind_estimator(input_data.timestamp, input_data.airspeed_true_raw, input_data.lpos_valid, input_data.lpos_vx,
-			      input_data.lpos_vy,
-			      input_data.lpos_vz, input_data.lpos_evh, input_data.lpos_evv, input_data.att_q);
+	update_wind_estimator(input_data.timestamp, input_data.airspeed_true_raw, input_data.lpos_valid,
+			      input_data.ground_velocity, input_data.lpos_evh, input_data.lpos_evv, input_data.att_q);
 	update_in_fixed_wing_flight(input_data.in_fixed_wing_flight);
-	check_airspeed_innovation(input_data.timestamp, input_data.vel_test_ratio, input_data.mag_test_ratio);
+	check_airspeed_data_stuck(input_data.timestamp);
 	check_load_factor(input_data.accel_z);
+	check_airspeed_innovation(input_data.timestamp, input_data.vel_test_ratio, input_data.mag_test_ratio,
+				  input_data.ground_velocity);
 	update_airspeed_valid_status(input_data.timestamp);
 }

@@ -66,14 +72,12 @@ AirspeedValidator::reset_airspeed_to_invalid(const uint64_t timestamp)

 void
 AirspeedValidator::update_wind_estimator(const uint64_t time_now_usec, float airspeed_true_raw, bool lpos_valid,
-		float lpos_vx, float lpos_vy,
-		float lpos_vz, float lpos_evh, float lpos_evv, const float att_q[4])
+		const matrix::Vector3f &vI, float lpos_evh, float lpos_evv, const float att_q[4])
 {
 	_wind_estimator.update(time_now_usec);

 	if (lpos_valid && _in_fixed_wing_flight) {

-		Vector3f vI(lpos_vx, lpos_vy, lpos_vz);
 		Quatf q(att_q);

 		// airspeed fusion (with raw TAS)
@@ -92,50 +96,124 @@ AirspeedValidator::get_wind_estimator_states(uint64_t timestamp)
 	airspeed_wind_s wind_est = {};

 	wind_est.timestamp = timestamp;
-	float wind[2];
-	_wind_estimator.get_wind(wind);
-	wind_est.windspeed_north = wind[0];
-	wind_est.windspeed_east = wind[1];
-	float wind_cov[2];
-	_wind_estimator.get_wind_var(wind_cov);
-	wind_est.variance_north = wind_cov[0];
-	wind_est.variance_east = wind_cov[1];
+	wind_est.windspeed_north = _wind_estimator.get_wind()(0);
+	wind_est.windspeed_east = _wind_estimator.get_wind()(1);
+	wind_est.variance_north = _wind_estimator.get_wind_var()(0);
+	wind_est.variance_east = _wind_estimator.get_wind_var()(1);
 	wind_est.tas_innov = _wind_estimator.get_tas_innov();
 	wind_est.tas_innov_var = _wind_estimator.get_tas_innov_var();
 	wind_est.beta_innov = _wind_estimator.get_beta_innov();
 	wind_est.beta_innov_var = _wind_estimator.get_beta_innov_var();
-	wind_est.tas_scale = _wind_estimator.get_tas_scale();
+	wind_est.tas_scale_raw = _wind_estimator.get_tas_scale();
+	wind_est.tas_scale_raw_var = _wind_estimator.get_tas_scale_var();
+	wind_est.tas_scale_validated = _CAS_scale_validated;
 	return wind_est;
 }

 void
-AirspeedValidator::set_airspeed_scale_manual(float airspeed_scale_manual)
+AirspeedValidator::update_CAS_scale_validated(bool lpos_valid, const matrix::Vector3f &vI, float airspeed_true_raw)
 {
-	_airspeed_scale_manual = airspeed_scale_manual;
-	_wind_estimator.enforce_airspeed_scale(1.0f / airspeed_scale_manual); // scale is inverted inside the wind estimator
+	if (!_in_fixed_wing_flight || !lpos_valid) {
+		return;
+	}
+
+	// reset every 100s as we assume that all the samples for current check are in similar wind conditions
+	if (hrt_elapsed_time(&_begin_current_scale_check) > 100_s) {
+		reset_CAS_scale_check();
+	}
+
+	const float course_over_ground_rad = matrix::wrap_2pi(atan2f(vI(0), vI(1)));
+	const int segment_index = int(SCALE_CHECK_SAMPLES * course_over_ground_rad / (2.f * M_PI_F));
+
+	_scale_check_groundspeed(segment_index) = vI.norm();
+	_scale_check_TAS(segment_index) = airspeed_true_raw;
+
+	// run check if all segments are filled
+	if (PX4_ISFINITE(_scale_check_groundspeed.norm_squared())) {
+
+		float ground_speed_sum = 0.f;
+		float TAS_sum = 0.f;
+
+		for (int i = 0; i < SCALE_CHECK_SAMPLES; i++) {
+			ground_speed_sum += _scale_check_groundspeed(i);
+			TAS_sum += _scale_check_TAS(i);
+		}
+
+		const float TAS_to_grounspeed_error_current = ground_speed_sum - TAS_sum * _CAS_scale_validated;
+		const float TAS_to_grounspeed_error_new = ground_speed_sum - TAS_sum * _wind_estimator.get_tas_scale();
+
+		// check passes if the average airspeed with the scale applied is closer to groundspeed than without
+		if (fabsf(TAS_to_grounspeed_error_new) < fabsf(TAS_to_grounspeed_error_current)) {
+
+			// constrain the scale update to max 0.01 at a time
+			const float new_scale_constrained = math::constrain(_wind_estimator.get_tas_scale(), _CAS_scale_validated - 0.01f,
+							    _CAS_scale_validated + 0.01f);
+
+			_CAS_scale_validated = new_scale_constrained;
+		}
+
+		reset_CAS_scale_check();
+	}
 }

 void
-AirspeedValidator::update_CAS_scale()
+AirspeedValidator::reset_CAS_scale_check()
 {
-	if (_wind_estimator.is_estimate_valid()) {
-		_CAS_scale = 1.0f / math::constrain(_wind_estimator.get_tas_scale(), 0.5f, 2.0f);

-	} else {
-		_CAS_scale = _airspeed_scale_manual;
+	_scale_check_groundspeed.setAll(NAN);
+	_scale_check_TAS.setAll(NAN);
+
+	_begin_current_scale_check = hrt_absolute_time();
+}
+
+void
+AirspeedValidator::update_CAS_scale_applied()
+{
+	switch (_tas_scale_apply) {
+	default:
+
+	/* fallthrough */
+	case 0:
+
+	/* fallthrough */
+	case 1:
+		_CAS_scale_applied = _tas_scale_init;
+		break;
+
+	case 2:
+		_CAS_scale_applied = _CAS_scale_validated;
+		break;
 	}
 }

 void
 AirspeedValidator::update_CAS_TAS(float air_pressure_pa, float air_temperature_celsius)
 {
-	_CAS = calc_CAS_from_IAS(_IAS, _CAS_scale);
+	_CAS = calc_CAS_from_IAS(_IAS, _CAS_scale_applied);
 	_TAS = calc_TAS_from_CAS(_CAS, air_pressure_pa, air_temperature_celsius);
 }

+void
+AirspeedValidator::check_airspeed_data_stuck(uint64_t time_now)
+{
+	// data stuck test: trigger when IAS is not changing for DATA_STUCK_TIMEOUT (2s)
+
+	if (!_data_stuck_check_enabled) {
+		_data_stuck_test_failed = false;
+		return;
+	}
+
+	if (fabsf(_IAS - _IAS_prev) > FLT_EPSILON || _time_last_unequal_data == 0) {
+		_time_last_unequal_data = time_now;
+		_IAS_prev = _IAS;
+	}
+
+	_data_stuck_test_failed = hrt_elapsed_time(&_time_last_unequal_data) > DATA_STUCK_TIMEOUT;
+}
+
 void
 AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_status_vel_test_ratio,
-		float estimator_status_mag_test_ratio)
+		float estimator_status_mag_test_ratio, const matrix::Vector3f &vI)
 {
 	// Check normalised innovation levels with requirement for continuous data and use of hysteresis
 	// to prevent false triggering.
@@ -144,8 +222,8 @@ AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_
 		_time_wind_estimator_initialized = time_now;
 	}

-	// reset states if we are not flying or wind estimator was just initialized/reset
-	if (!_in_fixed_wing_flight || (time_now - _time_wind_estimator_initialized) < 10_s
+	// reset states if check is disabled, we are not flying or wind estimator was just initialized/reset
+	if (!_innovation_check_enabled || !_in_fixed_wing_flight || (time_now - _time_wind_estimator_initialized) < 5_s
 	    || _tas_innov_integ_threshold <= 0.f) {
 		_innovations_check_failed = false;
 		_time_last_tas_pass = time_now;
@@ -159,13 +237,13 @@ AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_

 	} else {
 		// nav velocity data is likely good so airspeed innovations are able to be used
-		// compute the ratio of innovation to gate size
-		const float dt_s = math::constrain((time_now - _time_last_aspd_innov_check) / 1e6f, 0.01f, 0.2f); // limit to [100,5] Hz
-		const float tas_test_ratio = _wind_estimator.get_tas_innov() * _wind_estimator.get_tas_innov()
-					     / (fmaxf(_tas_gate, 1.0f) * fmaxf(_tas_gate, 1.f) * _wind_estimator.get_tas_innov_var());
+		const float dt_s = math::constrain((time_now - _time_last_aspd_innov_check) / 1e6f, 0.01f, 0.2f); // limit to [5,100] Hz
+		matrix::Vector2f wind_2d(_wind_estimator.get_wind());
+		matrix::Vector3f air_vel = vI - matrix::Vector3f {wind_2d(0), wind_2d(1), 0.f};
+		const float tas_innov = fabsf(_TAS - air_vel.norm());

-		if (tas_test_ratio > _tas_innov_threshold) {
-			_apsd_innov_integ_state += dt_s * (tas_test_ratio - _tas_innov_threshold); // integrate exceedance
+		if (tas_innov > _tas_innov_threshold) {
+			_apsd_innov_integ_state += dt_s * (tas_innov - _tas_innov_threshold); // integrate exceedance

 		} else {
 			// reset integrator used to trigger and record pass if integrator check is disabled
@@ -188,6 +266,12 @@ AirspeedValidator::check_load_factor(float accel_z)
 {
 	// Check if the airspeed reading is lower than physically possible given the load factor

+	if (!_load_factor_check_enabled) {
+		_load_factor_ratio = 0.5f;
+		_load_factor_check_failed = false;
+		return;
+	}
+
 	if (_in_fixed_wing_flight) {

 		float max_lift_ratio = fmaxf(_CAS, 0.7f) / fmaxf(_airspeed_stall, 1.0f);
@@ -205,12 +289,12 @@ AirspeedValidator::check_load_factor(float accel_z)
 void
 AirspeedValidator::update_airspeed_valid_status(const uint64_t timestamp)
 {
-	if (_innovations_check_failed || _load_factor_check_failed) {
-		// either innovation or load factor check failed, so record timestamp
+	if (_data_stuck_test_failed || _innovations_check_failed || _load_factor_check_failed) {
+		// at least one check (data stuck, innovation or load factor) failed, so record timestamp
 		_time_checks_failed = timestamp;

-	} else if (!_innovations_check_failed && !_load_factor_check_failed) {
-		// both innovation or load factor checks must pass to declare airspeed good
+	} else if (! _data_stuck_test_failed && !_innovations_check_failed && !_load_factor_check_failed) {
+		// all checks(data stuck, innovation and load factor) must pass to declare airspeed good
 		_time_checks_passed = timestamp;
 	}

@@ -223,7 +307,7 @@ AirspeedValidator::update_airspeed_valid_status(const uint64_t timestamp)
 		// a timeout period is applied.
 		const bool single_check_fail_timeout = (timestamp - _time_checks_passed) > _checks_fail_delay * 1_s;

-		if (both_checks_failed || single_check_fail_timeout) {
+		if (both_checks_failed || single_check_fail_timeout || _data_stuck_test_failed) {

 			_airspeed_valid = false;
 		}
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-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
@@ -55,9 +55,7 @@ struct airspeed_validator_update_data {
 	float airspeed_indicated_raw;
 	float airspeed_true_raw;
 	uint64_t airspeed_timestamp;
-	float lpos_vx;
-	float lpos_vy;
-	float lpos_vz;
+	matrix::Vector3f ground_velocity;
 	bool lpos_valid;
 	float lpos_evh;
 	float lpos_evv;
@@ -73,7 +71,7 @@ struct airspeed_validator_update_data {
 class AirspeedValidator
 {
 public:
-	AirspeedValidator() = default;
+	AirspeedValidator();
 	~AirspeedValidator() = default;

 	void update_airspeed_validator(const airspeed_validator_update_data &input_data);
@@ -84,13 +82,18 @@ public:
 	float get_CAS() { return _CAS; }
 	float get_TAS() { return _TAS; }
 	bool get_airspeed_valid() { return _airspeed_valid; }
-	float get_CAS_scale() {return _CAS_scale;}
+	float get_CAS_scale_validated() {return _CAS_scale_validated;}

 	airspeed_wind_s get_wind_estimator_states(uint64_t timestamp);

 	// setters wind estimator parameters
 	void set_wind_estimator_wind_p_noise(float wind_sigma) { _wind_estimator.set_wind_p_noise(wind_sigma); }
 	void set_wind_estimator_tas_scale_p_noise(float tas_scale_sigma) { _wind_estimator.set_tas_scale_p_noise(tas_scale_sigma); }
+	void set_wind_estimator_tas_scale_init(float tas_scale_init)
+	{
+		_tas_scale_init = tas_scale_init;
+	}
+
 	void set_wind_estimator_tas_noise(float tas_sigma) { _wind_estimator.set_tas_noise(tas_sigma); }
 	void set_wind_estimator_beta_noise(float beta_var) { _wind_estimator.set_beta_noise(beta_var); }
 	void set_wind_estimator_tas_gate(uint8_t gate_size)
@@ -100,9 +103,6 @@ public:
 	}

 	void set_wind_estimator_beta_gate(uint8_t gate_size) { _wind_estimator.set_beta_gate(gate_size); }
-	void set_wind_estimator_scale_estimation_on(bool scale_estimation_on) { _wind_estimator_scale_estimation_on = scale_estimation_on;}
-
-	void set_airspeed_scale_manual(float airspeed_scale_manual);

 	// setters for failure detection tuning parameters
 	void set_tas_innov_threshold(float tas_innov_threshold) { _tas_innov_threshold = tas_innov_threshold; }
@@ -112,20 +112,39 @@ public:

 	void set_airspeed_stall(float airspeed_stall) { _airspeed_stall = airspeed_stall; }

+	void set_tas_scale_apply(int tas_scale_apply) { _tas_scale_apply = tas_scale_apply; }
+	void set_CAS_scale_validated(float scale) { _CAS_scale_validated = scale; }
+	void set_scale_init(float scale) { _wind_estimator.set_scale_init(scale); }
+
+	void set_enable_data_stuck_check(bool enable) { _data_stuck_check_enabled = enable; }
+	void set_enable_innovation_check(bool enable) { _innovation_check_enabled = enable; }
+	void set_enable_load_factor_check(bool enable) { _load_factor_check_enabled = enable; }
+
 private:

 	WindEstimator _wind_estimator{}; ///< wind estimator instance running in this particular airspeedValidator

-	// wind estimator parameter
-	bool _wind_estimator_scale_estimation_on{false};	///< online scale estimation (IAS-->CAS) is on
-	float _airspeed_scale_manual{1.0f}; ///< manually entered airspeed scale
+	// Check enabling
+	bool _data_stuck_check_enabled{false};
+	bool _innovation_check_enabled{false};
+	bool _load_factor_check_enabled{false};
+
+	// airspeed scale validity check
+	static constexpr int SCALE_CHECK_SAMPLES = 12; ///< take samples from 12 segments (every 360/12=30°)

 	// general states
 	bool _in_fixed_wing_flight{false}; ///< variable to bypass innovation and load factor checks
 	float _IAS{0.0f}; ///< indicated airsped in m/s
 	float _CAS{0.0f}; ///< calibrated airspeed in m/s
 	float _TAS{0.0f}; ///< true airspeed in m/s
-	float _CAS_scale{1.0f}; ///< scale factor from IAS to CAS
+	float _CAS_scale_applied{1.0f}; ///< scale factor from IAS to CAS (currently applied value)
+	float _CAS_scale_validated{1.0f}; ///< scale factor from IAS to CAS (currently estimated value)
+
+	// data stuck check
+	uint64_t _time_last_unequal_data{0};
+	bool _data_stuck_test_failed{false};
+	float _IAS_prev{0.f};
+	static constexpr uint64_t DATA_STUCK_TIMEOUT{2_s}; ///< timeout after which data stuck check triggers when data is flat

 	// states of innovation check
 	float _tas_gate{1.0f}; ///< gate size of airspeed innovation (to calculate tas_test_ratio)
@@ -146,22 +165,32 @@ private:
 	// states of airspeed valid declaration
 	bool _airspeed_valid{true}; ///< airspeed valid (pitot or groundspeed-windspeed)
 	int _checks_fail_delay{3}; ///< delay for airspeed invalid declaration after single check failure (Sec)
-	int _checks_clear_delay{3}; ///< delay for airspeed valid declaration after all checks passed again (Sec)
+	int _checks_clear_delay{-1}; ///< delay for airspeed valid declaration after all checks passed again (Sec)
 	uint64_t	_time_checks_passed{0};	///< time the checks have last passed (uSec)
 	uint64_t	_time_checks_failed{0};	///< time the checks have last not passed (uSec)

+	hrt_abstime _begin_current_scale_check{0};
+
+	int _tas_scale_apply{0};
+	float _tas_scale_init{1.f};
+
+	matrix::Vector<float, SCALE_CHECK_SAMPLES> _scale_check_TAS {};
+	matrix::Vector<float, SCALE_CHECK_SAMPLES> _scale_check_groundspeed {};
+
 	void update_in_fixed_wing_flight(bool in_fixed_wing_flight) { _in_fixed_wing_flight = in_fixed_wing_flight; }

-	void update_wind_estimator(const uint64_t timestamp, float airspeed_true_raw, bool lpos_valid, float lpos_vx,
-				   float lpos_vy,
-				   float lpos_vz,
+	void update_wind_estimator(const uint64_t timestamp, float airspeed_true_raw, bool lpos_valid,
+				   const matrix::Vector3f &vI,
 				   float lpos_evh, float lpos_evv, const float att_q[4]);
-	void update_CAS_scale();
+	void update_CAS_scale_validated(bool lpos_valid, const matrix::Vector3f &vI, float airspeed_true_raw);
+	void update_CAS_scale_applied();
 	void update_CAS_TAS(float air_pressure_pa, float air_temperature_celsius);
+	void check_airspeed_data_stuck(uint64_t timestamp);
 	void check_airspeed_innovation(uint64_t timestamp, float estimator_status_vel_test_ratio,
-				       float estimator_status_mag_test_ratio);
+				       float estimator_status_mag_test_ratio, const matrix::Vector3f &vI);
 	void check_load_factor(float accel_z);
 	void update_airspeed_valid_status(const uint64_t timestamp);
 	void reset();
+	void reset_CAS_scale_check();

 };
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-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
@@ -149,13 +149,21 @@ private:
 	bool _in_takeoff_situation{true}; /**< in takeoff situation (defined as not yet stall speed reached) */
 	float _ground_minus_wind_TAS{0.0f}; /**< true airspeed from groundspeed minus windspeed */
 	float _ground_minus_wind_CAS{0.0f}; /**< calibrated airspeed from groundspeed minus windspeed */
-
-	bool _scale_estimation_previously_on{false}; /**< scale_estimation was on in the last cycle */
+	bool _armed_prev{false};

 	hrt_abstime _time_last_airspeed_update[MAX_NUM_AIRSPEED_SENSORS] {};

 	perf_counter_t _perf_elapsed{};

+	float _param_airspeed_scale[MAX_NUM_AIRSPEED_SENSORS] {}; /** array to save the airspeed scale params in */
+
+	enum CheckTypeBits {
+		CHECK_TYPE_ONLY_DATA_MISSING_BIT = (1 << 0),
+		CHECK_TYPE_DATA_STUCK_BIT = (1 << 1),
+		CHECK_TYPE_INNOVATION_BIT = (1 << 2),
+		CHECK_TYPE_LOAD_FACTOR_BIT = (1 << 3)
+	};
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::ASPD_W_P_NOISE>) _param_west_w_p_noise,
 		(ParamFloat<px4::params::ASPD_SC_P_NOISE>) _param_west_sc_p_noise,
@@ -163,8 +171,10 @@ private:
 		(ParamFloat<px4::params::ASPD_BETA_NOISE>) _param_west_beta_noise,
 		(ParamInt<px4::params::ASPD_TAS_GATE>) _param_west_tas_gate,
 		(ParamInt<px4::params::ASPD_BETA_GATE>) _param_west_beta_gate,
-		(ParamInt<px4::params::ASPD_SCALE_EST>) _param_west_scale_estimation_on,
-		(ParamFloat<px4::params::ASPD_SCALE>) _param_west_airspeed_scale,
+		(ParamInt<px4::params::ASPD_SCALE_APPLY>) _param_aspd_scale_apply,
+		(ParamFloat<px4::params::ASPD_SCALE_1>) _param_airspeed_scale_1,
+		(ParamFloat<px4::params::ASPD_SCALE_2>) _param_airspeed_scale_2,
+		(ParamFloat<px4::params::ASPD_SCALE_3>) _param_airspeed_scale_3,
 		(ParamInt<px4::params::ASPD_PRIMARY>) _param_airspeed_primary_index,
 		(ParamInt<px4::params::ASPD_DO_CHECKS>) _param_airspeed_checks_on,
 		(ParamInt<px4::params::ASPD_FALLBACK_GW>) _param_airspeed_fallback_gw,
@@ -291,6 +301,12 @@ AirspeedModule::Run()

 	if (!_initialized) {
 		init(); // initialize airspeed validator instances
+
+		for (int i = 0; i < MAX_NUM_AIRSPEED_SENSORS; i++) {
+			_airspeed_validator[i].set_CAS_scale_validated(_param_airspeed_scale[i]);
+			_airspeed_validator[i].set_scale_init(_param_airspeed_scale[i]);
+		}
+
 		_initialized = true;
 	}

@@ -300,7 +316,7 @@ AirspeedModule::Run()
 		update_params();
 	}

-	bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
+	const bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);

 	// check for new connected airspeed sensors as long as we're disarmed
 	if (!armed) {
@@ -320,12 +336,12 @@ AirspeedModule::Run()
 					       _position_setpoint.type != position_setpoint_s::SETPOINT_TYPE_LAND &&
 					       _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING;

+		const matrix::Vector3f vI(_vehicle_local_position.vx, _vehicle_local_position.vy, _vehicle_local_position.vz);
+
 		// Prepare data for airspeed_validator
 		struct airspeed_validator_update_data input_data = {};
 		input_data.timestamp = _time_now_usec;
-		input_data.lpos_vx = _vehicle_local_position.vx;
-		input_data.lpos_vy = _vehicle_local_position.vy;
-		input_data.lpos_vz = _vehicle_local_position.vz;
+		input_data.ground_velocity = vI;
 		input_data.lpos_valid = _vehicle_local_position_valid;
 		input_data.lpos_evh = _vehicle_local_position.evh;
 		input_data.lpos_evv = _vehicle_local_position.evv;
@@ -375,11 +391,44 @@ AirspeedModule::Run()
 				_airspeed_validator[i].reset_airspeed_to_invalid(_time_now_usec);

 			}
+
+			// save estimated airspeed scale after disarm
+			if (!armed && _armed_prev) {
+				if (_param_aspd_scale_apply.get() > 0) {
+					if (fabsf(_airspeed_validator[i].get_CAS_scale_validated() - _param_airspeed_scale[i]) > 0.01f) {
+						// apply the new scale if changed more than 0.01
+						mavlink_log_info(&_mavlink_log_pub, "Airspeed sensor Nr. %d ASPD_SCALE updated: %.2f --> %.2f", i + 1,
+								 (double)_param_airspeed_scale[i],
+								 (double)_airspeed_validator[i].get_CAS_scale_validated());
+
+						switch (i) {
+						case 0:
+							_param_airspeed_scale_1.set(_airspeed_validator[i].get_CAS_scale_validated());
+							_param_airspeed_scale_1.commit_no_notification();
+							break;
+
+						case 1:
+							_param_airspeed_scale_2.set(_airspeed_validator[i].get_CAS_scale_validated());
+							_param_airspeed_scale_2.commit_no_notification();
+							break;
+
+						case 2:
+							_param_airspeed_scale_3.set(_airspeed_validator[i].get_CAS_scale_validated());
+							_param_airspeed_scale_3.commit_no_notification();
+							break;
+						}
+					}
+				}
+
+				_airspeed_validator[i].set_scale_init(_param_airspeed_scale[i]);
+			}
 		}
 	}

 	select_airspeed_and_publish();

+	_armed_prev = armed;
+
 	perf_end(_perf_elapsed);

 	if (should_exit()) {
@@ -391,6 +440,10 @@ void AirspeedModule::update_params()
 {
 	updateParams();

+	_param_airspeed_scale[0] = _param_airspeed_scale_1.get();
+	_param_airspeed_scale[1] = _param_airspeed_scale_2.get();
+	_param_airspeed_scale[2] = _param_airspeed_scale_3.get();
+
 	_wind_estimator_sideslip.set_wind_p_noise(_param_west_w_p_noise.get());
 	_wind_estimator_sideslip.set_tas_scale_p_noise(_param_west_sc_p_noise.get());
 	_wind_estimator_sideslip.set_tas_noise(_param_west_tas_noise.get());
@@ -398,62 +451,30 @@ void AirspeedModule::update_params()
 	_wind_estimator_sideslip.set_tas_gate(_param_west_tas_gate.get());
 	_wind_estimator_sideslip.set_beta_gate(_param_west_beta_gate.get());

-	for (int i = 0; i < _number_of_airspeed_sensors; i++) {
+	for (int i = 0; i < MAX_NUM_AIRSPEED_SENSORS; i++) {
 		_airspeed_validator[i].set_wind_estimator_wind_p_noise(_param_west_w_p_noise.get());
 		_airspeed_validator[i].set_wind_estimator_tas_scale_p_noise(_param_west_sc_p_noise.get());
 		_airspeed_validator[i].set_wind_estimator_tas_noise(_param_west_tas_noise.get());
 		_airspeed_validator[i].set_wind_estimator_beta_noise(_param_west_beta_noise.get());
 		_airspeed_validator[i].set_wind_estimator_tas_gate(_param_west_tas_gate.get());
 		_airspeed_validator[i].set_wind_estimator_beta_gate(_param_west_beta_gate.get());
-		_airspeed_validator[i].set_wind_estimator_scale_estimation_on(_param_west_scale_estimation_on.get());

-		// only apply manual entered airspeed scale to first airspeed measurement
-		// TODO: enable multiple airspeed sensors
-		_airspeed_validator[0].set_airspeed_scale_manual(_param_west_airspeed_scale.get());
+		_airspeed_validator[i].set_tas_scale_apply(_param_aspd_scale_apply.get());
+		_airspeed_validator[i].set_wind_estimator_tas_scale_init(_param_airspeed_scale[i]);

 		_airspeed_validator[i].set_tas_innov_threshold(_tas_innov_threshold.get());
 		_airspeed_validator[i].set_tas_innov_integ_threshold(_tas_innov_integ_threshold.get());
 		_airspeed_validator[i].set_checks_fail_delay(_checks_fail_delay.get());
 		_airspeed_validator[i].set_checks_clear_delay(_checks_clear_delay.get());
 		_airspeed_validator[i].set_airspeed_stall(_param_fw_airspd_stall.get());
+
+		_airspeed_validator[i].set_enable_data_stuck_check(_param_airspeed_checks_on.get() &
+				CheckTypeBits::CHECK_TYPE_DATA_STUCK_BIT);
+		_airspeed_validator[i].set_enable_innovation_check(_param_airspeed_checks_on.get() &
+				CheckTypeBits::CHECK_TYPE_INNOVATION_BIT);
+		_airspeed_validator[i].set_enable_load_factor_check(_param_airspeed_checks_on.get() &
+				CheckTypeBits::CHECK_TYPE_LOAD_FACTOR_BIT);
 	}
-
-	// if the airspeed scale estimation is enabled and the airspeed is valid,
-	// then set the scale inside the wind estimator to -1 such that it starts to estimate it
-	if (!_scale_estimation_previously_on && _param_west_scale_estimation_on.get()) {
-		if (_valid_airspeed_index > 0) {
-			// set it to a negative value to start estimation inside wind estimator
-			_airspeed_validator[0].set_airspeed_scale_manual(-1.0f);
-
-		} else {
-			mavlink_log_info(&_mavlink_log_pub, "Airspeed: can't estimate scale as no valid sensor.\t");
-			events::send(events::ID("airspeed_selector_cannot_est_scale"), events::Log::Error,
-				     "Airspeed: cannot estimate scale as there is no valid sensor");
-			_param_west_scale_estimation_on.set(0); // reset this param to 0 as estimation was not turned on
-			_param_west_scale_estimation_on.commit_no_notification();
-		}
-
-	} else if (_scale_estimation_previously_on && !_param_west_scale_estimation_on.get()) {
-		if (_valid_airspeed_index > 0) {
-
-			_param_west_airspeed_scale.set(_airspeed_validator[_valid_airspeed_index - 1].get_CAS_scale());
-			_param_west_airspeed_scale.commit_no_notification();
-			_airspeed_validator[_valid_airspeed_index - 1].set_airspeed_scale_manual(_param_west_airspeed_scale.get());
-
-			mavlink_log_info(&_mavlink_log_pub, "Airspeed: estimated scale (ASPD_SCALE): %0.2f\t",
-					 (double)_airspeed_validator[_valid_airspeed_index - 1].get_CAS_scale());
-			events::send<float>(events::ID("airspeed_selector_scale"), events::Log::Info,
-					    "Airspeed: estimated scale (ASPD_SCALE): {1:.2}", _airspeed_validator[_valid_airspeed_index - 1].get_CAS_scale());
-
-		} else {
-			mavlink_log_info(&_mavlink_log_pub, "Airspeed: can't estimate scale as no valid sensor.\t");
-			events::send(events::ID("airspeed_selector_cannot_est_scale2"), events::Log::Error,
-				     "Airspeed: cannot estimate scale as there is no valid sensor");
-		}
-	}
-
-	_scale_estimation_previously_on = _param_west_scale_estimation_on.get();
-
 }

 void AirspeedModule::poll_topics()
@@ -497,19 +518,17 @@ void AirspeedModule::update_wind_estimator_sideslip()
 	}

 	_wind_estimate_sideslip.timestamp = _time_now_usec;
-	float wind[2];
-	_wind_estimator_sideslip.get_wind(wind);
-	_wind_estimate_sideslip.windspeed_north = wind[0];
-	_wind_estimate_sideslip.windspeed_east = wind[1];
-	float wind_cov[2];
-	_wind_estimator_sideslip.get_wind_var(wind_cov);
-	_wind_estimate_sideslip.variance_north = wind_cov[0];
-	_wind_estimate_sideslip.variance_east = wind_cov[1];
-	_wind_estimate_sideslip.tas_innov = _wind_estimator_sideslip.get_tas_innov();
-	_wind_estimate_sideslip.tas_innov_var = _wind_estimator_sideslip.get_tas_innov_var();
+	_wind_estimate_sideslip.windspeed_north = _wind_estimator_sideslip.get_wind()(0);
+	_wind_estimate_sideslip.windspeed_east = _wind_estimator_sideslip.get_wind()(1);
+	_wind_estimate_sideslip.variance_north = _wind_estimator_sideslip.get_wind_var()(0);
+	_wind_estimate_sideslip.variance_east = _wind_estimator_sideslip.get_wind_var()(1);
+	_wind_estimate_sideslip.tas_innov = NAN;
+	_wind_estimate_sideslip.tas_innov_var = NAN;
 	_wind_estimate_sideslip.beta_innov = _wind_estimator_sideslip.get_beta_innov();
 	_wind_estimate_sideslip.beta_innov_var = _wind_estimator_sideslip.get_beta_innov_var();
-	_wind_estimate_sideslip.tas_scale = _wind_estimator_sideslip.get_tas_scale();
+	_wind_estimate_sideslip.tas_scale_raw = NAN;
+	_wind_estimate_sideslip.tas_scale_raw_var = NAN;
+	_wind_estimate_sideslip.tas_scale_validated = NAN;
 	_wind_estimate_sideslip.source = airspeed_wind_s::SOURCE_AS_BETA_ONLY;
 }

@@ -527,10 +546,20 @@ void AirspeedModule::update_ground_minus_wind_airspeed()

 void AirspeedModule::select_airspeed_and_publish()
 {
-	const bool airspeed_sensor_switching_necessary = _prev_airspeed_index < airspeed_index::FIRST_SENSOR_INDEX ||
-			!_airspeed_validator[_prev_airspeed_index - 1].get_airspeed_valid();
+
+	// we need to re-evaluate the sensors if we're currently not on a phyisical sensor or the current sensor got invalid
+	bool airspeed_sensor_switching_necessary = false;
+
+	if (_prev_airspeed_index < airspeed_index::FIRST_SENSOR_INDEX) {
+		airspeed_sensor_switching_necessary = true;
+
+	} else {
+		airspeed_sensor_switching_necessary = !_airspeed_validator[_prev_airspeed_index - 1].get_airspeed_valid();
+	}
+
 	const bool airspeed_sensor_switching_allowed = _number_of_airspeed_sensors > 0 &&
 			_param_airspeed_primary_index.get() > airspeed_index::GROUND_MINUS_WIND_INDEX && _param_airspeed_checks_on.get();
+
 	const bool airspeed_sensor_added = _prev_number_of_airspeed_sensors < _number_of_airspeed_sensors;

 	if (airspeed_sensor_switching_necessary && (airspeed_sensor_switching_allowed || airspeed_sensor_added)) {
@@ -7,6 +7,7 @@
 * @min 0
 * @max 1
 * @unit m/s^2
+ * @decimal 2
 * @group Airspeed Validator
 */
 PARAM_DEFINE_FLOAT(ASPD_W_P_NOISE, 0.1f);
@@ -19,9 +20,10 @@ PARAM_DEFINE_FLOAT(ASPD_W_P_NOISE, 0.1f);
 * @min 0
 * @max 0.1
 * @unit Hz
+ * @decimal 5
 * @group Airspeed Validator
 */
-PARAM_DEFINE_FLOAT(ASPD_SC_P_NOISE, 0.0001);
+PARAM_DEFINE_FLOAT(ASPD_SC_P_NOISE, 0.0001f);

 /**
 * Airspeed Selector: Wind estimator true airspeed measurement noise
@@ -31,9 +33,10 @@ PARAM_DEFINE_FLOAT(ASPD_SC_P_NOISE, 0.0001);
 * @min 0
 * @max 4
 * @unit m/s
+ * @decimal 1
 * @group Airspeed Validator
 */
-PARAM_DEFINE_FLOAT(ASPD_TAS_NOISE, 1.4);
+PARAM_DEFINE_FLOAT(ASPD_TAS_NOISE, 1.4f);

 /**
 * Airspeed Selector: Wind estimator sideslip measurement noise
@@ -43,9 +46,10 @@ PARAM_DEFINE_FLOAT(ASPD_TAS_NOISE, 1.4);
 * @min 0
 * @max 1
 * @unit rad
+ * @decimal 3
 * @group Airspeed Validator
 */
-PARAM_DEFINE_FLOAT(ASPD_BETA_NOISE, 0.3);
+PARAM_DEFINE_FLOAT(ASPD_BETA_NOISE, 0.3f);

 /**
 * Airspeed Selector: Gate size for true airspeed fusion
@@ -72,27 +76,56 @@ PARAM_DEFINE_INT32(ASPD_TAS_GATE, 3);
 PARAM_DEFINE_INT32(ASPD_BETA_GATE, 1);

 /**
- * Automatic airspeed scale estimation on
+ * Controls when to apply the new estimated airspeed scale(s)
 *
- * Turns the automatic airspeed scale (scale from IAS to CAS) on or off. It is recommended to fly level
- * altitude while performing the estimation. Set to 1 to start estimation (best when already flying).
- * Set to 0 to end scale estimation. The estimated scale is then saved using the ASPD_SCALE parameter.
- *
- * @boolean
+ * @value 0 Do not automatically apply the estimated scale
+ * @value 1 Apply the estimated scale after disarm
+ * @value 2 Apply the estimated scale in air
 * @group Airspeed Validator
 */
-PARAM_DEFINE_INT32(ASPD_SCALE_EST, 0);
+PARAM_DEFINE_INT32(ASPD_SCALE_APPLY, 1);

 /**
- * Airspeed scale (scale from IAS to CAS)
+ * Scale of airspeed sensor 1
 *
- * Scale can either be entered manually, or estimated in-flight by setting ASPD_SCALE_EST to 1.
+ * This is the scale IAS --> CAS of the first airspeed sensor instance
 *
 * @min 0.5
- * @max 1.5
+ * @max 2.0
+ * @decimal 2
+ * @reboot_required true
 * @group Airspeed Validator
+ * @volatile
 */
-PARAM_DEFINE_FLOAT(ASPD_SCALE, 1.0f);
+PARAM_DEFINE_FLOAT(ASPD_SCALE_1, 1.0f);
+
+/**
+ * Scale of airspeed sensor 2
+ *
+ * This is the scale IAS --> CAS of the second airspeed sensor instance
+ *
+ * @min 0.5
+ * @max 2.0
+ * @decimal 2
+ * @reboot_required true
+ * @group Airspeed Validator
+ * @volatile
+ */
+PARAM_DEFINE_FLOAT(ASPD_SCALE_2, 1.0f);
+
+/**
+ * Scale of airspeed sensor 3
+ *
+ * This is the scale IAS --> CAS of the third airspeed sensor instance
+ *
+ * @min 0.5
+ * @max 2.0
+ * @decimal 2
+ * @reboot_required true
+ * @group Airspeed Validator
+ * @volatile
+ */
+PARAM_DEFINE_FLOAT(ASPD_SCALE_3, 1.0f);

 /**
 * Index or primary airspeed measurement source
@@ -112,12 +145,18 @@ PARAM_DEFINE_INT32(ASPD_PRIMARY, 1);
 /**
 * Enable checks on airspeed sensors
 *
- * If set to true then the data comming from the airspeed sensors is checked for validity. Only applied if ASPD_PRIMARY > 0.
+ * Controls which checks are run to check airspeed data for validity. Only applied if ASPD_PRIMARY > 0.
+ * Note that the data missing check is enabled if any of the options is set.
 *
- * @boolean
+ * @min 0
+ * @max 15
+ * @bit 0 Only data missing check (triggers if more than 1s no data)
+ * @bit 1 Data stuck (triggers if data is exactly constant for 2s)
+ * @bit 2 Innovation check (see ASPD_FS_INNOV)
+ * @bit 3 Load factor check (triggers if measurement is below stall speed)
 * @group Airspeed Validator
 */
-PARAM_DEFINE_INT32(ASPD_DO_CHECKS, 1);
+PARAM_DEFINE_INT32(ASPD_DO_CHECKS, 7);

 /**
 * Enable fallback to sensor-less airspeed estimation
@@ -133,32 +172,34 @@ PARAM_DEFINE_INT32(ASPD_DO_CHECKS, 1);
 PARAM_DEFINE_INT32(ASPD_FALLBACK_GW, 0);

 /**
- * Airspeed failsafe consistency threshold
+ * Airspeed failure innovation threshold
 *
- * This specifies the minimum airspeed test ratio required to trigger a failsafe. Larger values make the check less sensitive,
- * smaller values make it more sensitive. Start with a value of 1.0 when tuning. When tas_test_ratio is > 1.0 it indicates the
- * inconsistency between predicted and measured airspeed is large enough to cause the wind EKF to reject airspeed measurements.
+ * This specifies the minimum airspeed innovation required to trigger a failsafe. Larger values make the check less sensitive,
+ * smaller values make it more sensitive. Large innovations indicate an inconsistency between predicted (groundspeed - windspeeed)
+ * and measured airspeed.
 * The time required to detect a fault when the threshold is exceeded depends on the size of the exceedance and is controlled by the ASPD_FS_INTEG parameter.
-*
+ *
+ * @unit m/s
 * @min 0.5
- * @max 3.0
+ * @max 10.0
+ * @decimal 1
 * @group Airspeed Validator
 */
-PARAM_DEFINE_FLOAT(ASPD_FS_INNOV, 1.0f);
+PARAM_DEFINE_FLOAT(ASPD_FS_INNOV, 5.f);

 /**
- * Airspeed failsafe consistency delay
+ * Airspeed failure innovation integral threshold
 *
- * This sets the time integral of airspeed test ratio exceedance above ASPD_FS_INNOV required to trigger a failsafe.
- * For example if ASPD_FS_INNOV is 1 and estimator_status.tas_test_ratio is 2.0, then the exceedance is 1.0 and the integral will
- * rise at a rate of 1.0/second. A negative value disables the check. Larger positive values make the check less sensitive, smaller positive values
- * make it more sensitive.
+ * This sets the time integral of airspeed innovation exceedance above ASPD_FS_INNOV required to trigger a failsafe.
+ * Larger values make the check less sensitive, smaller positive values make it more sensitive.
 *
- * @unit s
- * @max 30.0
+ * @unit m
+ * @min 0.0
+ * @max 50.0
+ * @decimal 1
 * @group Airspeed Validator
 */
-PARAM_DEFINE_FLOAT(ASPD_FS_INTEG, 5.0f);
+PARAM_DEFINE_FLOAT(ASPD_FS_INTEG, 10.f);

 /**
 * Airspeed failsafe stop delay
@@ -49,8 +49,8 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &
 {
 	bool success = true; // start with a pass and change to a fail if any test fails

-	int32_t mag_strength_check_enabled = 1;
-	param_get(param_find("COM_ARM_MAG_STR"), &mag_strength_check_enabled);
+	int32_t mag_strength_check = 1;
+	param_get(param_find("COM_ARM_MAG_STR"), &mag_strength_check);

 	float hgt_test_ratio_limit = 1.f;
 	param_get(param_find("COM_ARM_EKF_HGT"), &hgt_test_ratio_limit);
@@ -107,13 +107,20 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &
 		goto out;
 	}

-	if ((mag_strength_check_enabled == 1) && status.pre_flt_fail_mag_field_disturbed) {
-		if (report_fail) {
-			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: strong magnetic interference detected");
-		}
+	if ((mag_strength_check >= 1) && status.pre_flt_fail_mag_field_disturbed) {
+		const char *message = "Preflight%s: Strong magnetic interference detected";

-		success = false;
-		goto out;
+		if (mag_strength_check == 1) {
+			if (report_fail) {
+				mavlink_log_critical(mavlink_log_pub, message, " Fail");
+			}
+
+			success = false;
+			goto out;
+
+		} else if (report_fail) {
+			mavlink_log_warning(mavlink_log_pub, message, "");
+		}
 	}

 	// check vertical position innovation test ratio
@@ -1211,13 +1211,28 @@ Commander::handle_command(const vehicle_command_s &cmd)

 	case vehicle_command_s::VEHICLE_CMD_DO_ORBIT:

-		// Switch to orbit state and let the orbit task handle the command further
-		if (TRANSITION_DENIED != main_state_transition(_status, commander_state_s::MAIN_STATE_ORBIT, _status_flags,
-				_internal_state)) {
+		transition_result_t main_ret;
+
+		if (_status.in_transition_mode) {
+			main_ret = TRANSITION_DENIED;
+
+		} else if (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+			// for fixed wings the behavior of orbit is the same as loiter
+			main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER,
+							 _status_flags, _internal_state);
+
+		} else {
+			// Switch to orbit state and let the orbit task handle the command further
+			main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_ORBIT, _status_flags,
+							 _internal_state);
+		}
+
+		if ((main_ret != TRANSITION_DENIED)) {
 			cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;

 		} else {
 			cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+			mavlink_log_critical(&_mavlink_log_pub, "Orbit command rejected");
 		}

 		break;
@@ -3897,7 +3912,7 @@ void Commander::estimator_check()

 		if (run_quality_checks && _status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {

-			if (_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
+			if (_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
 				_nav_test_failed = false;
 				_nav_test_passed = false;

@@ -620,10 +620,13 @@ PARAM_DEFINE_INT32(COM_ARM_MAG_ANG, 45);
 /**
 * Enable mag strength preflight check
 *
- * Deny arming if the estimator detects a strong magnetic
+ * Check if the estimator detects a strong magnetic
 * disturbance (check enabled by EKF2_MAG_CHECK)
 *
- * @boolean
+ * @value 0 Disabled
+ * @value 1 Deny arming
+ * @value 2 Warning only
+ *
 * @group Commander
 */
 PARAM_DEFINE_INT32(COM_ARM_MAG_STR, 1);
@@ -827,6 +827,8 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
 			status.nav_state = vehicle_status_s::NAVIGATION_STATE_OFFBOARD;
 		}

+		break;
+
 	default:
 		break;
 	}
@@ -0,0 +1,253 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "ActuatorEffectivenessCustom.hpp"
+
+ActuatorEffectivenessCustom::ActuatorEffectivenessCustom(ModuleParams *parent):
+	ModuleParams(parent)
+{
+}
+
+float getScaleParameter(uint8_t n)
+{
+	// CA_ACTn_SC: scale
+	char buffer[17];
+	sprintf(buffer, "CA_ACT%u_SC", n);
+	param_t param_handle = param_find(buffer);
+
+	if (param_handle == PARAM_INVALID) {
+		PX4_ERR("invalid parameter %s", buffer);
+		return 0.f;
+	}
+
+	float scale = 1.f;
+
+	if (param_get(param_handle, &scale) != PX4_OK) {
+		PX4_ERR("param_get %s failed", buffer);
+		return scale;
+	}
+
+	return 0.f;
+}
+
+bool ActuatorEffectivenessCustom::getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix,
+		bool force)
+{
+	// TODO:
+	//  - airspeed scaling
+	//  - battery scaling
+	//  - publish motors and servos (handle ranges)
+	//  - disable individual actuators
+	//      - feedback from output module (actual failures)
+	//      - VTOL transitions (cleanly blend in and out?)
+	//      -   eg disable all downward thrust
+	//      -   eg disable all control surfaces
+	//      -  blend in/out higher level by adjusting max? then completely purge from effectiveness once off
+	if (_updated || force) {
+		_updated = false;
+
+		int num_actuators = 0;
+		_effectiveness.zero();
+
+		for (int n = 0; n < NUM_ACTUATORS; n++) {
+			// CA_ACTn_FUNC: look up function
+			char buffer[17];
+			sprintf(buffer, "CA_ACT%u_FUNC", n);
+			int32_t ctrl_func = 0;
+			param_get(param_find(buffer), &ctrl_func);
+
+			switch (ctrl_func) {
+			case 0:
+				// disabled
+				break;
+
+			case 1: {
+					// 1: custom thrust (parameters)
+					matrix::Vector3f axis{};
+					matrix::Vector3f position{};
+					float thrust_coef = 0.f;
+					float moment_ratio = 0.f;
+
+					for (int i = 0; i < 3; i++) {
+						char axis_char = 'X' + i;
+
+						// CA_ACTn_P{X,Y,Z}
+						sprintf(buffer, "CA_ACT%u_P%c", n, axis_char);
+
+						if (param_get(param_find(buffer), &position(i)) != PX4_OK) {
+							PX4_ERR("unable to get %s", buffer);
+						}
+
+						// CA_ACTn_A_{X,Y,Z}
+						sprintf(buffer, "CA_ACT%u_A%c", n, axis_char);
+
+						if (param_get(param_find(buffer), &axis(i)) != PX4_OK) {
+							PX4_ERR("unable to get %s", buffer);
+						}
+					}
+
+					// CA_ACTn_CT TODO
+					sprintf(buffer, "CA_ACT%u_CT", n);
+					param_get(param_find(buffer), &thrust_coef);
+
+					// CA_ACTn_KM TODO
+					sprintf(buffer, "CA_ACT%u_KM", n);
+					param_get(param_find(buffer), &moment_ratio);
+
+					// Normalize axis
+					float axis_norm = axis.norm();
+
+					if (axis_norm > FLT_EPSILON) {
+						axis /= axis_norm;
+
+					} else {
+						// Bad axis definition, ignore this rotor
+						continue;
+					}
+
+					// Get coefficients
+					float ct = thrust_coef;
+					float km = moment_ratio;
+
+					if (fabsf(ct) < FLT_EPSILON) {
+						continue;
+					}
+
+					// Compute thrust generated by this rotor
+					const matrix::Vector3f thrust{ct * axis};
+
+					// Compute moment generated by this rotor
+					const matrix::Vector3f moment{ct * position.cross(axis) - ct *km * axis};
+
+					// Fill corresponding items in effectiveness matrix
+					for (size_t j = 0; j < 3; j++) {
+						_effectiveness(j, n) = moment(j);
+						_effectiveness(j + 3, n) = thrust(j);
+					}
+				}
+
+				break;
+
+			case 2: {
+					// 2: custom torque (parameters)
+
+					// CA_ACTn_TRQ_R
+					// CA_ACTn_TRQ_P
+					// CA_ACTn_TRQ_Y
+					char torque_str[3][17];
+					sprintf(torque_str[0], "CA_ACT%u_TRQ_R", n);
+					sprintf(torque_str[1], "CA_ACT%u_TRQ_P", n);
+					sprintf(torque_str[2], "CA_ACT%u_TRQ_Y", n);
+
+					for (int i = 0; i < 3; i++) {
+						// CA_ACTn_TRQ_{R,P,Y}
+						param_get(param_find(torque_str[i]), &_effectiveness(i, n));
+					}
+				}
+				break;
+
+			case 100: {
+					// 100: Aileron, scaled (CA_ACTn_SC) roll torque
+					_effectiveness(0, n) = getScaleParameter(n);
+				}
+				break;
+
+			case 101: {
+					// 101: Elevator, scaled (CA_ACTn_SC) pitch torque
+					_effectiveness(1, n) = getScaleParameter(n);
+				}
+				break;
+
+			case 102: {
+					// 102: Rudder, scaled (CA_ACTn_SC) yaw torque
+					_effectiveness(2, n) = getScaleParameter(n);
+				}
+				break;
+
+			case 103: {
+					// 103: Throttle, scaled (CA_ACTn_SC) X thrust
+					_effectiveness(4, n) = getScaleParameter(n);
+				}
+				break;
+
+			case 104: {
+					// 104: Elevon(+), scaled (CA_ACTn_SC) roll & pitch torque
+					float scale = getScaleParameter(n);
+					_effectiveness(0, n) = 0.5f * scale;
+					_effectiveness(1, n) = 0.5f * scale;
+				}
+				break;
+
+			case 105: {
+					// 105: Elevon(+), scaled (CA_ACTn_SC) -roll & pitch torque
+					float scale = getScaleParameter(n);
+					_effectiveness(0, n) = -0.5f * scale;
+					_effectiveness(1, n) =  0.5f * scale;
+				}
+				break;
+
+			case 106: {
+					// 106: V-Tail (+), scaled (CA_ACTn_SC) pitch & yaw torque
+					float scale = getScaleParameter(n);
+					_effectiveness(1, n) = 0.5f * scale;
+					_effectiveness(2, n) = 0.5f * scale;
+				}
+				break;
+
+			case 107: {
+					// 107: V-Tail (-), scaled (CA_ACTn_SC) pitch & -yaw torque
+					float scale = getScaleParameter(n);
+					_effectiveness(1, n) =  0.5f * scale;
+					_effectiveness(2, n) = -0.5f * scale;
+				}
+				break;
+
+			default:
+				PX4_ERR("%s invalid function %d", buffer, n);
+				break;
+			}
+
+			if (_effectiveness.col(n).longerThan(0.f)) {
+				num_actuators = n + 1;
+			}
+		}
+
+		_effectiveness.transpose().print();
+
+		_num_actuators = num_actuators;
+		matrix = _effectiveness;
+		return true;
+	}
+
+	return false;
+}
@@ -0,0 +1,67 @@
+/****************************************************************************
+ *
+ *   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 ActuatorEffectivenessCustom.hpp
+ *
+ * Actuator effectiveness computed from rotors position and orientation
+ *
+ * @author Julien Lecoeur <julien.lecoeur@gmail.com>
+ */
+
+#pragma once
+
+#include "ActuatorEffectiveness.hpp"
+
+#include <px4_platform_common/module_params.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionInterval.hpp>
+
+using namespace time_literals;
+
+class ActuatorEffectivenessCustom: public ModuleParams, public ActuatorEffectiveness
+{
+public:
+	ActuatorEffectivenessCustom(ModuleParams *parent);
+	virtual ~ActuatorEffectivenessCustom() = default;
+
+	bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix, bool force) override;
+
+	int numActuators() const override { return _num_actuators; }
+private:
+	matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> _effectiveness{};
+
+	int _num_actuators{0};
+
+	bool _updated{true};
+};
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
Daniel Agar	7e224d6bb5	[RFC] control_allocator: custom configuration parameters	2021-11-02 22:08:17 -04:00
Daniel Agar	8f6fd5f37b	sensors/vehicle_angular_velocity: gyro RPM dynamic notch filter handle negative RPM - some UAVCAN ESCs report negative RPM for reverse rotation - lower hard coded safety limit RPM limit to 10 Hz (600 RPM) - avoid disabling notch filters that weren't configured	2021-11-02 09:36:02 -04:00
Peter van der Perk	c6f249f7f1	Kconfig merge_config only set to 'n' when BOOL Run make updateconfig to fix delta's of derived configs	2021-11-02 09:34:21 -04:00
Daniel Agar	fec0d6c5ed	ekf2: change indication further reduce data precision	2021-11-02 10:21:54 +01:00
bresch	1317b1a6e1	[AUTO COMMIT] update change indication	2021-11-01 13:59:34 +01:00
bresch	9e54c6d1aa	ekf2: move generic functions to control.cpp these functions aren't specific to GPS fusion	2021-11-01 13:59:34 +01:00
bresch	e90734881b	ekf2_test: add more GPS fusion control tests	2021-11-01 13:59:34 +01:00
bresch	8aae39e82a	ekf2: move GPS control logic to separate source file	2021-11-01 13:59:34 +01:00
bresch	689ab12845	ekf2: refactor gps vel/pos fusion control logic	2021-11-01 13:59:34 +01:00
bresch	9afc390552	ekf2: move gps yaw reset in starting function	2021-11-01 13:59:34 +01:00
Daniel Agar	1461eb0e32	logger: increase optimization to ${MAX_CUSTOM_OPT_LEVEL} - ${MAX_CUSTOM_OPT_LEVEL} is -O2 on boards that aren't flash constrained	2021-11-01 09:13:12 +01:00
Thomas Debrunner	32be88404a	commander: Only run estimator navigation checks when armed	2021-10-28 12:06:31 -04:00
PX4 BuildBot	7006b0aee9	Update submodule jMAVSim to latest Thu Oct 28 12:38:55 UTC 2021 - jMAVSim in PX4/Firmware (`c9b89ee869`): https://github.com/PX4/jMAVSim/commit/0a5a8c6d9da05bee5f82faa25430213d97d396b0 - jMAVSim current upstream: https://github.com/PX4/jMAVSim/commit/b23dc53d558e801b214fbcb605a061c9773105e0 - Changes: https://github.com/PX4/jMAVSim/compare/0a5a8c6d9da05bee5f82faa25430213d97d396b0...b23dc53d558e801b214fbcb605a061c9773105e0 b23dc53 2021-10-08 Ilya Petrov - Use low resolution texture also on Windows	2021-10-28 11:41:02 -04:00
Harrison MG	c9b89ee869	fixed ulanding_radar autostart command	2021-10-28 00:01:37 -04:00
Daniel Agar	be5c5856e7	cmake: px4_add_module always add module to PX4_MODULE_PATHS so that parameters aren't pruned	2021-10-27 23:00:40 -04:00
Peter van der Perk	51abb804ac	UAVCANv1 Fix NodeClient header and Kconfig merge logic	2021-10-27 10:07:01 -04:00
Beat Küng	0decdb1c7b	github action: run ./Tools/generate_board_targets_json.py in container As it requires kconfiglib	2021-10-27 15:23:23 +02:00
Silvan Fuhrer	e715e6c245	Fixed-wing position control: set yaw_sp to yaw_current instead of nav_bearing when not controlled Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2021-10-27 14:35:00 +03:00
Silvan Fuhrer	b53808d11b	fixed-wing: set yaw_sp to yaw_current instead of 0 when not controlled Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2021-10-27 14:35:00 +03:00
Silvan Fuhrer	da4d6dc657	L1: increase the max allowed tangential velocity in the opposite direction to 2m/s There is logic in L1 that prevents the vehicle from trying to achieve an impossible loiter entry (e.g. due to wind). That check makes the vehicle track the loiter center if the tangential velocity is in the wrong direction while loitering. After the vehicle flies through the center, it can then turn the other way around to join the loiter. This check is though too sensitive if it purely checks for the wrong direction, and it can end in delayed loiter entry for no reason. This commit increases the threshold to 2m/s of tangential velocity in the wrong direction to trigger the check. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2021-10-27 12:32:58 +03:00
RomanBapst	eee5f501cd	navigator: fix flyaway when altitude change is commanded without a valid triplet Signed-off-by: RomanBapst <bapstroman@gmail.com>	2021-10-27 11:01:13 +03:00
RomanBapst	bf6a47ba6a	navigator: cleanup of set_loiter_item Unwraps the set_loiter_item() to solve the issue where the altitdue setpoint in a MC takeoff wasn't correctly used. Signed-off-by: RomanBapst <bapstroman@gmail.com>	2021-10-27 11:01:13 +03:00
Silvan Fuhrer	cb78ba34d7	Mission: for tangential loiter exit, set current position setpoint typ to position Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2021-10-27 11:01:13 +03:00
Silvan Fuhrer	4b21c0c49e	Fw Pos C: always reset pos_sp type from LOITER to POSITION if far away Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2021-10-27 11:01:13 +03:00
RomanBapst	d678e792cc	mission_block: don't require an exiting heading when loitering if the next waypoint is within the loiter radius of the current waypoint Signed-off-by: RomanBapst <bapstroman@gmail.com>	2021-10-27 11:01:13 +03:00
Michael Schaeuble	5e1f62e9d0	Add option to warn the pilot in case of strong magnetic interference but still allow arming. This PR changes the COM_ARM_MAG_STR parameter to accept values. If the parameter is set to 2, the check is performed and a warning is logged but the vehicle can still arm.	2021-10-27 09:59:18 +02:00
ponomarevda	2b6bd452df	fix hardpoint hardfault by checking argc before std::strcmp	2021-10-27 08:11:23 +02:00
Beat Küng	de488f0f40	omnibus/f4sd: add topic listener & change timer order So it matches the usage in the channel definition order	2021-10-27 08:03:55 +02:00
Beat Küng	8476875b4d	Kconfig: add missing serial ports	2021-10-27 08:03:55 +02:00
Beat Küng	48344c6e2a	state_machine_helper: add missing 'break' (no behavior change)	2021-10-27 08:03:55 +02:00
Daniel Agar	6d0c6bb6ce	lib/world_magnetic_model: cmake remove helper target BYPRODUCTS - otherwise ninja will try to rebuild these	2021-10-26 18:52:12 -04:00
dagar	a2801bab80	[AUTO COMMIT] update change indication	2021-10-26 14:39:58 -04:00
Daniel Agar	88a979cf1d	lib/world_magnetic_model: add cmake helpers for updating tables - `world_magnetic_model_update` to fetch latest geo_magnetic_tables.hpp - `world_magnetic_model_tests_update` to fetch latest test_geo_lookup.cpp	2021-10-26 14:39:58 -04:00
Peter van der Perk	24ab430466	Tools/generate_board_targets_json.py: fix json board targets regression from #17100	2021-10-26 16:15:39 +02:00
bresch	d0f89f7fff	ekf2: refactor wind reset functions	2021-10-26 10:18:56 +02:00
bresch	456dfcb4b9	ekf2: update getter for true airspeed	2021-10-26 10:18:56 +02:00
bresch	3927c183de	ekf2_test: adjust airspeed unit test an airpseed of > 2m/s is required to start the fusion (set by param) fw mode is also required Given the larger estimated windspeed after those changes, the change of static pressure is larger and the height estimate takes more time to reach the final value	2021-10-26 10:18:56 +02:00
bresch	6e8f0e92ff	ekf2: refactor airspeed fusion control logic	2021-10-26 10:18:56 +02:00
bresch	8873e92c7c	ekf: force fallback to baro if GPS is stopped while in GPS height mode Otherwise, no height aiding source is used	2021-10-26 10:05:28 +02:00
bresch	0a140ec59a	ekf2_test: add GPS height to baro fallback	2021-10-26 10:05:28 +02:00
bresch	f4c21cedd9	ekf2_test: use motion_planning for dynamic yaw emergency test	2021-10-25 18:06:38 -04:00
bresch	340a2caa8e	ekf2_test: use motion_planning library The VelocitySmoothing class from the motion_planning library is used to generate trajectories in order to test the EKF convergence during motion	2021-10-25 18:06:38 -04:00
David Sidrane	38e2e6a01f	Use NuttX MPU Reset (#18283 ) * NuttX with MPU reset backports * Use NuttX MPU reset	2021-10-25 18:05:31 -04:00
alexklimaj	8088c82b6a	Add CANNODE_FLOW_ROT	2021-10-25 16:31:00 -04:00
Jaeyoung-Lim	5dcaadf492	Fix px4vision defaults	2021-10-25 16:29:52 -04:00
Landon Haugh	24cd0c6fa3	Enablement of PX4 SPI driver for UCANS32K146	2021-10-25 08:36:54 -07:00
Daniel Agar	a548c94230	boards: holybro_durandal-v1_default disable modules to save flash	2021-10-25 13:41:31 +02:00
bresch	6ec9ab11f2	add fw auto-tune module to board configs	2021-10-25 13:41:31 +02:00
bresch	95e2941b17	fw att: inject system identification signal to controller	2021-10-25 13:41:31 +02:00
bresch	6af0856558	add FF to FW rate controllers	2021-10-25 13:41:31 +02:00
bresch	55f0860c31	fw atune: add fixed-wing auto-tuning module	2021-10-25 13:41:31 +02:00
bresch	8dfdb1e3db	compute and publish fixed-wing control power	2021-10-25 13:41:31 +02:00
RomanBapst	d84b0296d2	support orbit command in fixed wing mode Signed-off-by: RomanBapst <bapstroman@gmail.com> commander: support orbit mode for fixed wings Signed-off-by: RomanBapst <bapstroman@gmail.com> FwPositionControl: publish orbit status Signed-off-by: RomanBapst <bapstroman@gmail.com> commander:reject orbit mode while doing a vtol transition Signed-off-by: RomanBapst <bapstroman@gmail.com> FixedWingPositionControl: explicitly cast waypoint for Orbit status FixedwingPositionControl: fill missing orbit_status fields navigator_main: handle reposition/orbit corner cases - set orbit rotation direction correctly - send mavlink message when orbit is rejected FixedWingPositionControl: correctly report rotation direction in orbit_status navigator: hack to not break orbit while doing altitude changes Signed-off-by: RomanBapst <bapstroman@gmail.com> navigator: set cruise throttle for orbit command Signed-off-by: RomanBapst <bapstroman@gmail.com>	2021-10-25 09:48:27 +02:00
Daniel Agar	93eb0162e5	drivers/uavcan_v1: fix Kconfig trailing whitespace	2021-10-25 08:15:46 +02:00
Silvan Fuhrer	f9cfcc5cfa	AirspeedSelector: add bitmask to enable checks seperately in ASPD_DO_CHECKS Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2021-10-22 17:00:35 +02:00
Silvan Fuhrer	73fe300c00	WindEstimator: remove option to disable scale estimation The situation where this would be desired is unclear, plus it's basically the same as setting ASPD_SC_P_NOISE to a very small value. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2021-10-22 17:00:35 +02:00
Silvan Fuhrer	b38bf23d6e	WindEstimator: avoid division by 0 Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2021-10-22 17:00:35 +02:00
Silvan Fuhrer	c0754cf324	AirspeedValidator: pass vI as reference Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2021-10-22 17:00:35 +02:00
Silvan Fuhrer	f9682b86d1	AirspeedSelector: some clean up Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2021-10-22 17:00:35 +02:00
Silvan Fuhrer	7537fa36c8	AirspeedValidator: fix airspeed scale validation (feed raw TAS) Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2021-10-22 17:00:35 +02:00
Silvan Fuhrer	a2faac148f	AirspeedValidator: check_airspeed_innovation() check absolute innovations Do no longer use tas_innovation from wind estimator and test ratio, but calculate the innovation based on wind estimate, TAS measurement (including currently applied scale) and ground velocity. Use innovations directly to trigger failure. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2021-10-22 17:00:35 +02:00
Silvan Fuhrer	f6d37ecacf	AirspeedSelector: make sure we don't try to access a negative array index Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2021-10-22 17:00:35 +02:00
Silvan Fuhrer	ccab93e68b	AirspeedSelector: use Vector3f Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2021-10-22 17:00:35 +02:00
Silvan Fuhrer	259b851ba7	WindEstimator: remove filter reset due to airspeed measurement rejection As the purpose of this wind estimator is to (mainly) catch airspeed failures, we don't value estimator stability as much as the reliability to catch actual sensor issues, and thus do not reset the filter (as this may hide a real issue with the sensor) Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2021-10-22 17:00:35 +02:00
Silvan Fuhrer	3ad901e51d	Wind Estimator: use time_literals Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2021-10-22 17:00:35 +02:00
Silvan Fuhrer	cad7851774	AirspeedSelector: add _CAS_scale_validated to airspeed_wind for logging Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2021-10-22 17:00:35 +02:00
Silvan Fuhrer	625f556b0e	AirspeedSelector: airspeed scale estimation improvements and robustification - run airspeed scale estimation always, not in dedicated mode - add option to apply scale automatically, with extra feasibility check - add airspeed scale for all 3 possible airspeed instances - clean up parameters - add check for data stuck (non-changing airspeed data) Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2021-10-22 17:00:35 +02:00
RomanBapst	8e8c6efd66	Mission block: do not care for altitdue acceptance when approaching backtransition point Not accepting the waypoint causes the vehicle to perform a sharp turn after passing the land waypoint and this causes worse unexected behavior. Signed-off-by: RomanBapst <bapstroman@gmail.com>	2021-10-22 15:37:10 +02:00
Matthias Grob	3d50adc5fe	astyle: restore backwards compatibility with old pre-commit hook I removed the filtering logic from the shell script in #18482 because the new pre-commit hook already takes care of it. The problem is if you don't update the .git/hooks/pre-commit file and use the new shell script there's no filtering of files done and it checks all files for every file. This commit restores backwards compatibility because it does not hurt until I have an automatic way to update the pre-commit hook file.	2021-10-22 12:56:21 +02:00
bresch	78fe6e2152	VelocitySmoothing: fix issue when delta vel is the same on all axes When the change in velocity is exactly the same on several axes, the T1 of the recumputed trajectories after time sync was set to 0 because it was skipping both if and else if.	2021-10-22 10:54:36 +02:00
mcsauder	9301288d1f	Add parameter and logic to set RTL heading mode.	2021-10-22 09:28:30 +02:00