control_allocator: Fixes for standard VTOL frames

- Add 'actuatorType()' to ActuatorEffectiveness - Map outputs to uORB topics by type - Fix incorrect number of outputs in StandardVTOL - Fix 1050_standard_vtol_ctrlalloc airframe file
airframes: add standard_vtol_ctrlalloc model
2026-06-11 11:50:04 +08:00 · 2021-10-27 18:30:31 -07:00 · 2021-10-27 16:04:30 -07:00 · 2021-10-27 10:07:01 -04:00 · 2021-10-27 15:23:23 +02:00 · 2021-10-27 14:35:00 +03:00
164 changed files with 10160 additions and 6546 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"
@@ -0,0 +1,110 @@
+#!/bin/sh
+#
+# @name Standard VTOL
+#
+# @type Standard VTOL
+#
+
+. ${R}etc/init.d/rc.vtol_defaults
+
+param set-default SYS_CTRL_ALLOC 1
+
+param set-default VM_MASS 1.5
+param set-default VM_INERTIA_XX 0.03
+param set-default VM_INERTIA_YY 0.03
+param set-default VM_INERTIA_ZZ 0.05
+
+param set-default FW_L1_PERIOD 12
+param set-default FW_MAN_P_MAX 30
+param set-default FW_PR_FF 0.2
+param set-default FW_PR_P 0.9
+param set-default FW_PSP_OFF 2
+param set-default FW_P_LIM_MAX 32
+param set-default FW_P_LIM_MIN -15
+param set-default FW_RR_FF 0.1
+param set-default FW_RR_P 0.3
+param set-default FW_THR_CRUISE 0.25
+param set-default FW_THR_MAX 0.6
+param set-default FW_THR_MIN 0.05
+param set-default FW_T_ALT_TC 2
+param set-default FW_T_CLMB_MAX 8
+param set-default FW_T_HRATE_FF 0.5
+param set-default FW_T_SINK_MAX 2.7
+param set-default FW_T_SINK_MIN 2.2
+param set-default FW_T_TAS_TC 2
+
+param set-default MC_ROLLRATE_P 0.3
+param set-default MC_YAW_P 1.6
+
+param set-default MIS_TAKEOFF_ALT 10
+
+param set-default MPC_ACC_HOR_MAX 2
+param set-default MPC_XY_P 0.8
+param set-default MPC_XY_VEL_P_ACC 3
+param set-default MPC_XY_VEL_I_ACC 4
+param set-default MPC_XY_VEL_D_ACC 0.1
+
+param set-default NAV_ACC_RAD 5
+param set-default NAV_LOITER_RAD 80
+
+param set-default VT_FWD_THRUST_EN 4
+param set-default VT_F_TRANS_THR 0.75
+param set-default VT_MOT_ID 1234
+param set-default VT_FW_MOT_OFFID 1234
+param set-default VT_B_TRANS_DUR 8
+param set-default VT_TYPE 2
+
+param set-default CA_AIRFRAME 1
+param set-default CA_METHOD 1
+
+# VTOL Motors
+param set-default CA_ACT0_MIN 0.0
+param set-default CA_ACT1_MIN 0.0
+param set-default CA_ACT2_MIN 0.0
+param set-default CA_ACT3_MIN 0.0
+param set-default CA_ACT0_MAX 1.0
+param set-default CA_ACT1_MAX 1.0
+param set-default CA_ACT2_MAX 1.0
+param set-default CA_ACT3_MAX 1.0
+
+# Throttle
+param set-default CA_ACT4_MIN 0.0
+param set-default CA_ACT4_MAX 1.0
+
+# Ailerons/Ruddervator
+param set-default CA_ACT5_MIN -1.0
+param set-default CA_ACT5_MAX 1.0
+param set-default CA_ACT6_MIN -1.0
+param set-default CA_ACT6_MAX 1.0
+param set-default CA_ACT7_MIN -1.0
+param set-default CA_ACT7_MAX 1.0
+
+param set-default CA_MC_R0_PX 0.177
+param set-default CA_MC_R0_PY 0.177
+param set-default CA_MC_R0_CT 6.5
+param set-default CA_MC_R0_KM 0.05
+param set-default CA_MC_R1_PX -0.177
+param set-default CA_MC_R1_PY -0.177
+param set-default CA_MC_R1_CT 6.5
+param set-default CA_MC_R1_KM 0.05
+param set-default CA_MC_R2_PX 0.177
+param set-default CA_MC_R2_PY -0.177
+param set-default CA_MC_R2_CT 6.5
+param set-default CA_MC_R2_KM -0.05
+param set-default CA_MC_R3_PX -0.177
+param set-default CA_MC_R3_PY 0.177
+param set-default CA_MC_R3_CT 6.5
+param set-default CA_MC_R3_KM -0.05
+
+param set-default PWM_MAIN_FUNC1 101
+param set-default PWM_MAIN_FUNC2 102
+param set-default PWM_MAIN_FUNC3 103
+param set-default PWM_MAIN_FUNC4 104
+param set-default PWM_MAIN_FUNC5 105 # Setting pusher to Motor5 for now
+param set-default PWM_MAIN_FUNC6 201
+param set-default PWM_MAIN_FUNC7 202
+param set-default PWM_MAIN_FUNC8 203
+
+set MAV_TYPE 22
+
+set MIXER skip
@@ -67,6 +67,7 @@ px4_add_romfs_files(
 	1042_tiltrotor
 	1043_standard_vtol_drop
 	1043_standard_vtol_drop.post
+	1050_standard_vtol_ctrlalloc
 	1060_rover
 	1061_r1_rover
 	1062_tf-r1
@@ -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.
 #
@@ -15,7 +15,7 @@ param set-default RTL_RETURN_ALT 30
 param set-default RTL_DESCEND_ALT 10

 param set-default PWM_MAIN_MAX 1950
-param set-default PWM_MAIN_MIN1 1075
+param set-default PWM_MAIN_MIN 1075
 param set-default PWM_MAIN_RATE 400

 param set-default GPS_UBX_DYNMODEL 6
@@ -15,6 +15,22 @@ ekf2 start &
 #                        End Estimator group selection                        #
 ###############################################################################

+if param compare SYS_CTRL_ALLOC 1
+then
+	#
+	# Start Control Allocator
+	#
+	control_allocator start
+
+	#
+	# Disable hover thrust estimator and prearming
+	# These features are currently incompatible with control allocation
+	#
+	# TODO: fix
+	#
+	param set MPC_USE_HTE 0
+fi
+
 airspeed_selector start

 vtol_att_control start
@@ -24,9 +40,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,16 @@ 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()
+
+            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
@@ -44,6 +44,7 @@ CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
+CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_GYRO_CALIBRATION=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 */
@@ -3,6 +3,7 @@ CONFIG_BOARD_ARCHITECTURE="cortex-m4"
 CONFIG_BOARD_CONSTRAINED_FLASH=y
 CONFIG_BOARD_CONSTRAINED_MEMORY=y
 CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1"
+CONFIG_BOARD_SERIAL_URT6="/dev/ttyS2"
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_BMP280=y
 CONFIG_DRIVERS_DSHOT=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] = {
@@ -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
@@ -54,6 +54,7 @@ CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
+CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -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
@@ -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
@@ -7,3 +7,4 @@ bool lockdown		# Set to true if actuators are forced to being disabled (due to e
 bool manual_lockdown    # Set to true if manual throttle kill switch is engaged
 bool force_failsafe	# Set to true if the actuators are forced to the failsafe position
 bool in_esc_calibration_mode # IO/FMU should ignore messages from the actuator controls topics
+bool soft_stop # Set to true if we need to ESCs to remove the idle constraint
@@ -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;
+}
@@ -144,6 +144,7 @@ set(models
 	solo
 	standard_vtol
 	standard_vtol_drop
+	standard_vtol_ctrlalloc
 	tailsitter
 	techpod
 	tiltrotor
@@ -118,6 +118,11 @@ struct pwm_output_values {

 #endif // not PX4_PWM_ALTERNATE_RANGES

+/**
+ * Do not output a channel with this value
+ */
+#define PWM_IGNORE_THIS_CHANNEL UINT16_MAX
+
 /**
 * Servo output signal type, value is actual servo output pulse
 * width in microseconds.
@@ -165,9 +170,15 @@ typedef uint16_t	servo_position_t;
 /** start DSM bind */
 #define DSM_BIND_START	_PX4_IOC(_PWM_SERVO_BASE, 10)

+/** set the PWM value for failsafe */
+#define PWM_SERVO_SET_FAILSAFE_PWM	_PX4_IOC(_PWM_SERVO_BASE, 12)
+
 /** get the PWM value for failsafe */
 #define PWM_SERVO_GET_FAILSAFE_PWM	_PX4_IOC(_PWM_SERVO_BASE, 13)

+/** set the PWM value when disarmed - should be no PWM (zero) by default */
+#define PWM_SERVO_SET_DISARMED_PWM	_PX4_IOC(_PWM_SERVO_BASE, 14)
+
 /** get the PWM value when disarmed */
 #define PWM_SERVO_GET_DISARMED_PWM	_PX4_IOC(_PWM_SERVO_BASE, 15)

@@ -186,9 +197,21 @@ typedef uint16_t	servo_position_t;
 /** get the TRIM value the output will send */
 #define PWM_SERVO_GET_TRIM_PWM	_PX4_IOC(_PWM_SERVO_BASE, 21)

+/** set the lockdown override flag to enable outputs in HIL */
+#define PWM_SERVO_SET_DISABLE_LOCKDOWN		_PX4_IOC(_PWM_SERVO_BASE, 23)
+
+/** get the lockdown override flag to enable outputs in HIL */
+#define PWM_SERVO_GET_DISABLE_LOCKDOWN		_PX4_IOC(_PWM_SERVO_BASE, 24)
+
 /** force safety switch off (to disable use of safety switch) */
 #define PWM_SERVO_SET_FORCE_SAFETY_OFF		_PX4_IOC(_PWM_SERVO_BASE, 25)

+/** force failsafe mode (failsafe values are set immediately even if failsafe condition not met) */
+#define PWM_SERVO_SET_FORCE_FAILSAFE		_PX4_IOC(_PWM_SERVO_BASE, 26)
+
+/** make failsafe non-recoverable (termination) if it occurs */
+#define PWM_SERVO_SET_TERMINATION_FAILSAFE	_PX4_IOC(_PWM_SERVO_BASE, 27)
+
 /** force safety switch on (to enable use of safety switch) */
 #define PWM_SERVO_SET_FORCE_SAFETY_ON		_PX4_IOC(_PWM_SERVO_BASE, 28)

@@ -44,6 +44,10 @@ LinuxPWMOut::LinuxPWMOut() :
 	_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")),
 	_interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": interval"))
 {
+	if (!_mixing_output.useDynamicMixing()) {
+		_mixing_output.setAllMinValues(PWM_DEFAULT_MIN);
+		_mixing_output.setAllMaxValues(PWM_DEFAULT_MAX);
+	}
 }

 LinuxPWMOut::~LinuxPWMOut()
