control_allocator: add parameter based ActuatorEffectivenessCustom

- this allows creating a custom actuator effectiveness entirely from parameters of the form CA_ACTn_TRQ_{R,P,Y} and CA_ACTn_THR_{X,Y,Z}
control_allocator: push VTOL flight phase into ActuatorEffectiveness
2026-06-19 13:50:35 +08:00 · 2021-10-27 14:25:03 -04:00 · 2021-10-27 13:42:22 -04:00 · 2021-10-27 12:48:42 -04:00 · 2021-10-27 10:07:01 -04:00 · 2021-10-27 15:23:23 +02:00
136 changed files with 8509 additions and 6550 deletions
@@ -10,6 +10,7 @@ on:
 jobs:
  enumerate_targets:
    runs-on: ubuntu-latest
+    container: px4io/px4-dev-base-focal:2021-09-08
    outputs:
      matrix: ${{ steps.set-matrix.outputs.matrix }}
    steps:
@@ -128,6 +128,9 @@ config BOARD_KEYSTORE

 menu "Serial ports"

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

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

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

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

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

 fw_att_control start vtol
 fw_pos_control_l1 start vtol
+fw_autotune_attitude_control start vtol

 # Start Land Detector
 # Multicopter for now until we have something for VTOL
@@ -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
@@ -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;
+}
@@ -1205,7 +1205,7 @@ int uavcan_main(int argc, char *argv[])
 	}

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

@@ -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);
 	}

@@ -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, },
 };
@@ -49,8 +49,8 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &
 {
 	bool success = true; // start with a pass and change to a fail if any test fails

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

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

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

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

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

+		break;
+
 	default:
 		break;
 	}
@@ -44,7 +44,6 @@
 #include <ControlAllocation/ControlAllocation.hpp>

 #include <matrix/matrix/math.hpp>
