dshot: fix clearing out esc status

.editorconfig

@@ -1,14 +0,0 @@
-root = true
-
-[*]
-insert_final_newline = false
-
-[{*.{c,cpp,cc,h,hpp},CMakeLists.txt,Kconfig}]
-indent_style = tab
-tab_width = 8
-# Not in the official standard, but supported by many editors
-max_line_length = 120
-
-[*.yaml]
-indent_style = space
-indent_size = 2

.gitmodules

@@ -81,5 +81,5 @@
 	url = https://github.com/PX4/PX4-gazebo-models.git
 	branch = main
 [submodule "boards/modalai/voxl2/libfc-sensor-api"]
-	path = boards/modalai/voxl2/libfc-sensor-api
+	path = src/modules/muorb/apps/libfc-sensor-api
 	url = https://gitlab.com/voxl-public/voxl-sdk/core-libs/libfc-sensor-api.git

Tools/astyle/files_to_check_code_style.sh

@@ -30,5 +30,5 @@ exec find boards msg src platforms test \
     -path src/lib/cdrstream/cyclonedds -prune -o \
     -path src/lib/cdrstream/rosidl -prune -o \
     -path src/modules/zenoh/zenoh-pico -prune -o \
-    -path boards/modalai/voxl2/libfc-sensor-api -prune -o \
+    -path src/modules/muorb/apps/libfc-sensor-api -prune -o \
     -type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \) | grep $PATTERN

Tools/module_config/output_groups_from_timer_config.py

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

boards/modalai/fc-v2/default.px4board

@@ -60,7 +60,7 @@ CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
-# CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
+CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UXRCE_DDS_CLIENT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y

boards/modalai/voxl2-slpi/default.px4board

@@ -4,7 +4,6 @@ CONFIG_DRIVERS_ACTUATORS_VOXL_ESC=y
 CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP101XX=y
 CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L0X=y
 CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L1X=y
-CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
 CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y
 CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8308=y

boards/modalai/voxl2/cmake/init.cmake

@@ -1,34 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2024 ModalAI, Inc. 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_directories(${PX4_BOARD_DIR}/libfc-sensor-api/inc)

boards/modalai/voxl2/cmake/link_libraries.cmake

@@ -1,7 +1,7 @@
 
 # Link against the public stub version of the proprietary fc sensor library
 target_link_libraries(px4 PRIVATE
-		${PX4_BOARD_DIR}/libfc-sensor-api/build/libfc_sensor.so
+		${PX4_SOURCE_DIR}/src//modules/muorb/apps/libfc-sensor-api/build/libfc_sensor.so
 		px4_layer
 		${module_libraries}
 )

boards/modalai/voxl2/scripts/build-deps.sh

@@ -1,9 +1,10 @@
 #!/bin/bash
 
-cd boards/modalai/voxl2/libfc-sensor-api
+cd src/modules/muorb/apps/libfc-sensor-api
 rm -fR build
 mkdir build
 cd build
 CC=/home/4.1.0.4/tools/linaro64/bin/aarch64-linux-gnu-gcc cmake ..
 make
-cd ../../../../..
+cd ../../../../../..
+

boards/px4/fmu-v5x/default.px4board

@@ -81,6 +81,7 @@ CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF=y
 CONFIG_MODULES_PAYLOAD_DELIVERER=y
 CONFIG_MODULES_RC_UPDATE=y
+CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
@@ -101,6 +102,7 @@ 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_SYSTEM_TIME=y
 CONFIG_SYSTEMCMDS_TOP=y

boards/px4/fmu-v6xrt/nuttx-config/nsh/defconfig

@@ -83,7 +83,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

boards/px4/fmu-v6xrt/src/imxrt_clockconfig.c

@@ -114,7 +114,7 @@ const struct clock_configuration_s g_initial_clkconfig = {
 			.div           = 1,
 			.mux           = ACMP_CLK_ROOT_OSC_RC_48M_DIV2,
 		},