@@ -157,6 +161,128 @@ void LinuxPWMOut::update_params()
 		return;
 	}

+	int32_t pwm_min_default = PWM_DEFAULT_MIN;
+	int32_t pwm_max_default = PWM_DEFAULT_MAX;
+	int32_t pwm_disarmed_default = 0;
+
+	const char *prefix;
+
+	if (_class_instance == CLASS_DEVICE_PRIMARY) {
+		prefix = "PWM_MAIN";
+
+		param_get(param_find("PWM_MAIN_MIN"), &pwm_min_default);
+		param_get(param_find("PWM_MAIN_MAX"), &pwm_max_default);
+		param_get(param_find("PWM_MAIN_DISARM"), &pwm_disarmed_default);
+
+	} else if (_class_instance == CLASS_DEVICE_SECONDARY) {
+		prefix = "PWM_AUX";
+
+		param_get(param_find("PWM_AUX_MIN"), &pwm_min_default);
+		param_get(param_find("PWM_AUX_MAX"), &pwm_max_default);
+		param_get(param_find("PWM_AUX_DISARM"), &pwm_disarmed_default);
+
+	} else if (_class_instance == CLASS_DEVICE_TERTIARY) {
+		prefix = "PWM_EXTRA";
+
+		param_get(param_find("PWM_EXTRA_MIN"), &pwm_min_default);
+		param_get(param_find("PWM_EXTRA_MAX"), &pwm_max_default);
+		param_get(param_find("PWM_EXTRA_DISARM"), &pwm_disarmed_default);
+
+	} else {
+		PX4_ERR("invalid class instance %d", _class_instance);
+		return;
+	}
+
+	char str[17];
+
+	for (unsigned i = 0; i < MAX_ACTUATORS; i++) {
+		// PWM_MAIN_MINx
+		{
+			sprintf(str, "%s_MIN%u", prefix, i + 1);
+			int32_t pwm_min = -1;
+
+			if (param_get(param_find(str), &pwm_min) == PX4_OK && pwm_min >= 0) {
+				_mixing_output.minValue(i) = math::constrain(pwm_min, PWM_LOWEST_MIN, PWM_HIGHEST_MIN);
+
+				if (pwm_min != _mixing_output.minValue(i)) {
+					int32_t pwm_min_new = _mixing_output.minValue(i);
+					param_set(param_find(str), &pwm_min_new);
+				}
+
+			} else {
+				_mixing_output.minValue(i) = pwm_min_default;
+			}
+		}
+
+		// PWM_MAIN_MAXx
+		{
+			sprintf(str, "%s_MAX%u", prefix, i + 1);
+			int32_t pwm_max = -1;
+
+			if (param_get(param_find(str), &pwm_max) == PX4_OK && pwm_max >= 0) {
+				_mixing_output.maxValue(i) = math::constrain(pwm_max, PWM_LOWEST_MAX, PWM_HIGHEST_MAX);
+
+				if (pwm_max != _mixing_output.maxValue(i)) {
+					int32_t pwm_max_new = _mixing_output.maxValue(i);
+					param_set(param_find(str), &pwm_max_new);
+				}
+
+			} else {
+				_mixing_output.maxValue(i) = pwm_max_default;
+			}
+		}
+
+		// PWM_MAIN_FAILx
+		{
+			sprintf(str, "%s_FAIL%u", prefix, i + 1);
+			int32_t pwm_failsafe = -1;
+
+			if (param_get(param_find(str), &pwm_failsafe) == PX4_OK && pwm_failsafe >= 0) {
+				_mixing_output.failsafeValue(i) = math::constrain(pwm_failsafe, 0, PWM_HIGHEST_MAX);
+
+				if (pwm_failsafe != _mixing_output.failsafeValue(i)) {
+					int32_t pwm_fail_new = _mixing_output.failsafeValue(i);
+					param_set(param_find(str), &pwm_fail_new);
+				}
+			}
+		}
+
+		// PWM_MAIN_DISx
+		{
+			sprintf(str, "%s_DIS%u", prefix, i + 1);
+			int32_t pwm_dis = -1;
+
+			if (param_get(param_find(str), &pwm_dis) == PX4_OK && pwm_dis >= 0) {
+				_mixing_output.disarmedValue(i) = math::constrain(pwm_dis, 0, PWM_HIGHEST_MAX);
+
+				if (pwm_dis != _mixing_output.disarmedValue(i)) {
+					int32_t pwm_dis_new = _mixing_output.disarmedValue(i);
+					param_set(param_find(str), &pwm_dis_new);
+				}
+
+			} else {
+				_mixing_output.disarmedValue(i) = pwm_disarmed_default;
+			}
+		}
+
+		// PWM_MAIN_REVx
+		{
+			sprintf(str, "%s_REV%u", prefix, i + 1);
+			int32_t pwm_rev = 0;
+
+			if (param_get(param_find(str), &pwm_rev) == PX4_OK) {
+				uint16_t &reverse_pwm_mask = _mixing_output.reverseOutputMask();
+
+				if (pwm_rev >= 1) {
+					reverse_pwm_mask = reverse_pwm_mask | (2 << i);
+
+				} else {
+					reverse_pwm_mask = reverse_pwm_mask & ~(2 << i);
+				}
+			}
+		}
+	}
+
 	if (_mixing_output.mixers()) {
 		int16_t values[MAX_ACTUATORS] {};

@@ -124,6 +124,10 @@ PCA9685Wrapper::PCA9685Wrapper(int schd_rate_limit) :
 	_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")),
 	_schd_rate_limit(schd_rate_limit)
 {
+	if (!_mixing_output.useDynamicMixing()) {
+		_mixing_output.setAllMinValues(PWM_DEFAULT_MIN);
+		_mixing_output.setAllMaxValues(PWM_DEFAULT_MAX);
+	}
 }

 PCA9685Wrapper::~PCA9685Wrapper()
@@ -175,6 +179,151 @@ void PCA9685Wrapper::updatePWMParams()
 		return;
 	}