-#include <uORB/topics/vehicle_actuator_setpoint.h>

 class ActuatorEffectiveness
 {
@@ -55,23 +54,6 @@ public:
 	static constexpr uint8_t NUM_ACTUATORS = ControlAllocation::NUM_ACTUATORS;
 	static constexpr uint8_t NUM_AXES = ControlAllocation::NUM_AXES;

-	enum class FlightPhase {
-		HOVER_FLIGHT = 0,
-		FORWARD_FLIGHT = 1,
-		TRANSITION_HF_TO_FF = 2,
-		TRANSITION_FF_TO_HF = 3
-	};
-
-	/**
-	 * Set the current flight phase
-	 *
-	 * @param Flight phase
-	 */
-	virtual void setFlightPhase(const FlightPhase &flight_phase)
-	{
-		_flight_phase = flight_phase;
-	}
-
 	/**
 	 * Get the control effectiveness matrix if updated
 	 *
@@ -84,20 +66,7 @@ public:
 	 *
 	 * @return Actuator trims
 	 */
-	const matrix::Vector<float, NUM_ACTUATORS> &getActuatorTrim() const
-	{
-		return _trim;
-	}
-
-	/**
-	 * Get the current flight phase
-	 *
-	 * @return Flight phase
-	 */
-	const FlightPhase &getFlightPhase() const
-	{
-		return _flight_phase;
-	}
+	const matrix::Vector<float, NUM_ACTUATORS> &getActuatorTrim() const { return _trim; }

 	/**
 	 * Get the number of actuators
@@ -106,5 +75,4 @@ public:

 protected:
 	matrix::Vector<float, NUM_ACTUATORS> _trim;			///< Actuator trim
-	FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT};		///< Current flight phase
 };
@@ -0,0 +1,85 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "ActuatorEffectivenessCustom.hpp"
+
+ActuatorEffectivenessCustom::ActuatorEffectivenessCustom(ModuleParams *parent):
+	ModuleParams(parent)
+{
+}
+
+bool ActuatorEffectivenessCustom::getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix,
+		bool force)
+{
+	if (_updated || force) {
+		_updated = false;
+
+		int num_actuators = 0;
+
+		for (int n = 0; n < NUM_ACTUATORS; n++) {
+			// CA_ACTn_TRQ_R
+			// CA_ACTn_TRQ_P
+			// CA_ACTn_TRQ_Y
+			char torque_str[3][17];
+			sprintf(torque_str[0], "CA_ACT%u_TRQ_R", n);
+			sprintf(torque_str[1], "CA_ACT%u_TRQ_P", n);
+			sprintf(torque_str[2], "CA_ACT%u_TRQ_Y", n);
+
+			// CA_ACTn_THR_X
+			// CA_ACTn_THR_Y
+			// CA_ACTn_THR_Z
+			char thrust_str[3][17];
+			sprintf(thrust_str[0], "CA_ACT%u_THR_X", n);
+			sprintf(thrust_str[1], "CA_ACT%u_THR_Y", n);
+			sprintf(thrust_str[2], "CA_ACT%u_THR_Z", n);
+
+			for (int i = 0; i < 3; i++) {
+				// CA_ACTn_TRQ_{R,P,Y}
+				param_get(param_find(torque_str[i]), &_matrix(n, i));
+
+				// CA_ACTn_THR_{X,Y,Z}
+				param_get(param_find(thrust_str[i]), &_matrix(n, i + 3));
+			}
+
+			if (_matrix.row(n).longerThan(0.f)) {
+				num_actuators++;
+			}
+		}
+
+		_num_actuators = num_actuators;
+		matrix = _matrix;
+		return true;
+	}
+
+	return false;
+}
@@ -0,0 +1,68 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ActuatorEffectivenessCustom.hpp
+ *
+ * Actuator effectiveness computed from rotors position and orientation
+ *
+ * @author Julien Lecoeur <julien.lecoeur@gmail.com>
+ */
+
+#pragma once
+
+#include "ActuatorEffectiveness.hpp"
+
+#include <px4_platform_common/module_params.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionInterval.hpp>
+
+using namespace time_literals;
+
+class ActuatorEffectivenessCustom: public ModuleParams, public ActuatorEffectiveness
+{
+public:
+	ActuatorEffectivenessCustom(ModuleParams *parent);
+	virtual ~ActuatorEffectivenessCustom() = default;
+
+	bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix, bool force) override;
+
+	int numActuators() const override { return _num_actuators; }
+private:
+
+	matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> _matrix{};
+
+	int _num_actuators{0};
+
+	bool _updated{true};
+};
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -54,78 +54,33 @@ ActuatorEffectivenessMultirotor::getEffectivenessMatrix(matrix::Matrix<float, NU
 		_updated = false;

 		// Get multirotor geometry
-		MultirotorGeometry geometry = {};
-		geometry.rotors[0].position_x = _param_ca_mc_r0_px.get();
-		geometry.rotors[0].position_y = _param_ca_mc_r0_py.get();
-		geometry.rotors[0].position_z = _param_ca_mc_r0_pz.get();
-		geometry.rotors[0].axis_x = _param_ca_mc_r0_ax.get();
-		geometry.rotors[0].axis_y = _param_ca_mc_r0_ay.get();
-		geometry.rotors[0].axis_z = _param_ca_mc_r0_az.get();
-		geometry.rotors[0].thrust_coef = _param_ca_mc_r0_ct.get();
-		geometry.rotors[0].moment_ratio = _param_ca_mc_r0_km.get();
+		MultirotorGeometry geometry{};

-		geometry.rotors[1].position_x = _param_ca_mc_r1_px.get();
-		geometry.rotors[1].position_y = _param_ca_mc_r1_py.get();
-		geometry.rotors[1].position_z = _param_ca_mc_r1_pz.get();
-		geometry.rotors[1].axis_x = _param_ca_mc_r1_ax.get();
-		geometry.rotors[1].axis_y = _param_ca_mc_r1_ay.get();
-		geometry.rotors[1].axis_z = _param_ca_mc_r1_az.get();
-		geometry.rotors[1].thrust_coef = _param_ca_mc_r1_ct.get();
-		geometry.rotors[1].moment_ratio = _param_ca_mc_r1_km.get();
+		for (int n = 0; n < NUM_ROTORS_MAX; n++) {
+			char buffer[17];

-		geometry.rotors[2].position_x = _param_ca_mc_r2_px.get();
-		geometry.rotors[2].position_y = _param_ca_mc_r2_py.get();
-		geometry.rotors[2].position_z = _param_ca_mc_r2_pz.get();
-		geometry.rotors[2].axis_x = _param_ca_mc_r2_ax.get();
-		geometry.rotors[2].axis_y = _param_ca_mc_r2_ay.get();
-		geometry.rotors[2].axis_z = _param_ca_mc_r2_az.get();
-		geometry.rotors[2].thrust_coef = _param_ca_mc_r2_ct.get();
-		geometry.rotors[2].moment_ratio = _param_ca_mc_r2_km.get();
+			for (int i = 0; i < 3; i++) {
+				char axis_char = 'X' + i;

-		geometry.rotors[3].position_x = _param_ca_mc_r3_px.get();
-		geometry.rotors[3].position_y = _param_ca_mc_r3_py.get();
-		geometry.rotors[3].position_z = _param_ca_mc_r3_pz.get();
-		geometry.rotors[3].axis_x = _param_ca_mc_r3_ax.get();
-		geometry.rotors[3].axis_y = _param_ca_mc_r3_ay.get();
-		geometry.rotors[3].axis_z = _param_ca_mc_r3_az.get();
-		geometry.rotors[3].thrust_coef = _param_ca_mc_r3_ct.get();
-		geometry.rotors[3].moment_ratio = _param_ca_mc_r3_km.get();
+				// CA_MC_Rn_P{X,Y,Z}
+				sprintf(buffer, "CA_MC_R%u_P%c", n, axis_char);
+				param_get(param_find(buffer), &geometry.rotors[n].position(i));

-		geometry.rotors[4].position_x = _param_ca_mc_r4_px.get();
-		geometry.rotors[4].position_y = _param_ca_mc_r4_py.get();
-		geometry.rotors[4].position_z = _param_ca_mc_r4_pz.get();
-		geometry.rotors[4].axis_x = _param_ca_mc_r4_ax.get();
-		geometry.rotors[4].axis_y = _param_ca_mc_r4_ay.get();
-		geometry.rotors[4].axis_z = _param_ca_mc_r4_az.get();
-		geometry.rotors[4].thrust_coef = _param_ca_mc_r4_ct.get();
-		geometry.rotors[4].moment_ratio = _param_ca_mc_r4_km.get();
+				// CA_MC_Rn_A{X,Y,Z}
+				sprintf(buffer, "CA_MC_R%u_A%c", n, axis_char);
+				param_get(param_find(buffer), &geometry.rotors[n].axis(i));
+			}

-		geometry.rotors[5].position_x = _param_ca_mc_r5_px.get();
-		geometry.rotors[5].position_y = _param_ca_mc_r5_py.get();
-		geometry.rotors[5].position_z = _param_ca_mc_r5_pz.get();
-		geometry.rotors[5].axis_x = _param_ca_mc_r5_ax.get();
-		geometry.rotors[5].axis_y = _param_ca_mc_r5_ay.get();
-		geometry.rotors[5].axis_z = _param_ca_mc_r5_az.get();
-		geometry.rotors[5].thrust_coef = _param_ca_mc_r5_ct.get();
-		geometry.rotors[5].moment_ratio = _param_ca_mc_r5_km.get();
+			geometry.rotors[n].axis.normalize();

-		geometry.rotors[6].position_x = _param_ca_mc_r6_px.get();
-		geometry.rotors[6].position_y = _param_ca_mc_r6_py.get();
-		geometry.rotors[6].position_z = _param_ca_mc_r6_pz.get();
-		geometry.rotors[6].axis_x = _param_ca_mc_r6_ax.get();
-		geometry.rotors[6].axis_y = _param_ca_mc_r6_ay.get();
-		geometry.rotors[6].axis_z = _param_ca_mc_r6_az.get();
-		geometry.rotors[6].thrust_coef = _param_ca_mc_r6_ct.get();
-		geometry.rotors[6].moment_ratio = _param_ca_mc_r6_km.get();
+			// CA_MC_Rn_CT
+			sprintf(buffer, "CA_MC_R%u_CT", n);
+			param_get(param_find(buffer), &geometry.rotors[n].thrust_coef);

-		geometry.rotors[7].position_x = _param_ca_mc_r7_px.get();
-		geometry.rotors[7].position_y = _param_ca_mc_r7_py.get();
-		geometry.rotors[7].position_z = _param_ca_mc_r7_pz.get();
-		geometry.rotors[7].axis_x = _param_ca_mc_r7_ax.get();
-		geometry.rotors[7].axis_y = _param_ca_mc_r7_ay.get();
-		geometry.rotors[7].axis_z = _param_ca_mc_r7_az.get();
-		geometry.rotors[7].thrust_coef = _param_ca_mc_r7_ct.get();
-		geometry.rotors[7].moment_ratio = _param_ca_mc_r7_km.get();
+			// CA_MC_Rn_KM
+			sprintf(buffer, "CA_MC_R%u_KM", n);
+			param_get(param_find(buffer), &geometry.rotors[n].moment_ratio);
+		}

 		_num_actuators = computeEffectivenessMatrix(geometry, matrix);
 		return true;
@@ -145,11 +100,7 @@ ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(const MultirotorGeom
 	for (size_t i = 0; i < NUM_ROTORS_MAX; i++) {

 		// Get rotor axis
-		matrix::Vector3f axis(
-			geometry.rotors[i].axis_x,
-			geometry.rotors[i].axis_y,
-			geometry.rotors[i].axis_z
-		);
+		matrix::Vector3f axis{geometry.rotors[i].axis};

 		// Normalize axis
 		float axis_norm = axis.norm();
@@ -163,11 +114,7 @@ ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(const MultirotorGeom
 		}

 		// Get rotor position
-		matrix::Vector3f position(
-			geometry.rotors[i].position_x,
-			geometry.rotors[i].position_y,
-			geometry.rotors[i].position_z
-		);
+		const matrix::Vector3f position{geometry.rotors[i].position};

 		// Get coefficients
 		float ct = geometry.rotors[i].thrust_coef;
@@ -178,10 +125,10 @@ ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(const MultirotorGeom
 		}

 		// Compute thrust generated by this rotor
-		matrix::Vector3f thrust = ct * axis;
+		const matrix::Vector3f thrust{ct * axis};

 		// Compute moment generated by this rotor
-		matrix::Vector3f moment = ct * position.cross(axis) - ct * km * axis;
+		const matrix::Vector3f moment{ct * position.cross(axis) - ct *km * axis};

 		// Fill corresponding items in effectiveness matrix
 		for (size_t j = 0; j < 3; j++) {
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -57,20 +57,16 @@ public:

 	static constexpr int NUM_ROTORS_MAX = 8;

-	typedef struct {
-		float position_x;
-		float position_y;
-		float position_z;
-		float axis_x;
-		float axis_y;
-		float axis_z;
-		float thrust_coef;
-		float moment_ratio;
-	} RotorGeometry;
+	struct RotorGeometry {
+		matrix::Vector3f position{};
+		matrix::Vector3f axis{};
+		float thrust_coef{0.f};
+		float moment_ratio{0.f};
+	};

-	typedef struct {
+	struct MultirotorGeometry {
 		RotorGeometry rotors[NUM_ROTORS_MAX];
-	} MultirotorGeometry;
+	};

 	static int computeEffectivenessMatrix(const MultirotorGeometry &geometry,
 					      matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness);
@@ -81,78 +77,4 @@ public:
 private:
 	bool _updated{true};
 	int _num_actuators{0};
-
-	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::CA_MC_R0_PX>) _param_ca_mc_r0_px,
-		(ParamFloat<px4::params::CA_MC_R0_PY>) _param_ca_mc_r0_py,
-		(ParamFloat<px4::params::CA_MC_R0_PZ>) _param_ca_mc_r0_pz,
-		(ParamFloat<px4::params::CA_MC_R0_AX>) _param_ca_mc_r0_ax,
-		(ParamFloat<px4::params::CA_MC_R0_AY>) _param_ca_mc_r0_ay,
-		(ParamFloat<px4::params::CA_MC_R0_AZ>) _param_ca_mc_r0_az,
-		(ParamFloat<px4::params::CA_MC_R0_CT>) _param_ca_mc_r0_ct,
-		(ParamFloat<px4::params::CA_MC_R0_KM>) _param_ca_mc_r0_km,
-
-		(ParamFloat<px4::params::CA_MC_R1_PX>) _param_ca_mc_r1_px,
-		(ParamFloat<px4::params::CA_MC_R1_PY>) _param_ca_mc_r1_py,
-		(ParamFloat<px4::params::CA_MC_R1_PZ>) _param_ca_mc_r1_pz,
-		(ParamFloat<px4::params::CA_MC_R1_AX>) _param_ca_mc_r1_ax,
-		(ParamFloat<px4::params::CA_MC_R1_AY>) _param_ca_mc_r1_ay,
-		(ParamFloat<px4::params::CA_MC_R1_AZ>) _param_ca_mc_r1_az,
-		(ParamFloat<px4::params::CA_MC_R1_CT>) _param_ca_mc_r1_ct,
-		(ParamFloat<px4::params::CA_MC_R1_KM>) _param_ca_mc_r1_km,
-
-		(ParamFloat<px4::params::CA_MC_R2_PX>) _param_ca_mc_r2_px,
-		(ParamFloat<px4::params::CA_MC_R2_PY>) _param_ca_mc_r2_py,
-		(ParamFloat<px4::params::CA_MC_R2_PZ>) _param_ca_mc_r2_pz,
-		(ParamFloat<px4::params::CA_MC_R2_AX>) _param_ca_mc_r2_ax,
-		(ParamFloat<px4::params::CA_MC_R2_AY>) _param_ca_mc_r2_ay,
-		(ParamFloat<px4::params::CA_MC_R2_AZ>) _param_ca_mc_r2_az,
-		(ParamFloat<px4::params::CA_MC_R2_CT>) _param_ca_mc_r2_ct,
-		(ParamFloat<px4::params::CA_MC_R2_KM>) _param_ca_mc_r2_km,
-
-		(ParamFloat<px4::params::CA_MC_R3_PX>) _param_ca_mc_r3_px,
-		(ParamFloat<px4::params::CA_MC_R3_PY>) _param_ca_mc_r3_py,
-		(ParamFloat<px4::params::CA_MC_R3_PZ>) _param_ca_mc_r3_pz,
-		(ParamFloat<px4::params::CA_MC_R3_AX>) _param_ca_mc_r3_ax,
-		(ParamFloat<px4::params::CA_MC_R3_AY>) _param_ca_mc_r3_ay,
-		(ParamFloat<px4::params::CA_MC_R3_AZ>) _param_ca_mc_r3_az,
-		(ParamFloat<px4::params::CA_MC_R3_CT>) _param_ca_mc_r3_ct,
-		(ParamFloat<px4::params::CA_MC_R3_KM>) _param_ca_mc_r3_km,
-
-		(ParamFloat<px4::params::CA_MC_R4_PX>) _param_ca_mc_r4_px,
-		(ParamFloat<px4::params::CA_MC_R4_PY>) _param_ca_mc_r4_py,
-		(ParamFloat<px4::params::CA_MC_R4_PZ>) _param_ca_mc_r4_pz,
-		(ParamFloat<px4::params::CA_MC_R4_AX>) _param_ca_mc_r4_ax,
-		(ParamFloat<px4::params::CA_MC_R4_AY>) _param_ca_mc_r4_ay,
-		(ParamFloat<px4::params::CA_MC_R4_AZ>) _param_ca_mc_r4_az,
-		(ParamFloat<px4::params::CA_MC_R4_CT>) _param_ca_mc_r4_ct,
-		(ParamFloat<px4::params::CA_MC_R4_KM>) _param_ca_mc_r4_km,
-
-		(ParamFloat<px4::params::CA_MC_R5_PX>) _param_ca_mc_r5_px,
-		(ParamFloat<px4::params::CA_MC_R5_PY>) _param_ca_mc_r5_py,
-		(ParamFloat<px4::params::CA_MC_R5_PZ>) _param_ca_mc_r5_pz,
-		(ParamFloat<px4::params::CA_MC_R5_AX>) _param_ca_mc_r5_ax,
-		(ParamFloat<px4::params::CA_MC_R5_AY>) _param_ca_mc_r5_ay,
-		(ParamFloat<px4::params::CA_MC_R5_AZ>) _param_ca_mc_r5_az,
-		(ParamFloat<px4::params::CA_MC_R5_CT>) _param_ca_mc_r5_ct,
-		(ParamFloat<px4::params::CA_MC_R5_KM>) _param_ca_mc_r5_km,
-
-		(ParamFloat<px4::params::CA_MC_R6_PX>) _param_ca_mc_r6_px,
-		(ParamFloat<px4::params::CA_MC_R6_PY>) _param_ca_mc_r6_py,
-		(ParamFloat<px4::params::CA_MC_R6_PZ>) _param_ca_mc_r6_pz,
-		(ParamFloat<px4::params::CA_MC_R6_AX>) _param_ca_mc_r6_ax,
-		(ParamFloat<px4::params::CA_MC_R6_AY>) _param_ca_mc_r6_ay,
-		(ParamFloat<px4::params::CA_MC_R6_AZ>) _param_ca_mc_r6_az,
-		(ParamFloat<px4::params::CA_MC_R6_CT>) _param_ca_mc_r6_ct,
-		(ParamFloat<px4::params::CA_MC_R6_KM>) _param_ca_mc_r6_km,
-
-		(ParamFloat<px4::params::CA_MC_R7_PX>) _param_ca_mc_r7_px,
-		(ParamFloat<px4::params::CA_MC_R7_PY>) _param_ca_mc_r7_py,
-		(ParamFloat<px4::params::CA_MC_R7_PZ>) _param_ca_mc_r7_pz,
-		(ParamFloat<px4::params::CA_MC_R7_AX>) _param_ca_mc_r7_ax,
-		(ParamFloat<px4::params::CA_MC_R7_AY>) _param_ca_mc_r7_ay,
-		(ParamFloat<px4::params::CA_MC_R7_AZ>) _param_ca_mc_r7_az,
-		(ParamFloat<px4::params::CA_MC_R7_CT>) _param_ca_mc_r7_ct,
-		(ParamFloat<px4::params::CA_MC_R7_KM>) _param_ca_mc_r7_km
-	)
 };
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -41,15 +41,40 @@

 #include "ActuatorEffectivenessStandardVTOL.hpp"

-ActuatorEffectivenessStandardVTOL::ActuatorEffectivenessStandardVTOL()
-{
-	setFlightPhase(FlightPhase::HOVER_FLIGHT);
-}
-
 bool
 ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix,
 		bool force)
 {
+	vehicle_status_s vehicle_status;
+
+	if (_vehicle_status_sub.update(&vehicle_status)) {
+
+		FlightPhase flight_phase{FlightPhase::HOVER_FLIGHT};
+
+		// Check if the current flight phase is HOVER or FIXED_WING
+		if (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+			flight_phase = FlightPhase::HOVER_FLIGHT;
+
+		} else {
+			flight_phase = FlightPhase::FORWARD_FLIGHT;
+		}
+
+		// Special cases for VTOL in transition
+		if (vehicle_status.is_vtol && vehicle_status.in_transition_mode) {
+			if (vehicle_status.in_transition_to_fw) {
+				flight_phase = FlightPhase::TRANSITION_HF_TO_FF;
+
+			} else {
+				flight_phase = FlightPhase::TRANSITION_FF_TO_HF;
+			}
+		}
+
+		if (flight_phase != _flight_phase) {
+			_flight_phase = flight_phase;
+			_updated = true;
+		}
+	}
+
 	if (!(_updated || force)) {
 		return false;
 	}
@@ -99,11 +124,3 @@ ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(matrix::Matrix<float,
 	_updated = false;
 	return true;
 }
-
-void
-ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight_phase)
-{
-	ActuatorEffectiveness::setFlightPhase(flight_phase);
-	_updated = true;
-
-}
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -43,22 +43,30 @@

 #include "ActuatorEffectiveness.hpp"