-		.flexio1_clk_root =
+		.flexio1_clk_root = /* 240 / 2 = 120Mhz */
 		{
 			.enable        = 1,
 			.div           = 2,

boards/px4/fmu-v6xrt/src/timer_config.cpp

@@ -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, 23),
+	/* FMU_CH2  */  initIOTimerChannelDshot(io_timers, {PWM::PWM1_PWM_A, PWM::Submodule1}, IOMUX::Pad::GPIO_EMC_B1_25, GPIO_FLEXIO1_FLEXIO25_1, 25),
+	/* FMU_CH3  */  initIOTimerChannelDshot(io_timers, {PWM::PWM1_PWM_A, PWM::Submodule2}, IOMUX::Pad::GPIO_EMC_B1_27, GPIO_FLEXIO1_FLEXIO27_1, 27),
+	/* FMU_CH4  */  initIOTimerChannelDshot(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_EMC_B1_06, GPIO_FLEXIO1_FLEXIO06_1, 6),
+	/* FMU_CH5  */  initIOTimerChannelDshot(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule1}, IOMUX::Pad::GPIO_EMC_B1_08, GPIO_FLEXIO1_FLEXIO08_1, 8),
+	/* FMU_CH6  */  initIOTimerChannelDshot(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule2}, IOMUX::Pad::GPIO_EMC_B1_10, GPIO_FLEXIO1_FLEXIO10_1, 10),
+	/* FMU_CH7  */  initIOTimerChannelDshot(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule3}, IOMUX::Pad::GPIO_EMC_B1_19, GPIO_FLEXIO1_FLEXIO19_1, 19),
+	/* FMU_CH8  */  initIOTimerChannelDshot(io_timers, {PWM::PWM3_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_EMC_B1_29, GPIO_FLEXIO1_FLEXIO29_1, 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),

platforms/common/CMakeLists.txt

@@ -52,7 +52,6 @@ add_library(px4_platform STATIC
 	shutdown.cpp
 	spi.cpp
 	pab_manifest.c
-	Serial.cpp
 	${SRCS}
 )
 target_link_libraries(px4_platform prebuild_targets px4_work_queue)

platforms/common/Serial.cpp

@@ -1,138 +0,0 @@
-/****************************************************************************
- *
- *   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
- * 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 <px4_platform_common/Serial.hpp>
-
-namespace device
-{
-
-Serial::Serial(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
-	       FlowControl flowcontrol) :
-	_impl(port, baudrate, bytesize, parity, stopbits, flowcontrol)
-{
-	// If no baudrate was specified then set it to a reasonable default value
-	if (baudrate == 0) {
-		(void) _impl.setBaudrate(9600);
-	}
-}
-
-Serial::~Serial()
-{
-}
-
-bool Serial::open()
-{
-	return _impl.open();
-}
-
-bool Serial::isOpen() const
-{
-	return _impl.isOpen();
-}
-
-bool Serial::close()
-{
-	return _impl.close();
-}
-
-ssize_t Serial::read(uint8_t *buffer, size_t buffer_size)
-{
-	return _impl.read(buffer, buffer_size);
-}
-
-ssize_t Serial::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us)
-{
-	return _impl.readAtLeast(buffer, buffer_size, character_count, timeout_us);
-}
-
-ssize_t Serial::write(const void *buffer, size_t buffer_size)
-{
-	return _impl.write(buffer, buffer_size);
-}
-
-uint32_t Serial::getBaudrate() const
-{
-	return _impl.getBaudrate();
-}
-
-bool Serial::setBaudrate(uint32_t baudrate)
-{
-	return _impl.setBaudrate(baudrate);
-}
-
-ByteSize Serial::getBytesize() const
-{
-	return _impl.getBytesize();
-}
-
-bool Serial::setBytesize(ByteSize bytesize)
-{
-	return _impl.setBytesize(bytesize);
-}
-
-Parity Serial::getParity() const
-{
-	return _impl.getParity();
-}
-
-bool Serial::setParity(Parity parity)
-{
-	return _impl.setParity(parity);
-}
-
-StopBits Serial::getStopbits() const
-{
-	return _impl.getStopbits();
-}
-
-bool Serial::setStopbits(StopBits stopbits)
-{
-	return _impl.setStopbits(stopbits);
-}
-
-FlowControl Serial::getFlowcontrol() const
-{
-	return _impl.getFlowcontrol();
-}
-
-bool Serial::setFlowcontrol(FlowControl flowcontrol)
-{
-	return _impl.setFlowcontrol(flowcontrol);
-}
-
-const char *Serial::getPort() const
-{
-	return _impl.getPort();
-}
-
-} // namespace device

platforms/common/include/px4_platform_common/Serial.hpp

@@ -1,97 +0,0 @@
-/****************************************************************************
- *
- *   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
- * 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 <SerialImpl.hpp>
-
-#include <px4_platform_common/SerialCommon.hpp>
-
-using device::SerialConfig::ByteSize;
-using device::SerialConfig::Parity;
-using device::SerialConfig::StopBits;
-using device::SerialConfig::FlowControl;
-
-namespace device __EXPORT
-{
-
-class Serial
-{
-public:
-	Serial(const char *port, uint32_t baudrate = 57600,
-	       ByteSize bytesize = ByteSize::EightBits, Parity parity = Parity::None,
-	       StopBits stopbits = StopBits::One, FlowControl flowcontrol = FlowControl::Disabled);
-	virtual ~Serial();
-
-	// Open sets up the port and gets it configured based on desired configuration
-	bool open();
-	bool isOpen() const;
-
-	bool close();
-
-	ssize_t read(uint8_t *buffer, size_t buffer_size);
-	ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0);
-
-	ssize_t write(const void *buffer, size_t buffer_size);
-
-	// If port is already open then the following configuration functions
-	// will reconfigure the port. If the port is not yet open then they will
-	// simply store the configuration in preparation for the port to be opened.
-
-	uint32_t getBaudrate() const;
-	bool setBaudrate(uint32_t baudrate);
-
-	ByteSize getBytesize() const;
-	bool setBytesize(ByteSize bytesize);
-
-	Parity getParity() const;
-	bool setParity(Parity parity);
-
-	StopBits getStopbits() const;
-	bool setStopbits(StopBits stopbits);
-
-	FlowControl getFlowcontrol() const;
-	bool setFlowcontrol(FlowControl flowcontrol);
-
-	const char *getPort() const;
-
-private:
-	// Disable copy constructors
-	Serial(const Serial &);
-	Serial &operator=(const Serial &);
-
-	// platform implementation
-	SerialImpl _impl;
-};
-
-} // namespace device

platforms/common/include/px4_platform_common/time.h

@@ -13,21 +13,11 @@ __END_DECLS
 #define px4_clock_gettime system_clock_gettime
 #endif
 
-#if defined(ENABLE_LOCKSTEP_SCHEDULER) || defined(__PX4_QURT)
-
-__BEGIN_DECLS
-__EXPORT int px4_clock_settime(clockid_t clk_id, const struct timespec *tp);
-__END_DECLS
-
-#else
-
-#define px4_clock_settime system_clock_settime
-
-#endif
-
 #if defined(ENABLE_LOCKSTEP_SCHEDULER)
 
 __BEGIN_DECLS
+__EXPORT int px4_clock_settime(clockid_t clk_id, const struct timespec *tp);
+
 __EXPORT int px4_usleep(useconds_t usec);
 __EXPORT unsigned int px4_sleep(unsigned int seconds);
 __EXPORT int px4_pthread_cond_timedwait(pthread_cond_t *cond,
@@ -37,6 +27,7 @@ __END_DECLS
 
 #else
 
+#define px4_clock_settime system_clock_settime
 #define px4_usleep system_usleep
 #define px4_sleep system_sleep
 #define px4_pthread_cond_timedwait system_pthread_cond_timedwait

platforms/common/uORB/SubscriptionBlocking.hpp

@@ -56,6 +56,31 @@ public:
 	SubscriptionBlocking(const orb_metadata *meta, uint32_t interval_us = 0, uint8_t instance = 0) :
 		SubscriptionCallback(meta, interval_us, instance)
 	{
+		// pthread_mutexattr_init
+		pthread_mutexattr_t attr;
+		int ret_attr_init = pthread_mutexattr_init(&attr);
+
+		if (ret_attr_init != 0) {
+			PX4_ERR("pthread_mutexattr_init failed, status=%d", ret_attr_init);
+		}
+
+#if defined(PTHREAD_PRIO_NONE)
+		// pthread_mutexattr_settype
+		//  PTHREAD_PRIO_NONE not available on cygwin
+		int ret_mutexattr_settype = pthread_mutexattr_settype(&attr, PTHREAD_PRIO_NONE);
+
+		if (ret_mutexattr_settype != 0) {
+			PX4_ERR("pthread_mutexattr_settype failed, status=%d", ret_mutexattr_settype);
+		}
+
+#endif // PTHREAD_PRIO_NONE
+
+		// pthread_mutex_init
+		int ret_mutex_init = pthread_mutex_init(&_mutex, &attr);
+
+		if (ret_mutex_init != 0) {
+			PX4_ERR("pthread_mutex_init failed, status=%d", ret_mutex_init);
+		}
 	}
 
 	virtual ~SubscriptionBlocking()

platforms/common/uORB/uORBDeviceNode.cpp

@@ -412,10 +412,7 @@ int16_t uORB::DeviceNode::process_add_subscription()
 	uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator();
 
 	if (_data != nullptr && ch != nullptr) { // _data will not be null if there is a publisher.
-		// Only send the most recent data to initialize the remote end.
-		if (_data_valid) {
-			ch->send_message(_meta->o_name, _meta->o_size, _data + (_meta->o_size * ((_generation.load() - 1) % _meta->o_queue)));
-		}
+		ch->send_message(_meta->o_name, _meta->o_size, _data);
 	}
 
 	return PX4_OK;

platforms/nuttx/NuttX/nuttx

@@ -1 +1 @@
-Subproject commit 693b9e782535f12e6a4ab657c7a0c3bd92b45fb1
+Subproject commit 837e0eccab44a8e4cb44e076111eae43f92f61aa

platforms/nuttx/src/px4/common/SerialImpl.cpp

@@ -1,394 +0,0 @@
-/****************************************************************************
- *
- *   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
- * 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 <SerialImpl.hpp>
-#include <string.h> // strncpy
-#include <termios.h>
-#include <px4_log.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <poll.h>
-#include <drivers/drv_hrt.h>
-
-#define MODULE_NAME "SerialImpl"
-
-namespace device
-{
-
-SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
-		       FlowControl flowcontrol) :
-	_baudrate(baudrate),
-	_bytesize(bytesize),
-	_parity(parity),
-	_stopbits(stopbits),
-	_flowcontrol(flowcontrol)
-{
-	if (port) {
-		strncpy(_port, port, sizeof(_port) - 1);
-		_port[sizeof(_port) - 1] = '\0';
-
-	} else {
-		_port[0] = 0;
-	}
-}
-
-SerialImpl::~SerialImpl()
-{
-	if (isOpen()) {
-		close();
-	}
-}
-
-bool SerialImpl::validateBaudrate(uint32_t baudrate)
-{
-	return ((baudrate == 9600) ||
-		(baudrate == 19200) ||
-		(baudrate == 38400) ||
-		(baudrate == 57600) ||
-		(baudrate == 115200) ||
-		(baudrate == 230400) ||
-		(baudrate == 460800) ||
-		(baudrate == 921600));
-}
-
-bool SerialImpl::configure()
-{
-	/* process baud rate */
-	int speed;
-
-	if (! validateBaudrate(_baudrate)) {
-		PX4_ERR("ERR: unknown baudrate: %lu", _baudrate);
-		return false;
-	}
-
-	switch (_baudrate) {
-	case 9600:   speed = B9600;   break;
-
-	case 19200:  speed = B19200;  break;
-
-	case 38400:  speed = B38400;  break;
-
-	case 57600:  speed = B57600;  break;
-
-	case 115200: speed = B115200; break;
-
-	case 230400: speed = B230400; break;
-
-#ifndef B460800
-#define B460800 460800
-#endif
-
-	case 460800: speed = B460800; break;
-
-#ifndef B921600
-#define B921600 921600
-#endif
-
-	case 921600: speed = B921600; break;
-
-	default:
-		PX4_ERR("ERR: unknown baudrate: %lu", _baudrate);
-		return false;
-	}
-
-	struct termios uart_config;
-
-	int termios_state;
-
-	/* fill the struct for the new configuration */
-	if ((termios_state = tcgetattr(_serial_fd, &uart_config)) < 0) {
-		PX4_ERR("ERR: %d (tcgetattr)", termios_state);
-		return false;
-	}
-
-	/* properly configure the terminal (see also https://en.wikibooks.org/wiki/Serial_Programming/termios ) */
-
-	//
-	// Input flags - Turn off input processing
-	//
-	// convert break to null byte, no CR to NL translation,
-	// no NL to CR translation, don't mark parity errors or breaks
-	// no input parity check, don't strip high bit off,
-	// no XON/XOFF software flow control
-	//
-	uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL |
-				 INLCR | PARMRK | INPCK | ISTRIP | IXON);
-
-	//
-	// Output flags - Turn off output processing
-	//
-	// no CR to NL translation, no NL to CR-NL translation,
-	// no NL to CR translation, no column 0 CR suppression,
-	// no Ctrl-D suppression, no fill characters, no case mapping,
-	// no local output processing
-	//
-	// config.c_oflag &= ~(OCRNL | ONLCR | ONLRET |
-	//                     ONOCR | ONOEOT| OFILL | OLCUC | OPOST);
-	uart_config.c_oflag = 0;
-
-	//
-	// No line processing
-	//
-	// echo off, echo newline off, canonical mode off,
-	// extended input processing off, signal chars off
-	//
-	uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
-
-	/* no parity, one stop bit, disable flow control */
-	uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS);
-
-	/* set baud rate */
-	if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
-		PX4_ERR("ERR: %d (cfsetispeed)", termios_state);
-		return false;
-	}
-
-	if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
-		PX4_ERR("ERR: %d (cfsetospeed)", termios_state);
-		return false;
-	}
-
-	if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) {
-		PX4_ERR("ERR: %d (tcsetattr)", termios_state);
-		return false;
-	}
-
-	return true;
-}
-
-bool SerialImpl::open()
-{
-	if (isOpen()) {
-		return true;
-	}
-
-	// Open the serial port
-	int serial_fd = ::open(_port, O_RDWR | O_NOCTTY);
-
-	if (serial_fd < 0) {
-		PX4_ERR("failed to open %s err: %d", _port, errno);
-		return false;
-	}
-
-	_serial_fd = serial_fd;
-
-	// Configure the serial port
-	if (! configure()) {
-		PX4_ERR("failed to configure %s err: %d", _port, errno);
-		return false;
-	}
-
-	_open = true;
-
-	return _open;
-}
-
-bool SerialImpl::isOpen() const
-{
-	return _open;
-}
-
-bool SerialImpl::close()
-{
-
-	if (_serial_fd >= 0) {
-		::close(_serial_fd);
-	}
-
-	_serial_fd = -1;
-	_open = false;
-
-	return true;
-}
-
-ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size)
-{
-	if (!_open) {
-		PX4_ERR("Cannot read from serial device until it has been opened");
-		return -1;
-	}
-
-	int ret = ::read(_serial_fd, buffer, buffer_size);
-
-	if (ret < 0) {
-		PX4_DEBUG("%s read error %d", _port, ret);
-	}
-
-	return ret;
-}
-
-ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us)
-{
-	if (!_open) {
-		PX4_ERR("Cannot readAtLeast from serial device until it has been opened");
-		return -1;
-	}
-
-	if (buffer_size < character_count) {
-		PX4_ERR("%s: Buffer not big enough to hold desired amount of read data", __FUNCTION__);
-		return -1;
-	}
-
-	const hrt_abstime start_time_us = hrt_absolute_time();
-	int total_bytes_read = 0;
-
-	while ((total_bytes_read < (int) character_count) && (hrt_elapsed_time(&start_time_us) < timeout_us)) {
-		// Poll for incoming UART data.
-		pollfd fds[1];
-		fds[0].fd = _serial_fd;
-		fds[0].events = POLLIN;
-
-		hrt_abstime remaining_time = timeout_us - hrt_elapsed_time(&start_time_us);
-
-		if (remaining_time <= 0) { break; }
-
-		int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), remaining_time);
-
-		if (ret > 0) {
-			if (fds[0].revents & POLLIN) {
-				const unsigned sleeptime = character_count * 1000000 / (_baudrate / 10);
-
-				int err = 0;
-				int bytes_available = 0;
-				err = ::ioctl(_serial_fd, FIONREAD, (unsigned long)&bytes_available);
-
-				if (err != 0 || bytes_available < (int)character_count) {
-					px4_usleep(sleeptime);
-				}
-
-				ret = read(&buffer[total_bytes_read], buffer_size - total_bytes_read);
-
-				if (ret > 0) {
-					total_bytes_read += ret;
-				}
-
-			} else {
-				PX4_ERR("Got a poll error");
-				return -1;
-			}
-		}
-	}
-
-	return total_bytes_read;
-}
-
-ssize_t SerialImpl::write(const void *buffer, size_t buffer_size)
-{
-	if (!_open) {
-		PX4_ERR("Cannot write to serial device until it has been opened");
-		return -1;
-	}
-
-	int written = ::write(_serial_fd, buffer, buffer_size);
-	::fsync(_serial_fd);
-
-	if (written < 0) {
-		PX4_ERR("%s write error %d", _port, written);
-	}
-
-	return written;
-}
-
-const char *SerialImpl::getPort() const
-{
-	return _port;
-}
-
-uint32_t SerialImpl::getBaudrate() const
-{
-	return _baudrate;
-}
-
-bool SerialImpl::setBaudrate(uint32_t baudrate)
-{
-	if (! validateBaudrate(baudrate)) {
-		PX4_ERR("ERR: invalid baudrate: %lu", baudrate);
-		return false;
-	}
-
-	// check if already configured
-	if ((baudrate == _baudrate) && _open) {
-		return true;
-	}
-
-	_baudrate = baudrate;
-
-	// process baud rate change now if port is already open
-	if (_open) {
-		return configure();
-	}
-
-	return true;
-}
-
-ByteSize SerialImpl::getBytesize() const
-{
-	return _bytesize;
-}
-
-bool SerialImpl::setBytesize(ByteSize bytesize)
-{
-	return bytesize == ByteSize::EightBits;
-}
-
-Parity SerialImpl::getParity() const
-{
-	return _parity;
-}
-
-bool SerialImpl::setParity(Parity parity)
-{
-	return parity == Parity::None;
-}
-
-StopBits SerialImpl::getStopbits() const
-{
-	return _stopbits;
-}
-
-bool SerialImpl::setStopbits(StopBits stopbits)
-{
-	return stopbits == StopBits::One;
-}
-
-FlowControl SerialImpl::getFlowcontrol() const
-{
-	return _flowcontrol;
-}
-
-bool SerialImpl::setFlowcontrol(FlowControl flowcontrol)
-{
-	return flowcontrol == FlowControl::Disabled;
-}
-
-} // namespace device