+	// update pwm params
+	const char *pname_format_pwm_ch_max[2] = {"PWM_MAIN_MAX%d", "PWM_AUX_MAX%d"};
+	const char *pname_format_pwm_ch_min[2] = {"PWM_MAIN_MIN%d", "PWM_AUX_MIN%d"};
+	const char *pname_format_pwm_ch_fail[2] = {"PWM_MAIN_FAIL%d", "PWM_AUX_FAIL%d"};
+	const char *pname_format_pwm_ch_dis[2] = {"PWM_MAIN_DIS%d", "PWM_AUX_DIS%d"};
+	const char *pname_format_pwm_ch_rev[2] = {"PWM_MAIN_REV%d", "PWM_AUX_REV%d"};
+
+	int32_t default_pwm_max = PWM_DEFAULT_MAX,
+		default_pwm_min = PWM_DEFAULT_MIN,
+		default_pwm_fail = PWM_DEFAULT_MIN,
+		default_pwm_dis = PWM_MOTOR_OFF;
+
+	param_t param_h = param_find("PWM_MAIN_MAX");
+
+	if (param_h != PARAM_INVALID) {
+		param_get(param_h, &default_pwm_max);
+
+	} else {
+		PX4_DEBUG("PARAM_INVALID: %s", "PWM_MAIN_MAX");
+	}
+
+	param_h = param_find("PWM_MAIN_MIN");
+
+	if (param_h != PARAM_INVALID) {
+		param_get(param_h, &default_pwm_min);
+
+	} else {
+		PX4_DEBUG("PARAM_INVALID: %s", "PWM_MAIN_MIN");
+	}
+
+	param_h = param_find("PWM_MAIN_RATE");
+
+	if (param_h != PARAM_INVALID) {
+		int32_t pval = 0;
+		param_get(param_h, &pval);
+
+		if (_last_fetched_Freq != pval) {
+			_last_fetched_Freq = pval;
+			_targetFreq = (float)pval;  // update only if changed
+		}
+
+	} else {
+		PX4_DEBUG("PARAM_INVALID: %s", "PWM_MAIN_RATE");
+	}
+
+	for (int i = 0; i < PCA9685_PWM_CHANNEL_COUNT; i++) {
+		char pname[16];
+		uint8_t param_group, param_index;
+
+		if (i <= 7) {	// Main channel
+			param_group = 0;
+			param_index = i + 1;
+
+		} else {	// AUX
+			param_group = 1;
+			param_index = i - 8 + 1;
+		}
+
+		sprintf(pname, pname_format_pwm_ch_max[param_group], param_index);
+		param_h = param_find(pname);
+
+		if (param_h != PARAM_INVALID) {
+			int32_t pval = 0;
+			param_get(param_h, &pval);
+
+			if (pval != -1) {
+				_mixing_output.maxValue(i) = pval;
+
+			} else {
+				_mixing_output.maxValue(i) = default_pwm_max;
+			}
+
+		} else {
+			PX4_DEBUG("PARAM_INVALID: %s", pname);
+		}
+
+		sprintf(pname, pname_format_pwm_ch_min[param_group], param_index);
+		param_h = param_find(pname);
+
+		if (param_h != PARAM_INVALID) {
+			int32_t pval = 0;
+			param_get(param_h, &pval);
+
+			if (pval != -1) {
+				_mixing_output.minValue(i) = pval;
+
+			} else {
+				_mixing_output.minValue(i) = default_pwm_min;
+			}
+
+		} else {
+			PX4_DEBUG("PARAM_INVALID: %s", pname);
+		}
+
+		sprintf(pname, pname_format_pwm_ch_fail[param_group], param_index);
+		param_h = param_find(pname);
+
+		if (param_h != PARAM_INVALID) {
+			int32_t pval = 0;
+			param_get(param_h, &pval);
+
+			if (pval != -1) {
+				_mixing_output.failsafeValue(i) = pval;
+
+			} else {
+				_mixing_output.failsafeValue(i) = default_pwm_fail;
+			}
+
+		} else {
+			PX4_DEBUG("PARAM_INVALID: %s", pname);
+		}
+
+		sprintf(pname, pname_format_pwm_ch_dis[param_group], param_index);
+		param_h = param_find(pname);
+
+		if (param_h != PARAM_INVALID) {
+			int32_t pval = 0;
+			param_get(param_h, &pval);
+
+			if (pval != -1) {
+				_mixing_output.disarmedValue(i) = pval;
+
+			} else {
+				_mixing_output.disarmedValue(i) = default_pwm_dis;
+			}
+
+		} else {
+			PX4_DEBUG("PARAM_INVALID: %s", pname);
+		}
+
+		sprintf(pname, pname_format_pwm_ch_rev[param_group], param_index);
+		param_h = param_find(pname);
+
+		if (param_h != PARAM_INVALID) {
+			uint16_t &reverse_pwm_mask = _mixing_output.reverseOutputMask();
+			int32_t pval = 0;
+			param_get(param_h, &pval);
+			reverse_pwm_mask &= (0xfffe << i);  // clear this bit
+			reverse_pwm_mask |= (((uint16_t)(pval != 0)) << i); // set to new val
+
+		} else {
+			PX4_DEBUG("PARAM_INVALID: %s", pname);
+		}
+	}
+
 	if (_mixing_output.mixers()) { // only update trims if mixer loaded
 		updatePWMParamTrim();
 	}
@@ -56,6 +56,10 @@ PWMOut::PWMOut(int instance, uint8_t output_base) :
 	_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")),
 	_interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": interval"))
 {
+	if (!_mixing_output.useDynamicMixing()) {
+		_mixing_output.setAllMinValues(PWM_DEFAULT_MIN);
+		_mixing_output.setAllMaxValues(PWM_DEFAULT_MAX);
+	}
 }

 PWMOut::~PWMOut()
@@ -483,6 +487,10 @@ void PWMOut::Run()
 		}
 	}