+#include <uORB/Subscription.hpp>
+#include <uORB/topics/vehicle_status.h>
+
 class ActuatorEffectivenessStandardVTOL: public ActuatorEffectiveness
 {
 public:
-	ActuatorEffectivenessStandardVTOL();
+	ActuatorEffectivenessStandardVTOL() = default;
 	virtual ~ActuatorEffectivenessStandardVTOL() = default;

 	bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix, bool force) override;

-	/**
-	 * Set the current flight phase
-	 *
-	 * @param Flight phase
-	 */
-	void setFlightPhase(const FlightPhase &flight_phase) override;
-
 	int numActuators() const override { return 7; }
 protected:
+
+	enum class FlightPhase {
+		HOVER_FLIGHT = 0,
+		FORWARD_FLIGHT = 1,
+		TRANSITION_HF_TO_FF = 2,
+		TRANSITION_FF_TO_HF = 3
+	};
+
+	FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT};
+
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+
 	bool _updated{true};
 };
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -41,14 +41,40 @@

 #include "ActuatorEffectivenessTiltrotorVTOL.hpp"

-ActuatorEffectivenessTiltrotorVTOL::ActuatorEffectivenessTiltrotorVTOL()
-{
-	setFlightPhase(FlightPhase::HOVER_FLIGHT);
-}
 bool
 ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix,
 		bool force)
 {
+	vehicle_status_s vehicle_status;
+
+	if (_vehicle_status_sub.update(&vehicle_status)) {
+
+		FlightPhase flight_phase{FlightPhase::HOVER_FLIGHT};
+
+		// Check if the current flight phase is HOVER or FIXED_WING
+		if (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+			flight_phase = FlightPhase::HOVER_FLIGHT;
+
+		} else {
+			flight_phase = FlightPhase::FORWARD_FLIGHT;
+		}
+
+		// Special cases for VTOL in transition
+		if (vehicle_status.is_vtol && vehicle_status.in_transition_mode) {
+			if (vehicle_status.in_transition_to_fw) {
+				flight_phase = FlightPhase::TRANSITION_HF_TO_FF;
+
+			} else {
+				flight_phase = FlightPhase::TRANSITION_FF_TO_HF;
+			}
+		}
+
+		if (flight_phase != _flight_phase) {
+			_flight_phase = flight_phase;
+			_updated = true;
+		}
+	}
+
 	if (!(_updated || force)) {
 		return false;
 	}
@@ -106,11 +132,3 @@ ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(matrix::Matrix<float,
 	_updated = false;
 	return true;
 }
-
-void
-ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &flight_phase)
-{
-	ActuatorEffectiveness::setFlightPhase(flight_phase);
-
-	_updated = true;
-}
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -43,22 +43,30 @@

 #include "ActuatorEffectiveness.hpp"