platforms/nuttx/src/px4/common/include/SerialImpl.hpp

@@ -1,104 +0,0 @@
-/****************************************************************************
- *
- *   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
- * 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 <stdint.h>
-#include <unistd.h>
-
-#include <px4_platform_common/SerialCommon.hpp>
-
-using device::SerialConfig::ByteSize;
-using device::SerialConfig::Parity;
-using device::SerialConfig::StopBits;
-using device::SerialConfig::FlowControl;
-
-namespace device
-{
-
-class SerialImpl
-{
-public:
-
-	SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
-		   FlowControl flowcontrol);
-	virtual ~SerialImpl();
-
-	bool open();
-	bool isOpen() const;
-
-	bool close();
-
-	ssize_t read(uint8_t *buffer, size_t buffer_size);
-	ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0);
-
-	ssize_t write(const void *buffer, size_t buffer_size);
-
-	const char *getPort() const;
-
-	uint32_t getBaudrate() const;
-	bool setBaudrate(uint32_t baudrate);
-
-	ByteSize getBytesize() const;
-	bool setBytesize(ByteSize bytesize);
-
-	Parity getParity() const;
-	bool setParity(Parity parity);
-
-	StopBits getStopbits() const;
-	bool setStopbits(StopBits stopbits);
-
-	FlowControl getFlowcontrol() const;
-	bool setFlowcontrol(FlowControl flowcontrol);
-
-private:
-
-	int _serial_fd{-1};
-
-	bool _open{false};
-
-	char _port[32] {};
-
-	uint32_t _baudrate{0};
-
-	ByteSize _bytesize{ByteSize::EightBits};
-	Parity _parity{Parity::None};
-	StopBits _stopbits{StopBits::One};
-	FlowControl _flowcontrol{FlowControl::Disabled};
-
-	bool validateBaudrate(uint32_t baudrate);
-	bool configure();
-
-};
-
-} // namespace device

platforms/nuttx/src/px4/common/px4_layer.cmake

@@ -3,8 +3,6 @@
 add_library(px4_layer
 		${KERNEL_SRCS}
 		cdc_acm_check.cpp
-		${PX4_SOURCE_DIR}/platforms/common/Serial.cpp
-		SerialImpl.cpp
 	)
 
 target_link_libraries(px4_layer

platforms/nuttx/src/px4/common/px4_protected_layers.cmake

@@ -15,8 +15,6 @@ add_library(px4_layer
 	usr_board_ctrl.c
 	usr_hrt.cpp
 	usr_mcu_version.cpp
-	${PX4_SOURCE_DIR}/platforms/common/Serial.cpp
-	SerialImpl.cpp
 )
 
 target_link_libraries(px4_layer

platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c

@@ -33,145 +33,424 @@
  ****************************************************************************/
 #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_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;
 } dshot_handler_t;
 
 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].irq_data != 0) {
+				flexio_putreg32(dshot_inst[channel].irq_data, IMXRT_FLEXIO_SHIFTBUF0_OFFSET + channel * 0x4);
+				dshot_inst[channel].irq_data = 0;
 
-			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].irq_data == 0 && 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 < DSHOT_TIMERS; channel++) {
+		clear_timer_status_flags(1 << channel);
+
+		if (flags & (1 << channel)) {
+			if (dshot_inst[channel].state == DSHOT_START) {
+				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;
+	/* 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);
 
-	if (dshot_pwm_freq == 150000) {
-		timer_compare = 0x2F8A;
+	/* Clock FlexIO peripheral */
+	imxrt_clockall_flexio1();
 
-	} else if (dshot_pwm_freq == 300000) {
-		timer_compare = 0x2F45;
+	/* Reset FlexIO peripheral */
+	flexio_modifyreg32(IMXRT_FLEXIO_CTRL_OFFSET, 0,
+			   FLEXIO_CTRL_SWRST_MASK);
+	flexio_putreg32(0, IMXRT_FLEXIO_CTRL_OFFSET);
 
-	} else if (dshot_pwm_freq == 600000) {
-		timer_compare = 0x2F22;
+	/* 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)));
 
-	} else if (dshot_pwm_freq == 1200000) {
-		timer_compare = 0x2F11;
-
-	} else {
-		// Not supported Dshot frequency
-		return 0;
-	}
-
-	/* Init FlexIO peripheral */
-
-	flexio_s = imxrt_flexio_initialize(1);
+	/* 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_dshot_trigger(void)
+void up_bdshot_erpm(void)
 {
-	uint32_t buf_adr;
+	uint32_t value;
+	uint32_t erpm;
+	uint32_t csum_data;
 
-	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);
+	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 */
+				erpm = gcr_decode[value & 0x1fU];
+				erpm |= gcr_decode[(value >> 5U) & 0x1fU] << 4U;
+				erpm |= gcr_decode[(value >> 10U) & 0x1fU] << 8U;
+				erpm |= gcr_decode[(value >> 15U) & 0x1fU] << 12U;
+
+				/* Calculate checksum */
+				csum_data = erpm;
+				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 {
+					dshot_inst[channel].erpm = ~(erpm >> 4) & 0xFFF;
+					//TODO store this or foward this
+				}
+
+			} else {
+				dshot_inst[channel].frame_error_cnt++;
+			}
 		}
 	}
 
-	flexio_s->ops->clear_timer_status_flags(flexio_s, 0xFF);
-	flexio_s->ops->enable_shifter_status_interrupts(flexio_s, 0xFF);
+	bdshot_parsed_recv_mask = bdshot_recv_mask;
 }
 
+
+
+int up_bdshot_get_erpm(uint8_t channel, int *erpm)
+{
+	if (bdshot_parsed_recv_mask & (1 << channel)) {
+		*erpm = dshot_inst[channel].erpm;
+		return 0;
+	}
+
+	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 Last erpm %i value", channel, 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)
+{
+	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);
+		}
+	}
+
+	// Calc data now since we're not event driven
+	if (bdshot_recv_mask != 0x0) {
+		up_bdshot_erpm();
+		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;
@@ -197,16 +476,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;
@@ -219,8 +504,19 @@ 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);
+		}
 	}
 }
 

platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/io_timer.h

@@ -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

platforms/nuttx/src/px4/nxp/imxrt/io_pins/io_timer.c

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

platforms/nuttx/src/px4/nxp/rt106x/CMakeLists.txt

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

platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/dshot.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2023 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,41 +30,7 @@
  * POSSIBILITY OF SUCH DAMAGE.
  *
  ****************************************************************************/
-
 #pragma once
 
-namespace device
-{
-namespace SerialConfig
-{
 
-
-// ByteSize: number of data bits
-enum class ByteSize {
-	FiveBits  = 5,
-	SixBits   = 6,
-	SevenBits = 7,
-	EightBits = 8,
-};
-
-// Parity: enable parity checking
-enum class Parity {
-	None = 0,
-	Odd  = 1,
-	Even = 2,
-};
-
-// StopBits: number of stop bits
-enum class StopBits {
-	One = 1,
-	Two = 2
-};
-
-// FlowControl: enable flow control
-enum class FlowControl {
-	Disabled = 0,
-	Enabled  = 1,
-};
-
-} // namespace SerialConfig
-} // namespace device
+#include "../../../imxrt/include/px4_arch/dshot.h"

platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/io_timer.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

platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/io_timer_hw_description.h

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

platforms/nuttx/src/px4/nxp/rt117x/include/px4_arch/io_timer_hw_description.h

@@ -690,6 +690,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{};
@@ -699,14 +709,13 @@ static inline constexpr io_timers_t initIOPWM(PWM::FlexPWM pwm, PWM::FlexPWMSubm
 	return ret;
 }
 
-static inline constexpr io_timers_t initIOPWMDshot(PWM::FlexPWM pwm, PWM::FlexPWMSubmodule sub, uint32_t pinmux,
-		uint32_t flexio_pin)
+
+
+static inline constexpr io_timers_t initIOPWMDshot(PWM::FlexPWM pwm, PWM::FlexPWMSubmodule sub)
 {
 	io_timers_t ret{};
 
 	ret.base = getFlexPWMBaseRegister(pwm);
 	ret.submodle = sub;
-	ret.dshot.pinmux = pinmux;
-	ret.dshot.flexio_pin = flexio_pin;
 	return ret;
 }

platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c

@@ -82,7 +82,7 @@ static uint8_t dshot_burst_buffer_array[DSHOT_TIMERS * DSHOT_BURST_BUFFER_SIZE(M
 px4_cache_aligned_data() = {};
 static uint32_t *dshot_burst_buffer[DSHOT_TIMERS] = {};
 
-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)
 {
 	unsigned buffer_offset = 0;
 
@@ -152,6 +152,16 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq)
 	return ret_val == OK ? channels_init_mask : ret_val;
 }
 