+	if (_pwm_initialized && _current_update_rate == 0 && !_mixing_output.useDynamicMixing()) {
+		update_current_rate();
+	}
+
 	// check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread)
 	_mixing_output.updateSubscriptions(true, true);

@@ -497,6 +505,9 @@ void PWMOut::update_params()
 		return;
 	}

+	int32_t pwm_min_default = PWM_DEFAULT_MIN;
+	int32_t pwm_max_default = PWM_DEFAULT_MAX;
+	int32_t pwm_disarmed_default = 0;
 	int32_t pwm_rate_default = 50;
 	int32_t pwm_default_channels = 0;

@@ -505,20 +516,28 @@ void PWMOut::update_params()
 	if (_class_instance == CLASS_DEVICE_PRIMARY) {
 		prefix = "PWM_MAIN";

+		param_get(param_find("PWM_MAIN_MIN"), &pwm_min_default);
+		param_get(param_find("PWM_MAIN_MAX"), &pwm_max_default);
+		param_get(param_find("PWM_MAIN_DISARM"), &pwm_disarmed_default);
 		param_get(param_find("PWM_MAIN_RATE"), &pwm_rate_default);
 		param_get(param_find("PWM_MAIN_OUT"), &pwm_default_channels);

 	} else if (_class_instance == CLASS_DEVICE_SECONDARY) {
 		prefix = "PWM_AUX";

+		param_get(param_find("PWM_AUX_MIN"), &pwm_min_default);
+		param_get(param_find("PWM_AUX_MAX"), &pwm_max_default);
+		param_get(param_find("PWM_AUX_DISARM"), &pwm_disarmed_default);
 		param_get(param_find("PWM_AUX_RATE"), &pwm_rate_default);
 		param_get(param_find("PWM_AUX_OUT"), &pwm_default_channels);

 	} else if (_class_instance == CLASS_DEVICE_TERTIARY) {
 		prefix = "PWM_EXTRA";

+		param_get(param_find("PWM_EXTRA_MIN"), &pwm_min_default);
+		param_get(param_find("PWM_EXTRA_MAX"), &pwm_max_default);
+		param_get(param_find("PWM_EXTRA_DISARM"), &pwm_disarmed_default);
 		param_get(param_find("PWM_EXTRA_RATE"), &pwm_rate_default);
-		param_get(param_find("PWM_EXTRA_OUT"), &pwm_default_channels);

 	} else {
 		PX4_ERR("invalid class instance %d", _class_instance);
@@ -539,6 +558,106 @@ void PWMOut::update_params()

 	char str[17];

+	for (unsigned i = 0; i < _num_outputs; i++) {
+		// PWM_MAIN_MINx
+		{
+			sprintf(str, "%s_MIN%u", prefix, i + 1);
+			int32_t pwm_min = -1;
+
+			if (param_get(param_find(str), &pwm_min) == PX4_OK) {
+				if (pwm_min >= 0 && pwm_min != 1000) {
+					_mixing_output.minValue(i) = math::constrain(pwm_min, (int32_t) PWM_LOWEST_MIN, (int32_t) PWM_HIGHEST_MIN);
+
+					if (pwm_min != _mixing_output.minValue(i)) {
+						int32_t pwm_min_new = _mixing_output.minValue(i);
+						param_set(param_find(str), &pwm_min_new);
+					}
+
+				} else if (pwm_default_channel_mask & 1 << i) {
+					_mixing_output.minValue(i) = pwm_min_default;
+				}
+			}
+		}
+
+		// PWM_MAIN_MAXx
+		{
+			sprintf(str, "%s_MAX%u", prefix, i + 1);
+			int32_t pwm_max = -1;
+
+			if (param_get(param_find(str), &pwm_max) == PX4_OK) {
+				if (pwm_max >= 0 && pwm_max != 2000) {
+					_mixing_output.maxValue(i) = math::constrain(pwm_max, (int32_t) PWM_LOWEST_MAX, (int32_t) PWM_HIGHEST_MAX);
+
+					if (pwm_max != _mixing_output.maxValue(i)) {
+						int32_t pwm_max_new = _mixing_output.maxValue(i);
+						param_set(param_find(str), &pwm_max_new);
+					}
+
+				} else if (pwm_default_channel_mask & 1 << i) {
+					_mixing_output.maxValue(i) = pwm_max_default;
+				}
+			}
+		}
+
+		// PWM_MAIN_FAILx
+		{
+			sprintf(str, "%s_FAIL%u", prefix, i + 1);
+			int32_t pwm_failsafe = -1;
+
+			if (param_get(param_find(str), &pwm_failsafe) == PX4_OK) {
+				if (pwm_failsafe >= 0) {
+					_mixing_output.failsafeValue(i) = math::constrain(pwm_failsafe, (int32_t) 0, (int32_t) PWM_HIGHEST_MAX);
+
+					if (pwm_failsafe != _mixing_output.failsafeValue(i)) {
+						int32_t pwm_fail_new = _mixing_output.failsafeValue(i);
+						param_set(param_find(str), &pwm_fail_new);
+					}
+				}
+			}
+		}
+
+		// PWM_MAIN_DISx
+		{
+			sprintf(str, "%s_DIS%u", prefix, i + 1);
+			int32_t pwm_dis = -1;
+
+			if (param_get(param_find(str), &pwm_dis) == PX4_OK) {
+				if (pwm_dis >= 0 && pwm_dis != 900) {
+					_mixing_output.disarmedValue(i) = math::constrain(pwm_dis, (int32_t) 0, (int32_t) PWM_HIGHEST_MAX);
+
+					if (pwm_dis != _mixing_output.disarmedValue(i)) {
+						int32_t pwm_dis_new = _mixing_output.disarmedValue(i);
+						param_set(param_find(str), &pwm_dis_new);
+					}
+
+				} else if (pwm_default_channel_mask & 1 << i) {
+					_mixing_output.disarmedValue(i) = pwm_disarmed_default;
+				}
+			}
+
+			if (_mixing_output.disarmedValue(i) > 0) {
+				num_disarmed_set++;
+			}
+		}
+
+		// PWM_MAIN_REVx
+		{
+			sprintf(str, "%s_REV%u", prefix, i + 1);
+			int32_t pwm_rev = 0;
+
+			if (param_get(param_find(str), &pwm_rev) == PX4_OK) {
+				uint16_t &reverse_pwm_mask = _mixing_output.reverseOutputMask();
+
+				if (pwm_rev >= 1) {
+					reverse_pwm_mask = reverse_pwm_mask | (1 << i);
+
+				} else {
+					reverse_pwm_mask = reverse_pwm_mask & ~(1 << i);
+				}
+			}
+		}
+	}
+
 	if (_mixing_output.mixers()) {
 		int16_t values[FMU_MAX_ACTUATORS] {};

@@ -617,6 +736,40 @@ int PWMOut::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		*(uint32_t *)arg = _pwm_alt_rate_channels;
 		break;

+	case PWM_SERVO_SET_FAILSAFE_PWM: {
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+			/* discard if too many values are sent */
+			if (pwm->channel_count > FMU_MAX_ACTUATORS || _mixing_output.useDynamicMixing()) {
+				ret = -EINVAL;
+				break;
+			}
+
+			for (unsigned i = 0; i < pwm->channel_count; i++) {
+				if (pwm->values[i] == 0) {
+					/* ignore 0 */
+				} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
+					_mixing_output.failsafeValue(i) = PWM_HIGHEST_MAX;
+
+				}
+
+#if PWM_LOWEST_MIN > 0
+
+				else if (pwm->values[i] < PWM_LOWEST_MIN) {
+					_mixing_output.failsafeValue(i) = PWM_LOWEST_MIN;
+
+				}
+
+#endif
+
+				else {
+					_mixing_output.failsafeValue(i) = pwm->values[i];
+				}
+			}
+
+			break;
+		}
+
 	case PWM_SERVO_GET_FAILSAFE_PWM: {
 			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;

@@ -628,6 +781,50 @@ int PWMOut::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 			break;
 		}

+	case PWM_SERVO_SET_DISARMED_PWM: {
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+			/* discard if too many values are sent */
+			if (pwm->channel_count > FMU_MAX_ACTUATORS || _mixing_output.useDynamicMixing()) {
+				ret = -EINVAL;
+				break;
+			}
+
+			for (unsigned i = 0; i < pwm->channel_count; i++) {
+				if (pwm->values[i] == 0) {
+					/* ignore 0 */
+				} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
+					_mixing_output.disarmedValue(i) = PWM_HIGHEST_MAX;
+				}
+
+#if PWM_LOWEST_MIN > 0
+
+				else if (pwm->values[i] < PWM_LOWEST_MIN) {
+					_mixing_output.disarmedValue(i) = PWM_LOWEST_MIN;
+				}
+
+#endif
+
+				else {
+					_mixing_output.disarmedValue(i) = pwm->values[i];
+				}
+			}
+
+			/*
+			 * update the counter
+			 * this is needed to decide if disarmed PWM output should be turned on or not
+			 */
+			_num_disarmed_set = 0;
+
+			for (unsigned i = 0; i < FMU_MAX_ACTUATORS; i++) {
+				if (_mixing_output.disarmedValue(i) > 0) {
+					_num_disarmed_set++;
+				}
+			}
+
+			break;
+		}
+
 	case PWM_SERVO_GET_DISARMED_PWM: {
 			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;

@@ -178,6 +178,7 @@ private:

 	static int checkcrc(int argc, char *argv[]);
 	static int bind(int argc, char *argv[]);
+	static int lockdown(int argc, char *argv[]);
 	static int monitor();

 	static constexpr int PX4IO_MAX_ACTUATORS = 8;
@@ -238,6 +239,12 @@ private:

 	MixingOutput _mixing_output{"PWM_MAIN", PX4IO_MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, true};

+	bool _pwm_min_configured{false};
+	bool _pwm_max_configured{false};
+	bool _pwm_fail_configured{false};
+	bool _pwm_dis_configured{false};
+	bool _pwm_rev_configured{false};
+
 	/**
 	 * Update IO's arming-related state
 	 */
@@ -359,6 +366,11 @@ PX4IO::PX4IO(device::Device *interface) :
 	OutputModuleInterface(MODULE_NAME, px4::serial_port_to_wq(PX4IO_SERIAL_DEVICE)),
 	_interface(interface)
 {
+	if (!_mixing_output.useDynamicMixing()) {
+		_mixing_output.setAllMinValues(PWM_DEFAULT_MIN);
+		_mixing_output.setAllMaxValues(PWM_DEFAULT_MAX);
+	}
+
 	_mixing_output.setLowrateSchedulingInterval(20_ms);
 }

@@ -745,14 +757,11 @@ void PX4IO::updateTimerRateGroups()

 void PX4IO::update_params()
 {
-	if (!_mixing_output.armed().armed) {
-		updateFailsafe();
-		updateDisarmed();
-	}
-
 	if (!_mixing_output.armed().armed && _mixing_output.useDynamicMixing()) {
 		// sync params to IO
 		updateTimerRateGroups();
+		updateFailsafe();
+		updateDisarmed();
 		return;
 	}

@@ -761,11 +770,17 @@ void PX4IO::update_params()
 		return;
 	}

+	int32_t pwm_min_default = PWM_DEFAULT_MIN;
+	int32_t pwm_max_default = PWM_DEFAULT_MAX;
+	int32_t pwm_disarmed_default = 0;
 	int32_t pwm_rate_default = 50;
 	int32_t pwm_default_channels = 0;

 	const char *prefix = "PWM_MAIN";

+	param_get(param_find("PWM_MAIN_MIN"), &pwm_min_default);
+	param_get(param_find("PWM_MAIN_MAX"), &pwm_max_default);
+	param_get(param_find("PWM_MAIN_DISARM"), &pwm_disarmed_default);
 	param_get(param_find("PWM_MAIN_RATE"), &pwm_rate_default);
 	param_get(param_find("PWM_MAIN_OUT"), &pwm_default_channels);

@@ -779,6 +794,125 @@ void PX4IO::update_params()

 	char str[17];

+	// PWM_MAIN_MINx
+	if (!_pwm_min_configured) {
+		for (unsigned i = 0; i < _max_actuators; i++) {
+			sprintf(str, "%s_MIN%u", prefix, i + 1);
+			int32_t pwm_min = -1;
+
+			if (param_get(param_find(str), &pwm_min) == PX4_OK) {
+				if (pwm_min >= 0 && pwm_min != 1000) {
+					_mixing_output.minValue(i) = math::constrain(pwm_min, static_cast<int32_t>(PWM_LOWEST_MIN),
+								     static_cast<int32_t>(PWM_HIGHEST_MIN));
+
+					if (pwm_min != _mixing_output.minValue(i)) {
+						int32_t pwm_min_new = _mixing_output.minValue(i);
+						param_set(param_find(str), &pwm_min_new);
+					}
+
+				} else if (pwm_default_channel_mask & 1 << i) {
+					_mixing_output.minValue(i) = pwm_min_default;
+				}
+			}
+		}
+
+		_pwm_min_configured = true;
+	}
+
+	// PWM_MAIN_MAXx
+	if (!_pwm_max_configured) {
+		for (unsigned i = 0; i < _max_actuators; i++) {
+			sprintf(str, "%s_MAX%u", prefix, i + 1);
+			int32_t pwm_max = -1;
+
+			if (param_get(param_find(str), &pwm_max) == PX4_OK) {
+				if (pwm_max >= 0 && pwm_max != 2000) {
+					_mixing_output.maxValue(i) = math::constrain(pwm_max, static_cast<int32_t>(PWM_LOWEST_MAX),
+								     static_cast<int32_t>(PWM_HIGHEST_MAX));
+
+					if (pwm_max != _mixing_output.maxValue(i)) {
+						int32_t pwm_max_new = _mixing_output.maxValue(i);
+						param_set(param_find(str), &pwm_max_new);
+					}
+
+				} else if (pwm_default_channel_mask & 1 << i) {
+					_mixing_output.maxValue(i) = pwm_max_default;
+				}
+			}
+		}
+
+		_pwm_max_configured = true;
+	}
+
+	// PWM_MAIN_FAILx
+	if (!_pwm_fail_configured) {
+		for (unsigned i = 0; i < _max_actuators; i++) {
+			sprintf(str, "%s_FAIL%u", prefix, i + 1);
+			int32_t pwm_fail = -1;
+
+			if (param_get(param_find(str), &pwm_fail) == PX4_OK) {
+				if (pwm_fail >= 0) {
+					_mixing_output.failsafeValue(i) = math::constrain(pwm_fail, static_cast<int32_t>(0),
+									  static_cast<int32_t>(PWM_HIGHEST_MAX));
+
+					if (pwm_fail != _mixing_output.failsafeValue(i)) {
+						int32_t pwm_fail_new = _mixing_output.failsafeValue(i);
+						param_set(param_find(str), &pwm_fail_new);
+					}
+				}
+			}
+		}
+
+		_pwm_fail_configured = true;
+		updateFailsafe();
+	}
+
+	// PWM_MAIN_DISx
+	if (!_pwm_dis_configured) {
+		for (unsigned i = 0; i < _max_actuators; i++) {
+			sprintf(str, "%s_DIS%u", prefix, i + 1);
+			int32_t pwm_dis = -1;
+
+			if (param_get(param_find(str), &pwm_dis) == PX4_OK) {
+				if (pwm_dis >= 0 && pwm_dis != 900) {
+					_mixing_output.disarmedValue(i) = math::constrain(pwm_dis, static_cast<int32_t>(0),
+									  static_cast<int32_t>(PWM_HIGHEST_MAX));
+
+					if (pwm_dis != _mixing_output.disarmedValue(i)) {
+						int32_t pwm_dis_new = _mixing_output.disarmedValue(i);
+						param_set(param_find(str), &pwm_dis_new);
+					}
+
+				} else if (pwm_default_channel_mask & 1 << i) {
+					_mixing_output.disarmedValue(i) = pwm_disarmed_default;
+				}
+			}
+		}
+
+		_pwm_dis_configured = true;
+		updateDisarmed();
+	}
+
+	// PWM_MAIN_REVx
+	if (!_pwm_rev_configured) {
+		uint16_t &reverse_pwm_mask = _mixing_output.reverseOutputMask();
+		reverse_pwm_mask = 0;
+
+		for (unsigned i = 0; i < _max_actuators; i++) {
+			sprintf(str, "%s_REV%u", prefix, i + 1);
+			int32_t pwm_rev = -1;
+
+			if (param_get(param_find(str), &pwm_rev) == PX4_OK) {
+				if (pwm_rev >= 1) {
+					reverse_pwm_mask |= (1 << i);
+				}
+
+			}
+		}
+
+		_pwm_rev_configured = true;
+	}
+
 	// PWM_MAIN_TRIMx
 	if (_mixing_output.mixers()) {
 		int16_t values[8] {};
@@ -1514,6 +1648,27 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 		*(unsigned *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES);
 		break;

+	case PWM_SERVO_SET_FAILSAFE_PWM: {
+			PX4_DEBUG("PWM_SERVO_SET_FAILSAFE_PWM");
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+			if (pwm->channel_count > _max_actuators)
+				/* fail with error */
+			{
+				return -E2BIG;
+			}
+
+			for (unsigned i = 0; i < pwm->channel_count; i++) {
+				if (pwm->values[i] != 0 && !_mixing_output.useDynamicMixing()) {
+					_mixing_output.failsafeValue(i) = math::constrain(pwm->values[i], (uint16_t)PWM_LOWEST_MIN, (uint16_t)PWM_HIGHEST_MAX);
+				}
+			}
+
+			updateFailsafe();
+
+			break;
+		}
+
 	case PWM_SERVO_GET_FAILSAFE_PWM: {
 			PX4_DEBUG("PWM_SERVO_GET_FAILSAFE_PWM");
 			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
@@ -1528,6 +1683,26 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 			break;
 		}

+	case PWM_SERVO_SET_DISARMED_PWM: {
+			PX4_DEBUG("PWM_SERVO_SET_DISARMED_PWM");
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+			if (pwm->channel_count > _max_actuators) {
+				/* fail with error */
+				return -E2BIG;
+			}
+
+			for (unsigned i = 0; i < pwm->channel_count; i++) {
+				if (pwm->values[i] != 0 && !_mixing_output.useDynamicMixing()) {
+					_mixing_output.disarmedValue(i) = math::constrain(pwm->values[i], (uint16_t)PWM_LOWEST_MIN, (uint16_t)PWM_HIGHEST_MAX);
+				}
+			}
+
+			updateDisarmed();
+
+			break;
+		}
+
 	case PWM_SERVO_GET_DISARMED_PWM: {
 			PX4_DEBUG("PWM_SERVO_GET_DISARMED_PWM");
 			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
@@ -1619,6 +1794,16 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 		*(unsigned *)arg = _max_actuators;
 		break;

+	case PWM_SERVO_SET_DISABLE_LOCKDOWN:
+		PX4_DEBUG("PWM_SERVO_SET_DISABLE_LOCKDOWN");
+		_lockdown_override = arg;
+		break;
+
+	case PWM_SERVO_GET_DISABLE_LOCKDOWN:
+		PX4_DEBUG("PWM_SERVO_GET_DISABLE_LOCKDOWN");
+		*(unsigned *)arg = _lockdown_override;
+		break;
+
 	case PWM_SERVO_SET_FORCE_SAFETY_OFF:
 		PX4_DEBUG("PWM_SERVO_SET_FORCE_SAFETY_OFF");
 		/* force safety swith off */
@@ -1631,6 +1816,36 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 		ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_ON, PX4IO_FORCE_SAFETY_MAGIC);
 		break;

+	case PWM_SERVO_SET_FORCE_FAILSAFE:
+		PX4_DEBUG("PWM_SERVO_SET_FORCE_FAILSAFE");
+
+		/* force failsafe mode instantly */
+		if (arg == 0) {
+			/* clear force failsafe flag */
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE, 0);
+
+		} else {
+			/* set force failsafe flag */
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE);
+		}
+
+		break;
+
+	case PWM_SERVO_SET_TERMINATION_FAILSAFE:
+		PX4_DEBUG("PWM_SERVO_SET_TERMINATION_FAILSAFE");
+
+		/* if failsafe occurs, do not allow the system to recover */
+		if (arg == 0) {
+			/* clear termination failsafe flag */
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE, 0);
+
+		} else {
+			/* set termination failsafe flag */
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE);
+		}
+
+		break;
+
 	case DSM_BIND_START:
 		/* bind a DSM receiver */
 		ret = dsm_bind_ioctl(arg);