+#include <uORB/Subscription.hpp>
+#include <uORB/topics/vehicle_status.h>
+
 class ActuatorEffectivenessTiltrotorVTOL: public ActuatorEffectiveness
 {
 public:
-	ActuatorEffectivenessTiltrotorVTOL();
+	ActuatorEffectivenessTiltrotorVTOL() = default;
 	virtual ~ActuatorEffectivenessTiltrotorVTOL() = default;

 	bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix, bool force) override;

-	/**
-	 * Set the current flight phase
-	 *
-	 * @param Flight phase
-	 */
-	void setFlightPhase(const FlightPhase &flight_phase) override;
-
 	int numActuators() const override { return 10; }
 protected:
+
+	enum class FlightPhase {
+		HOVER_FLIGHT = 0,
+		FORWARD_FLIGHT = 1,
+		TRANSITION_HF_TO_FF = 2,
+		TRANSITION_FF_TO_HF = 3
+	};
+
+	FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT};
+
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+
 	bool _updated{true};
 };
@@ -1,52 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2019 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-px4_add_library(ActuatorEffectiveness
-	ActuatorEffectiveness.hpp
-	ActuatorEffectivenessMultirotor.cpp
-	ActuatorEffectivenessMultirotor.hpp
-	ActuatorEffectivenessStandardVTOL.cpp
-	ActuatorEffectivenessStandardVTOL.hpp
-	ActuatorEffectivenessTiltrotorVTOL.cpp
-	ActuatorEffectivenessTiltrotorVTOL.hpp
-)
-
-target_compile_options(ActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
-target_include_directories(ActuatorEffectiveness PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
-target_link_libraries(ActuatorEffectiveness
-	PRIVATE
-		mathlib
-		ControlAllocation
-)
-
-# px4_add_unit_gtest(SRC ActuatorEffectivenessMultirotorTest.cpp LINKLIBS ActuatorEffectiveness)
@@ -31,22 +31,45 @@
 #
 ############################################################################

-include_directories(${CMAKE_CURRENT_SOURCE_DIR})
-add_subdirectory(ActuatorEffectiveness)
-add_subdirectory(ControlAllocation)
-
 px4_add_module(
 	MODULE modules__control_allocator
 	MAIN control_allocator
 	COMPILE_FLAGS
 		${MAX_CUSTOM_OPT_LEVEL}
+	INCLUDES
+		${CMAKE_CURRENT_SOURCE_DIR}
 	SRCS
+		ActuatorEffectiveness/ActuatorEffectiveness.hpp
+		ActuatorEffectiveness/ActuatorEffectivenessCustom.cpp
+		ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp
+		ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp
+		ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp
+		ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp
+		ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp
+		ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp
+		ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp
+
+		ControlAllocation/ControlAllocation.cpp
+		ControlAllocation/ControlAllocation.hpp
+		ControlAllocation/ControlAllocationPseudoInverse.cpp
+		ControlAllocation/ControlAllocationPseudoInverse.hpp
+		ControlAllocation/ControlAllocationSequentialDesaturation.cpp
+		ControlAllocation/ControlAllocationSequentialDesaturation.hpp
+
 		ControlAllocator.cpp
 		ControlAllocator.hpp
 	DEPENDS
 		mathlib
-		ActuatorEffectiveness
-		ControlAllocation
-		mixer
 		px4_work_queue
+	MODULE_CONFIG
+		module.yaml
 )
+
+px4_add_library(ControlAllocation_testing
+	ControlAllocation/ControlAllocation.cpp
+	ControlAllocation/ControlAllocationPseudoInverse.cpp
+)
+target_link_libraries(ControlAllocation_testing PRIVATE mathlib)
+
+px4_add_unit_gtest(SRC ControlAllocation/ControlAllocationPseudoInverseTest.cpp LINKLIBS ControlAllocation_testing)
+# px4_add_unit_gtest(SRC ActuatorEffectivenessMultirotorTest.cpp LINKLIBS ActuatorEffectiveness)
@@ -70,7 +70,6 @@
 #pragma once

 #include <matrix/matrix/math.hpp>
-#include <uORB/topics/vehicle_actuator_setpoint.h>

 class ControlAllocation
 {
@@ -81,7 +80,7 @@ public:
 	static constexpr uint8_t NUM_ACTUATORS = 16;
 	static constexpr uint8_t NUM_AXES = 6;

-	typedef matrix::Vector<float, NUM_ACTUATORS> ActuatorVector;
+	using ActuatorVector = matrix::Vector<float, NUM_ACTUATORS>;

 	enum ControlAxis {
 		ROLL = 0,
@@ -40,7 +40,8 @@
 */

 #include <gtest/gtest.h>
-#include <ControlAllocationPseudoInverse.hpp>
+
+#include "ControlAllocationPseudoInverse.hpp"

 using namespace matrix;

@@ -212,6 +212,10 @@ ControlAllocator::update_effectiveness_source()
 			tmp = new ActuatorEffectivenessTiltrotorVTOL();
 			break;

+		case EffectivenessSource::CUSTOM:
+			tmp = new ActuatorEffectivenessCustom(this);
+			break;
+
 		default:
 			PX4_ERR("Unknown airframe");
 			break;
@@ -260,34 +264,6 @@ ControlAllocator::Run()
 		return;
 	}

-	vehicle_status_s vehicle_status;
-
-	if (_vehicle_status_sub.update(&vehicle_status)) {
-
-		ActuatorEffectiveness::FlightPhase flight_phase{ActuatorEffectiveness::FlightPhase::HOVER_FLIGHT};
-
-		// Check if the current flight phase is HOVER or FIXED_WING
-		if (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
-			flight_phase = ActuatorEffectiveness::FlightPhase::HOVER_FLIGHT;
-
-		} else {
-			flight_phase = ActuatorEffectiveness::FlightPhase::FORWARD_FLIGHT;
-		}
-
-		// Special cases for VTOL in transition
-		if (vehicle_status.is_vtol && vehicle_status.in_transition_mode) {
-			if (vehicle_status.in_transition_to_fw) {
-				flight_phase = ActuatorEffectiveness::FlightPhase::TRANSITION_HF_TO_FF;
-
-			} else {
-				flight_phase = ActuatorEffectiveness::FlightPhase::TRANSITION_FF_TO_HF;
-			}
-		}
-
-		// Forward to effectiveness source
-		_actuator_effectiveness->setFlightPhase(flight_phase);
-	}
-
 	// Guard against too small (< 0.2ms) and too large (> 20ms) dt's.
 	const hrt_abstime now = hrt_absolute_time();
 	const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f);
@@ -512,6 +488,10 @@ int ControlAllocator::print_status()
 	case EffectivenessSource::TILTROTOR_VTOL:
 		PX4_INFO("EffectivenessSource: Tiltrotor VTOL");
 		break;
+
+	case EffectivenessSource::CUSTOM:
+		PX4_INFO("EffectivenessSource: Custom");
+		break;
 	}

 	// Print current effectiveness matrix
@@ -41,14 +41,15 @@

 #pragma once

-#include <ActuatorEffectiveness.hpp>
-#include <ActuatorEffectivenessMultirotor.hpp>
-#include <ActuatorEffectivenessStandardVTOL.hpp>
-#include <ActuatorEffectivenessTiltrotorVTOL.hpp>
+#include <ActuatorEffectiveness/ActuatorEffectiveness.hpp>
+#include <ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp>
+#include <ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp>
+#include <ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp>
+#include <ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp>

-#include <ControlAllocation.hpp>
-#include <ControlAllocationPseudoInverse.hpp>
-#include <ControlAllocationSequentialDesaturation.hpp>
+#include <ControlAllocation/ControlAllocation.hpp>
+#include <ControlAllocation/ControlAllocationPseudoInverse.hpp>
+#include <ControlAllocation/ControlAllocationSequentialDesaturation.hpp>

 #include <lib/matrix/matrix/math.hpp>
 #include <lib/perf/perf_counter.h>
@@ -68,7 +69,6 @@
 #include <uORB/topics/vehicle_torque_setpoint.h>
 #include <uORB/topics/vehicle_thrust_setpoint.h>
 #include <uORB/topics/vehicle_actuator_setpoint.h>
-#include <uORB/topics/vehicle_status.h>

 class ControlAllocator : public ModuleBase<ControlAllocator>, public ModuleParams, public px4::WorkItem
 {
@@ -124,10 +124,11 @@ private:
 	ControlAllocation *_control_allocation{nullptr}; 	///< class for control allocation calculations

 	enum class EffectivenessSource {
-		NONE = -1,
-		MULTIROTOR = 0,
-		STANDARD_VTOL = 1,
+		NONE           = -1,
+		MULTIROTOR     = 0,
+		STANDARD_VTOL  = 1,
 		TILTROTOR_VTOL = 2,
+		CUSTOM         = 3,
 	};

 	EffectivenessSource _effectiveness_source_id{EffectivenessSource::NONE};
@@ -147,7 +148,6 @@ private:
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};

 	uORB::Subscription _airspeed_sub{ORB_ID(airspeed)};				/**< airspeed subscription */
-	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};

 	matrix::Vector3f _torque_sp;
 	matrix::Vector3f _thrust_sp;