+int up_bdshot_get_erpm(uint8_t channel, int *erpm)
+{
+	// Not implemented
+	return -1;
+}
+
+void up_bdshot_status(void)
+{
+}
+
 void up_dshot_trigger(void)
 {
 	for (uint8_t timer = 0; (timer < DSHOT_TIMERS); timer++) {

platforms/posix/include/SerialImpl.hpp

@@ -1,103 +0,0 @@
-/****************************************************************************
- *
- *   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
- * 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 <stdint.h>
-#include <unistd.h>
-
-#include <px4_platform_common/SerialCommon.hpp>
-
-using device::SerialConfig::ByteSize;
-using device::SerialConfig::Parity;
-using device::SerialConfig::StopBits;
-using device::SerialConfig::FlowControl;
-
-namespace device
-{
-
-class SerialImpl
-{
-public:
-
-	SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
-		   FlowControl flowcontrol);
-	virtual ~SerialImpl();
-
-	bool open();
-	bool isOpen() const;
-
-	bool close();
-
-	ssize_t read(uint8_t *buffer, size_t buffer_size);
-	ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0);
-
-	ssize_t write(const void *buffer, size_t buffer_size);
-
-	const char *getPort() const;
-
-	uint32_t getBaudrate() const;
-	bool setBaudrate(uint32_t baudrate);
-
-	ByteSize getBytesize() const;
-	bool setBytesize(ByteSize bytesize);
-
-	Parity getParity() const;
-	bool setParity(Parity parity);
-
-	StopBits getStopbits() const;
-	bool setStopbits(StopBits stopbits);
-
-	FlowControl getFlowcontrol() const;
-	bool setFlowcontrol(FlowControl flowcontrol);
-
-private:
-
-	int _serial_fd{-1};
-
-	bool _open{false};
-
-	char _port[32] {};
-
-	uint32_t _baudrate{0};
-
-	ByteSize _bytesize{ByteSize::EightBits};
-	Parity _parity{Parity::None};
-	StopBits _stopbits{StopBits::One};
-	FlowControl _flowcontrol{FlowControl::Disabled};
-
-	bool validateBaudrate(uint32_t baudrate);
-	bool configure();
-};
-
-} // namespace device

platforms/posix/src/px4/common/CMakeLists.txt

@@ -46,8 +46,6 @@ add_library(px4_layer
 	drv_hrt.cpp
 	cpuload.cpp
 	print_load.cpp
-	${PX4_SOURCE_DIR}/platforms/common/Serial.cpp
-	SerialImpl.cpp
 )
 target_compile_definitions(px4_layer PRIVATE MODULE_NAME="px4")
 target_compile_options(px4_layer PRIVATE -Wno-cast-align) # TODO: fix and enable

platforms/posix/src/px4/common/SerialImpl.cpp

@@ -1,387 +0,0 @@
-/****************************************************************************
- *
- *   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
- * 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 <SerialImpl.hpp>
-#include <string.h> // strncpy
-#include <termios.h>
-#include <px4_log.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <poll.h>
-#include <drivers/drv_hrt.h>
-
-namespace device
-{
-
-SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
-		       FlowControl flowcontrol) :
-	_baudrate(baudrate),
-	_bytesize(bytesize),
-	_parity(parity),
-	_stopbits(stopbits),
-	_flowcontrol(flowcontrol)
-{
-	if (port) {
-		strncpy(_port, port, sizeof(_port) - 1);
-		_port[sizeof(_port) - 1] = '\0';
-
-	} else {
-		_port[0] = 0;
-	}
-}
-
-SerialImpl::~SerialImpl()
-{
-	if (isOpen()) {
-		close();
-	}
-}
-
-bool SerialImpl::validateBaudrate(uint32_t baudrate)
-{
-	return ((baudrate == 9600) ||
-		(baudrate == 19200) ||
-		(baudrate == 38400) ||
-		(baudrate == 57600) ||
-		(baudrate == 115200) ||
-		(baudrate == 230400) ||
-		(baudrate == 460800) ||
-		(baudrate == 921600));
-}
-
-bool SerialImpl::configure()
-{
-	/* process baud rate */
-	int speed;
-
-	if (! validateBaudrate(_baudrate)) {
-		PX4_ERR("ERR: unknown baudrate: %u", _baudrate);
-		return false;
-	}
-
-	switch (_baudrate) {
-	case 9600:   speed = B9600;   break;
-
-	case 19200:  speed = B19200;  break;
-
-	case 38400:  speed = B38400;  break;
-
-	case 57600:  speed = B57600;  break;
-
-	case 115200: speed = B115200; break;
-
-	case 230400: speed = B230400; break;
-
-#ifndef B460800
-#define B460800 460800
-#endif
-
-	case 460800: speed = B460800; break;
-
-#ifndef B921600
-#define B921600 921600
-#endif
-
-	case 921600: speed = B921600; break;
-
-	default:
-		PX4_ERR("ERR: unknown baudrate: %d", _baudrate);
-		return false;
-	}
-
-	struct termios uart_config;
-
-	int termios_state;
-
-	/* fill the struct for the new configuration */
-	if ((termios_state = tcgetattr(_serial_fd, &uart_config)) < 0) {
-		PX4_ERR("ERR: %d (tcgetattr)", termios_state);
-		return false;
-	}
-
-	/* properly configure the terminal (see also https://en.wikibooks.org/wiki/Serial_Programming/termios ) */
-
-	//
-	// Input flags - Turn off input processing
-	//
-	// convert break to null byte, no CR to NL translation,
-	// no NL to CR translation, don't mark parity errors or breaks
-	// no input parity check, don't strip high bit off,
-	// no XON/XOFF software flow control
-	//
-	uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL |
-				 INLCR | PARMRK | INPCK | ISTRIP | IXON);
-
-	//
-	// Output flags - Turn off output processing
-	//
-	// no CR to NL translation, no NL to CR-NL translation,
-	// no NL to CR translation, no column 0 CR suppression,
-	// no Ctrl-D suppression, no fill characters, no case mapping,
-	// no local output processing
-	//
-	// config.c_oflag &= ~(OCRNL | ONLCR | ONLRET |
-	//                     ONOCR | ONOEOT| OFILL | OLCUC | OPOST);
-	uart_config.c_oflag = 0;
-
-	//
-	// No line processing
-	//
-	// echo off, echo newline off, canonical mode off,
-	// extended input processing off, signal chars off
-	//
-	uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
-
-	/* no parity, one stop bit, disable flow control */
-	uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS);
-
-	/* set baud rate */
-	if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
-		PX4_ERR("ERR: %d (cfsetispeed)", termios_state);
-		return false;
-	}
-
-	if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
-		PX4_ERR("ERR: %d (cfsetospeed)", termios_state);
-		return false;
-	}
-
-	if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) {
-		PX4_ERR("ERR: %d (tcsetattr)", termios_state);
-		return false;
-	}
-
-	return true;
-}
-
-bool SerialImpl::open()
-{
-	if (isOpen()) {
-		return true;
-	}
-
-	// Open the serial port
-	int serial_fd = ::open(_port, O_RDWR | O_NOCTTY);
-
-	if (serial_fd < 0) {
-		PX4_ERR("failed to open %s err: %d", _port, errno);
-		return false;
-	}
-
-	_serial_fd = serial_fd;
-
-	// Configure the serial port
-	if (! configure()) {
-		PX4_ERR("failed to configure %s err: %d", _port, errno);
-		return false;
-	}
-
-	_open = true;
-
-	return _open;
-}
-
-bool SerialImpl::isOpen() const
-{
-	return _open;
-}
-
-bool SerialImpl::close()
-{
-
-	if (_serial_fd >= 0) {
-		::close(_serial_fd);
-	}
-
-	_serial_fd = -1;
-	_open = false;
-
-	return true;
-}
-
-ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size)
-{
-	if (!_open) {
-		PX4_ERR("Cannot read from serial device until it has been opened");
-		return -1;
-	}
-
-	int ret = ::read(_serial_fd, buffer, buffer_size);
-
-	if (ret < 0) {
-		PX4_DEBUG("%s read error %d", _port, ret);
-
-	}
-
-	return ret;
-}
-
-ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us)
-{
-	if (!_open) {
-		PX4_ERR("Cannot readAtLeast from serial device until it has been opened");
-		return -1;
-	}
-
-	if (buffer_size < character_count) {
-		PX4_ERR("%s: Buffer not big enough to hold desired amount of read data", __FUNCTION__);
-		return -1;
-	}
-
-	const hrt_abstime start_time_us = hrt_absolute_time();
-	int total_bytes_read = 0;
-
-	while ((total_bytes_read < (int) character_count) && (hrt_elapsed_time(&start_time_us) < timeout_us)) {
-		// Poll for incoming UART data.
-		pollfd fds[1];
-		fds[0].fd = _serial_fd;
-		fds[0].events = POLLIN;
-
-		hrt_abstime remaining_time = timeout_us - hrt_elapsed_time(&start_time_us);
-
-		if (remaining_time <= 0) { break; }
-
-		int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), remaining_time);
-
-		if (ret > 0) {
-			if (fds[0].revents & POLLIN) {
-				const unsigned sleeptime = character_count * 1000000 / (_baudrate / 10);
-				px4_usleep(sleeptime);
-
-				ret = read(&buffer[total_bytes_read], buffer_size - total_bytes_read);
-
-				if (ret > 0) {
-					total_bytes_read += ret;
-				}
-
-			} else {
-				PX4_ERR("Got a poll error");
-				return -1;
-			}
-		}
-	}
-
-	return total_bytes_read;
-}
-
-ssize_t SerialImpl::write(const void *buffer, size_t buffer_size)
-{
-	if (!_open) {
-		PX4_ERR("Cannot write to serial device until it has been opened");
-		return -1;
-	}
-
-	int written = ::write(_serial_fd, buffer, buffer_size);
-	::fsync(_serial_fd);
-
-	if (written < 0) {
-		PX4_ERR("%s write error %d", _port, written);
-
-	}
-
-	return written;
-}
-
-const char *SerialImpl::getPort() const
-{
-	return _port;
-}
-
-uint32_t SerialImpl::getBaudrate() const
-{
-	return _baudrate;
-}
-
-bool SerialImpl::setBaudrate(uint32_t baudrate)
-{
-	if (! validateBaudrate(baudrate)) {
-		PX4_ERR("ERR: invalid baudrate: %u", baudrate);
-		return false;
-	}
-
-	// check if already configured
-	if ((baudrate == _baudrate) && _open) {
-		return true;
-	}
-
-	_baudrate = baudrate;
-
-	// process baud rate change now if port is already open
-	if (_open) {
-		return configure();
-	}
-
-	return true;
-}
-
-ByteSize SerialImpl::getBytesize() const
-{
-	return _bytesize;
-}
-
-bool SerialImpl::setBytesize(ByteSize bytesize)
-{
-	return bytesize == ByteSize::EightBits;
-}
-
-Parity SerialImpl::getParity() const
-{
-	return _parity;
-}
-
-bool SerialImpl::setParity(Parity parity)
-{
-	return parity == Parity::None;
-}
-
-StopBits SerialImpl::getStopbits() const
-{
-	return _stopbits;
-}
-
-bool SerialImpl::setStopbits(StopBits stopbits)
-{
-	return stopbits == StopBits::One;
-}
-
-FlowControl SerialImpl::getFlowcontrol() const
-{
-	return _flowcontrol;
-}
-
-bool SerialImpl::setFlowcontrol(FlowControl flowcontrol)
-{
-	return flowcontrol == FlowControl::Disabled;
-}
-
-} // namespace device