@@ -1996,6 +2211,60 @@ int PX4IO::monitor()
 	return 0;
 }

+int PX4IO::lockdown(int argc, char *argv[])
+{
+	if (argc > 1 && !strcmp(argv[1], "disable")) {
+
+		PX4_WARN("WARNING: ACTUATORS WILL BE LIVE IN HIL! PROCEED?");
+		PX4_WARN("Press 'y' to enable, any other key to abort.");
+
+		/* check if user wants to abort */
+		char c;
+
+		struct pollfd fds;
+		int ret;
+		hrt_abstime start = hrt_absolute_time();
+		const unsigned long timeout = 5000000;
+
+		while (hrt_elapsed_time(&start) < timeout) {
+			fds.fd = 0; /* stdin */
+			fds.events = POLLIN;
+			ret = ::poll(&fds, 1, 0);
+
+			if (ret > 0) {
+
+				if (::read(0, &c, 1) > 0) {
+
+					if (c != 'y') {
+						return 0;
+
+					} else if (c == 'y') {
+						break;
+					}
+				}
+			}
+
+			px4_usleep(10000);
+		}
+
+		if (hrt_elapsed_time(&start) > timeout) {
+			PX4_ERR("TIMEOUT! ABORTED WITHOUT CHANGES.");
+			return 1;
+		}
+
+		get_instance()->ioctl(0, PWM_SERVO_SET_DISABLE_LOCKDOWN, 1);
+
+		PX4_WARN("ACTUATORS ARE NOW LIVE IN HIL!");
+
+	} else {
+		get_instance()->ioctl(0, PWM_SERVO_SET_DISABLE_LOCKDOWN, 0);
+		PX4_WARN("ACTUATORS ARE NOW SAFE IN HIL.");
+	}
+
+	return 0;
+}
+
+
 int PX4IO::task_spawn(int argc, char *argv[])
 {
 	device::Device *interface = get_interface();
@@ -2199,6 +2468,10 @@ int PX4IO::custom_command(int argc, char *argv[])
 		return bind(argc - 1, argv + 1);
 	}