@@ -50,6 +50,7 @@
 * @value 0 Multirotor
 * @value 1 Standard VTOL (WIP)
 * @value 2 Tiltrotor VTOL (WIP)
+ * @value 3 Custom (CA_ACTn* torque and thrust parameters)
 * @group Control Allocation
 */
 PARAM_DEFINE_INT32(CA_AIRFRAME, 0);
@@ -0,0 +1,72 @@
+__max_num_config_instances: &max_num_config_instances 16
+
+module_name: control_allocator
+
+parameters:
+    - group: Control Allocator
+      definitions:
+        CA_ACT${i}_TRQ_R:
+            description:
+                short: actuator ${i} roll
+                long: Actuator ${i} roll torque
+            type: float
+            decimal: 2
+            reboot_required: false
+            num_instances: *max_num_config_instances
+            instance_start: 0
+            default: 0.0
+
+        CA_ACT${i}_TRQ_P:
+            description:
+                short: actuator ${i} pitch
+                long: Actuator ${i} pitch torque
+            type: float
+            decimal: 2
+            reboot_required: false
+            num_instances: *max_num_config_instances
+            instance_start: 0
+            default: 0.0
+
+        CA_ACT${i}_TRQ_Y:
+            description:
+                short: actuator ${i} yaw
+                long: Actuator ${i} yaw torque
+            type: float
+            decimal: 2
+            reboot_required: false
+            num_instances: *max_num_config_instances
+            instance_start: 0
+            default: 0.0
+
+        CA_ACT${i}_THR_X:
+            description:
+                short: actuator ${i} thrust x
+                long: Actuator ${i} thrust x
+            type: float
+            decimal: 2
+            reboot_required: false
+            num_instances: *max_num_config_instances
+            instance_start: 0
+            default: 0.0
+
+        CA_ACT${i}_THR_Y:
+            description:
+                short: actuator ${i} thrust y
+                long: Actuator ${i} thrust y
+            type: float
+            decimal: 2
+            reboot_required: false
+            num_instances: *max_num_config_instances
+            instance_start: 0
+            default: 0.0
+
+        CA_ACT${i}_THR_Z:
+            description:
+                short: actuator ${i} thrust y
+                long: Actuator ${i} thrust y
+            type: float
+            decimal: 2
+            reboot_required: false
+            num_instances: *max_num_config_instances
+            instance_start: 0
+            default: 0.0
@@ -57,7 +57,7 @@ void Ekf::fuseAirspeed()
 	const float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) *
 			       math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f, 10.0f));