platforms/posix/src/px4/common/drv_hrt.cpp

@@ -51,11 +51,6 @@
 #include <errno.h>
 #include "hrt_work.h"
 
-// Voxl2 board specific API definitions to get time offset
-#if defined(CONFIG_MUORB_APPS_SYNC_TIMESTAMP)
-#include "fc_sensor.h"
-#endif
-
 #if defined(ENABLE_LOCKSTEP_SCHEDULER)
 #include <lockstep_scheduler/lockstep_scheduler.h>
 static LockstepScheduler lockstep_scheduler {true};
@@ -112,29 +107,6 @@ hrt_abstime hrt_absolute_time()
 #else // defined(ENABLE_LOCKSTEP_SCHEDULER)
 	struct timespec ts;
 	px4_clock_gettime(CLOCK_MONOTONIC, &ts);
-
-# if defined(CONFIG_MUORB_APPS_SYNC_TIMESTAMP)
-	hrt_abstime temp_abstime = ts_to_abstime(&ts);
-	int apps_time_offset = fc_sensor_get_time_offset();
-
-	if (apps_time_offset < 0) {
-		hrt_abstime temp_offset = -apps_time_offset;
-
-		if (temp_offset >= temp_abstime) {
-			temp_abstime = 0;
-
-		} else {
-			temp_abstime -= temp_offset;
-		}
-
-	} else {
-		temp_abstime += (hrt_abstime) apps_time_offset;
-	}
-
-	ts.tv_sec = temp_abstime / 1000000;
-	ts.tv_nsec = (temp_abstime % 1000000) * 1000;
-# endif // defined(CONFIG_MUORB_APPS_SYNC_TIMESTAMP)
-
 	return ts_to_abstime(&ts);
 #endif // defined(ENABLE_LOCKSTEP_SCHEDULER)
 }
@@ -477,7 +449,6 @@ int px4_clock_gettime(clockid_t clk_id, struct timespec *tp)
 
 #endif // defined(ENABLE_LOCKSTEP_SCHEDULER)
 	return system_clock_gettime(clk_id, tp);
-
 }
 
 #if defined(ENABLE_LOCKSTEP_SCHEDULER)

platforms/qurt/CMakeLists.txt

@@ -50,4 +50,5 @@ add_library(px4 SHARED
 target_link_libraries(px4
 	modules__muorb__slpi
 	${module_libraries}
+	px4_layer
 	)

platforms/qurt/include/SerialImpl.hpp

@@ -1,108 +0,0 @@
-/****************************************************************************
- *
- *   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
- * 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 <unistd.h>
-
-#include <px4_platform_common/SerialCommon.hpp>
-
-using device::SerialConfig::ByteSize;
-using device::SerialConfig::Parity;
-using device::SerialConfig::StopBits;
-using device::SerialConfig::FlowControl;
-
-namespace device
-{
-
-class SerialImpl
-{
-public:
-
-	SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
-		   FlowControl flowcontrol);
-	virtual ~SerialImpl();
-
-	bool open();
-	bool isOpen() const;
-
-	bool close();
-
-	ssize_t read(uint8_t *buffer, size_t buffer_size);
-	ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0);
-
-	ssize_t write(const void *buffer, size_t buffer_size);
-
-	const char *getPort() const;
-	bool setPort(const char *port);
-
-	uint32_t getBaudrate() const;
-	bool setBaudrate(uint32_t baudrate);
-
-	ByteSize getBytesize() const;
-	bool setBytesize(ByteSize bytesize);
-
-	Parity getParity() const;
-	bool setParity(Parity parity);
-
-	StopBits getStopbits() const;
-	bool setStopbits(StopBits stopbits);
-
-	FlowControl getFlowcontrol() const;
-	bool setFlowcontrol(FlowControl flowcontrol);
-
-private:
-
-	int _serial_fd{-1};
-
-	bool _open{false};
-
-	char _port[32] {};
-
-	uint32_t _baudrate{0};
-
-	ByteSize _bytesize{ByteSize::EightBits};
-	Parity _parity{Parity::None};
-	StopBits _stopbits{StopBits::One};
-	FlowControl _flowcontrol{FlowControl::Disabled};
-
-	bool validateBaudrate(uint32_t baudrate);
-
-	// Mutex used to lock the read functions
-	//pthread_mutex_t read_mutex;
-
-	// Mutex used to lock the write functions
-	//pthread_mutex_t write_mutex;
-};
-
-} // namespace device

platforms/qurt/src/px4/CMakeLists.txt

@@ -38,7 +38,6 @@ set(QURT_LAYER_SRCS
 	px4_qurt_impl.cpp
 	main.cpp
 	qurt_log.cpp
-	SerialImpl.cpp
 	)
 
 add_library(px4_layer

platforms/qurt/src/px4/SerialImpl.cpp

@@ -1,326 +0,0 @@
-/****************************************************************************
- *
- *   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
- * 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 <SerialImpl.hpp>
-#include <string.h> // strncpy
-#include <px4_log.h>
-#include <drivers/device/qurt/uart.h>
-#include <drivers/drv_hrt.h>
-
-namespace device
-{
-
-SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
-		       FlowControl flowcontrol) :
-	_baudrate(baudrate),
-	_bytesize(bytesize),
-	_parity(parity),
-	_stopbits(stopbits),
-	_flowcontrol(flowcontrol)
-{
-	if (port) {
-		strncpy(_port, port, sizeof(_port) - 1);
-		_port[sizeof(_port) - 1] = '\0';
-
-	} else {
-		_port[0] = 0;
-	}
-}
-
-SerialImpl::~SerialImpl()
-{
-	if (isOpen()) {
-		close();
-	}
-}
-
-bool SerialImpl::validateBaudrate(uint32_t baudrate)
-{
-	if ((baudrate != 9600)   &&
-	    (baudrate != 38400)  &&
-	    (baudrate != 57600)  &&
-	    (baudrate != 115200) &&
-	    (baudrate != 230400) &&
-	    (baudrate != 250000) &&
-	    (baudrate != 420000) &&
-	    (baudrate != 460800) &&
-	    (baudrate != 921600) &&
-	    (baudrate != 1000000) &&
-	    (baudrate != 1843200) &&
-	    (baudrate != 2000000)) {
-		return false;
-	}
-
-	return true;
-}
-
-bool SerialImpl::open()
-{
-	// There's no harm in calling open multiple times on the same port.
-	// In fact, that's the only way to change the baudrate
-
-	_open = false;
-	_serial_fd = -1;
-
-	if (! validateBaudrate(_baudrate)) {
-		PX4_ERR("Invalid baudrate: %u", _baudrate);
-		return false;
-	}
-
-	if (_bytesize != ByteSize::EightBits) {
-		PX4_ERR("Qurt platform only supports ByteSize::EightBits");
-		return false;
-	}
-
-	if (_parity != Parity::None) {
-		PX4_ERR("Qurt platform only supports Parity::None");
-		return false;
-	}
-
-	if (_stopbits != StopBits::One) {
-		PX4_ERR("Qurt platform only supports StopBits::One");
-		return false;
-	}
-
-	if (_flowcontrol != FlowControl::Disabled) {
-		PX4_ERR("Qurt platform only supports FlowControl::Disabled");
-		return false;
-	}
-
-	// qurt_uart_open will check validity of port and baudrate
-	int serial_fd = qurt_uart_open(_port, _baudrate);
-
-	if (serial_fd < 0) {
-		PX4_ERR("failed to open %s, fd returned: %d", _port, serial_fd);
-		return false;
-
-	} else {
-		PX4_INFO("Successfully opened UART %s with baudrate %u", _port, _baudrate);
-	}
-
-	_serial_fd = serial_fd;
-	_open = true;
-
-	return _open;
-}
-
-bool SerialImpl::isOpen() const
-{
-	return _open;
-}
-
-bool SerialImpl::close()
-{
-	// No close defined for qurt uart yet
-	// if (_serial_fd >= 0) {
-	// 	qurt_uart_close(_serial_fd);
-	// }
-
-	_serial_fd = -1;
-	_open = false;
-
-	return true;
-}
-
-ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size)
-{
-	if (!_open) {
-		PX4_ERR("Cannot read from serial device until it has been opened");
-		return -1;
-	}
-
-	int ret_read = qurt_uart_read(_serial_fd, (char *) buffer, buffer_size, 500);
-
-	if (ret_read < 0) {
-		PX4_DEBUG("%s read error %d", _port, ret_read);
-
-	}
-
-	return ret_read;
-}
-
-ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us)
-{
-	if (!_open) {
-		PX4_ERR("Cannot readAtLeast from serial device until it has been opened");
-		return -1;
-	}
-
-	if (buffer_size < character_count) {
-		PX4_ERR("%s: Buffer not big enough to hold desired amount of read data", __FUNCTION__);
-		return -1;
-	}
-
-	const hrt_abstime start_time_us = hrt_absolute_time();
-	int total_bytes_read = 0;
-
-	while (total_bytes_read < (int) character_count) {
-
-		if (timeout_us > 0) {
-			const uint64_t elapsed_us = hrt_elapsed_time(&start_time_us);
-
-			if (elapsed_us >= timeout_us) {
-				// If there was a partial read but not enough to satisfy the minimum then they will be lost
-				// but this really should never happen when everything is working normally.
-				// PX4_WARN("%s timeout %d bytes read (%llu us elapsed)", __FUNCTION__, total_bytes_read, elapsed_us);
-				// Or, instead of returning an error, should we return the number of bytes read (assuming it is greater than zero)?
-				return total_bytes_read;
-			}
-		}
-
-		int current_bytes_read = read(&buffer[total_bytes_read], buffer_size - total_bytes_read);
-
-		if (current_bytes_read < 0) {
-			// Again, if there was a partial read but not enough to satisfy the minimum then they will be lost
-			// but this really should never happen when everything is working normally.
-			PX4_ERR("%s failed to read uart", __FUNCTION__);
-			// Or, instead of returning an error, should we return the number of bytes read (assuming it is greater than zero)?
-			return -1;
-		}
-
-		// Current bytes read could be zero
-		total_bytes_read += current_bytes_read;
-
-		// If we have at least reached our desired minimum number of characters
-		// then we can return now
-		if (total_bytes_read >= (int) character_count) {
-			return total_bytes_read;
-		}
-
-		// Wait a set amount of time before trying again or the remaining time
-		// until the timeout if we are getting close
-		const uint64_t elapsed_us = hrt_elapsed_time(&start_time_us);
-		int64_t time_until_timeout = timeout_us - elapsed_us;
-		uint64_t time_to_sleep = 5000;
-
-		if ((time_until_timeout >= 0) &&
-		    (time_until_timeout < (int64_t) time_to_sleep)) {
-			time_to_sleep = time_until_timeout;
-		}
-
-		px4_usleep(time_to_sleep);
-	}
-
-	return -1;
-}
-
-ssize_t SerialImpl::write(const void *buffer, size_t buffer_size)
-{
-	if (!_open) {
-		PX4_ERR("Cannot write to serial device until it has been opened");
-		return -1;
-	}
-
-	int ret_write = qurt_uart_write(_serial_fd, (const char *) buffer, buffer_size);
-
-	if (ret_write < 0) {
-		PX4_ERR("%s write error %d", _port, ret_write);
-
-	}
-
-	return ret_write;
-}
-
-const char *SerialImpl::getPort() const
-{
-	return _port;
-}
-
-uint32_t SerialImpl::getBaudrate() const
-{
-	return _baudrate;
-}
-
-bool SerialImpl::setBaudrate(uint32_t baudrate)
-{
-	if (! validateBaudrate(baudrate)) {
-		PX4_ERR("Invalid baudrate: %u", baudrate);
-		return false;
-	}
-
-	// check if already configured
-	if (baudrate == _baudrate) {
-		return true;
-	}
-
-	_baudrate = baudrate;
-
-	// process baud rate change now if port is already open
-	if (_open) {
-		return open();
-	}
-
-	return true;
-}
-
-ByteSize SerialImpl::getBytesize() const
-{
-	return _bytesize;
-}
-
-bool SerialImpl::setBytesize(ByteSize bytesize)
-{
-	return bytesize == ByteSize::EightBits;
-}
-
-Parity SerialImpl::getParity() const
-{
-	return _parity;
-}
-
-bool SerialImpl::setParity(Parity parity)
-{
-	return parity == Parity::None;
-}
-
-StopBits SerialImpl::getStopbits() const
-{
-	return _stopbits;
-}
-
-bool SerialImpl::setStopbits(StopBits stopbits)
-{
-	return stopbits == StopBits::One;
-}
-
-FlowControl SerialImpl::getFlowcontrol() const
-{
-	return _flowcontrol;
-}
-
-bool SerialImpl::setFlowcontrol(FlowControl flowcontrol)
-{
-	return flowcontrol == FlowControl::Disabled;
-}
-
-} // namespace device

platforms/qurt/src/px4/drv_hrt.cpp

@@ -81,7 +81,7 @@ static void hrt_unlock()
 	px4_sem_post(&_hrt_lock);
 }
 
-int px4_clock_settime(clockid_t clk_id, const struct timespec *tp)
+int px4_clock_settime(clockid_t clk_id, struct timespec *tp)
 {
 	return 0;
 }

src/drivers/drv_dshot.h

@@ -91,7 +91,7 @@ typedef enum {
  * @param dshot_pwm_freq	Frequency of DSHOT signal. Usually DSHOT150, DSHOT300, DSHOT600 or DSHOT1200
  * @return <0 on error, the initialized channels mask.
  */
-__EXPORT extern int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq);
+__EXPORT extern int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bidirectional_dshot);
 
 /**
  * Set Dshot motor data, used by up_dshot_motor_data_set() and up_dshot_motor_command() (internal method)
@@ -137,4 +137,19 @@ __EXPORT extern void up_dshot_trigger(void);
  */
 __EXPORT extern int up_dshot_arm(bool armed);
 