+	if (!strcmp(verb, "lockdown")) {
+		return lockdown(argc, argv);
+	}
+
 	if (!strcmp(verb, "sbus1_out")) {
 		int ret = get_instance()->ioctl(nullptr, SBUS_SET_PROTO_VERSION, 1);

@@ -2261,6 +2534,8 @@ Output driver communicating with the IO co-processor.
 	PRINT_MODULE_USAGE_COMMAND_DESCR("monitor", "continuously monitor status");
 	PRINT_MODULE_USAGE_COMMAND_DESCR("bind", "DSM bind");
 	PRINT_MODULE_USAGE_ARG("dsm2|dsmx|dsmx8", "protocol", false);
+	PRINT_MODULE_USAGE_COMMAND_DESCR("lockdown", "enable (or disable) lockdown");
+	PRINT_MODULE_USAGE_ARG("disable", "disable lockdown", true);
 	PRINT_MODULE_USAGE_COMMAND_DESCR("sbus1_out", "enable sbus1 out");
 	PRINT_MODULE_USAGE_COMMAND_DESCR("sbus2_out", "enable sbus2 out");
 	PRINT_MODULE_USAGE_COMMAND_DESCR("test_fmu_fail", "test: turn off IO updates");
@@ -676,6 +676,17 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events)
 		return -EINVAL;
 	}