-	// determine if we need the sideslip fusion to correct states other than wind
+	// determine if we need the airspeed fusion to correct states other than wind
 	const bool update_wind_only = !_is_wind_dead_reckoning;

 	// Intermediate variables
@@ -96,8 +96,7 @@ void Ekf::fuseAirspeed()
 		const char *action_string = nullptr;

 		if (update_wind_only) {
-			resetWindStates();
-			resetWindCovariance();
+			resetWindUsingAirspeed();
 			action_string = "wind";

 		} else {
@@ -165,33 +164,46 @@ void Ekf::fuseAirspeed()

 	if (is_fused) {
 		_time_last_arsp_fuse = _time_last_imu;
-		_control_status.flags.fuse_aspd = true;
 	}
 }

-void Ekf::get_true_airspeed(float *tas) const
+float Ekf::getTrueAirspeed() const
 {
-	const float tempvar = sqrtf(sq(_state.vel(0) - _state.wind_vel(0)) + sq(_state.vel(1) - _state.wind_vel(1)) + sq(
-					    _state.vel(2)));
-	memcpy(tas, &tempvar, sizeof(float));
+	return (_state.vel - Vector3f(_state.wind_vel(0), _state.wind_vel(1), 0.f)).norm();
+}
+
+void Ekf::resetWind()
+{
+	if (_control_status.flags.fuse_aspd) {
+		resetWindUsingAirspeed();
+
+	} else {
+		resetWindToZero();
+	}
 }

 /*
 * Reset the wind states using the current airspeed measurement, ground relative nav velocity, yaw angle and assumption of zero sideslip
 */
-void Ekf::resetWindStates()
+void Ekf::resetWindUsingAirspeed()
 {
 	const float euler_yaw = shouldUse321RotationSequence(_R_to_earth)
 				? getEuler321Yaw(_state.quat_nominal)
 				: getEuler312Yaw(_state.quat_nominal);

-	if (_tas_data_ready && (_imu_sample_delayed.time_us - _airspeed_sample_delayed.time_us < (uint64_t)5e5)) {
-		// estimate wind using zero sideslip assumption and airspeed measurement if airspeed available
-		_state.wind_vel(0) = _state.vel(0) - _airspeed_sample_delayed.true_airspeed * cosf(euler_yaw);
-		_state.wind_vel(1) = _state.vel(1) - _airspeed_sample_delayed.true_airspeed * sinf(euler_yaw);
+	// estimate wind using zero sideslip assumption and airspeed measurement if airspeed available
+	_state.wind_vel(0) = _state.vel(0) - _airspeed_sample_delayed.true_airspeed * cosf(euler_yaw);
+	_state.wind_vel(1) = _state.vel(1) - _airspeed_sample_delayed.true_airspeed * sinf(euler_yaw);

-	} else {
-		// If we don't have an airspeed measurement, then assume the wind is zero
-		_state.wind_vel.setZero();
-	}
+	resetWindCovarianceUsingAirspeed();
+
+	_time_last_arsp_fuse = _time_last_imu;
+}
+
+void Ekf::resetWindToZero()
+{
+	// If we don't have an airspeed measurement, then assume the wind is zero
+	_state.wind_vel.setZero();
+	// start with a small initial uncertainty to improve the initial estimate
+	P.uncorrelateCovarianceSetVariance<2>(22, _params.initial_wind_uncertainty);
 }
@@ -1302,29 +1302,43 @@ void Ekf::controlAirDataFusion()
 	const bool airspeed_timed_out = isTimedOut(_time_last_arsp_fuse, (uint64_t)10e6);
 	const bool sideslip_timed_out = isTimedOut(_time_last_beta_fuse, (uint64_t)10e6);

-	if (_control_status.flags.wind &&
-	    (_using_synthetic_position || (airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)))) {
+	if (_using_synthetic_position || (airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG))) {
 		_control_status.flags.wind = false;
 	}

-	if (_control_status.flags.fuse_aspd && airspeed_timed_out) {
-		_control_status.flags.fuse_aspd = false;
+	if (_params.arsp_thr <= 0.f) {
+		stopAirspeedFusion();
+		return;
 	}

-	// Always try to fuse airspeed data if available and we are in flight
-	if (!_using_synthetic_position && _tas_data_ready && _control_status.flags.in_air) {
-		// If starting wind state estimation, reset the wind states and covariances before fusing any data
-		if (!_control_status.flags.wind) {
-			// activate the wind states
-			_control_status.flags.wind = true;
-			// reset the timout timer to prevent repeated resets
-			_time_last_arsp_fuse = _time_last_imu;
-			// reset the wind speed states and corresponding covariances
-			resetWindStates();
-			resetWindCovariance();
+	if (_tas_data_ready) {
+		const bool continuing_conditions_passing = _control_status.flags.in_air && _control_status.flags.fixed_wing && !_using_synthetic_position;
+		const bool is_airspeed_significant = _airspeed_sample_delayed.true_airspeed > _params.arsp_thr;
+		const bool starting_conditions_passing = continuing_conditions_passing && is_airspeed_significant;
+
+		if (_control_status.flags.fuse_aspd) {
+			if (continuing_conditions_passing) {
+				if (is_airspeed_significant) {
+					fuseAirspeed();
+				}
+
+				const bool is_fusion_failing = isTimedOut(_time_last_arsp_fuse, (uint64_t)10e6);
+
+				if (is_fusion_failing) {
+					stopAirspeedFusion();
+				}
+
+			} else {
+				stopAirspeedFusion();
+			}
+
+		} else if (starting_conditions_passing) {
+			startAirspeedFusion();
 		}

-		fuseAirspeed();
+	} else if (_control_status.flags.fuse_aspd && (_imu_sample_delayed.time_us - _airspeed_sample_delayed.time_us > (uint64_t) 1e6)) {
+		ECL_WARN("Airspeed data stopped");
+		stopAirspeedFusion();
 	}
 }

@@ -1346,9 +1360,7 @@ void Ekf::controlBetaFusion()
 			_control_status.flags.wind = true;
 			// reset the timeout timers to prevent repeated resets
 			_time_last_beta_fuse = _time_last_imu;
-			// reset the wind speed states and corresponding covariances
-			resetWindStates();
-			resetWindCovariance();
+			resetWind();
 		}

 		fuseSideslip();
@@ -1365,8 +1377,7 @@ void Ekf::controlDragFusion()
 		if (!_control_status.flags.wind) {
 			// reset the wind states and covariances when starting drag accel fusion
 			_control_status.flags.wind = true;
-			resetWindStates();
-			resetWindCovariance();
+			resetWind();

 		} else if (_drag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_drag_sample_delayed)) {
 			fuseDrag();
@@ -1140,39 +1140,33 @@ void Ekf::resetZDeltaAngBiasCov()
 	P.uncorrelateCovarianceSetVariance<1>(12, init_delta_ang_bias_var);
 }