+/**
+ * Print bidrectional dshot status
+ */
+__EXPORT extern void up_bdshot_status(void);
+
+
+/**
+ * Get bidrectional dshot erpm for a channel
+ * @param channel	Dshot channel
+ * @param erpm		pointer to write the erpm value
+ * @return <0 on error, OK on succes
+ */
+__EXPORT extern int up_bdshot_get_erpm(uint8_t channel, int *erpm);
+
+
 __END_DECLS

src/drivers/drv_hrt.h

@@ -162,16 +162,7 @@ static inline void abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
  */
 static inline hrt_abstime hrt_elapsed_time(const hrt_abstime *then)
 {
-	hrt_abstime now = hrt_absolute_time();
-
-	// Cannot allow a negative elapsed time as this would appear
-	// to be a huge positive elapsed time when represented as an
-	// unsigned value!
-	if (*then > now) {
-		return 0;
-	}
-
-	return now - *then;
+	return hrt_absolute_time() - *then;
 }
 
 /**

src/drivers/dshot/DShot.cpp

@@ -144,7 +144,9 @@ void DShot::enable_dshot_outputs(const bool enabled)
 			}
 		}
 
-		int ret = up_dshot_init(_output_mask, dshot_frequency);
+		_bdshot = _param_bidirectional_enable.get();
+
+		int ret = up_dshot_init(_output_mask, dshot_frequency, _bdshot);
 
 		if (ret < 0) {
 			PX4_ERR("up_dshot_init failed (%i)", ret);
@@ -167,6 +169,10 @@ void DShot::enable_dshot_outputs(const bool enabled)
 		}
 
 		_outputs_initialized = true;
+
+		if (_bdshot) {
+			init_telemetry(NULL);
+		}
 	}
 
 	if (_outputs_initialized) {
@@ -206,17 +212,20 @@ void DShot::init_telemetry(const char *device)
 
 	_telemetry->esc_status_pub.advertise();
 
-	int ret = _telemetry->handler.init(device);
+	if (device != NULL) {
+		int ret = _telemetry->handler.init(device);
 
-	if (ret != 0) {
-		PX4_ERR("telemetry init failed (%i)", ret);
+		if (ret != 0) {
+			PX4_ERR("telemetry init failed (%i)", ret);
+		}
 	}
 
 	update_telemetry_num_motors();
 }
 
-void DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data)
+int DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data)
 {
+	int ret = 0;
 	// fill in new motor data
 	esc_status_s &esc_status = _telemetry->esc_status_pub.get();
 
@@ -243,14 +252,59 @@ void DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTele
 		esc_status.esc_online_flags = (1 << esc_status.esc_count) - 1;
 		esc_status.esc_armed_flags = (1 << esc_status.esc_count) - 1;
 
-		_telemetry->esc_status_pub.update();
-
-		// reset esc data (in case a motor times out, so we won't send stale data)
-		memset(&esc_status.esc, 0, sizeof(_telemetry->esc_status_pub.get().esc));
-		esc_status.esc_online_flags = 0;
+		ret = 1; // Indicate we wrapped, so we publish data
 	}
 
 	_telemetry->last_telemetry_index = telemetry_index;
+
+	return ret;
+}
+
+void DShot::publish_esc_status(void)
+{
+	esc_status_s &esc_status = _telemetry->esc_status_pub.get();
+
+	// clear data of the esc that are offline
+	for (uint8_t channel = 0; (channel < _telemetry->last_telemetry_index); channel++) {
+		if ((esc_status.esc_online_flags & (1 << channel)) == 0) {
+			memset(&esc_status.esc[channel], 0, sizeof(struct esc_report_s));
+		}
+	}
+
+	// ESC telem wrap around or bdshot update
+	_telemetry->esc_status_pub.update();
+
+	// reset esc online flags
+	esc_status.esc_online_flags = 0;
+}
+
+int DShot::handle_new_bdshot_erpm(void)
+{
+	int num_erpms = 0;
+	int erpm;
+	uint8_t channel;
+	esc_status_s &esc_status = _telemetry->esc_status_pub.get();
+
+	esc_status.timestamp = hrt_absolute_time();
+	esc_status.counter = _esc_status_counter++;
+	esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_DSHOT;
+	esc_status.esc_armed_flags = _outputs_on;
+
+	for (channel = 0; channel < 8; channel++) {
+		if (up_bdshot_get_erpm(channel, &erpm) == 0) {
+			num_erpms++;
+			esc_status.esc_online_flags |= 1 << channel;
+			esc_status.esc[channel].timestamp = hrt_absolute_time();
+			esc_status.esc[channel].esc_rpm = (erpm * 100) /
+							  (_param_mot_pole_count.get() / 2);
+			esc_status.esc[channel].actuator_function = _telemetry->actuator_functions[channel];
+		}
+
+	}
+
+	esc_status.esc_count = num_erpms;
+
+	return num_erpms;
 }
 
 int DShot::send_command_thread_safe(const dshot_command_t command, const int num_repetitions, const int motor_index)
@@ -463,6 +517,7 @@ void DShot::Run()
 
 	if (_telemetry) {
 		int telem_update = _telemetry->handler.update();
+		int need_to_publish = 0;
 
 		// Are we waiting for ESC info?
 		if (_waiting_for_esc_info) {
@@ -472,10 +527,21 @@ void DShot::Run()
 			}
 
 		} else if (telem_update >= 0) {
-			handle_new_telemetry_data(telem_update, _telemetry->handler.latestESCData());
+			need_to_publish = handle_new_telemetry_data(telem_update, _telemetry->handler.latestESCData());
+		}
+
+		if (_bdshot) {
+			// Add bdshot data to esc status
+			need_to_publish += handle_new_bdshot_erpm();
+		}
+
+		if (need_to_publish > 0) {
+			// ESC telem wrap around or bdshot update
+			publish_esc_status();
 		}
 	}
 
+
 	if (_parameter_update_sub.updated()) {
 		update_params();
 	}
@@ -713,6 +779,9 @@ int DShot::print_status()
 		_telemetry->handler.printStatus();
 	}
 
+	/* Print dshot status */
+	up_bdshot_status();
+
 	return 0;
 }
 

src/drivers/dshot/DShot.h

@@ -131,7 +131,11 @@ private:
 
 	void init_telemetry(const char *device);
 
-	void handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data);
+	int handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data);
+
+	void publish_esc_status(void);
+
+	int handle_new_bdshot_erpm(void);
 
 	int request_esc_info();
 
@@ -158,6 +162,7 @@ private:
 	bool _outputs_initialized{false};
 	bool _outputs_on{false};
 	bool _waiting_for_esc_info{false};
+	bool _bdshot{false};
 
 	static constexpr unsigned _num_outputs{DIRECT_PWM_OUTPUT_CHANNELS};
 	uint32_t _output_mask{0};
@@ -169,12 +174,14 @@ private:
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
 	uORB::Publication<vehicle_command_ack_s> _command_ack_pub{ORB_ID(vehicle_command_ack)};
+	uint16_t _esc_status_counter{0};
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::DSHOT_MIN>)    _param_dshot_min,
 		(ParamBool<px4::params::DSHOT_3D_ENABLE>) _param_dshot_3d_enable,
 		(ParamInt<px4::params::DSHOT_3D_DEAD_H>) _param_dshot_3d_dead_h,
 		(ParamInt<px4::params::DSHOT_3D_DEAD_L>) _param_dshot_3d_dead_l,
-		(ParamInt<px4::params::MOT_POLE_COUNT>) _param_mot_pole_count
+		(ParamInt<px4::params::MOT_POLE_COUNT>) _param_mot_pole_count,
+		(ParamBool<px4::params::DSHOT_BIDIR_EN>) _param_bidirectional_enable
 	)
 };

src/drivers/dshot/DShotTelemetry.cpp