+	_mixing_interface_esc.mixingOutput().setAllDisarmedValues(UavcanEscController::DISARMED_OUTPUT_VALUE);
+
+	if (!_mixing_interface_esc.mixingOutput().useDynamicMixing()) {
+		// these are configurable with dynamic mixing
+		_mixing_interface_esc.mixingOutput().setAllMinValues(0); // Can be changed to 1 later, according to UAVCAN_ESC_IDLT
+		_mixing_interface_esc.mixingOutput().setAllMaxValues(UavcanEscController::max_output_value());
+
+		param_get(param_find("UAVCAN_ESC_IDLT"), &_idle_throttle_when_armed_param);
+		enable_idle_throttle_when_armed(true);
+	}
+
 	/*  Start the Node   */
 	return _node.start();
 }
@@ -780,6 +791,10 @@ UavcanNode::Run()

 	node_spin_once(); // expected to be non-blocking

+	// Check arming state
+	const actuator_armed_s &armed = _mixing_interface_esc.mixingOutput().armed();
+	enable_idle_throttle_when_armed(!armed.soft_stop);
+
 	// check for parameter updates
 	if (_parameter_update_sub.updated()) {
 		// clear update
@@ -823,6 +838,19 @@ UavcanNode::Run()
 	}
 }

+void
+UavcanNode::enable_idle_throttle_when_armed(bool value)
+{
+	value &= _idle_throttle_when_armed_param > 0;
+
+	if (!_mixing_interface_esc.mixingOutput().useDynamicMixing()) {
+		if (value != _idle_throttle_when_armed) {
+			_mixing_interface_esc.mixingOutput().setAllMinValues(value ? 1 : 0);
+			_idle_throttle_when_armed = value;
+		}
+	}
+}
+
 int
 UavcanNode::teardown()
 {
@@ -1177,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]);

@@ -216,6 +216,8 @@ private:
 	void		set_setget_response(uavcan::protocol::param::GetSet::Response *resp) { _setget_response = resp; }
 	void		free_setget_response(void) { _setget_response = nullptr; }

+	void enable_idle_throttle_when_armed(bool value);
+
 	px4::atomic_bool	_task_should_exit{false};	///< flag to indicate to tear down the CAN driver
 	px4::atomic<int>	_fw_server_action{None};
 	int			 _fw_server_status{-1};
@@ -77,6 +77,15 @@ PARAM_DEFINE_INT32(UAVCAN_NODE_ID, 1);
 */
 PARAM_DEFINE_INT32(UAVCAN_BITRATE, 1000000);

