rc_input: singlewire use push_pull

fix commander: make sure to count all valid mags in preflight check
Previously, if a mag was not required (not index 0 and not used by ekf), it was not counted in num_enabled_and_valid_calibration. If a user set SYS_HAS_MAG to e.g. 3, it would then trigger a preflight failure, even if there were 3 calibrated and enabled mags.
2026-05-26 09:50:04 +08:00 · 2024-06-01 19:15:42 +02:00 · 2024-05-31 12:30:34 -04:00 · 2024-05-31 10:38:17 -04:00 · 2024-05-31 10:38:17 -04:00 · 2024-05-31 10:38:17 -04:00
280 changed files with 4188 additions and 6653 deletions
@@ -15,6 +15,7 @@
 	"extensions": [
 		"chiehyu.vscode-astyle",
 		"dan-c-underwood.arm",
+		"editorconfig.editorconfig",
 		"fredericbonnet.cmake-test-adapter",
 		"github.vscode-pull-request-github",
 		"marus25.cortex-debug",
@@ -1,7 +1,7 @@
 root = true

 [*]
-insert_final_newline = false
+insert_final_newline = true

 [{*.{c,cpp,cc,h,hpp},CMakeLists.txt,Kconfig}]
 indent_style = tab
@@ -4,6 +4,7 @@
    "recommendations": [
        "chiehyu.vscode-astyle",
        "dan-c-underwood.arm",
+        "editorconfig.editorconfig",
        "fredericbonnet.cmake-test-adapter",
        "github.vscode-pull-request-github",
        "marus25.cortex-debug",
@@ -1,6 +1,6 @@
 ############################################################################
 #
-# Copyright (c) 2015 - 2024 PX4 Development Team. All rights reserved.
+# Copyright (c) 2015 - 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
@@ -323,32 +323,7 @@ px4io_update:
 	cp build/cubepilot_io-v2_default/cubepilot_io-v2_default.bin boards/cubepilot/cubeyellow/extras/cubepilot_io-v2_default.bin
 	git status

-bootloaders_update: \
-	ark_fmu-v6x_bootloader \
-	ark_pi6x_bootloader \
-	cuav_nora_bootloader \
-	cuav_x7pro_bootloader \
-	cubepilot_cubeorange_bootloader \
-	cubepilot_cubeorangeplus_bootloader \
-	hkust_nxt-dual_bootloader \
-	hkust_nxt-v1_bootloader \
-	holybro_durandal-v1_bootloader \
-	holybro_kakuteh7_bootloader \
-	holybro_kakuteh7mini_bootloader \
-	holybro_kakuteh7v2_bootloader \
-	matek_h743_bootloader \
-	matek_h743-mini_bootloader \
-	matek_h743-slim_bootloader \
-	modalai_fc-v2_bootloader \
-	mro_ctrl-zero-classic_bootloader \
-	mro_ctrl-zero-h7_bootloader \
-	mro_ctrl-zero-h7-oem_bootloader \
-	mro_pixracerpro_bootloader \
-	px4_fmu-v6c_bootloader \
-	px4_fmu-v6u_bootloader \
-	px4_fmu-v6x_bootloader \
-	px4_fmu-v6xrt_bootloader \
-	siyi_n7_bootloader
+bootloaders_update: ark_fmu-v6x_bootloader cuav_nora_bootloader cuav_x7pro_bootloader cubepilot_cubeorange_bootloader holybro_durandal-v1_bootloader holybro_kakuteh7_bootloader matek_h743_bootloader matek_h743-mini_bootloader matek_h743-slim_bootloader modalai_fc-v2_bootloader mro_ctrl-zero-classic_bootloader mro_ctrl-zero-h7_bootloader mro_ctrl-zero-h7-oem_bootloader mro_pixracerpro_bootloader px4_fmu-v6c_bootloader px4_fmu-v6u_bootloader px4_fmu-v6x_bootloader
 	git status

 .PHONY: coverity_scan
@@ -24,7 +24,6 @@ param set-default FW_RR_P 0.085
 param set-default FW_W_EN 1

 param set-default MIS_TAKEOFF_ALT 20
-param set-default MIS_DIST_1WP 2500

 param set-default NAV_ACC_RAD 15
 param set-default NAV_DLL_ACT 2
@@ -24,7 +24,6 @@ param set-default FW_RR_P 0.085
 param set-default FW_W_EN 1

 param set-default MIS_TAKEOFF_ALT 20
-param set-default MIS_DIST_1WP 2500

 param set-default NAV_ACC_RAD 15
 param set-default NAV_DLL_ACT 2
@@ -24,7 +24,6 @@ param set-default FW_RR_P 0.085
 param set-default FW_W_EN 1

 param set-default MIS_TAKEOFF_ALT 20
-param set-default MIS_DIST_1WP 2500

 param set-default NAV_ACC_RAD 15
 param set-default NAV_DLL_ACT 2
@@ -25,7 +25,6 @@ param set-default FW_W_EN 1

 param set-default MIS_LTRMIN_ALT 30
 param set-default MIS_TAKEOFF_ALT 20
-param set-default MIS_DIST_1WP 2500

 param set-default NAV_ACC_RAD 15
 param set-default NAV_DLL_ACT 2
@@ -66,7 +66,6 @@ param set-default MC_PITCHRATE_I 0.05
 param set-default MC_YAWRATE_MAX 20
 param set-default MC_AIRMODE 1

-param set-default MIS_DIST_1WP 100
 param set-default MIS_TAKEOFF_ALT 15

 param set-default MPC_XY_P 0.8
@@ -138,6 +138,12 @@ then
 	adis16507 -S start
 fi

+# SCH16T spi external IMU
+if param compare -s SENS_EN_SCH16T 1
+then
+	sch16t -S start
+fi
+
 # Eagle Tree airspeed sensor external I2C
 if param compare -s SENS_EN_ETSASPD 1
 then
@@ -123,6 +123,13 @@ else
 		set PARAM_FILE /fs/mtd_params
 	fi

+	# Check if /fs/mtd_params is a valid BSON file
+	if ! bsondump docsize /fs/mtd_caldata
+	then
+		echo "New /fs/mtd_caldata size is:"
+		bsondump docsize /fs/mtd_caldata
+	fi
+
 	#
 	# Load parameters.
 	#
@@ -6,7 +6,7 @@ function check_git_submodule {
 if [[ -f $1"/.git" || -d $1"/.git" ]]; then

 	# always update within CI environment or configuring withing VSCode CMake where you can't interact
-	if [ "$CI" == "true" ] || [ -n "${VSCODE_PID+set}" ]; then
+	if [ "$CI" == "true" ] || [ -n "${VSCODE_PID+set}" ] || [ -n "${CLION_IDE+set}" ]; then
 		git submodule --quiet sync --recursive -- $1
 		git submodule --quiet update --init --recursive --jobs=8 -- $1  || true
 		git submodule --quiet sync --recursive -- $1
@@ -34,33 +34,41 @@ def extract_timer(line):
    if search:
        return search.group(1), 'generic'

-    # nxp rt1062 format: initIOPWM(PWM::FlexPWM2),
-    search = re.search('PWM::Flex([0-9a-zA-Z_]+)[,\)]', line, re.IGNORECASE)
+    # NXP FlexPWM format format: initIOPWM(PWM::FlexPWM2),
+    search = re.search('PWM::Flex([0-9a-zA-Z_]+)..PWM::Submodule([0-9])[,\)]', line, re.IGNORECASE)
    if search:
-        return search.group(1), 'imxrt'
+        return (search.group(1) + '_' +  search.group(2)), 'imxrt'

    return None, 'unknown'

-def extract_timer_from_channel(line, num_channels_already_found):
+def extract_timer_from_channel(line, timer_names):
    # Try format: initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel1}, {GPIO::PortA, GPIO::Pin0}),
    search = re.search('Timer::([0-9a-zA-Z_]+), ', line, re.IGNORECASE)
    if search:
        return search.group(1)

-    # nxp rt1062 format: initIOTimerChannel(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_B0_06),
-    search = re.search('PWM::(PWM[0-9]+)[_,\)]', line, re.IGNORECASE)
+    # NXP FlexPWM format: initIOTimerChannel(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_B0_06),
+    search = re.search('PWM::(PWM[0-9]+).*PWM::Submodule([0-9])', line, re.IGNORECASE)
    if search:
-        # imxrt uses a 1:1 timer group to channel association
-        return str(num_channels_already_found)
+        return str(timer_names.index((search.group(1) + '_' +  search.group(2))))

    return None

+def imxrt_is_dshot(line):
+
+    # NXP FlexPWM format format: initIOPWM(PWM::FlexPWM2),
+    search = re.search('(initIOPWMDshot)', line, re.IGNORECASE)
+    if search:
+        return True
+
+    return False
+
 def get_timer_groups(timer_config_file, verbose=False):
    with open(timer_config_file, 'r') as f:
        timer_config = f.read()

    # timers
-    dshot_support = {} # key: timer
+    dshot_support = {str(i): False for i in range(16)}
    timers_start_marker = 'io_timers_t io_timers'
    timers_start = timer_config.find(timers_start_marker)
    if timers_start == -1:
@@ -69,6 +77,7 @@ def get_timer_groups(timer_config_file, verbose=False):
    open_idx, close_idx = find_matching_brackets(('{', '}'), timer_config, verbose)
    timers_str = timer_config[open_idx:close_idx]
    timers = []
+    timer_names = []
    for line in timers_str.splitlines():
        line = line.strip()
        if len(line) == 0 or line.startswith('//'):
@@ -77,14 +86,11 @@ def get_timer_groups(timer_config_file, verbose=False):

        if timer_type == 'imxrt':
            if verbose: print('imxrt timer found')
-            max_num_channels = 16 # Just add a fixed number of timers
-            timers = [str(i) for i in range(max_num_channels)]
-            dshot_support = {str(i): False for i in range(max_num_channels)}
-            for i in range(8): # First 8 channels support dshot
-                dshot_support[str(i)] = True
-            break
-
-        if timer:
+            timer_names.append(timer)
+            if imxrt_is_dshot(line):
+                dshot_support[str(len(timers))] = True
+            timers.append(str(len(timers)))
+        elif timer:
            if verbose: print('found timer def: {:}'.format(timer))
            dshot_support[timer] = 'DMA' in line
            timers.append(timer)
@@ -111,7 +117,7 @@ def get_timer_groups(timer_config_file, verbose=False):
            continue

        if verbose: print('--'+line+'--')
-        timer = extract_timer_from_channel(line, len(channel_timers))
+        timer = extract_timer_from_channel(line, timer_names)

        if timer:
            if verbose: print('Found timer: {:} in channel line {:}'.format(timer, line))
@@ -15,7 +15,7 @@ class ModuleDocumentation(object):
    # TOC in https://github.com/PX4/PX4-user_guide/blob/main/en/SUMMARY.md
    valid_categories = ['driver', 'estimator', 'controller', 'system',
                        'communication', 'command', 'template', 'simulation', 'autotune']
-    valid_subcategories = ['', 'distance_sensor', 'imu', 'ins', 'airspeed_sensor',
+    valid_subcategories = ['', 'camera', 'distance_sensor', 'imu', 'ins', 'airspeed_sensor',
                           'magnetometer', 'baro', 'optical_flow', 'rpm_sensor', 'transponder']

    max_line_length = 80 # wrap lines that are longer than this
@@ -6,6 +6,7 @@
 param set-default CBRK_IO_SAFETY 0
 param set-default CANNODE_SUB_MBD 1
 param set-default CANNODE_SUB_RTCM 1
+param set-default GPS_1_GNSS 63
 param set-default MBE_ENABLE 1
 param set-default SENS_IMU_CLPNOTI 0

@@ -47,7 +47,7 @@
 #define GPIO_BTN_SAFETY       /* PB15 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTB|GPIO_PIN15)

 /* Safety LED */
-#define GPIO_LED_SAFETY       /* PA1  */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN1)
+#define GPIO_LED_SAFETY       /* PA1  */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN1)

 /* Tone alarm output. */
 #define TONE_ALARM_TIMER             2    /* timer 2 */
@@ -14,6 +14,7 @@ CONFIG_DRIVERS_DSHOT=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_COMMON_HYGROMETERS=y
 CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y
+CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16507=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
 CONFIG_DRIVERS_IRLOCK=y
@@ -19,6 +19,7 @@ CONFIG_DRIVERS_DSHOT=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_HEATER=y
 CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16507=y
+CONFIG_DRIVERS_IMU_MURATA_SCH16T=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
 CONFIG_DRIVERS_IMU_INVENSENSE_IIM42652=y
 CONFIG_DRIVERS_IMU_INVENSENSE_IIM42653=y
@@ -58,6 +59,7 @@ CONFIG_MODULES_LOGGER=y
 CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
+CONFIG_MAVLINK_DIALECT="development"
 CONFIG_MODULES_MC_ATT_CONTROL=y
 CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
@@ -10,7 +10,6 @@ CONFIG_DRIVERS_BAROMETER_BMP388=y
 CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
 CONFIG_DRIVERS_CDCACM_AUTOSTART=y
 CONFIG_DRIVERS_DISTANCE_SENSOR_BROADCOM_AFBRS50=y
-CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L0X=y
 CONFIG_DRIVERS_DSHOT=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_HEATER=y
@@ -41,6 +40,7 @@ CONFIG_MODULES_LOGGER=y
 CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
+CONFIG_MAVLINK_DIALECT="development"
 CONFIG_MODULES_MC_ATT_CONTROL=y
 CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
@@ -32,10 +32,19 @@ then
 	param set-default SENS_TEMP_ID 2490378
 fi

+param set-default EKF2_BARO_DELAY 39
+param set-default EKF2_BARO_NOISE 0.9
+param set-default EKF2_HGT_REF 0
+param set-default EKF2_MAG_DELAY 60
+param set-default EKF2_MAG_NOISE 0.06
 param set-default EKF2_MULTI_IMU 2
 param set-default EKF2_OF_CTRL 1
+param set-default EKF2_OF_DELAY 28
 param set-default EKF2_OF_N_MIN 0.05
 param set-default EKF2_RNG_A_HMAX 25
+param set-default EKF2_RNG_DELAY 105
+param set-default EKF2_RNG_NOISE 0.03
 param set-default EKF2_RNG_QLTY_T 0.1
+param set-default EKF2_RNG_SFE 0.03

 param set-default SENS_FLOW_RATE 150
@@ -1,13 +1,13 @@
 CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
 CONFIG_BOARD_ARCHITECTURE="cortex-m4"
 CONFIG_BOARD_ROMFSROOT="cannode"
-CONFIG_BOARD_NO_HELP=y
 CONFIG_BOARD_CONSTRAINED_MEMORY=y
 CONFIG_DRIVERS_BAROMETER_BMP388=y
 CONFIG_DRIVERS_BOOTLOADERS=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
 CONFIG_DRIVERS_MAGNETOMETER_BOSCH_BMM150=y
+CONFIG_DRIVERS_MAGNETOMETER_ST_IIS2MDC=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
@@ -8,4 +8,7 @@ icm42688p -R 0 -s start

 bmp388 -I -b 1 start

-bmm150 -I -b 1 start
+if ! iis2mdc -R 4 -I -b 1 start
+then
+	bmm150 -I -b 1 start
+fi
@@ -14,7 +14,6 @@ CONFIG_DRIVERS_CAMERA_TRIGGER=y
 CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
 CONFIG_COMMON_DISTANCE_SENSOR=y
 CONFIG_DRIVERS_DSHOT=y
-CONFIG_DRIVERS_GNSS_SEPTENTRIO=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_HEATER=y
 CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y
@@ -15,7 +15,6 @@ CONFIG_DRIVERS_CDCACM_AUTOSTART=y
 CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
 CONFIG_COMMON_DISTANCE_SENSOR=y
 CONFIG_DRIVERS_DSHOT=y
-CONFIG_DRIVERS_GNSS_SEPTENTRIO=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
@@ -194,12 +194,6 @@

 #define STM32_RCC_D3CCIPR_ADCSRC     RCC_D3CCIPR_ADCSEL_PLL2     /* ADC 1 2 3 clock source */

-/* UART clock selection */
-/* reset to default to overwrite any changes done by any bootloader */
-
-#define STM32_RCC_D2CCIP2R_USART234578_SEL RCC_D2CCIP2R_USART234578SEL_RCC
-#define STM32_RCC_D2CCIP2R_USART16_SEL     RCC_D2CCIP2R_USART16SEL_RCC
-
 /* FLASH wait states */
 #define BOARD_FLASH_WAITSTATES 2

@@ -15,7 +15,6 @@ CONFIG_DRIVERS_CDCACM_AUTOSTART=y
 CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
 CONFIG_COMMON_DISTANCE_SENSOR=y
 CONFIG_DRIVERS_DSHOT=y
-CONFIG_DRIVERS_GNSS_SEPTENTRIO=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y
@@ -195,12 +195,6 @@

 #define STM32_RCC_D3CCIPR_ADCSRC     RCC_D3CCIPR_ADCSEL_PLL2     /* ADC 1 2 3 clock source */

-/* UART clock selection */
-/* reset to default to overwrite any changes done by any bootloader */
-
-#define STM32_RCC_D2CCIP2R_USART234578_SEL RCC_D2CCIP2R_USART234578SEL_RCC
-#define STM32_RCC_D2CCIP2R_USART16_SEL     RCC_D2CCIP2R_USART16SEL_RCC
-
 /* FLASH wait states */
 #define BOARD_FLASH_WAITSTATES 2

@@ -9,7 +9,7 @@ When running PX4 directly on the QRB5165 SoC it runs partially on the Sensor Low
 The portion running on the DSP hosts the flight critical portions of PX4 such as
 the IMU, barometer, magnetometer, GPS, ESC, and power management drivers, and the
 state estimation. The DSP acts as the real time portion of the system. Non flight
-critical applications such as Mavlink, logging, and commander are running on the
+critical applications such as Mavlink, and logging are running on the
 ARM CPU cluster (aka apps proc). The DSP and ARM CPU cluster communicate via a
 Qualcomm proprietary shared memory interface.

@@ -27,6 +27,7 @@ The full instructions are available here:
 ```
 px4$ boards/modalai/voxl2/scripts/run-docker.sh
 root@9373fa1401b8:/usr/local/workspace# boards/modalai/voxl2/scripts/clean.sh
+root@9373fa1401b8:/usr/local/workspace# boards/modalai/voxl2/scripts/build-deps.sh
 root@9373fa1401b8:/usr/local/workspace# boards/modalai/voxl2/scripts/build-apps.sh
 root@9373fa1401b8:/usr/local/workspace# boards/modalai/voxl2/scripts/build-slpi.sh
 root@9373fa1401b8:/usr/local/workspace# exit
@@ -69,16 +70,15 @@ pxh>
 ## Notes

 You cannot cleanly shutdown PX4 with the shutdown command on VOXL 2. You have
-to power cycle the board and restart everything.
+to power cycle the board and restart everything. Starting with SDK 1.3.0 it is possible
+to cleanly shutdown PX4 on VOXL 2.

 ## Tips

-Start with a VOXL 2 that only has the system image installed, not the SDK
-
-Run the command ```voxl-px4 -s``` on target to run the self-test
+Always use the latest SDK release

 In order to see DSP specific debug messages the mini-dm tool in the Hexagon SDK
-can be used:
+can be used (Most messages are passed to the apps proc but certain low level messages are not):
 ```
 modalai@modalai-XPS-15-9570:/local/mnt/workspace/Qualcomm/Hexagon_SDK/4.1.0.4/tools/debug/mini-dm/Ubuntu18$ sudo ./mini-dm
 [sudo] password for modalai:
@@ -20,7 +20,6 @@ adb shell chmod a+x /usr/bin/voxl-px4-hitl-start

 # Push configuration file
 adb shell mkdir -p /etc/modalai
-adb push boards/modalai/voxl2/target/voxl-px4-set-default-parameters.config /etc/modalai
 adb push boards/modalai/voxl2/target/voxl-px4-fake-imu-calibration.config /etc/modalai
 adb push boards/modalai/voxl2/target/voxl-px4-hitl-set-default-parameters.config /etc/modalai

@@ -129,12 +129,6 @@ else
    DAEMON="-d"
 fi

-if [ ! -f /data/px4/param/parameters ]; then
-   echo "[INFO] Setting default parameters for PX4 on voxl"
-   px4 $DAEMON -s /etc/modalai/voxl-px4-set-default-parameters.config
-   /bin/sync
-fi
-
 if [ $SENSOR_CAL == "FAKE" ]; then
   /bin/echo "[INFO] Setting up fake sensor calibration values"
   px4 $DAEMON -s /etc/modalai/voxl-px4-fake-imu-calibration.config
@@ -1,192 +0,0 @@
-#!/bin/sh
-# PX4 commands need the 'px4-' prefix in bash.
-# (px4-alias.sh is expected to be in the PATH)
-. px4-alias.sh
-
-param select /data/px4/param/parameters
-
-# Make sure we are running at 800Hz on IMU
-param set IMU_GYRO_RATEMAX 800
-
-# EKF2 Parameters
-param set EKF2_IMU_POS_X 0.027
-param set EKF2_IMU_POS_Y 0.009
-param set EKF2_IMU_POS_Z -0.019
-param set EKF2_EV_DELAY 5
-param set EKF2_AID_MASK 280
-param set EKF2_ABL_LIM 0.8
-param set EKF2_TAU_POS 0.25
-param set EKF2_TAU_VEL 0.25
-
-param set MC_AIRMODE 0
-
-param set MC_YAW_P 2.0
-param set MC_YAWRATE_P 0.15
-param set MC_YAWRATE_I 0.1
-param set MC_YAWRATE_D 0.0
-param set MC_YAWRATE_K 1.0
-
-param set MC_PITCH_P 5.5
-param set MC_PITCHRATE_P 0.08
-param set MC_PITCHRATE_I 0.2
-param set MC_PITCHRATE_D 0.0013
-param set MC_PITCHRATE_K 1.0
-
-param set MC_ROLL_P 5.5
-param set MC_ROLLRATE_P 0.08
-param set MC_ROLLRATE_I 0.2
-param set MC_ROLLRATE_D 0.0013
-param set MC_ROLLRATE_K 1.0
-
-param set MPC_VELD_LP 5.0
-
-# tweak MPC_THR_MIN to prevent roll/pitch losing control
-# authority under rapid downward acceleration
-param set MPC_THR_MAX 0.75
-param set MPC_THR_MIN 0.08
-param set MPC_THR_HOVER 0.42
-param set MPC_MANTHR_MIN 0.05
-
-# default position mode with a little expo, smooth mode is terrible
-param set MPC_POS_MODE 0
-param set MPC_YAW_EXPO 0.20
-param set MPC_XY_MAN_EXPO 0.20
-param set MPC_Z_MAN_EXPO 0.20
-
-# max velocities
-param set MPC_VEL_MANUAL 5.0
-param set MPC_XY_VEL_MAX 5.0
-param set MPC_XY_CRUISE 5.0
-param set MPC_Z_VEL_MAX_DN 1.5
-param set MPC_Z_VEL_MAX_UP 4.0
-param set MPC_LAND_SPEED 1.0
-
-# Horizontal position PID
-param set MPC_XY_P 0.95
-param set MPC_XY_VEL_P_ACC 3.00
-param set MPC_XY_VEL_I_ACC 0.10
-param set MPC_XY_VEL_D_ACC 0.00
-
-# Vertical position PID
-# PX4 Defaults
-param set MPC_Z_P 1.0
-param set MPC_Z_VEL_P_ACC 8.0
-param set MPC_Z_VEL_I_ACC 2.0
-param set MPC_Z_VEL_D_ACC 0.0
-
-param set MPC_TKO_RAMP_T 1.50
-param set MPC_TKO_SPEED	1.50
-
-# Set the ESC outputs to function as motors
-param set VOXL_ESC_FUNC1 101
-param set VOXL_ESC_FUNC2 103
-param set VOXL_ESC_FUNC3 104
-param set VOXL_ESC_FUNC4 102
-
-param set VOXL_ESC_SDIR1 0
-param set VOXL_ESC_SDIR2 0
-param set VOXL_ESC_SDIR3 0
-param set VOXL_ESC_SDIR4 0
-
-param set VOXL_ESC_CONFIG 1
-param set VOXL_ESC_REV 0
-param set VOXL_ESC_MODE 0
-param set VOXL_ESC_BAUD 2000000
-param set VOXL_ESC_RPM_MAX 10500
-param set VOXL_ESC_RPM_MIN 1000
-
-# Set the Voxl2 IO outputs to function as motors
-param set VOXL2_IO_FUNC1 101
-param set VOXL2_IO_FUNC2 102
-param set VOXL2_IO_FUNC3 103
-param set VOXL2_IO_FUNC4 104
-
-param set VOXL2_IO_BAUD 921600
-param set VOXL2_IO_MIN 1000
-param set VOXL2_IO_MAX 2000
-
-# Some parameters for control allocation
-param set CA_ROTOR_COUNT 4
-param set CA_R_REV 0
-param set CA_AIRFRAME 0
-param set CA_ROTOR_COUNT 4
-param set CA_ROTOR0_PX 0.15
-param set CA_ROTOR0_PY 0.15
-param set CA_ROTOR1_PX -0.15
-param set CA_ROTOR1_PY -0.15
-param set CA_ROTOR2_PX 0.15
-param set CA_ROTOR2_PY -0.15
-param set CA_ROTOR2_KM -0.05
-param set CA_ROTOR3_PX -0.15
-param set CA_ROTOR3_PY 0.15
-param set CA_ROTOR3_KM -0.05
-
-# Some parameter settings to disable / ignore certain preflight checks
-
-# No GPS driver yet so disable it
-param set SYS_HAS_GPS 0
-
-# Allow arming wihtout a magnetometer (Need magnetometer driver)
-param set SYS_HAS_MAG 0
-param set EKF2_MAG_TYPE	5
-
-# Allow arming without battery check (Need voxlpm driver)
-param set CBRK_SUPPLY_CHK 894281
-
-# Allow arming without an SD card
-param set COM_ARM_SDCARD 0
-
-# Allow arming wihtout CPU load information
-param set COM_CPU_MAX 0.0
-
-# Disable auto disarm. This is number of seconds to wait for takeoff
-# after arming. If no takeoff happens then it will disarm. A negative
-# value disables this.
-param set COM_DISARM_PRFLT -1
-
-# This parameter doesn't exist anymore. Need to see what the new method is.
-# param set MAV_BROADCAST 0
-
-# Doesn't work without setting this to Quadcopter
-param set MAV_TYPE 2
-
-# Parameters that we don't use but QGC complains about if they aren't there
-param set SYS_AUTOSTART 4001
-
-# Default RC channel mappings
-param set RC_MAP_ACRO_SW 0
-param set RC_MAP_ARM_SW 0
-param set RC_MAP_AUX1 0
-param set RC_MAP_AUX2 0
-param set RC_MAP_AUX3 0
-param set RC_MAP_AUX4 0
-param set RC_MAP_AUX5 0
-param set RC_MAP_AUX6 0
-param set RC_MAP_FAILSAFE 0
-param set RC_MAP_FLAPS 0
-param set RC_MAP_FLTMODE 6
-param set RC_MAP_GEAR_SW 0
-param set RC_MAP_KILL_SW 7
-param set RC_MAP_LOITER_SW 0
-param set RC_MAP_MAN_SW 0
-param set RC_MAP_MODE_SW 0
-param set RC_MAP_OFFB_SW 0
-param set RC_MAP_PARAM1 0
-param set RC_MAP_PARAM2 0
-param set RC_MAP_PARAM3 0
-param set RC_MAP_PITCH 2
-param set RC_MAP_POSCTL_SW 0
-param set RC_MAP_RATT_SW 0
-param set RC_MAP_RETURN_SW 0
-param set RC_MAP_ROLL 1
-param set RC_MAP_STAB_SW 0
-param set RC_MAP_THROTTLE 3
-param set RC_MAP_TRANS_SW 0
-param set RC_MAP_YAW 4
-
-param save
-
-sleep 2
-
-# Need px4-shutdown otherwise Linux system shutdown is called
-px4-shutdown
@@ -13,7 +13,6 @@ CONFIG_DRIVERS_CDCACM_AUTOSTART=y
 CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
 CONFIG_COMMON_DISTANCE_SENSOR=y
 CONFIG_DRIVERS_DSHOT=y
-CONFIG_DRIVERS_GNSS_SEPTENTRIO=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_IMU_BOSCH_BMI085=y
 CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
@@ -10,7 +10,6 @@ CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS6"
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_MS5611=y
 CONFIG_DRIVERS_CDCACM_AUTOSTART=y
-CONFIG_DRIVERS_GNSS_SEPTENTRIO=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y
 CONFIG_DRIVERS_IMU_ST_L3GD20=y
@@ -15,7 +15,6 @@ CONFIG_DRIVERS_CDCACM_AUTOSTART=y
 CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
 CONFIG_COMMON_DISTANCE_SENSOR=y
 CONFIG_DRIVERS_DSHOT=y
-CONFIG_DRIVERS_GNSS_SEPTENTRIO=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_HEATER=y
 CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y
@@ -15,7 +15,6 @@ CONFIG_DRIVERS_CDCACM_AUTOSTART=y
 CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
 CONFIG_COMMON_DISTANCE_SENSOR=y
 CONFIG_DRIVERS_DSHOT=y
-CONFIG_DRIVERS_GNSS_SEPTENTRIO=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_HEATER=y
 CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y
@@ -8,7 +8,7 @@ CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n
 CONFIG_DRIVERS_IRLOCK=n
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
 CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n
-CONFIG_MODULES_ROVER_POS_CONTROL=n
+CONFIG_MODULES_PAYLOAD_DELIVERER=n
 CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=n
 CONFIG_BOARD_TESTING=y
 CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_SERIAL=y
@@ -48,7 +48,6 @@ CONFIG_MODULES_FW_POS_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=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
@@ -246,12 +246,6 @@

 #define STM32_RCC_D2CCIP2R_USBSRC    RCC_D2CCIP2R_USBSEL_PLL3

-/* UART clock selection */
-/* reset to default to overwrite any changes done by any bootloader */
-
-#define STM32_RCC_D2CCIP2R_USART234578_SEL RCC_D2CCIP2R_USART234578SEL_RCC
-#define STM32_RCC_D2CCIP2R_USART16_SEL     RCC_D2CCIP2R_USART16SEL_RCC
-
 /* ADC 1 2 3 clock source */

 #define STM32_RCC_D3CCIPR_ADCSRC     RCC_D3CCIPR_ADCSEL_PLL2
@@ -67,6 +67,7 @@ CONFIG_MODULES_LOGGER=y
 CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
+CONFIG_MAVLINK_DIALECT="development"
 CONFIG_MODULES_MC_ATT_CONTROL=y
 CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
@@ -250,12 +250,6 @@

 #define STM32_RCC_D3CCIPR_ADCSRC     RCC_D3CCIPR_ADCSEL_PLL2

-/* UART clock selection */
-/* reset to default to overwrite any changes done by any bootloader */
-
-#define STM32_RCC_D2CCIP2R_USART234578_SEL RCC_D2CCIP2R_USART234578SEL_RCC
-#define STM32_RCC_D2CCIP2R_USART16_SEL     RCC_D2CCIP2R_USART16SEL_RCC
-
 /* FDCAN 1 2 clock source */

 #define STM32_RCC_D2CCIP1R_FDCANSEL  RCC_D2CCIP1R_FDCANSEL_HSE   /* FDCAN 1 2 clock source */
@@ -52,7 +52,6 @@
 #define IMXRT_IPG_PODF_DIVIDER 5
 #define BOARD_GPT_FREQUENCY  24000000
 #define BOARD_XTAL_FREQUENCY 24000000
-#define BOARD_FLEXIO_PREQ    108000000

 /* SDIO *********************************************************************/

@@ -268,6 +267,10 @@

 // This is the ENET_1G interface.

+/* Dshot Disambiguation *******************************************************/
+
+#define IOMUX_DSHOT_DEFAULT             (IOMUX_DRIVE_HIGHSTRENGTH | IOMUX_SLEW_FAST)
+
 // Compile time selection
 #if defined(CONFIG_ETH0_PHY_TJA1103)
 #  define BOARD_PHY_ADDR (18)
@@ -87,7 +87,6 @@ CONFIG_IMXRT_ENET=y
 CONFIG_IMXRT_FLEXCAN1=y
 CONFIG_IMXRT_FLEXCAN2=y
 CONFIG_IMXRT_FLEXCAN3=y
-CONFIG_IMXRT_FLEXIO1=y
 CONFIG_IMXRT_FLEXSPI2=y
 CONFIG_IMXRT_GPIO13_IRQ=y
 CONFIG_IMXRT_GPIO1_0_15_IRQ=y
@@ -6,6 +6,7 @@
 *(.text.board_autoled_on)
 *(.text.clock_timer)
 *(.text.exception_common)
+*(.text.flexio_irq_handler)
 *(.text.hrt_absolute_time)
 *(.text.hrt_call_enter)
 *(.text.hrt_tim_isr)
@@ -455,7 +455,6 @@
 #define RC_SERIAL_SINGLEWIRE            1 // Suport Single wire wiring
 #define RC_SERIAL_SWAP_RXTX             1 // Set Swap (but not supported in HW) to use Single wire
 #define RC_SERIAL_SWAP_USING_SINGLEWIRE 1 // Set to use Single wire swap as HW does not support swap
-#define BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT

 /* FLEXSPI4 */

@@ -114,11 +114,11 @@ const struct clock_configuration_s g_initial_clkconfig = {
 			.div           = 1,
 			.mux           = ACMP_CLK_ROOT_OSC_RC_48M_DIV2,
 		},
-		.flexio1_clk_root = /* 432 / 4 = 108Mhz */
+		.flexio1_clk_root = /* 240 / 2 = 120Mhz */
 		{
 			.enable        = 1,
-			.div           = 4,
-			.mux           = FLEXIO1_CLK_ROOT_SYS_PLL2_PFD3,
+			.div           = 2,
+			.mux           = FLEXIO1_CLK_ROOT_SYS_PLL3_DIV2,
 		},
 		.flexio2_clk_root =
 		{
@@ -492,9 +492,9 @@ const struct clock_configuration_s g_initial_clkconfig = {
 		.mfd             = 268435455,
 		.ss_enable       = 0,
 		.pfd0            = 27, /* (528 * 18) / 27 = 352 MHz */
-		.pfd1            = 16, /* (528 * 18) / 16 = 594 MHz */
-		.pfd2            = 24, /* (528 * 18) / 24 = 396 MHz */
-		.pfd3            = 22, /* (528 * 18) / 22 = 216 MHz */
+		.pfd1            = 16, /* (528 * 16) / 16 = 594 MHz */
+		.pfd2            = 24, /* (528 * 24) / 27 = 396 MHz */
+		.pfd3            = 32, /* (528 * 32) / 27 = 297 MHz */
 	},
 	.sys_pll3 =
 	{
@@ -104,10 +104,9 @@ const struct flexspi_nor_config_s g_flash_fast_config = {
 		.busyBitPolarity = 0u,
 		.lookupTable =
 		{
-			/* Read */// EEH+11H+32bit addr+20dummy cycles+ 4Bytes read data
-			/* Macronix manual says 20 dummy cycles @ 200Mhz, FlexSPI peripheral Operand value needs to be 2N in DDR mode hence 0x28 */
+			/* Read */// EEH+11H+32bit addr+20dummy cycles+ 4Bytes read data    //200Mhz 18 dummy=10+8
 			[0 + 0] = FLEXSPI_LUT_SEQ(CMD_DDR, FLEXSPI_8PAD, 0xEE, CMD_DDR, FLEXSPI_8PAD, 0x11),    //0x871187ee,
-			[0 + 1] = FLEXSPI_LUT_SEQ(RADDR_DDR, FLEXSPI_8PAD, 0x20, DUMMY_DDR, FLEXSPI_8PAD, 0x28),//0xb3288b20,
+			[0 + 1] = FLEXSPI_LUT_SEQ(RADDR_DDR, FLEXSPI_8PAD, 0x20, DUMMY_DDR, FLEXSPI_8PAD, 0x04),//0xb3048b20,
 			[0 + 2] = FLEXSPI_LUT_SEQ(READ_DDR, FLEXSPI_8PAD, 0x04, STOP_EXE, FLEXSPI_1PAD, 0x00), //0xa704,

 			/* Read status */
@@ -53,12 +53,18 @@ static const px4_mft_device_t i2c6 = {             // 24LC64T on BASE  8K 32 X 2

 static const px4_mtd_entry_t fmum_fram = {
 	.device = &qspi_flash,
-	.npart = 1,
+	.npart = 2,
 	.partd = {
 		{
 			.type = MTD_PARAMETERS,
 			.path = "/fs/mtd_params",
-			.nblocks = 256
+			.nblocks = 32
+		},
+		{
+			.type = MTD_WAYPOINTS,
+			.path = "/fs/mtd_waypoints",
+			.nblocks = 32
+
 		}
 	},
 };
@@ -91,14 +91,14 @@


 constexpr io_timers_t io_timers[MAX_IO_TIMERS] = {
-	initIOPWMDshot(PWM::FlexPWM1, PWM::Submodule0, GPIO_FLEXIO1_FLEXIO23_1, 23),
-	initIOPWMDshot(PWM::FlexPWM1, PWM::Submodule1, GPIO_FLEXIO1_FLEXIO25_1, 25),
-	initIOPWMDshot(PWM::FlexPWM1, PWM::Submodule2, GPIO_FLEXIO1_FLEXIO27_1, 27),
-	initIOPWMDshot(PWM::FlexPWM2, PWM::Submodule0, GPIO_FLEXIO1_FLEXIO06_1, 6),
-	initIOPWMDshot(PWM::FlexPWM2, PWM::Submodule1, GPIO_FLEXIO1_FLEXIO08_1, 8),
-	initIOPWMDshot(PWM::FlexPWM2, PWM::Submodule2, GPIO_FLEXIO1_FLEXIO10_1, 10),
-	initIOPWMDshot(PWM::FlexPWM2, PWM::Submodule3, GPIO_FLEXIO1_FLEXIO19_1, 19),
-	initIOPWMDshot(PWM::FlexPWM3, PWM::Submodule0, GPIO_FLEXIO1_FLEXIO29_1, 29),
+	initIOPWMDshot(PWM::FlexPWM1, PWM::Submodule0),
+	initIOPWMDshot(PWM::FlexPWM1, PWM::Submodule1),
+	initIOPWMDshot(PWM::FlexPWM1, PWM::Submodule2),
+	initIOPWMDshot(PWM::FlexPWM2, PWM::Submodule0),
+	initIOPWMDshot(PWM::FlexPWM2, PWM::Submodule1),
+	initIOPWMDshot(PWM::FlexPWM2, PWM::Submodule2),
+	initIOPWMDshot(PWM::FlexPWM2, PWM::Submodule3),
+	initIOPWMDshot(PWM::FlexPWM3, PWM::Submodule0),
 	initIOPWM(PWM::FlexPWM3, PWM::Submodule1),
 	initIOPWM(PWM::FlexPWM3, PWM::Submodule3),
 	initIOPWM(PWM::FlexPWM4, PWM::Submodule0),
@@ -106,14 +106,14 @@ constexpr io_timers_t io_timers[MAX_IO_TIMERS] = {
 };

 constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
-	/* FMU_CH1  */  initIOTimerChannel(io_timers, {PWM::PWM1_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_EMC_B1_23),
-	/* FMU_CH2  */  initIOTimerChannel(io_timers, {PWM::PWM1_PWM_A, PWM::Submodule1}, IOMUX::Pad::GPIO_EMC_B1_25),
-	/* FMU_CH3  */  initIOTimerChannel(io_timers, {PWM::PWM1_PWM_A, PWM::Submodule2}, IOMUX::Pad::GPIO_EMC_B1_27),
-	/* FMU_CH4  */  initIOTimerChannel(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_EMC_B1_06),
-	/* FMU_CH5  */  initIOTimerChannel(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule1}, IOMUX::Pad::GPIO_EMC_B1_08),
-	/* FMU_CH6  */  initIOTimerChannel(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule2}, IOMUX::Pad::GPIO_EMC_B1_10),
-	/* FMU_CH7  */  initIOTimerChannel(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule3}, IOMUX::Pad::GPIO_EMC_B1_19),
-	/* FMU_CH8  */  initIOTimerChannel(io_timers, {PWM::PWM3_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_EMC_B1_29),
+	/* FMU_CH1  */  initIOTimerChannelDshot(io_timers, {PWM::PWM1_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_EMC_B1_23, GPIO_FLEXIO1_FLEXIO23_1 | IOMUX_DSHOT_DEFAULT, 23),
+	/* FMU_CH2  */  initIOTimerChannelDshot(io_timers, {PWM::PWM1_PWM_A, PWM::Submodule1}, IOMUX::Pad::GPIO_EMC_B1_25, GPIO_FLEXIO1_FLEXIO25_1 | IOMUX_DSHOT_DEFAULT, 25),
+	/* FMU_CH3  */  initIOTimerChannelDshot(io_timers, {PWM::PWM1_PWM_A, PWM::Submodule2}, IOMUX::Pad::GPIO_EMC_B1_27, GPIO_FLEXIO1_FLEXIO27_1 | IOMUX_DSHOT_DEFAULT, 27),
+	/* FMU_CH4  */  initIOTimerChannelDshot(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_EMC_B1_06, GPIO_FLEXIO1_FLEXIO06_1 | IOMUX_DSHOT_DEFAULT, 6),
+	/* FMU_CH5  */  initIOTimerChannelDshot(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule1}, IOMUX::Pad::GPIO_EMC_B1_08, GPIO_FLEXIO1_FLEXIO08_1 | IOMUX_DSHOT_DEFAULT, 8),
+	/* FMU_CH6  */  initIOTimerChannelDshot(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule2}, IOMUX::Pad::GPIO_EMC_B1_10, GPIO_FLEXIO1_FLEXIO10_1 | IOMUX_DSHOT_DEFAULT, 10),
+	/* FMU_CH7  */  initIOTimerChannelDshot(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule3}, IOMUX::Pad::GPIO_EMC_B1_19, GPIO_FLEXIO1_FLEXIO19_1 | IOMUX_DSHOT_DEFAULT, 19),
+	/* FMU_CH8  */  initIOTimerChannelDshot(io_timers, {PWM::PWM3_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_EMC_B1_29, GPIO_FLEXIO1_FLEXIO29_1 | IOMUX_DSHOT_DEFAULT, 29),
 	/* FMU_CH9  */  initIOTimerChannel(io_timers, {PWM::PWM3_PWM_A, PWM::Submodule1}, IOMUX::Pad::GPIO_EMC_B1_31),
 	/* FMU_CH10 */  initIOTimerChannel(io_timers, {PWM::PWM3_PWM_A, PWM::Submodule3}, IOMUX::Pad::GPIO_EMC_B1_21),
 	/* FMU_CH11 */  initIOTimerChannel(io_timers, {PWM::PWM4_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_EMC_B1_00),
@@ -50,7 +50,6 @@ CONFIG_MODULES_FW_POS_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=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
@@ -87,7 +86,6 @@ CONFIG_SYSTEMCMDS_NSHTERM=y
 CONFIG_SYSTEMCMDS_PARAM=y
 CONFIG_SYSTEMCMDS_PERF=y
 CONFIG_SYSTEMCMDS_REBOOT=y
-CONFIG_SYSTEMCMDS_REFLECT=y
 CONFIG_SYSTEMCMDS_SD_BENCH=y
 CONFIG_SYSTEMCMDS_SD_STRESS=y
 CONFIG_SYSTEMCMDS_SERIAL_TEST=y
@@ -364,7 +364,7 @@ __ramfunc__ void stm32_board_clockconfig(void)
 		 */

 		regval = getreg32(STM32_PWR_CR3);
-		regval |= STM32_PWR_CR3_LDOEN | STM32_PWR_CR3_SCUEN;
+		regval |= STM32_PWR_CR3_LDOEN | STM32_PWR_CR3_LDOESCUEN;
 		putreg32(regval, STM32_PWR_CR3);

 		/* Set the voltage output scale */
@@ -74,13 +74,13 @@ add_custom_target(metadata_parameters
 		--markdown ${PX4_BINARY_DIR}/docs/parameters.md

 	COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/src/lib/parameters/px_process_params.py
-		--src-path `find ${PX4_SOURCE_DIR}/src -maxdepth 4 -type d` ${generated_params_dir}
+		--src-path `find ${PX4_SOURCE_DIR}/src -maxdepth 4 -type d`
 		--inject-xml ${PX4_SOURCE_DIR}/src/lib/parameters/parameters_injected.xml
 		--json ${PX4_BINARY_DIR}/docs/parameters.json
 		--compress

 	COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/src/lib/parameters/px_process_params.py
-		--src-path `find ${PX4_SOURCE_DIR}/src -maxdepth 4 -type d` ${generated_params_dir}
+		--src-path `find ${PX4_SOURCE_DIR}/src -maxdepth 4 -type d`
 		--inject-xml ${PX4_SOURCE_DIR}/src/lib/parameters/parameters_injected.xml
 		--xml ${PX4_BINARY_DIR}/docs/parameters.xml

@@ -308,7 +308,7 @@ class MavrosMissionTest(MavrosTestCommon):
        self.assertTrue(res['pitch_error_std'] < 5.0, str(res))

        # TODO: fix by excluding initial heading init and reset preflight
-        self.assertTrue(res['yaw_error_std'] < 10.0, str(res))
+        self.assertTrue(res['yaw_error_std'] < 15.0, str(res))


 if __name__ == '__main__':
@@ -1,10 +1,12 @@
 # This message is used to dump the raw gps communication to the log.
+# Set the parameter GPS_DUMP_COMM to 1 to use this.

-uint64 timestamp # time since system start (microseconds)
+uint64 timestamp		# time since system start (microseconds)

-uint8 instance   # Instance of GNSS receiver
-uint8 len        # length of data, MSB bit set = message to the gps device,
-                 # clear = message from the device
-uint8[79] data   # data to write to the log
+uint8 instance 		# Instance of GNSS receiver
+
+uint8 len			# length of data, MSB bit set = message to the gps device,
+				# clear = message from the device
+uint8[79] data		# data to write to the log

 uint8 ORB_QUEUE_LENGTH = 8
@@ -1,5 +1,5 @@
 uint64 timestamp				# time since system start (microseconds)
-uint64 last_heartbeat				# timestamp of the last successful sbd session
+uint64 last_at_ok_timestamp			# timestamp of the last "OK" received after the "AT" command
 uint16 tx_buf_write_index			# current size of the tx buffer
 uint16 rx_buf_read_index			# the rx buffer is parsed up to that index
 uint16 rx_buf_end_index				# current size of the rx buffer
@@ -1,4 +1,4 @@
-# A logging message, output with PX4_{WARN,ERR,INFO}
+# A logging message, output with PX4_WARN, PX4_ERR, PX4_INFO

 uint64 timestamp		# time since system start (microseconds)

@@ -9,6 +9,8 @@ uint8 BACKEND_MAVLINK = 2
 uint8 BACKEND_ALL     = 3
 uint8 backend

+bool is_logging
+
 float32 total_written_kb       # total written to log in kiloBytes
 float32 write_rate_kb_s        # write rate in kiloBytes/s

@@ -8,8 +8,8 @@ bool has_vtol_approach 		      # flag if approaches are defined for current RTL_
 uint8 rtl_type	      		      # Type of RTL chosen
 uint8 safe_point_index 		      # index of the chosen safe point, if in RTL_STATUS_TYPE_DIRECT_SAFE_POINT mode

-uint8 RTL_STATUS_TYPE_NONE=0       # RTL type is pending if evaluation can't pe performed currently e.g. when it is still loading the safe points
-uint8 RTL_STATUS_TYPE_DIRECT_SAFE_POINT=1 # RTL type is chosen to directly go to a safe point or home position
-uint8 RTL_STATUS_TYPE_DIRECT_MISSION_LAND=2 # RTL type is going straight to the beginning of the mission landing
-uint8 RTL_STATUS_TYPE_FOLLOW_MISSION=3 	# RTL type is following the mission from closest point to mission landing
-uint8 RTL_STATUS_TYPE_FOLLOW_MISSION_REVERSE=4 # RTL type is following the mission in reverse to the start position
+uint8 RTL_STATUS_TYPE_NONE=0       		# pending if evaluation can't pe performed currently e.g. when it is still loading the safe points
+uint8 RTL_STATUS_TYPE_DIRECT_SAFE_POINT=1 	# chosen to directly go to a safe point or home position
+uint8 RTL_STATUS_TYPE_DIRECT_MISSION_LAND=2 	# going straight to the beginning of the mission landing
+uint8 RTL_STATUS_TYPE_FOLLOW_MISSION=3 		# Following the mission from start index to mission landing. Start index is current WP if in Mission mode, and closest WP otherwise.
+uint8 RTL_STATUS_TYPE_FOLLOW_MISSION_REVERSE=4 	# Following the mission in reverse from start index to the beginning of the mission. Start index is previous WP if in Mission mode, and closest WP otherwise.
@@ -12,14 +12,7 @@ float64 altitude_ellipsoid_m	# Altitude above Ellipsoid, meters

 float32 s_variance_m_s		# GPS speed accuracy estimate, (metres/sec)
 float32 c_variance_rad		# GPS course accuracy estimate, (radians)
-uint8 FIX_TYPE_NONE                   = 1       # Value 0 is also valid to represent no fix.
-uint8 FIX_TYPE_2D                     = 2
-uint8 FIX_TYPE_3D                     = 3
-uint8 FIX_TYPE_RTCM_CODE_DIFFERENTIAL = 4
-uint8 FIX_TYPE_RTK_FLOAT              = 5
-uint8 FIX_TYPE_RTK_FIXED              = 6
-uint8 FIX_TYPE_EXTRAPOLATED           = 8
-uint8 fix_type                  # Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.
+uint8 fix_type # 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic, float, 6: Real-Time Kinematic, fixed, 8: Extrapolated. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.

 float32 eph			# GPS horizontal position accuracy (metres)
 float32 epv			# GPS vertical position accuracy (metres)
@@ -82,7 +82,6 @@ uint16 VEHICLE_CMD_RUN_PREARM_CHECKS = 401      	# Instructs a target system to
 uint16 VEHICLE_CMD_INJECT_FAILURE = 420			# Inject artificial failure for testing purposes
 uint16 VEHICLE_CMD_START_RX_PAIR = 500			# Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX|
 uint16 VEHICLE_CMD_REQUEST_MESSAGE = 512    		# Request to send a single instance of the specified message
-uint16 VEHICLE_CMD_REQUEST_CAMERA_INFORMATION = 521     # Request camera information (CAMERA_INFORMATION). |0: No action 1: Request camera capabilities| Reserved (all remaining params)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)|
 uint16 VEHICLE_CMD_SET_CAMERA_MODE = 530		# Set camera capture mode (photo, video, etc.)
 uint16 VEHICLE_CMD_SET_CAMERA_ZOOM = 531		# Set camera zoom
 uint16 VEHICLE_CMD_SET_CAMERA_FOCUS = 532
@@ -1,6 +1,6 @@
 uint64 timestamp	# time since system start (microseconds)

-# body angular rates in NED frame
+# body angular rates in FRD frame
 float32 roll		# [rad/s] roll rate setpoint
 float32 pitch		# [rad/s] pitch rate setpoint
 float32 yaw		# [rad/s] yaw rate setpoint
@@ -50,3 +50,15 @@
 * @return 0 on success, -errno otherwise
 */
 int px4_get_parameter_value(const char *option, int &value);
+
+/**
+ * Parse a CLI argument to a float. There are 2 valid formats:
+ * - 'p:<param_name>'
+ *   in this case the parameter is loaded from an integer parameter
+ * - <float>
+ *   a floating-point value, so just a string to float conversion is done
+ * @param option CLI argument
+ * @param value returned value
+ * @return 0 on success, -errno otherwise
+ */
+int px4_get_parameter_value(const char *option, float &value);
@@ -45,7 +45,7 @@
 #endif

 #include <events/events_generated.h>
-#include <libevents_definitions.h>
+#include <uORB/topics/event.h>

 #include <stdint.h>

@@ -93,7 +93,7 @@ constexpr unsigned sizeofArguments(const T &t, const Args &... args)
 /**
 * publish/send an event
 */
-void send(EventType &event);
+void send(event_s &event);

 /**
 * Generate event ID from an event name
@@ -109,7 +109,7 @@ constexpr uint32_t ID(const char (&name)[N])
 template<typename... Args>
 inline void send(uint32_t id, const LogLevels &log_levels, const char *message, Args... args)
 {
-	EventType e{};
+	event_s e{};
 	e.log_levels = ((uint8_t)log_levels.internal << 4) | (uint8_t)log_levels.external;
 	e.id = id;
 	static_assert(util::sizeofArguments(args...) <= sizeof(e.arguments), "Too many arguments");
@@ -120,7 +120,7 @@ inline void send(uint32_t id, const LogLevels &log_levels, const char *message,

 inline void send(uint32_t id, const LogLevels &log_levels, const char *message)
 {
-	EventType e{};
+	event_s e{};
 	e.log_levels = ((uint8_t)log_levels.internal << 4) | (uint8_t)log_levels.external;
 	e.id = id;
 	CONSOLE_PRINT_EVENT(e.log_level_external, e.id, message);
@@ -89,7 +89,7 @@ static constexpr wq_config_t ttyS9{"wq:ttyS9", 1728, -30};
 static constexpr wq_config_t ttyACM0{"wq:ttyACM0", 1728, -31};
 static constexpr wq_config_t ttyUnknown{"wq:ttyUnknown", 1728, -32};

-static constexpr wq_config_t lp_default{"wq:lp_default", 1920, -50};
+static constexpr wq_config_t lp_default{"wq:lp_default", 2000, -50};

 static constexpr wq_config_t test1{"wq:test1", 2000, 0};
 static constexpr wq_config_t test2{"wq:test2", 2000, 0};
@@ -59,6 +59,7 @@ int px4_get_parameter_value(const char *option, int &value)
 		if (param_handle != PARAM_INVALID) {

 			if (param_type(param_handle) != PARAM_TYPE_INT32) {
+				PX4_ERR("Type of param '%s' is different from INT32", param_name);
 				return -EINVAL;
 			}

@@ -87,3 +88,48 @@ int px4_get_parameter_value(const char *option, int &value)

 	return 0;
 }
+
+int px4_get_parameter_value(const char *option, float &value)
+{
+	value = 0;
+
+	/* check if this is a param name */
+	if (strncmp("p:", option, 2) == 0) {
+
+		const char *param_name = option + 2;
+
+		/* user wants to use a param name */
+		param_t param_handle = param_find(param_name);
+
+		if (param_handle != PARAM_INVALID) {
+
+			if (param_type(param_handle) != PARAM_TYPE_FLOAT) {
+				PX4_ERR("Type of param '%s' is different from FLOAT", param_name);
+				return -EINVAL;
+			}
+
+			float pwm_parm;
+			int ret = param_get(param_handle, &pwm_parm);
+
+			if (ret != 0) {
+				return ret;
+			}
+
+			value = pwm_parm;
+
+		} else {
+			PX4_ERR("param name '%s' not found", param_name);
+			return -ENXIO;
+		}
+
+	} else {
+		char *ep;
+		value = strtof(option, &ep);
+
+		if (*ep != '\0') {
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
@@ -47,7 +47,6 @@ set(SRCS_COMMON
 	Subscription.cpp
 	Subscription.hpp
 	SubscriptionCallback.hpp
-	SubscriptionInterval.cpp
 	SubscriptionInterval.hpp
 	SubscriptionMultiArray.hpp
 	uORB.cpp
@@ -62,6 +62,11 @@ public:
 	{
 		Node **p;

+		// Don't allow duplicates to be inserted
+		if (find(node_name)) {
+			return;
+		}
+
 		if (_top == nullptr) {
 			p = &_top;

@@ -93,21 +93,45 @@ public:
 	/**
 	 * Check if there is a new update.
 	 * */
-	bool updated();
+	bool updated()
+	{
+		if (advertised() && (hrt_elapsed_time(&_last_update) >= _interval_us)) {
+			return _subscription.updated();
+		}
+
+		return false;
+	}

 	/**
 	 * Copy the struct if updated.
 	 * @param dst The destination pointer where the struct will be copied.
 	 * @return true only if topic was updated and copied successfully.
 	 */
-	bool update(void *dst);
+	bool update(void *dst)
+	{
+		if (updated()) {
+			return copy(dst);
+		}
+
+		return false;
+	}

 	/**
 	 * Copy the struct
 	 * @param dst The destination pointer where the struct will be copied.
 	 * @return true only if topic was copied successfully.
 	 */
-	bool copy(void *dst);
+	bool copy(void *dst)
+	{
+		if (_subscription.copy(dst)) {
+			const hrt_abstime now = hrt_absolute_time();
+			// shift last update time forward, but don't let it get further behind than the interval
+			_last_update = math::constrain(_last_update + _interval_us, now - _interval_us, now);
+			return true;
+		}
+
+		return false;
+	}

 	bool		valid() const { return _subscription.valid(); }

@@ -136,7 +160,7 @@ public:
 protected:

 	Subscription	_subscription;
-	uint64_t	_last_update{0};	// last subscription update in microseconds
+	uint64_t	_last_update{0};	// last update in microseconds
 	uint32_t	_interval_us{0};	// maximum update interval in microseconds

 };
@@ -33,129 +33,454 @@
 ****************************************************************************/
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/micro_hal.h>
+#include <px4_platform_common/log.h>
 #include <imxrt_flexio.h>
+#include <hardware/imxrt_flexio.h>
+#include <imxrt_periphclks.h>
 #include <px4_arch/dshot.h>
 #include <px4_arch/io_timer.h>
 #include <drivers/drv_dshot.h>
 #include <stdio.h>
+#include "barriers.h"

 #include "arm_internal.h"

+#define FLEXIO_BASE			IMXRT_FLEXIO1_BASE
+#define FLEXIO_PREQ			120000000
 #define DSHOT_TIMERS			FLEXIO_SHIFTBUFNIS_COUNT
 #define DSHOT_THROTTLE_POSITION		5u
 #define DSHOT_TELEMETRY_POSITION	4u
 #define NIBBLES_SIZE 			4u
 #define DSHOT_NUMBER_OF_NIBBLES		3u

+#if defined(IOMUX_PULL_UP_47K)
+#define IOMUX_PULL_UP IOMUX_PULL_UP_47K
+#endif
+
+static const uint32_t gcr_decode[32] = {
+	0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
+	0x0, 0x9, 0xA, 0xB, 0x0, 0xD, 0xE, 0xF,
+	0x0, 0x0, 0x2, 0x3, 0x0, 0x5, 0x6, 0x7,
+	0x0, 0x0, 0x8, 0x1, 0x0, 0x4, 0xC, 0x0
+};
+
+uint32_t erpms[DSHOT_TIMERS];
+
+typedef enum {
+	DSHOT_START = 0,
+	DSHOT_12BIT_FIFO,
+	DSHOT_12BIT_TRANSFERRED,
+	DSHOT_TRANSMIT_COMPLETE,
+	BDSHOT_RECEIVE,
+	BDSHOT_RECEIVE_COMPLETE,
+} dshot_state;
+
 typedef struct dshot_handler_t {
 	bool			init;
 	uint32_t 		data_seg1;
 	uint32_t 		irq_data;
+	dshot_state             state;
+	bool			bdshot;
+	uint32_t                raw_response;
+	uint16_t                erpm;
+	uint32_t		crc_error_cnt;
+	uint32_t		frame_error_cnt;
+	uint32_t		no_response_cnt;
+	uint32_t		last_no_response_cnt;
 } dshot_handler_t;

+#define BDSHOT_OFFLINE_COUNT 400 // If there are no responses for 400 setpoints ESC is offline
+
 static dshot_handler_t dshot_inst[DSHOT_TIMERS] = {};

-struct flexio_dev_s *flexio_s;
+static uint32_t dshot_tcmp;
+static uint32_t bdshot_tcmp;
+static uint32_t dshot_mask;
+static uint32_t bdshot_recv_mask;
+static uint32_t bdshot_parsed_recv_mask;
+
+static inline uint32_t flexio_getreg32(uint32_t offset)
+{
+	return getreg32(FLEXIO_BASE + offset);
+}
+
+static inline void flexio_modifyreg32(unsigned int offset,
+				      uint32_t clearbits,
+				      uint32_t setbits)
+{
+	modifyreg32(FLEXIO_BASE + offset, clearbits, setbits);
+}
+
+static inline void flexio_putreg32(uint32_t value, uint32_t offset)
+{
+	putreg32(value, FLEXIO_BASE + offset);
+}
+
+static inline void enable_shifter_status_interrupts(uint32_t mask)
+{
+	flexio_modifyreg32(IMXRT_FLEXIO_SHIFTSIEN_OFFSET, 0, mask);
+}
+
+static inline void disable_shifter_status_interrupts(uint32_t mask)
+{
+	flexio_modifyreg32(IMXRT_FLEXIO_SHIFTSIEN_OFFSET, mask, 0);
+}
+
+static inline uint32_t get_shifter_status_flags(void)
+{
+	return flexio_getreg32(IMXRT_FLEXIO_SHIFTSTAT_OFFSET);
+}
+
+static inline void clear_shifter_status_flags(uint32_t mask)
+{
+	flexio_putreg32(mask, IMXRT_FLEXIO_SHIFTSTAT_OFFSET);
+}
+
+static inline void enable_timer_status_interrupts(uint32_t mask)
+{
+	flexio_modifyreg32(IMXRT_FLEXIO_TIMIEN_OFFSET, 0, mask);
+}
+
+static inline void disable_timer_status_interrupts(uint32_t mask)
+{
+	flexio_modifyreg32(IMXRT_FLEXIO_TIMIEN_OFFSET, mask, 0);
+}
+
+static inline uint32_t get_timer_status_flags(void)
+{
+	return flexio_getreg32(IMXRT_FLEXIO_TIMSTAT_OFFSET);
+}
+
+static inline void clear_timer_status_flags(uint32_t mask)
+{
+	flexio_putreg32(mask, IMXRT_FLEXIO_TIMSTAT_OFFSET);
+}
+
+static void flexio_dshot_output(uint32_t channel, uint32_t pin, uint32_t timcmp, bool inverted)
+{
+	/* Disable Shifter */
+	flexio_putreg32(0, IMXRT_FLEXIO_SHIFTCTL0_OFFSET + channel * 0x4);
+
+	/* No start bit, stop bit low */
+	flexio_putreg32(FLEXIO_SHIFTCFG_INSRC(FLEXIO_SHIFTER_INPUT_FROM_PIN) |
+			FLEXIO_SHIFTCFG_PWIDTH(0) |
+			FLEXIO_SHIFTCFG_SSTOP(FLEXIO_SHIFTER_STOP_BIT_LOW) |
+			FLEXIO_SHIFTCFG_SSTART(FLEXIO_SHIFTER_START_BIT_DISABLED_LOAD_DATA_ON_ENABLE),
+			IMXRT_FLEXIO_SHIFTCFG0_OFFSET + channel * 0x4);
+
+	/* Transmit mode, output to FXIO pin, inverted output for bdshot */
+	flexio_putreg32(FLEXIO_SHIFTCTL_TIMSEL(channel) |
+			FLEXIO_SHIFTCTL_TIMPOL(FLEXIO_SHIFTER_TIMER_POLARITY_ON_POSITIVE) |
+			FLEXIO_SHIFTCTL_PINCFG(FLEXIO_PIN_CONFIG_OUTPUT) |
+			FLEXIO_SHIFTCTL_PINSEL(pin) |
+			FLEXIO_SHIFTCTL_PINPOL(inverted) |
+			FLEXIO_SHIFTCTL_SMOD(FLEXIO_SHIFTER_MODE_TRANSMIT),
+			IMXRT_FLEXIO_SHIFTCTL0_OFFSET + channel * 0x4);
+
+	/* Start transmitting on trigger, disable on compare */
+	flexio_putreg32(FLEXIO_TIMCFG_TIMOUT(FLEXIO_TIMER_OUTPUT_ONE_NOT_AFFECTED_BY_RESET) |
+			FLEXIO_TIMCFG_TIMDEC(FLEXIO_TIMER_DEC_SRC_ON_FLEX_IO_CLOCK_SHIFT_TIMER_OUTPUT) |
+			FLEXIO_TIMCFG_TIMRST(FLEXIO_TIMER_RESET_NEVER) |
+			FLEXIO_TIMCFG_TIMDIS(FLEXIO_TIMER_DISABLE_ON_TIMER_COMPARE) |
+			FLEXIO_TIMCFG_TIMENA(FLEXIO_TIMER_ENABLE_ON_TRIGGER_HIGH) |
+			FLEXIO_TIMCFG_TSTOP(FLEXIO_TIMER_STOP_BIT_DISABLED) |
+			FLEXIO_TIMCFG_TSTART(FLEXIO_TIMER_START_BIT_DISABLED),
+			IMXRT_FLEXIO_TIMCFG0_OFFSET + channel * 0x4);
+
+	flexio_putreg32(timcmp, IMXRT_FLEXIO_TIMCMP0_OFFSET + channel * 0x4);
+
+	/* Baud mode, Trigger on shifter write */
+	flexio_putreg32(FLEXIO_TIMCTL_TRGSEL((4 * channel) + 1) |
+			FLEXIO_TIMCTL_TRGPOL(FLEXIO_TIMER_TRIGGER_POLARITY_ACTIVE_LOW) |
+			FLEXIO_TIMCTL_TRGSRC(FLEXIO_TIMER_TRIGGER_SOURCE_INTERNAL) |
+			FLEXIO_TIMCTL_PINCFG(FLEXIO_PIN_CONFIG_OUTPUT_DISABLED) |
+			FLEXIO_TIMCTL_PINSEL(0) |
+			FLEXIO_TIMCTL_PINPOL(FLEXIO_PIN_ACTIVE_LOW) |
+			FLEXIO_TIMCTL_TIMOD(FLEXIO_TIMER_MODE_DUAL8_BIT_BAUD_BIT),
+			IMXRT_FLEXIO_TIMCTL0_OFFSET + channel * 0x4);
+
+}

 static int flexio_irq_handler(int irq, void *context, void *arg)
 {
+	uint32_t flags = get_shifter_status_flags();
+	uint32_t channel;

-	uint32_t flags = flexio_s->ops->get_shifter_status_flags(flexio_s);
-	uint32_t instance;
+	for (channel = 0; flags && channel < DSHOT_TIMERS; channel++) {
+		if (flags & (1 << channel)) {
+			disable_shifter_status_interrupts(1 << channel);

-	for (instance = 0; flags && instance < DSHOT_TIMERS; instance++) {
-		if (flags & (1 << instance)) {
-			flexio_s->ops->disable_shifter_status_interrupts(flexio_s, (1 << instance));
-			flexio_s->ops->disable_timer_status_interrupts(flexio_s, (1 << instance));
+			if (dshot_inst[channel].state == DSHOT_START) {
+				dshot_inst[channel].state = DSHOT_12BIT_FIFO;
+				flexio_putreg32(dshot_inst[channel].irq_data, IMXRT_FLEXIO_SHIFTBUF0_OFFSET + channel * 0x4);

-			if (dshot_inst[instance].irq_data != 0) {
-				uint32_t buf_adr = flexio_s->ops->get_shifter_buffer_address(flexio_s, FLEXIO_SHIFTER_BUFFER, instance);
-				putreg32(dshot_inst[instance].irq_data, IMXRT_FLEXIO1_BASE + buf_adr);
-				dshot_inst[instance].irq_data = 0;
+			} else if (dshot_inst[channel].state == BDSHOT_RECEIVE) {
+				dshot_inst[channel].state = BDSHOT_RECEIVE_COMPLETE;
+				dshot_inst[channel].raw_response = flexio_getreg32(IMXRT_FLEXIO_SHIFTBUFBIS0_OFFSET + channel * 0x4);
+
+				bdshot_recv_mask |= (1 << channel);
+
+				if (bdshot_recv_mask == dshot_mask) {
+					// Received telemetry on all channels
+					// Schedule workqueue?
+				}
 			}
 		}
 	}

+	flags = get_timer_status_flags();
+
+	for (channel = 0; flags; (channel = (channel + 1) % DSHOT_TIMERS)) {
+		flags = get_timer_status_flags();
+
+		if (flags & (1 << channel)) {
+			clear_timer_status_flags(1 << channel);
+
+			if (dshot_inst[channel].state == DSHOT_12BIT_FIFO) {
+				dshot_inst[channel].state = DSHOT_12BIT_TRANSFERRED;
+
+			} else if (!dshot_inst[channel].bdshot && dshot_inst[channel].state == DSHOT_12BIT_TRANSFERRED) {
+				dshot_inst[channel].state = DSHOT_TRANSMIT_COMPLETE;
+
+			} else if (dshot_inst[channel].bdshot && dshot_inst[channel].state == DSHOT_12BIT_TRANSFERRED) {
+				disable_shifter_status_interrupts(1 << channel);
+				dshot_inst[channel].state = BDSHOT_RECEIVE;
+
+				/* Transmit done, disable timer and reconfigure to receive*/
+				flexio_putreg32(0x0, IMXRT_FLEXIO_TIMCTL0_OFFSET + channel * 0x4);
+
+				/* Input data from pin, no start/stop bit*/
+				flexio_putreg32(FLEXIO_SHIFTCFG_INSRC(FLEXIO_SHIFTER_INPUT_FROM_PIN) |
+						FLEXIO_SHIFTCFG_PWIDTH(0) |
+						FLEXIO_SHIFTCFG_SSTOP(FLEXIO_SHIFTER_STOP_BIT_DISABLE) |
+						FLEXIO_SHIFTCFG_SSTART(FLEXIO_SHIFTER_START_BIT_DISABLED_LOAD_DATA_ON_SHIFT),
+						IMXRT_FLEXIO_SHIFTCFG0_OFFSET + channel * 0x4);
+
+				/* Shifter receive mdoe, on FXIO pin input */
+				flexio_putreg32(FLEXIO_SHIFTCTL_TIMSEL(channel) |
+						FLEXIO_SHIFTCTL_TIMPOL(FLEXIO_SHIFTER_TIMER_POLARITY_ON_POSITIVE) |
+						FLEXIO_SHIFTCTL_PINCFG(FLEXIO_PIN_CONFIG_OUTPUT_DISABLED) |
+						FLEXIO_SHIFTCTL_PINSEL(timer_io_channels[channel].dshot.flexio_pin) |
+						FLEXIO_SHIFTCTL_PINPOL(FLEXIO_PIN_ACTIVE_LOW) |
+						FLEXIO_SHIFTCTL_SMOD(FLEXIO_SHIFTER_MODE_RECEIVE),
+						IMXRT_FLEXIO_SHIFTCTL0_OFFSET + channel * 0x4);
+
+				/* Make sure there no shifter flags high from transmission */
+				clear_shifter_status_flags(1 << channel);
+
+				/* Enable on pin transition, resychronize through reset on rising edge */
+				flexio_putreg32(FLEXIO_TIMCFG_TIMOUT(FLEXIO_TIMER_OUTPUT_ONE_AFFECTED_BY_RESET) |
+						FLEXIO_TIMCFG_TIMDEC(FLEXIO_TIMER_DEC_SRC_ON_FLEX_IO_CLOCK_SHIFT_TIMER_OUTPUT) |
+						FLEXIO_TIMCFG_TIMRST(FLEXIO_TIMER_RESET_ON_TIMER_PIN_RISING_EDGE) |
+						FLEXIO_TIMCFG_TIMDIS(FLEXIO_TIMER_DISABLE_ON_TIMER_COMPARE) |
+						FLEXIO_TIMCFG_TIMENA(FLEXIO_TIMER_ENABLE_ON_TRIGGER_BOTH_EDGE) |
+						FLEXIO_TIMCFG_TSTOP(FLEXIO_TIMER_STOP_BIT_ENABLE_ON_TIMER_DISABLE) |
+						FLEXIO_TIMCFG_TSTART(FLEXIO_TIMER_START_BIT_ENABLED),
+						IMXRT_FLEXIO_TIMCFG0_OFFSET + channel * 0x4);
+
+				/* Enable on pin transition, resychronize through reset on rising edge */
+				flexio_putreg32(bdshot_tcmp, IMXRT_FLEXIO_TIMCMP0_OFFSET + channel * 0x4);
+
+				/* Trigger on FXIO pin transition, Baud mode */
+				flexio_putreg32(FLEXIO_TIMCTL_TRGSEL(2 * timer_io_channels[channel].dshot.flexio_pin) |
+						FLEXIO_TIMCTL_TRGPOL(FLEXIO_TIMER_TRIGGER_POLARITY_ACTIVE_HIGH) |
+						FLEXIO_TIMCTL_TRGSRC(FLEXIO_TIMER_TRIGGER_SOURCE_INTERNAL) |
+						FLEXIO_TIMCTL_PINCFG(FLEXIO_PIN_CONFIG_OUTPUT_DISABLED) |
+						FLEXIO_TIMCTL_PINSEL(0) |
+						FLEXIO_TIMCTL_PINPOL(FLEXIO_PIN_ACTIVE_LOW) |
+						FLEXIO_TIMCTL_TIMOD(FLEXIO_TIMER_MODE_DUAL8_BIT_BAUD_BIT),
+						IMXRT_FLEXIO_TIMCTL0_OFFSET + channel * 0x4);
+
+				/* Enable shifter interrupt for receiving data */
+				enable_shifter_status_interrupts(1 << channel);
+			}
+		}
+
+	}
+
 	return OK;
 }

-int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq)
+
+int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bidirectional_dshot)
 {
-	uint32_t timer_compare = 0x2F00 | (((BOARD_FLEXIO_PREQ / (dshot_pwm_freq * 3) / 2) - 1) & 0xFF);
+	/* Calculate dshot timings based on dshot_pwm_freq */
+	dshot_tcmp = 0x2F00 | (((FLEXIO_PREQ / (dshot_pwm_freq * 3) / 2)) & 0xFF);
+	bdshot_tcmp = 0x2900 | (((FLEXIO_PREQ / (dshot_pwm_freq * 5 / 4) / 2) - 1) & 0xFF);

+	/* Clock FlexIO peripheral */
+	imxrt_clockall_flexio1();

-	/* Init FlexIO peripheral */
+	/* Reset FlexIO peripheral */
+	flexio_modifyreg32(IMXRT_FLEXIO_CTRL_OFFSET, 0,
+			   FLEXIO_CTRL_SWRST_MASK);
+	flexio_putreg32(0, IMXRT_FLEXIO_CTRL_OFFSET);

-	flexio_s = imxrt_flexio_initialize(1);
+	/* Initialize FlexIO peripheral */
+	flexio_modifyreg32(IMXRT_FLEXIO_CTRL_OFFSET,
+			   (FLEXIO_CTRL_DOZEN_MASK |
+			    FLEXIO_CTRL_DBGE_MASK |
+			    FLEXIO_CTRL_FASTACC_MASK |
+			    FLEXIO_CTRL_FLEXEN_MASK),
+			   (FLEXIO_CTRL_DBGE(1) |
+			    FLEXIO_CTRL_FASTACC(1) |
+			    FLEXIO_CTRL_FLEXEN(0)));
+
+	/* FlexIO IRQ handling */
 	up_enable_irq(IMXRT_IRQ_FLEXIO1);
-	irq_attach(IMXRT_IRQ_FLEXIO1, flexio_irq_handler, flexio_s);
+	irq_attach(IMXRT_IRQ_FLEXIO1, flexio_irq_handler, 0);
+
+	dshot_mask = 0x0;

 	for (unsigned channel = 0; (channel_mask != 0) && (channel < DSHOT_TIMERS); channel++) {
 		if (channel_mask & (1 << channel)) {
-			uint8_t timer = timer_io_channels[channel].timer_index;

-			if (io_timers[timer].dshot.pinmux == 0) { // board does not configure dshot on this pin
+			if (timer_io_channels[channel].dshot.pinmux == 0) { // board does not configure dshot on this pin
 				continue;
 			}

-			imxrt_config_gpio(io_timers[timer].dshot.pinmux);
+			imxrt_config_gpio(timer_io_channels[channel].dshot.pinmux | IOMUX_PULL_UP);

-			struct flexio_shifter_config_s shft_cfg;
-			shft_cfg.timer_select = channel;
-			shft_cfg.timer_polarity = FLEXIO_SHIFTER_TIMER_POLARITY_ON_POSITIVE;
-			shft_cfg.pin_config = FLEXIO_PIN_CONFIG_OUTPUT;
-			shft_cfg.pin_select = io_timers[timer].dshot.flexio_pin;
-			shft_cfg.pin_polarity = FLEXIO_PIN_ACTIVE_HIGH;
-			shft_cfg.shifter_mode = FLEXIO_SHIFTER_MODE_TRANSMIT;
-			shft_cfg.parallel_width = 0;
-			shft_cfg.input_source = FLEXIO_SHIFTER_INPUT_FROM_PIN;
-			shft_cfg.shifter_stop = FLEXIO_SHIFTER_STOP_BIT_LOW;
-			shft_cfg.shifter_start = FLEXIO_SHIFTER_START_BIT_DISABLED_LOAD_DATA_ON_ENABLE;
+			dshot_inst[channel].bdshot = enable_bidirectional_dshot;

-			flexio_s->ops->set_shifter_config(flexio_s, channel, &shft_cfg);
-
-			struct flexio_timer_config_s tmr_cfg;
-			tmr_cfg.trigger_select = (4 * channel) + 1;
-			tmr_cfg.trigger_polarity = FLEXIO_TIMER_TRIGGER_POLARITY_ACTIVE_LOW;
-			tmr_cfg.trigger_source = FLEXIO_TIMER_TRIGGER_SOURCE_INTERNAL;
-			tmr_cfg.pin_config = FLEXIO_PIN_CONFIG_OUTPUT_DISABLED;
-			tmr_cfg.pin_select = 0;
-			tmr_cfg.pin_polarity = FLEXIO_PIN_ACTIVE_LOW;
-			tmr_cfg.timer_mode = FLEXIO_TIMER_MODE_DUAL8_BIT_BAUD_BIT;
-			tmr_cfg.timer_output = FLEXIO_TIMER_OUTPUT_ONE_NOT_AFFECTED_BY_RESET;
-			tmr_cfg.timer_decrement = FLEXIO_TIMER_DEC_SRC_ON_FLEX_IO_CLOCK_SHIFT_TIMER_OUTPUT;
-			tmr_cfg.timer_reset = FLEXIO_TIMER_RESET_NEVER;
-			tmr_cfg.timer_disable = FLEXIO_TIMER_DISABLE_ON_TIMER_COMPARE;
-			tmr_cfg.timer_enable = FLEXIO_TIMER_ENABLE_ON_TRIGGER_HIGH;
-			tmr_cfg.timer_stop = FLEXIO_TIMER_STOP_BIT_DISABLED;
-			tmr_cfg.timer_start = FLEXIO_TIMER_START_BIT_DISABLED;
-			tmr_cfg.timer_compare = timer_compare;
-			flexio_s->ops->set_timer_config(flexio_s, channel, &tmr_cfg);
+			flexio_dshot_output(channel, timer_io_channels[channel].dshot.flexio_pin, dshot_tcmp, dshot_inst[channel].bdshot);

 			dshot_inst[channel].init = true;
+
+			// Mask channel to be active on dshot
+			dshot_mask |= (1 << channel);
 		}
 	}

-	flexio_s->ops->enable(flexio_s, true);
+	flexio_modifyreg32(IMXRT_FLEXIO_CTRL_OFFSET, 0,
+			   FLEXIO_CTRL_FLEXEN_MASK);

 	return channel_mask;
 }

+void up_bdshot_erpm(void)
+{
+	uint32_t value;
+	uint32_t data;
+	uint32_t csum_data;
+	uint8_t exponent;
+	uint16_t period;
+	uint16_t erpm;
+
+	bdshot_parsed_recv_mask = 0;
+
+	// Decode each individual channel
+	for (uint8_t channel = 0; (channel < DSHOT_TIMERS); channel++) {
+		if (bdshot_recv_mask & (1 << channel)) {
+			value = ~dshot_inst[channel].raw_response & 0xFFFFF;
+
+			/* if lowest significant isn't 1 we've got a framing error */
+			if (value & 0x1) {
+				/* Decode RLL */
+				value = (value ^ (value >> 1));
+
+				/* Decode GCR */
+				data = gcr_decode[value & 0x1fU];
+				data |= gcr_decode[(value >> 5U) & 0x1fU] << 4U;
+				data |= gcr_decode[(value >> 10U) & 0x1fU] << 8U;
+				data |= gcr_decode[(value >> 15U) & 0x1fU] << 12U;
+
+				/* Calculate checksum */
+				csum_data = data;
+				csum_data = csum_data ^ (csum_data >> 8U);
+				csum_data = csum_data ^ (csum_data >> NIBBLES_SIZE);
+
+				if ((csum_data & 0xFU) != 0xFU) {
+					dshot_inst[channel].crc_error_cnt++;
+
+				} else {
+					data = (data >> 4) & 0xFFF;
+
+					if (data == 0xFFF) {
+						erpm = 0;
+
+					} else {
+						exponent = ((data >> 9U) & 0x7U); /* 3 bit: exponent */
+						period = (data & 0x1ffU); /* 9 bit: period base */
+						period = period << exponent; /* Period in usec */
+						erpm = ((1000000U * 60U / 100U + period / 2U) / period);
+					}
+
+					dshot_inst[channel].erpm = erpm;
+					bdshot_parsed_recv_mask |= (1 << channel);
+					dshot_inst[channel].last_no_response_cnt = dshot_inst[channel].no_response_cnt;
+				}
+
+			} else {
+				dshot_inst[channel].frame_error_cnt++;
+			}
+		}
+	}
+}
+
+
+
+int up_bdshot_get_erpm(uint8_t channel, int *erpm)
+{
+	if (bdshot_parsed_recv_mask & (1 << channel)) {
+		*erpm = (int)dshot_inst[channel].erpm;
+		return 0;
+	}
+
+	return -1;
+}
+
+int up_bdshot_channel_status(uint8_t channel)
+{
+	if (channel < DSHOT_TIMERS) {
+		return ((dshot_inst[channel].no_response_cnt - dshot_inst[channel].last_no_response_cnt) < BDSHOT_OFFLINE_COUNT);
+	}
+
+	return -1;
+}
+
+void up_bdshot_status(void)
+{
+
+	for (uint8_t channel = 0; (channel < DSHOT_TIMERS); channel++) {
+
+		if (dshot_inst[channel].init) {
+			PX4_INFO("Channel %i %s Last erpm %i value", channel, up_bdshot_channel_status(channel) ? "online" : "offline",
+				 dshot_inst[channel].erpm);
+			PX4_INFO("CRC errors Frame error No response");
+			PX4_INFO("%10lu %11lu %11lu", dshot_inst[channel].crc_error_cnt, dshot_inst[channel].frame_error_cnt,
+				 dshot_inst[channel].no_response_cnt);
+		}
+	}
+}
+
 void up_dshot_trigger(void)
 {
-	uint32_t buf_adr;
+	// Calc data now since we're not event driven
+	if (bdshot_recv_mask != 0x0) {
+		up_bdshot_erpm();
+	}

-	for (uint8_t motor_number = 0; (motor_number < DSHOT_TIMERS); motor_number++) {
-		if (dshot_inst[motor_number].init && dshot_inst[motor_number].data_seg1 != 0) {
-			buf_adr = flexio_s->ops->get_shifter_buffer_address(flexio_s, FLEXIO_SHIFTER_BUFFER, motor_number);
-			putreg32(dshot_inst[motor_number].data_seg1, IMXRT_FLEXIO1_BASE + buf_adr);
+	clear_timer_status_flags(0xFF);
+
+	for (uint8_t channel = 0; (channel < DSHOT_TIMERS); channel++) {
+		if (dshot_inst[channel].bdshot && (bdshot_recv_mask & (1 << channel)) == 0) {
+			dshot_inst[channel].no_response_cnt++;
+		}
+
+		if (dshot_inst[channel].init && dshot_inst[channel].data_seg1 != 0) {
+			flexio_putreg32(dshot_inst[channel].data_seg1, IMXRT_FLEXIO_SHIFTBUF0_OFFSET + channel * 0x4);
 		}
 	}

-	flexio_s->ops->clear_timer_status_flags(flexio_s, 0xFF);
-	flexio_s->ops->enable_shifter_status_interrupts(flexio_s, 0xFF);
+	bdshot_recv_mask = 0x0;
+
+	clear_timer_status_flags(0xFF);
+	enable_shifter_status_interrupts(0xFF);
+	enable_timer_status_interrupts(0xFF);
 }

+/* Expand packet from 16 bits 48 to get T0H and T1H timing */
 uint64_t dshot_expand_data(uint16_t packet)
 {
 	unsigned int mask;
@@ -181,16 +506,22 @@ uint64_t dshot_expand_data(uint16_t packet)
 * bit 	12		- dshot telemetry enable/disable
 * bits 	13-16	- XOR checksum
 **/
-void dshot_motor_data_set(unsigned motor_number, uint16_t throttle, bool telemetry)
+void dshot_motor_data_set(unsigned channel, uint16_t throttle, bool telemetry)
 {
-	if (motor_number < DSHOT_TIMERS && dshot_inst[motor_number].init) {
+	if (channel < DSHOT_TIMERS && dshot_inst[channel].init) {
+		uint16_t csum_data;
 		uint16_t packet = 0;
 		uint16_t checksum = 0;

 		packet |= throttle << DSHOT_THROTTLE_POSITION;
 		packet |= ((uint16_t)telemetry & 0x01) << DSHOT_TELEMETRY_POSITION;

-		uint16_t csum_data = packet;
+		if (dshot_inst[channel].bdshot) {
+			csum_data = ~packet;
+
+		} else {
+			csum_data = packet;
+		}

 		/* XOR checksum calculation */
 		csum_data >>= NIBBLES_SIZE;
@@ -203,8 +534,20 @@ void dshot_motor_data_set(unsigned motor_number, uint16_t throttle, bool telemet
 		packet |= (checksum & 0x0F);

 		uint64_t dshot_expanded = dshot_expand_data(packet);
-		dshot_inst[motor_number].data_seg1 = (uint32_t)(dshot_expanded & 0xFFFFFF);
-		dshot_inst[motor_number].irq_data = (uint32_t)(dshot_expanded >> 24);
+
+		dshot_inst[channel].data_seg1 = (uint32_t)(dshot_expanded & 0xFFFFFF);
+		dshot_inst[channel].irq_data = (uint32_t)(dshot_expanded >> 24);
+		dshot_inst[channel].state = DSHOT_START;
+
+		if (dshot_inst[channel].bdshot) {
+
+			flexio_putreg32(0x0, IMXRT_FLEXIO_TIMCTL0_OFFSET + channel * 0x4);
+			disable_shifter_status_interrupts(1 << channel);
+
+			flexio_dshot_output(channel, timer_io_channels[channel].dshot.flexio_pin, dshot_tcmp, dshot_inst[channel].bdshot);
+
+			clear_timer_status_flags(0xFF);
+		}
 	}
 }

@@ -87,7 +87,6 @@ typedef struct io_timers_t {
 	uint32_t  clock_register;      /* SIM_SCGCn */
 	uint32_t  clock_bit;           /* SIM_SCGCn bit pos */
 	uint32_t  vectorno;            /* IRQ number */
-	dshot_conf_t	dshot;
 } io_timers_t;

 typedef struct io_timers_channel_mapping_element_t {
@@ -112,6 +111,7 @@ typedef struct timer_io_channels_t {
 	uint8_t   sub_module;          /* 0 based sub module offset */
 	uint8_t   sub_module_bits;     /* LDOK and CLDOK bits */
 	uint8_t   timer_channel;       /* Unused */
+	dshot_conf_t	dshot;
 } timer_io_channels_t;

 #define SM0           0
@@ -255,6 +255,34 @@ static inline int channels_timer(unsigned channel)
 	return timer_io_channels[channel].timer_index;
 }

+static uint32_t get_timer_channels(unsigned timer)
+{
+	uint32_t channels = 0;
+	static uint32_t channels_cache[MAX_IO_TIMERS] = {0};
+
+	if (validate_timer_index(timer) < 0) {
+		return channels;
+
+	} else {
+		if (channels_cache[timer] == 0) {
+			/* Gather the channel bits that belong to the timer */
+
+			uint32_t first_channel_index = io_timers_channel_mapping.element[timer].first_channel_index;
+			uint32_t last_channel_index = first_channel_index + io_timers_channel_mapping.element[timer].channel_count;
+
+			for (unsigned chan_index = first_channel_index; chan_index < last_channel_index; chan_index++) {
+				channels |= 1 << chan_index;
+			}
+
+			/* cache them */
+
+			channels_cache[timer] = channels;
+		}
+	}
+
+	return channels_cache[timer];
+}
+
 static uint32_t get_channel_mask(unsigned channel)
 {
 	return io_timer_validate_channel_index(channel) == 0 ? 1 << channel : 0;
@@ -391,41 +419,66 @@ static int allocate_channel(unsigned channel, io_timer_channel_mode_t mode)
 	return rv;
 }

-static int timer_set_rate(unsigned channel, unsigned rate)
+static int timer_set_rate(unsigned timer, unsigned rate)
 {
+	int channels = get_timer_channels(timer);
+
 	irqstate_t flags = px4_enter_critical_section();
-	rMCTRL(channels_timer(channel)) |= (timer_io_channels[channel].sub_module_bits >> MCTRL_LDOK_SHIFT) << MCTRL_CLDOK_SHIFT
-					   ;
-	rVAL1(channels_timer(channel), timer_io_channels[channel].sub_module) = (BOARD_PWM_FREQ / rate) - 1;
-	rMCTRL(channels_timer(channel)) |= timer_io_channels[channel].sub_module_bits;
+
+	for (uint32_t channel = 0; channel < DIRECT_PWM_OUTPUT_CHANNELS; ++channel) {
+		if ((1 << channel) & channels) {
+			rMCTRL(channels_timer(channel)) |= (timer_io_channels[channel].sub_module_bits >> MCTRL_LDOK_SHIFT) << MCTRL_CLDOK_SHIFT
+							   ;
+			rVAL1(channels_timer(channel), timer_io_channels[channel].sub_module) = (BOARD_PWM_FREQ / rate) - 1;
+			rMCTRL(channels_timer(channel)) |= timer_io_channels[channel].sub_module_bits;
+
+		}
+	}
+
 	px4_leave_critical_section(flags);
 	return 0;
 }

-static inline void io_timer_set_oneshot_mode(unsigned channel)
+static inline void io_timer_set_oneshot_mode(unsigned timer)
 {
+	int channels = get_timer_channels(timer);
+
 	irqstate_t flags = px4_enter_critical_section();
-	uint16_t rvalue = rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module);
-	rvalue &= ~SMCTRL_PRSC_MASK;
-	rvalue |= SMCTRL_PRSC_DIV2 | SMCTRL_LDMOD;
-	rMCTRL(channels_timer(channel)) |= (timer_io_channels[channel].sub_module_bits >> MCTRL_LDOK_SHIFT) << MCTRL_CLDOK_SHIFT
-					   ;
-	rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module)  = rvalue;
-	rMCTRL(channels_timer(channel)) |= timer_io_channels[channel].sub_module_bits;
+
+	for (uint32_t channel = 0; channel < DIRECT_PWM_OUTPUT_CHANNELS; ++channel) {
+		if ((1 << channel) & channels) {
+			uint16_t rvalue = rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module);
+			rvalue &= ~SMCTRL_PRSC_MASK;
+			rvalue |= SMCTRL_PRSC_DIV2 | SMCTRL_LDMOD;
+			rMCTRL(channels_timer(channel)) |= (timer_io_channels[channel].sub_module_bits >> MCTRL_LDOK_SHIFT) << MCTRL_CLDOK_SHIFT
+							   ;
+			rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module)  = rvalue;
+			rMCTRL(channels_timer(channel)) |= timer_io_channels[channel].sub_module_bits;
+		}
+	}
+
 	px4_leave_critical_section(flags);

 }

-static inline void io_timer_set_PWM_mode(unsigned channel)
+static inline void io_timer_set_PWM_mode(unsigned timer)
 {
+	int channels = get_timer_channels(timer);
+
 	irqstate_t flags = px4_enter_critical_section();
-	uint16_t rvalue = rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module);
-	rvalue &= ~(SMCTRL_PRSC_MASK | SMCTRL_LDMOD);
-	rvalue |= SMCTRL_PRSC_DIV16;
-	rMCTRL(channels_timer(channel)) |= (timer_io_channels[channel].sub_module_bits >> MCTRL_LDOK_SHIFT) << MCTRL_CLDOK_SHIFT
-					   ;
-	rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module)  = rvalue;
-	rMCTRL(channels_timer(channel)) |= timer_io_channels[channel].sub_module_bits;
+
+	for (uint32_t channel = 0; channel < DIRECT_PWM_OUTPUT_CHANNELS; ++channel) {
+		if ((1 << channel) & channels) {
+			uint16_t rvalue = rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module);
+			rvalue &= ~(SMCTRL_PRSC_MASK | SMCTRL_LDMOD);
+			rvalue |= SMCTRL_PRSC_DIV16;
+			rMCTRL(channels_timer(channel)) |= (timer_io_channels[channel].sub_module_bits >> MCTRL_LDOK_SHIFT) << MCTRL_CLDOK_SHIFT
+							   ;
+			rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module)  = rvalue;
+			rMCTRL(channels_timer(channel)) |= timer_io_channels[channel].sub_module_bits;
+		}
+	}
+
 	px4_leave_critical_section(flags);
 }

@@ -530,33 +583,35 @@ int io_timer_init_timer(unsigned timer, io_timer_channel_mode_t mode)
 			break;
 		}

-		uint32_t first_channel_index = io_timers_channel_mapping.element[timer].first_channel_index;
-		uint32_t last_channel_index = first_channel_index + io_timers_channel_mapping.element[timer].channel_count;
+		int channels = get_timer_channels(timer);

-		for (uint32_t chan = first_channel_index; chan < last_channel_index; chan++) {
+		for (uint32_t chan = 0; chan < DIRECT_PWM_OUTPUT_CHANNELS; ++chan) {
+			if ((1 << chan) & channels) {

-			/* Clear all Faults */
-			rFSTS0(timer) = FSTS_FFLAG_MASK;
+				/* Clear all Faults */
+				rFSTS0(timer) = FSTS_FFLAG_MASK;

-			rCTRL2(timer, timer_io_channels[chan].sub_module) = SMCTRL2_CLK_SEL_EXT_CLK | SMCTRL2_DBGEN | SMCTRL2_INDEP;
-			rCTRL(timer, timer_io_channels[chan].sub_module)  = SMCTRL_PRSC_DIV16 | SMCTRL_FULL;
-			/* Edge aligned at 0 */
-			rINIT(channels_timer(chan), timer_io_channels[chan].sub_module) = 0;
-			rVAL0(channels_timer(chan), timer_io_channels[chan].sub_module) = 0;
-			rVAL2(channels_timer(chan), timer_io_channels[chan].sub_module) = 0;
-			rVAL4(channels_timer(chan), timer_io_channels[chan].sub_module) = 0;
-			rFFILT0(timer) &= ~FFILT_FILT_PER_MASK;
-			rDISMAP0(timer, timer_io_channels[chan].sub_module) = 0xf000;
-			rDISMAP1(timer, timer_io_channels[chan].sub_module) = 0xf000;
-			rOUTEN(timer) |= timer_io_channels[chan].val_offset == PWMA_VAL ? OUTEN_PWMA_EN(1 << timer_io_channels[chan].sub_module)
-					 : OUTEN_PWMB_EN(1 << timer_io_channels[chan].sub_module);
-			rDTSRCSEL(timer) = 0;
-			rMCTRL(timer) = MCTRL_LDOK(1 << timer_io_channels[chan].sub_module);
-			rMCTRL(timer) = timer_io_channels[chan].sub_module_bits;
-			io_timer_set_PWM_mode(chan);
-			timer_set_rate(chan, 50);
+				rCTRL2(timer, timer_io_channels[chan].sub_module) = SMCTRL2_CLK_SEL_EXT_CLK | SMCTRL2_DBGEN | SMCTRL2_INDEP;
+				rCTRL(timer, timer_io_channels[chan].sub_module)  = SMCTRL_PRSC_DIV16 | SMCTRL_FULL;
+				/* Edge aligned at 0 */
+				rINIT(channels_timer(chan), timer_io_channels[chan].sub_module) = 0;
+				rVAL0(channels_timer(chan), timer_io_channels[chan].sub_module) = 0;
+				rVAL2(channels_timer(chan), timer_io_channels[chan].sub_module) = 0;
+				rVAL4(channels_timer(chan), timer_io_channels[chan].sub_module) = 0;
+				rFFILT0(timer) &= ~FFILT_FILT_PER_MASK;
+				rDISMAP0(timer, timer_io_channels[chan].sub_module) = 0xf000;
+				rDISMAP1(timer, timer_io_channels[chan].sub_module) = 0xf000;
+				rOUTEN(timer) |= timer_io_channels[chan].val_offset == PWMA_VAL ? OUTEN_PWMA_EN(1 << timer_io_channels[chan].sub_module)
+						 : OUTEN_PWMB_EN(1 << timer_io_channels[chan].sub_module);
+				rDTSRCSEL(timer) = 0;
+				rMCTRL(timer) = MCTRL_LDOK(1 << timer_io_channels[chan].sub_module);
+				rMCTRL(timer) = timer_io_channels[chan].sub_module_bits;
+			}
 		}

+		io_timer_set_PWM_mode(timer);
+		timer_set_rate(timer, 50);
+
 		px4_leave_critical_section(flags);
 	}

@@ -818,8 +873,7 @@ uint16_t io_channel_get_ccr(unsigned channel)
 	return value;
 }

-// The rt has 1:1 group to channel
-uint32_t io_timer_get_group(unsigned group)
+uint32_t io_timer_get_group(unsigned timer)
 {
-	return get_channel_mask(group);
+	return get_timer_channels(timer);
 }
@@ -36,7 +36,7 @@ add_subdirectory(../imxrt/adc adc)
 add_subdirectory(../imxrt/board_critmon board_critmon)
 add_subdirectory(../imxrt/board_hw_info board_hw_info)
 add_subdirectory(../imxrt/board_reset board_reset)
-#add_subdirectory(../imxrt/dshot dshot)
+add_subdirectory(../imxrt/dshot dshot)
 add_subdirectory(../imxrt/hrt hrt)
 add_subdirectory(../imxrt/led_pwm led_pwm)
 add_subdirectory(../imxrt/io_pins io_pins)
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2023 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
@@ -30,19 +30,7 @@
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/
-
-/*
- * This header defines the events::EventType type.
- */
-
 #pragma once

-#include <uORB/topics/event.h>
-
-namespace events
-{
-using EventType = event_s;
-} // namespace events
-
-

+#include "../../../imxrt/include/px4_arch/dshot.h"
@@ -41,6 +41,7 @@
 #include <nuttx/irq.h>

 #include <drivers/drv_hrt.h>
+#include "dshot.h"

 #pragma once
 __BEGIN_DECLS
@@ -110,6 +111,7 @@ typedef struct timer_io_channels_t {
 	uint8_t   sub_module;          /* 0 based sub module offset */
 	uint8_t   sub_module_bits;     /* LDOK and CLDOK bits */
 	uint8_t   timer_channel;       /* Unused */
+	dshot_conf_t dshot;
 } timer_io_channels_t;

 #define SM0           0
@@ -601,6 +601,16 @@ static inline constexpr timer_io_channels_t initIOTimerChannel(const io_timers_t
 	return ret;
 }

+static inline constexpr timer_io_channels_t initIOTimerChannelDshot(const io_timers_t io_timers_conf[MAX_IO_TIMERS],
+		PWM::FlexPWMConfig pwm_config, IOMUX::Pad pad, uint32_t dshot_pinmux, uint32_t flexio_pin)
+{
+	timer_io_channels_t ret = initIOTimerChannel(io_timers_conf, pwm_config, pad);
+
+	ret.dshot.pinmux = dshot_pinmux;
+	ret.dshot.flexio_pin = flexio_pin;
+	return ret;
+}
+
 static inline constexpr io_timers_t initIOPWM(PWM::FlexPWM pwm, PWM::FlexPWMSubmodule sub)
 {
 	io_timers_t ret{};
@@ -609,3 +619,14 @@ static inline constexpr io_timers_t initIOPWM(PWM::FlexPWM pwm, PWM::FlexPWMSubm
 	ret.submodle = sub;
 	return ret;
 }
+
+
+
+static inline constexpr io_timers_t initIOPWMDshot(PWM::FlexPWM pwm, PWM::FlexPWMSubmodule sub)
+{
+	io_timers_t ret{};
+
+	ret.base = getFlexPWMBaseRegister(pwm);
+	ret.submodle = sub;
+	return ret;
+}
--- a/Show More
+++ b/Show More