@@ -183,6 +183,9 @@ bool DShotTelemetry::decodeByte(uint8_t byte, bool &successful_decoding)
 				  _latest_data.erpm);
 			++_num_successful_responses;
 			successful_decoding = true;
+
+		} else {
+			++_num_checksum_errors;
 		}
 
 		return true;
@@ -195,6 +198,7 @@ void DShotTelemetry::printStatus() const
 {
 	PX4_INFO("Number of successful ESC frames: %i", _num_successful_responses);
 	PX4_INFO("Number of timeouts: %i", _num_timeouts);
+	PX4_INFO("Number of CRC errors: %i", _num_checksum_errors);
 }
 
 uint8_t DShotTelemetry::updateCrc8(uint8_t crc, uint8_t crc_seed)

src/drivers/dshot/DShotTelemetry.h

@@ -140,4 +140,5 @@ private:
 	// statistics
 	int _num_timeouts{0};
 	int _num_successful_responses{0};
+	int _num_checksum_errors{0};
 };

src/drivers/dshot/module.yaml

@@ -33,6 +33,16 @@ parameters:
                     When mixer outputs 1000 or value inside DSHOT 3D deadband, DShot 0 is sent.
             type: boolean
             default: 0
+        DSHOT_BIDIR_EN:
+            description:
+                short: Enable bidirectional DShot
+                long: |
+                    This parameter enables bidirectional DShot which provides RPM feedback.
+                    Note that this requires ESCs that support bidirectional DSHot, e.g. BlHeli32.
+                    This is not the same as DShot telemetry which requires an additional serial connection.
+            type: boolean
+            default: 0
+            reboot_required: true
         DSHOT_3D_DEAD_H:
             description:
                 short: DSHOT 3D deadband high

src/drivers/gps/gps.cpp

@@ -45,6 +45,7 @@
 #include <poll.h>
 #endif
 
+#include <termios.h>
 #include <cstring>
 
 #include <drivers/drv_sensor.h>
@@ -56,8 +57,6 @@
 #include <px4_platform_common/cli.h>
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/module.h>
-#include <px4_platform_common/time.h>
-#include <px4_platform_common/Serial.hpp>
 #include <uORB/Publication.hpp>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/Subscription.hpp>
@@ -82,7 +81,6 @@
 #include <linux/spi/spidev.h>
 #endif /* __PX4_LINUX */
 
-using namespace device;
 using namespace time_literals;
 
 #define TIMEOUT_1HZ		1300	//!< Timeout time in mS, 1000 mS (1Hz) + 300 mS delta for error
@@ -171,10 +169,7 @@ public:
 	void reset_if_scheduled();
 
 private:
-#ifdef __PX4_LINUX
-	int				_spi_fd {-1};					///< SPI interface to GPS
-#endif
-	Serial 			*_uart = nullptr;				///< UART interface to GPS
+	int				_serial_fd{-1};					///< serial interface to GPS
 	unsigned			_baudrate{0};					///< current baudrate
 	const unsigned			_configured_baudrate{0};			///< configured baudrate (0=auto-detect)
 	char				_port[20] {};					///< device / serial port path
@@ -334,11 +329,8 @@ GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interfac
 		char c = _port[strlen(_port) - 1]; // last digit of path (eg /dev/ttyS2)
 		set_device_bus(c - 48); // sub 48 to convert char to integer
 
-#ifdef __PX4_LINUX
-
 	} else if (_interface == GPSHelper::Interface::SPI) {
 		set_device_bus_type(device::Device::DeviceBusType::DeviceBusType_SPI);
-#endif
 	}
 
 	if (_mode == gps_driver_mode_t::None) {
@@ -411,23 +403,10 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user)
 			return num_read;
 		}
 
-	case GPSCallbackType::writeDeviceData: {
-			gps->dumpGpsData((uint8_t *)data1, (size_t)data2, gps_dump_comm_mode_t::Full, true);
+	case GPSCallbackType::writeDeviceData:
+		gps->dumpGpsData((uint8_t *)data1, (size_t)data2, gps_dump_comm_mode_t::Full, true);
 
-			int ret = 0;
-
-			if (gps->_uart) {
-				ret = gps->_uart->write((void *) data1, (size_t) data2);
-
-#ifdef __PX4_LINUX
-
-			} else if (gps->_spi_fd >= 0) {
-				ret = ::write(gps->_spi_fd, data1, (size_t)data2);
-#endif
-			}
-
-			return ret;
-		}
+		return ::write(gps->_serial_fd, data1, (size_t)data2);
 
 	case GPSCallbackType::setBaudrate:
 		return gps->setBaudrate(data2);
@@ -470,64 +449,72 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user)
 
 int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout)
 {
-	int ret = 0;
-	const unsigned character_count = 32; // minimum bytes that we want to read
-	const int max_timeout = 50;
-	int timeout_adjusted = math::min(max_timeout, timeout);
-
 	handleInjectDataTopic();
 
-	if ((_interface == GPSHelper::Interface::UART) && (_uart)) {
-		ret = _uart->readAtLeast(buf, buf_length, character_count, timeout_adjusted);
+#if !defined(__PX4_QURT)
 
-// SPI is only supported on LInux
-#if defined(__PX4_LINUX)
+	/* For non QURT, use the usual polling. */
 
-	} else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd >= 0)) {
+	//Poll only for the serial data. In the same thread we also need to handle orb messages,
+	//so ideally we would poll on both, the serial fd and orb subscription. Unfortunately the
+	//two pollings use different underlying mechanisms (at least under posix), which makes this
+	//impossible. Instead we limit the maximum polling interval and regularly check for new orb
+	//messages.
+	//FIXME: add a unified poll() API
+	const int max_timeout = 50;
 
-		//Poll only for the SPI data. In the same thread we also need to handle orb messages,
-		//so ideally we would poll on both, the SPI fd and orb subscription. Unfortunately the
-		//two pollings use different underlying mechanisms (at least under posix), which makes this
-		//impossible. Instead we limit the maximum polling interval and regularly check for new orb
-		//messages.
-		//FIXME: add a unified poll() API
+	pollfd fds[1];
+	fds[0].fd = _serial_fd;
+	fds[0].events = POLLIN;
 
-		pollfd fds[1];
-		fds[0].fd = _spi_fd;
-		fds[0].events = POLLIN;
+	int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), math::min(max_timeout, timeout));
 
-		ret = poll(fds, sizeof(fds) / sizeof(fds[0]), timeout_adjusted);
+	if (ret > 0) {
+		/* if we have new data from GPS, go handle it */
+		if (fds[0].revents & POLLIN) {
+			/*
+			 * We are here because poll says there is some data, so this
+			 * won't block even on a blocking device. But don't read immediately
+			 * by 1-2 bytes, wait for some more data to save expensive read() calls.
+			 * If we have all requested data available, read it without waiting.
+			 * If more bytes are available, we'll go back to poll() again.
+			 */
+			const unsigned character_count = 32; // minimum bytes that we want to read
+			unsigned baudrate = _baudrate == 0 ? 115200 : _baudrate;
+			const unsigned sleeptime = character_count * 1000000 / (baudrate / 10);
 
-		if (ret > 0) {
-			/* if we have new data from GPS, go handle it */
-			if (fds[0].revents & POLLIN) {
-				/*
-				 * We are here because poll says there is some data, so this
-				 * won't block even on a blocking device. But don't read immediately
-				 * by 1-2 bytes, wait for some more data to save expensive read() calls.
-				 * If we have all requested data available, read it without waiting.
-				 * If more bytes are available, we'll go back to poll() again.
-				 */
-				unsigned baudrate = _baudrate == 0 ? 115200 : _baudrate;
-				const unsigned sleeptime = character_count * 1000000 / (baudrate / 10);
+#ifdef __PX4_NUTTX
+			int err = 0;
+			int bytes_available = 0;
+			err = ::ioctl(_serial_fd, FIONREAD, (unsigned long)&bytes_available);
 
+			if (err != 0 || bytes_available < (int)character_count) {
 				px4_usleep(sleeptime);
-
-				ret = ::read(_spi_fd, buf, buf_length);
-
-				if (ret > 0) {
-					_num_bytes_read += ret;
-				}
-
-			} else {
-				ret = -1;
 			}
-		}
 
+#else
+			px4_usleep(sleeptime);
 #endif
+
+			ret = ::read(_serial_fd, buf, buf_length);
+
+			if (ret > 0) {
+				_num_bytes_read += ret;
+			}
+
+		} else {
+			ret = -1;
+		}
 	}
 
 	return ret;
+
+#else
+	/* For QURT, just use read for now, since this doesn't block, we need to slow it down
+	 * just a bit. */
+	px4_usleep(10000);
+	return ::read(_serial_fd, buf, buf_length);
+#endif
 }
 
 void GPS::handleInjectDataTopic()
@@ -596,38 +583,105 @@ bool GPS::injectData(uint8_t *data, size_t len)
 {
 	dumpGpsData(data, len, gps_dump_comm_mode_t::Full, true);
 
-	size_t written = 0;
-
-	if ((_interface == GPSHelper::Interface::UART) && (_uart)) {
-		written = _uart->write((const void *) data, len);
-
-#ifdef __PX4_LINUX
-
-	} else if (_interface == GPSHelper::Interface::SPI) {
-		written = ::write(_spi_fd, data, len);
-		::fsync(_spi_fd);
-#endif
-	}
-
+	size_t written = ::write(_serial_fd, data, len);
+	::fsync(_serial_fd);
 	return written == len;
 }
 
 int GPS::setBaudrate(unsigned baud)
 {
-	if (_interface == GPSHelper::Interface::UART) {
-		if ((_uart) && (_uart->setBaudrate(baud))) {
-			return 0;
-		}
+	/* process baud rate */
+	int speed;
 
-#ifdef __PX4_LINUX
+	switch (baud) {
+	case 9600:   speed = B9600;   break;
 
-	} else if (_interface == GPSHelper::Interface::SPI) {
-		// Can't set the baudrate on a SPI port but just return a success
-		return 0;
+	case 19200:  speed = B19200;  break;
+
+	case 38400:  speed = B38400;  break;
+
+	case 57600:  speed = B57600;  break;
+
+	case 115200: speed = B115200; break;
+
+	case 230400: speed = B230400; break;
+
+#ifndef B460800
+#define B460800 460800
 #endif
+
+	case 460800: speed = B460800; break;
+
+#ifndef B921600
+#define B921600 921600
+#endif
+
+	case 921600: speed = B921600; break;
+
+	default:
+		PX4_ERR("ERR: unknown baudrate: %d", baud);
+		return -EINVAL;
 	}
 
-	return -1;
+	struct termios uart_config;
+
+	int termios_state;
+
+	/* fill the struct for the new configuration */
+	tcgetattr(_serial_fd, &uart_config);
+
+	/* properly configure the terminal (see also https://en.wikibooks.org/wiki/Serial_Programming/termios ) */
+
+	//
+	// Input flags - Turn off input processing
+	//
+	// convert break to null byte, no CR to NL translation,
+	// no NL to CR translation, don't mark parity errors or breaks
+	// no input parity check, don't strip high bit off,
+	// no XON/XOFF software flow control
+	//
+	uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL |
+				 INLCR | PARMRK | INPCK | ISTRIP | IXON);
+	//
+	// Output flags - Turn off output processing
+	//
+	// no CR to NL translation, no NL to CR-NL translation,
+	// no NL to CR translation, no column 0 CR suppression,
+	// no Ctrl-D suppression, no fill characters, no case mapping,
+	// no local output processing
+	//
+	// config.c_oflag &= ~(OCRNL | ONLCR | ONLRET |
+	//                     ONOCR | ONOEOT| OFILL | OLCUC | OPOST);
+	uart_config.c_oflag = 0;
+
+	//
+	// No line processing
+	//
+	// echo off, echo newline off, canonical mode off,
+	// extended input processing off, signal chars off
+	//
+	uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
+
+	/* no parity, one stop bit, disable flow control */
+	uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS);
+
+	/* set baud rate */
+	if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
+		GPS_ERR("ERR: %d (cfsetispeed)", termios_state);
+		return -1;
+	}
+
+	if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
+		GPS_ERR("ERR: %d (cfsetospeed)", termios_state);
+		return -1;
+	}
+
+	if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) {
+		GPS_ERR("ERR: %d (tcsetattr)", termios_state);
+		return -1;
+	}
+
+	return 0;
 }
 
 void GPS::initializeCommunicationDump()