-void Ekf::resetWindCovariance()
+void Ekf::resetWindCovarianceUsingAirspeed()
 {
-	if (_tas_data_ready && (_imu_sample_delayed.time_us - _airspeed_sample_delayed.time_us < (uint64_t)5e5)) {
-		// Derived using EKF/matlab/scripts/Inertial Nav EKF/wind_cov.py
-		// TODO: explicitly include the sideslip angle in the derivation
-		const float euler_yaw = getEuler321Yaw(_state.quat_nominal);
-		const float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f, 10.0f));
-		constexpr float initial_sideslip_uncertainty = math::radians(15.0f);
-		const float initial_wind_var_body_y = sq(_airspeed_sample_delayed.true_airspeed * sinf(initial_sideslip_uncertainty));
-		constexpr float R_yaw = sq(math::radians(10.0f));
+	// Derived using EKF/matlab/scripts/Inertial Nav EKF/wind_cov.py
+	// TODO: explicitly include the sideslip angle in the derivation
+	const float euler_yaw = getEuler321Yaw(_state.quat_nominal);
+	const float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f, 10.0f));
+	constexpr float initial_sideslip_uncertainty = math::radians(15.0f);
+	const float initial_wind_var_body_y = sq(_airspeed_sample_delayed.true_airspeed * sinf(initial_sideslip_uncertainty));
+	constexpr float R_yaw = sq(math::radians(10.0f));

-		const float cos_yaw = cosf(euler_yaw);
-		const float sin_yaw = sinf(euler_yaw);
+	const float cos_yaw = cosf(euler_yaw);
+	const float sin_yaw = sinf(euler_yaw);

-		// rotate wind velocity into earth frame aligned with vehicle yaw
-		const float Wx = _state.wind_vel(0) * cos_yaw + _state.wind_vel(1) * sin_yaw;
-		const float Wy = -_state.wind_vel(0) * sin_yaw + _state.wind_vel(1) * cos_yaw;
+	// rotate wind velocity into earth frame aligned with vehicle yaw
+	const float Wx = _state.wind_vel(0) * cos_yaw + _state.wind_vel(1) * sin_yaw;
+	const float Wy = -_state.wind_vel(0) * sin_yaw + _state.wind_vel(1) * cos_yaw;

-		// it is safer to remove all existing correlations to other states at this time
-		P.uncorrelateCovarianceSetVariance<2>(22, 0.0f);
+	// it is safer to remove all existing correlations to other states at this time
+	P.uncorrelateCovarianceSetVariance<2>(22, 0.0f);

-		P(22, 22) = R_TAS * sq(cos_yaw) + R_yaw * sq(-Wx * sin_yaw - Wy * cos_yaw) + initial_wind_var_body_y * sq(sin_yaw);
-		P(22, 23) = R_TAS * sin_yaw * cos_yaw + R_yaw * (-Wx * sin_yaw - Wy * cos_yaw) * (Wx * cos_yaw - Wy * sin_yaw) -
-			    initial_wind_var_body_y * sin_yaw * cos_yaw;
-		P(23, 22) = P(22, 23);
-		P(23, 23) = R_TAS * sq(sin_yaw) + R_yaw * sq(Wx * cos_yaw - Wy * sin_yaw) + initial_wind_var_body_y * sq(cos_yaw);
+	P(22, 22) = R_TAS * sq(cos_yaw) + R_yaw * sq(-Wx * sin_yaw - Wy * cos_yaw) + initial_wind_var_body_y * sq(sin_yaw);
+	P(22, 23) = R_TAS * sin_yaw * cos_yaw + R_yaw * (-Wx * sin_yaw - Wy * cos_yaw) * (Wx * cos_yaw - Wy * sin_yaw) -
+		    initial_wind_var_body_y * sin_yaw * cos_yaw;
+	P(23, 22) = P(22, 23);
+	P(23, 23) = R_TAS * sq(sin_yaw) + R_yaw * sq(Wx * cos_yaw - Wy * sin_yaw) + initial_wind_var_body_y * sq(cos_yaw);

-		// Now add the variance due to uncertainty in vehicle velocity that was used to calculate the initial wind speed
-		P(22, 22) += P(4, 4);
-		P(23, 23) += P(5, 5);
-
-	} else {
-		// without airspeed, start with a small initial uncertainty to improve the initial estimate
-		P.uncorrelateCovarianceSetVariance<2>(22, _params.initial_wind_uncertainty);
-	}
+	// Now add the variance due to uncertainty in vehicle velocity that was used to calculate the initial wind speed
+	P(22, 22) += P(4, 4);
+	P(23, 23) += P(5, 5);
 }
@@ -134,7 +134,7 @@ public:
 	Vector2f getWindVelocityVariance() const { return P.slice<2, 2>(22, 22).diag(); }

 	// get the true airspeed in m/s
-	void get_true_airspeed(float *tas) const;
+	float getTrueAirspeed() const;

 	// get the full covariance matrix
 	const matrix::SquareMatrix<float, 24> &covariances() const { return P; }
@@ -917,10 +917,12 @@ private:
 	void resetMagCov();

 	// perform a limited reset of the wind state covariances
-	void resetWindCovariance();
+	void resetWindCovarianceUsingAirspeed();

-	// perform a reset of the wind states
-	void resetWindStates();
+	// perform a reset of the wind states and related covariances
+	void resetWind();
+	void resetWindUsingAirspeed();
+	void resetWindToZero();

 	// check that the range finder data is continuous
 	void updateRangeDataContinuity();
@@ -954,6 +956,9 @@ private:
 		return sensor_timestamp + acceptance_interval > _time_last_imu;
 	}

+	void startAirspeedFusion();
+	void stopAirspeedFusion();
+
 	void startGpsFusion();
 	void stopGpsFusion();
 	void stopGpsPosFusion();
@@ -1480,6 +1480,24 @@ void Ekf::loadMagCovData()
 	P.slice<2, 2>(16, 16) = _saved_mag_ef_covmat;
 }