+/**
+ * UAVCAN ESC will spin at idle throttle when armed, even if the mixer outputs zero setpoints.
+ *
+ * @boolean
+ * @reboot_required true
+ * @group UAVCAN
+ */
+PARAM_DEFINE_INT32(UAVCAN_ESC_IDLT, 1);
+
 /**
 * UAVCAN rangefinder minimum range
 *
@@ -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);
 	}

@@ -104,13 +104,15 @@ _param_prefix(param_prefix)

 	_use_dynamic_mixing = _param_sys_ctrl_alloc.get();

-	initParamHandles();
+	if (_use_dynamic_mixing) {
+		initParamHandles();

-	for (unsigned i = 0; i < MAX_ACTUATORS; i++) {
-		_failsafe_value[i] = UINT16_MAX;
+		for (unsigned i = 0; i < MAX_ACTUATORS; i++) {
+			_failsafe_value[i] = UINT16_MAX;
+		}
+
+		updateParams();
 	}
-
-	updateParams();
 }

 MixingOutput::~MixingOutput()
@@ -127,6 +129,8 @@ void MixingOutput::initParamHandles()
 	char param_name[17];

 	for (unsigned i = 0; i < _max_num_outputs; ++i) {
+		snprintf(param_name, sizeof(param_name), "%s_%s%d", _param_prefix, "FUNC", i + 1);
+		_param_handles[i].function = param_find(param_name);
 		snprintf(param_name, sizeof(param_name), "%s_%s%d", _param_prefix, "DIS", i + 1);
 		_param_handles[i].disarmed = param_find(param_name);
 		snprintf(param_name, sizeof(param_name), "%s_%s%d", _param_prefix, "MIN", i + 1);
@@ -136,13 +140,6 @@ void MixingOutput::initParamHandles()
 		snprintf(param_name, sizeof(param_name), "%s_%s%d", _param_prefix, "FAIL", i + 1);
 		_param_handles[i].failsafe = param_find(param_name);
 	}
-
-	if (_use_dynamic_mixing) {
-		for (unsigned i = 0; i < _max_num_outputs; ++i) {
-			snprintf(param_name, sizeof(param_name), "%s_%s%d", _param_prefix, "FUNC", i + 1);
-			_param_handles[i].function = param_find(param_name);
-		}
-	}
 }

 void MixingOutput::printStatus() const
@@ -193,40 +190,13 @@ void MixingOutput::updateParams()
 		_mixers->set_airmode((Mixer::Airmode)_param_mc_airmode.get());
 	}

-	_reverse_output_mask = 0;
-
-	for (unsigned i = 0; i < _max_num_outputs; i++) {
-		int32_t val;
-
-		if (_param_handles[i].disarmed != PARAM_INVALID && param_get(_param_handles[i].disarmed, &val) == 0) {
-			_disarmed_value[i] = val;
-		}
-
-		if (_param_handles[i].min != PARAM_INVALID && param_get(_param_handles[i].min, &val) == 0) {
-			_min_value[i] = val;
-		}
-
-		if (_param_handles[i].max != PARAM_INVALID && param_get(_param_handles[i].max, &val) == 0) {
-			_max_value[i] = val;
-		}
-
-		if (_min_value[i] > _max_value[i]) {
-			_reverse_output_mask |= 1 << i;
-			uint16_t tmp = _min_value[i];
-			_min_value[i] = _max_value[i];
-			_max_value[i] = tmp;
-		}
-
-		if (_param_handles[i].failsafe != PARAM_INVALID && param_get(_param_handles[i].failsafe, &val) == 0) {
-			_failsafe_value[i] = val;
-		}
-	}
-
 	if (_use_dynamic_mixing) {
 		_param_mot_ordering.set(0); // not used with dynamic mixing

 		bool function_changed = false;

+		_reverse_output_mask = 0;
+
 		for (unsigned i = 0; i < _max_num_outputs; i++) {
 			int32_t val;

@@ -237,6 +207,29 @@ void MixingOutput::updateParams()

 				// we set _function_assignment[i] later to ensure _functions[i] is updated at the same time
 			}
+
+			if (_param_handles[i].disarmed != PARAM_INVALID && param_get(_param_handles[i].disarmed, &val) == 0) {
+				_disarmed_value[i] = val;
+			}
+
+			if (_param_handles[i].min != PARAM_INVALID && param_get(_param_handles[i].min, &val) == 0) {
+				_min_value[i] = val;
+			}
+
+			if (_param_handles[i].max != PARAM_INVALID && param_get(_param_handles[i].max, &val) == 0) {
+				_max_value[i] = val;
+			}
+
+			if (_min_value[i] > _max_value[i]) {
+				_reverse_output_mask |= 1 << i;
+				uint16_t tmp = _min_value[i];
+				_min_value[i] = _max_value[i];
+				_max_value[i] = tmp;
+			}
+
+			if (_param_handles[i].failsafe != PARAM_INVALID && param_get(_param_handles[i].failsafe, &val) == 0) {
+				_failsafe_value[i] = val;
+			}
 		}

 		if (function_changed) {
@@ -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
@@ -155,6 +155,29 @@ bool param_modify_on_import(bson_node_t node)
 		}
 	}

+	// 2021-01-31 (v1.12 alpha): translate PWM_MIN/PWM_MAX/PWM_DISARMED to PWM_MAIN
+	{
+		if (strcmp("PWM_MIN", node->name) == 0) {
+			strcpy(node->name, "PWM_MAIN_MIN");
+			PX4_INFO("copying %s -> %s", "PWM_MIN", "PWM_MAIN_MIN");
+		}
+
+		if (strcmp("PWM_MAX", node->name) == 0) {
+			strcpy(node->name, "PWM_MAIN_MAX");
+			PX4_INFO("copying %s -> %s", "PWM_MAX", "PWM_MAIN_MAX");
+		}
+
+		if (strcmp("PWM_RATE", node->name) == 0) {
+			strcpy(node->name, "PWM_MAIN_RATE");
+			PX4_INFO("copying %s -> %s", "PWM_RATE", "PWM_MAIN_RATE");
+		}
+
+		if (strcmp("PWM_DISARMED", node->name) == 0) {
+			strcpy(node->name, "PWM_MAIN_DISARM");
+			PX4_INFO("copying %s -> %s", "PWM_DISARMED", "PWM_MAIN_DISARM");
+		}
+	}
+
 	// 2021-04-30: translate ASPD_STALL to FW_AIRSPD_STALL
 	{
 		if (strcmp("ASPD_STALL", node->name) == 0) {
@@ -29,6 +29,40 @@ parameters:
            max: 2000
            default: 50

+        PWM_AUX_MIN:
+            description:
+                short: PWM aux minimum value
+                long: |
+                    Set to 1000 for industry default or 900 to increase servo travel.
+            type: int32
+            unit: us
+            min: 800
+            max: 1400
+            default: 1000
+
+        PWM_AUX_MAX:
+            description:
+                short: PWM aux maximum value
+                long: |
+                    Set to 2000 for industry default or 2100 to increase servo travel.
+            type: int32
+            unit: us
+            min: 1600
+            max: 2200
+            default: 2000
+
+        PWM_AUX_DISARM:
+            description:
+                short: PWM aux disarmed value
+                long: |
+                    This is the PWM pulse the autopilot is outputting if not armed.
+                    The main use of this parameter is to silence ESCs when they are disarmed.
+            type: int32
+            unit: us
+            min: 0
+            max: 2200
+            default: 1500
+
        PWM_AUX_TRIM${i}:
            description:
                short: PWM aux ${i} trim value
@@ -41,3 +75,27 @@ parameters:
            num_instances: *max_num_config_instances
            instance_start: 1
            default: 0
+
+        PWM_AUX_REV${i}:
+            description:
+                short: PWM aux ${i} reverse value
+                long: |
+                    Enable to invert the channel.
+                    Warning: Use this parameter when connected to a servo only.
+                    For a brushless motor, invert manually two phases to reverse the direction.
+            type: boolean
+            num_instances: *max_num_config_instances
+            instance_start: 1
+            default: 0
+
+        PWM_AUX_RATE${i}:
+            description:
+                short: PWM aux ${i} rate
+                long: |
+                    Set the default PWM output frequency for the aux outputs
+            type: int32
+            unit: Hz
+            min: 0
+            max: 400
+            instance_start: 1
+            default: 50
@@ -121,3 +121,27 @@ parameters:
            num_instances: *max_num_config_instances
            instance_start: 1
            default: 0
+
+        PWM_EXTRA_REV${i}:
+            description:
+                short: PWM extra ${i} reverse value
+                long: |
+                    Enable to invert the channel.
+                    Warning: Use this parameter when connected to a servo only.
+                    For a brushless motor, invert manually two phases to reverse the direction.
+            type: boolean
+            num_instances: *max_num_config_instances
+            instance_start: 1
+            default: 0
+
+        PWM_EXTRA_RATE${i}:
+            description:
+                short: PWM extra ${i} rate
+                long: |
+                    Set the default PWM output frequency for the main outputs
+            type: int32
+            unit: Hz
+            min: 0
+            max: 400
+            instance_start: 1
+            default: 50
@@ -29,6 +29,40 @@ parameters:
            max: 2000
            default: 400

+        PWM_MAIN_MIN:
+            description:
+                short: PWM main minimum value
+                long: |
+                    Set to 1000 for industry default or 900 to increase servo travel.
+            type: int32
+            unit: us
+            min: 800
+            max: 1400
+            default: 1000
+
+        PWM_MAIN_MAX:
+            description:
+                short: PWM main maximum value
+                long: |
+                    Set to 2000 for industry default or 2100 to increase servo travel.
+            type: int32
+            unit: us
+            min: 1600
+            max: 2200
+            default: 2000
+
+        PWM_MAIN_DISARM:
+            description:
+                short: PWM main disarmed value
+                long: |
+                    This is the PWM pulse the autopilot is outputting if not armed.
+                    The main use of this parameter is to silence ESCs when they are disarmed.
+            type: int32
+            unit: us
+            min: 0
+            max: 2200
+            default: 900
+
        PWM_MAIN_TRIM${i}:
            description:
                short: PWM main ${i} trim value
@@ -41,3 +75,27 @@ parameters:
            num_instances: *max_num_config_instances
            instance_start: 1
            default: 0
+
+        PWM_MAIN_REV${i}:
+            description:
+                short: PWM main ${i} reverse value
+                long: |
+                    Enable to invert the channel.
+                    Warning: Use this parameter when connected to a servo only.
+                    For a brushless motor, invert manually two phases to reverse the direction.
+            type: boolean
+            num_instances: *max_num_config_instances
+            instance_start: 1
+            default: 0
+
+        PWM_MAIN_RATE${i}:
+            description:
+                short: PWM main ${i} rate
+                long: |
+                    Set the default PWM output frequency for the main outputs
+            type: int32
+            unit: Hz
+            min: 0
+            max: 400
+            instance_start: 1
+            default: 50
@@ -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, },
 };
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
JacobCrabill	c1ca5b50a4	control_allocator: Fixes for standard VTOL frames - Add 'actuatorType()' to ActuatorEffectiveness - Map outputs to uORB topics by type - Fix incorrect number of outputs in StandardVTOL - Fix 1050_standard_vtol_ctrlalloc airframe file	2021-10-27 18:30:31 -07:00
Julian Oes	81590f137e	airframes: add standard_vtol_ctrlalloc model	2021-10-27 16:04:30 -07: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