@@ -786,58 +840,31 @@ GPS::run()
 			_helper = nullptr;
 		}
 
-		if ((_interface == GPSHelper::Interface::UART) && (_uart == nullptr)) {
+		if (_serial_fd < 0) {
+			/* open the serial port */
+			_serial_fd = ::open(_port, O_RDWR | O_NOCTTY);
 
-			// Create the UART port instance
-			_uart = new Serial(_port);
-
-			if (_uart == nullptr) {
-				PX4_ERR("Error creating serial device %s", _port);
-				px4_sleep(1);
-				continue;
-			}
-		}
-
-		if ((_interface == GPSHelper::Interface::UART) && (! _uart->isOpen())) {
-			// Configure the desired baudrate if one was specified by the user.
-			// Otherwise the default baudrate will be used.
-			if (_configured_baudrate) {
-				if (! _uart->setBaudrate(_configured_baudrate)) {
-					PX4_ERR("Error setting baudrate to %u on %s", _configured_baudrate, _port);
-					px4_sleep(1);
-					continue;
-				}
-			}
-
-			// Open the UART. If this is successful then the UART is ready to use.
-			if (! _uart->open()) {
-				PX4_ERR("Error opening serial device  %s", _port);
+			if (_serial_fd < 0) {
+				PX4_ERR("failed to open %s err: %d", _port, errno);
 				px4_sleep(1);
 				continue;
 			}
 
 #ifdef __PX4_LINUX
 
-		} else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd < 0)) {
-			_spi_fd = ::open(_port, O_RDWR | O_NOCTTY);
+			if (_interface == GPSHelper::Interface::SPI) {
+				int spi_speed = 1000000; // make sure the bus speed is not too high (required on RPi)
+				int status_value = ::ioctl(_serial_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed);
 
-			if (_spi_fd < 0) {
-				PX4_ERR("failed to open SPI port %s err: %d", _port, errno);
-				px4_sleep(1);
-				continue;
-			}
+				if (status_value < 0) {
+					PX4_ERR("SPI_IOC_WR_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
+				}
 
-			int spi_speed = 1000000; // make sure the bus speed is not too high (required on RPi)
-			int status_value = ::ioctl(_spi_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed);
+				status_value = ::ioctl(_serial_fd, SPI_IOC_RD_MAX_SPEED_HZ, &spi_speed);
 
-			if (status_value < 0) {
-				PX4_ERR("SPI_IOC_WR_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
-			}
-
-			status_value = ::ioctl(_spi_fd, SPI_IOC_RD_MAX_SPEED_HZ, &spi_speed);
-
-			if (status_value < 0) {
-				PX4_ERR("SPI_IOC_RD_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
+				if (status_value < 0) {
+					PX4_ERR("SPI_IOC_RD_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
+				}
 			}
 
 #endif /* __PX4_LINUX */
@@ -1029,17 +1056,9 @@ GPS::run()
 			}
 		}
 
-		if ((_interface == GPSHelper::Interface::UART) && (_uart)) {
-			(void) _uart->close();
-			delete _uart;
-			_uart = nullptr;
-
-#ifdef __PX4_LINUX
-
-		} else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd >= 0)) {
-			::close(_spi_fd);
-			_spi_fd = -1;
-#endif
+		if (_serial_fd >= 0) {
+			::close(_serial_fd);
+			_serial_fd = -1;
 		}
 
 		if (_mode_auto) {
@@ -1387,7 +1406,7 @@ int GPS::task_spawn(int argc, char *argv[], Instance instance)
 				   entry_point, (char *const *)argv);
 
 	if (task_id < 0) {
-		_task_id = -1;
+		task_id = -1;
 		return -errno;
 	}
 
@@ -1458,12 +1477,12 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance)
 			break;
 
 		case 'i':
-			if (!strcmp(myoptarg, "uart")) {
-				interface = GPSHelper::Interface::UART;
-#ifdef __PX4_LINUX
-			} else if (!strcmp(myoptarg, "spi")) {
+			if (!strcmp(myoptarg, "spi")) {
 				interface = GPSHelper::Interface::SPI;
-#endif
+
+			} else if (!strcmp(myoptarg, "uart")) {
+				interface = GPSHelper::Interface::UART;
+
 			} else {
 				PX4_ERR("unknown interface: %s", myoptarg);
 				error_flag = true;
@@ -1471,12 +1490,12 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance)
 			break;
 
 		case 'j':
-			if (!strcmp(myoptarg, "uart")) {
-				interface_secondary = GPSHelper::Interface::UART;
-#ifdef __PX4_LINUX
-			} else if (!strcmp(myoptarg, "spi")) {
+			if (!strcmp(myoptarg, "spi")) {
 				interface_secondary = GPSHelper::Interface::SPI;
-#endif
+
+			} else if (!strcmp(myoptarg, "uart")) {
+				interface_secondary = GPSHelper::Interface::UART;
+
 			} else {
 				PX4_ERR("unknown interface for secondary: %s", myoptarg);
 				error_flag = true;

src/drivers/px4io/px4io.cpp

@@ -364,13 +364,6 @@ PX4IO::~PX4IO()
 bool PX4IO::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 			  unsigned num_outputs, unsigned num_control_groups_updated)
 {
-	for (size_t i = 0; i < num_outputs; i++) {
-		if (!_mixing_output.isFunctionSet(i)) {
-			// do not run any signal on disabled channels
-			outputs[i] = 0;
-		}
-	}
-
 	if (!_test_fmu_fail) {
 		/* output to the servos */
 		io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, outputs, num_outputs);

src/lib/mixer_module/mixer_module.cpp

@@ -455,8 +455,7 @@ bool MixingOutput::update()
 		}
 	}
 
-	// Send output if any function mapped or one last disabling sample
-	if (!all_disabled || !_was_all_disabled) {
+	if (!all_disabled) {
 		if (!_armed.armed && !_armed.manual_lockdown) {
 			_actuator_test.overrideValues(outputs, _max_num_outputs);
 		}
@@ -464,8 +463,6 @@ bool MixingOutput::update()
 		limitAndUpdateOutputs(outputs, has_updates);
 	}
 
-	_was_all_disabled = all_disabled;
-
 	return true;
 }
 

src/lib/mixer_module/mixer_module.hpp

@@ -288,7 +288,6 @@ private:
 	hrt_abstime _lowrate_schedule_interval{300_ms};
 	ActuatorTest _actuator_test{_function_assignment};
 	uint32_t _reversible_mask{0}; ///< per-output bits. If set, the output is configured to be reversible (motors only)
-	bool _was_all_disabled{false};
 
 	uORB::SubscriptionCallbackWorkItem *_subscription_callback{nullptr}; ///< current scheduling callback
 

src/lib/mixer_module/mixer_module_tests.cpp

@@ -191,15 +191,11 @@ TEST_F(MixerModuleTest, basic)
 	mixing_output.setAllMaxValues(MAX_VALUE);
 	EXPECT_EQ(test_module.num_updates, 0);
 
-	// all functions disabled: expect to get one single update to process disabling the output signal
+	// all functions disabled: not expected to get an update
 	mixing_output.update();
 	mixing_output.updateSubscriptions(false);
 	mixing_output.update();
-	EXPECT_EQ(test_module.num_updates, 1);
-	mixing_output.update();
-	mixing_output.updateSubscriptions(false);
-	mixing_output.update();
-	EXPECT_EQ(test_module.num_updates, 1);
+	EXPECT_EQ(test_module.num_updates, 0);
 	test_module.reset();
 
 	// configure motor, ensure all still disarmed

src/modules/muorb/apps/CMakeLists.txt

@@ -39,7 +39,7 @@ px4_add_module(
 	INCLUDES
 		../test
 		../aggregator
-		${PX4_BOARD_DIR}/libfc-sensor-api/inc
+		libfc-sensor-api/inc
 	SRCS
 		uORBAppsProtobufChannel.cpp
 		muorb_main.cpp

src/modules/muorb/apps/Kconfig

@@ -4,11 +4,3 @@ menuconfig MODULES_MUORB_APPS
 	depends on PLATFORM_POSIX
 	---help---
 		Enable support for muorb apps
-
-
-    config MUORB_APPS_SYNC_TIMESTAMP
-        bool "Sync timestamp with external processor"
-        depends on MODULES_MUORB_APPS
-        default y
-        help
-            causes HRT timestamp to use an externally calculated offset for synchronization

src/modules/px4iofirmware/mixer.cpp

@@ -180,19 +180,15 @@ mixer_tick()
 	 * Run the mixers.
 	 */
 	if (source == MIX_FAILSAFE) {
-		// Set failsafe value if the PWM output isn't disabled
+		/* copy failsafe values to the servo outputs */
 		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
-			if (r_page_servos[i] != 0) {
-				r_page_servos[i] = r_page_servo_failsafe[i];
-			}
+			r_page_servos[i] = r_page_servo_failsafe[i];
 		}
 
 	} else if (source == MIX_DISARMED) {
-		// Set disarmed value if the PWM output isn't disabled
+		/* copy disarmed values to the servo outputs */
 		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
-			if (r_page_servos[i] != 0) {
-				r_page_servos[i] = r_page_servo_disarmed[i];
-			}
+			r_page_servos[i] = r_page_servo_disarmed[i];
 		}
 	}