+void Ekf::startAirspeedFusion()
+{
+	// If starting wind state estimation, reset the wind states and covariances before fusing any data
+	if (!_control_status.flags.wind) {
+		// activate the wind states
+		_control_status.flags.wind = true;
+		// reset the wind speed states and corresponding covariances
+		resetWindUsingAirspeed();
+	}
+
+	_control_status.flags.fuse_aspd = true;
+}
+
+void Ekf::stopAirspeedFusion()
+{
+	_control_status.flags.fuse_aspd = false;
+}
+
 void Ekf::startGpsFusion()
 {
 	resetHorizontalPositionToGps();
@@ -1509,7 +1527,11 @@ void Ekf::stopGpsFusion()
 void Ekf::stopGpsPosFusion()
 {
 	_control_status.flags.gps = false;
-	_control_status.flags.gps_hgt = false;
+
+	if (_control_status.flags.gps_hgt) {
+		startBaroHgtFusion();
+	}
+
 	_gps_pos_innov.setZero();
 	_gps_pos_innov_var.setZero();
 	_gps_pos_test_ratio.setZero();
@@ -1719,13 +1741,15 @@ bool Ekf::getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_M

 void Ekf::runYawEKFGSF()
 {
-	float TAS;
+	float TAS = 0.f;

-	if (isTimedOut(_airspeed_sample_delayed.time_us, 1000000) && _control_status.flags.fixed_wing) {
-		TAS = _params.EKFGSF_tas_default;
+	if (_control_status.flags.fixed_wing) {
+		if (isTimedOut(_airspeed_sample_delayed.time_us, 1000000)) {
+			TAS = _params.EKFGSF_tas_default;

-	} else {
-		TAS = _airspeed_sample_delayed.true_airspeed;
+		} else if (_airspeed_sample_delayed.true_airspeed >= _params.arsp_thr) {
+			TAS = _airspeed_sample_delayed.true_airspeed;
+		}
 	}

 	const Vector3f imu_gyro_bias = getGyroBias();
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
Daniel Agar	a88c6434b6	control_allocator: add parameter based ActuatorEffectivenessCustom - this allows creating a custom actuator effectiveness entirely from parameters of the form CA_ACTn_TRQ_{R,P,Y} and CA_ACTn_THR_{X,Y,Z}	2021-10-27 14:25:03 -04:00
Daniel Agar	43d7d83a4b	control_allocator: push VTOL flight phase into ActuatorEffectiveness	2021-10-27 13:42:22 -04:00
Daniel Agar	eb362f2f63	control_allocator: replace ModuleParams with generated parameter strings as needed	2021-10-27 12:48:42 -04:00
Peter van der Perk	51abb804ac	UAVCANv1 Fix NodeClient header and Kconfig merge logic	2021-10-27 10:07:01 -04:00
Beat Küng	0decdb1c7b	github action: run ./Tools/generate_board_targets_json.py in container As it requires kconfiglib	2021-10-27 15:23:23 +02:00
Silvan Fuhrer	e715e6c245	Fixed-wing position control: set yaw_sp to yaw_current instead of nav_bearing when not controlled Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2021-10-27 14:35:00 +03:00
Silvan Fuhrer	b53808d11b	fixed-wing: set yaw_sp to yaw_current instead of 0 when not controlled Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2021-10-27 14:35:00 +03:00
Silvan Fuhrer	da4d6dc657	L1: increase the max allowed tangential velocity in the opposite direction to 2m/s There is logic in L1 that prevents the vehicle from trying to achieve an impossible loiter entry (e.g. due to wind). That check makes the vehicle track the loiter center if the tangential velocity is in the wrong direction while loitering. After the vehicle flies through the center, it can then turn the other way around to join the loiter. This check is though too sensitive if it purely checks for the wrong direction, and it can end in delayed loiter entry for no reason. This commit increases the threshold to 2m/s of tangential velocity in the wrong direction to trigger the check. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2021-10-27 12:32:58 +03:00
RomanBapst	eee5f501cd	navigator: fix flyaway when altitude change is commanded without a valid triplet Signed-off-by: RomanBapst <bapstroman@gmail.com>	2021-10-27 11:01:13 +03:00
RomanBapst	bf6a47ba6a	navigator: cleanup of set_loiter_item Unwraps the set_loiter_item() to solve the issue where the altitdue setpoint in a MC takeoff wasn't correctly used. Signed-off-by: RomanBapst <bapstroman@gmail.com>	2021-10-27 11:01:13 +03:00
Silvan Fuhrer	cb78ba34d7	Mission: for tangential loiter exit, set current position setpoint typ to position Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2021-10-27 11:01:13 +03:00
Silvan Fuhrer	4b21c0c49e	Fw Pos C: always reset pos_sp type from LOITER to POSITION if far away Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2021-10-27 11:01:13 +03:00
RomanBapst	d678e792cc	mission_block: don't require an exiting heading when loitering if the next waypoint is within the loiter radius of the current waypoint Signed-off-by: RomanBapst <bapstroman@gmail.com>	2021-10-27 11:01:13 +03:00
Michael Schaeuble	5e1f62e9d0	Add option to warn the pilot in case of strong magnetic interference but still allow arming. This PR changes the COM_ARM_MAG_STR parameter to accept values. If the parameter is set to 2, the check is performed and a warning is logged but the vehicle can still arm.	2021-10-27 09:59:18 +02:00
ponomarevda	2b6bd452df	fix hardpoint hardfault by checking argc before std::strcmp	2021-10-27 08:11:23 +02:00
Beat Küng	de488f0f40	omnibus/f4sd: add topic listener & change timer order So it matches the usage in the channel definition order	2021-10-27 08:03:55 +02:00
Beat Küng	8476875b4d	Kconfig: add missing serial ports	2021-10-27 08:03:55 +02:00
Beat Küng	48344c6e2a	state_machine_helper: add missing 'break' (no behavior change)	2021-10-27 08:03:55 +02:00
Daniel Agar	6d0c6bb6ce	lib/world_magnetic_model: cmake remove helper target BYPRODUCTS - otherwise ninja will try to rebuild these	2021-10-26 18:52:12 -04:00
dagar	a2801bab80	[AUTO COMMIT] update change indication	2021-10-26 14:39:58 -04:00
Daniel Agar	88a979cf1d	lib/world_magnetic_model: add cmake helpers for updating tables - `world_magnetic_model_update` to fetch latest geo_magnetic_tables.hpp - `world_magnetic_model_tests_update` to fetch latest test_geo_lookup.cpp	2021-10-26 14:39:58 -04:00
Peter van der Perk	24ab430466	Tools/generate_board_targets_json.py: fix json board targets regression from #17100	2021-10-26 16:15:39 +02:00
bresch	d0f89f7fff	ekf2: refactor wind reset functions	2021-10-26 10:18:56 +02:00
bresch	456dfcb4b9	ekf2: update getter for true airspeed	2021-10-26 10:18:56 +02:00
bresch	3927c183de	ekf2_test: adjust airspeed unit test an airpseed of > 2m/s is required to start the fusion (set by param) fw mode is also required Given the larger estimated windspeed after those changes, the change of static pressure is larger and the height estimate takes more time to reach the final value	2021-10-26 10:18:56 +02:00
bresch	6e8f0e92ff	ekf2: refactor airspeed fusion control logic	2021-10-26 10:18:56 +02:00
bresch	8873e92c7c	ekf: force fallback to baro if GPS is stopped while in GPS height mode Otherwise, no height aiding source is used	2021-10-26 10:05:28 +02:00
bresch	0a140ec59a	ekf2_test: add GPS height to baro fallback	2021-10-26 10:05:28 +02:00
bresch	f4c21cedd9	ekf2_test: use motion_planning for dynamic yaw emergency test	2021-10-25 18:06:38 -04:00
bresch	340a2caa8e	ekf2_test: use motion_planning library The VelocitySmoothing class from the motion_planning library is used to generate trajectories in order to test the EKF convergence during motion	2021-10-25 18:06:38 -04:00
David Sidrane	38e2e6a01f	Use NuttX MPU Reset (#18283 ) * NuttX with MPU reset backports * Use NuttX MPU reset	2021-10-25 18:05:31 -04:00
alexklimaj	8088c82b6a	Add CANNODE_FLOW_ROT	2021-10-25 16:31:00 -04:00
Jaeyoung-Lim	5dcaadf492	Fix px4vision defaults	2021-10-25 16:29:52 -04:00
Landon Haugh	24cd0c6fa3	Enablement of PX4 SPI driver for UCANS32K146	2021-10-25 08:36:54 -07:00
Daniel Agar	a548c94230	boards: holybro_durandal-v1_default disable modules to save flash	2021-10-25 13:41:31 +02:00
bresch	6ec9ab11f2	add fw auto-tune module to board configs	2021-10-25 13:41:31 +02:00
bresch	95e2941b17	fw att: inject system identification signal to controller	2021-10-25 13:41:31 +02:00
bresch	6af0856558	add FF to FW rate controllers	2021-10-25 13:41:31 +02:00
bresch	55f0860c31	fw atune: add fixed-wing auto-tuning module	2021-10-25 13:41:31 +02:00
bresch	8dfdb1e3db	compute and publish fixed-wing control power	2021-10-25 13:41:31 +02:00