[WIP] use HRT for UAVCAN monotonic time and introduce HRT time sync

.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

boards/ark/fmu-v6x/default.px4board

@@ -33,7 +33,6 @@ CONFIG_DRIVERS_RC_INPUT=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_DRIVERS_UAVCAN=y
-CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y

boards/ark/septentrio-gps/default.px4board

@@ -20,7 +20,6 @@ CONFIG_UAVCANNODE_RTK_DATA=y
 CONFIG_UAVCANNODE_SAFETY_BUTTON=y
 CONFIG_UAVCANNODE_STATIC_PRESSURE=y
 CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y
-CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_SENSORS=y

boards/ark/septentrio-gps/init/rc.board_defaults

@@ -11,5 +11,3 @@ param set-default SENS_IMU_CLPNOTI 0
 
 safety_button start
 tone_alarm start
-
-ekf2 start

boards/ark/septentrio-gps/nuttx-config/canbootloader/defconfig

@@ -50,7 +50,6 @@ CONFIG_STACK_COLORATION=y
 CONFIG_START_DAY=30
 CONFIG_START_MONTH=11
 CONFIG_STDIO_DISABLE_BUFFERING=y
-CONFIG_STM32_FLASH_CONFIG_G=y
 CONFIG_STM32_NOEXT_VECTORS=y
 CONFIG_STM32_TIM8=y
 CONFIG_TASK_NAME_SIZE=0

boards/ark/septentrio-gps/nuttx-config/nsh/defconfig

@@ -123,7 +123,6 @@ CONFIG_STM32_ADC1=y
 CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y
 CONFIG_STM32_DMA1=y
 CONFIG_STM32_DMA2=y
-CONFIG_STM32_FLASH_CONFIG_G=y
 CONFIG_STM32_FLASH_PREFETCH=y
 CONFIG_STM32_FLOWCONTROL_BROKEN=y
 CONFIG_STM32_I2C1=y

boards/ark/septentrio-gps/nuttx-config/scripts/canbootloader_script.ld

@@ -33,7 +33,7 @@
  *
  ****************************************************************************/
 
-/* The STM32F412 has 1M of FLASH beginning at address 0x0800:0000 and
+/* The STM32F412 has 512Kb of FLASH beginning at address 0x0800:0000 and
  * 256Kb of SRAM. SRAM is split up into three blocks:
  *
  * 1) 112Kb of SRAM beginning at address 0x2000:0000

boards/ark/septentrio-gps/nuttx-config/scripts/script.ld

@@ -33,7 +33,7 @@
  *
  ****************************************************************************/
 
-/* The STM32F412 has 1M of FLASH beginning at address 0x0800:0000 and
+/* The STM32F412 has 512Kb of FLASH beginning at address 0x0800:0000 and
  * 256Kb of SRAM. SRAM is split up into three blocks:
  *
  * 1) 112Kb of SRAM beginning at address 0x2000:0000

boards/cuav/nora/default.px4board

@@ -35,7 +35,6 @@ CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_DRIVERS_UAVCAN=y
-CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_BATTERY_STATUS=y

boards/cuav/x7pro/default.px4board

@@ -36,7 +36,6 @@ CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_DRIVERS_UAVCAN=y
-CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_BATTERY_STATUS=y

boards/cuav/x7pro/test.px4board

@@ -1,4 +1,3 @@
-# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
 CONFIG_DRIVERS_ADC_ADS1115=n
 CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n
 CONFIG_DRIVERS_IRLOCK=n

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/default.px4board

@@ -11,6 +11,7 @@ CONFIG_DRIVERS_VOXL2_IO=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOGGER=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MUORB_APPS=y

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/modalai/voxl2/target/voxl-px4-hitl-start

@@ -76,6 +76,7 @@ qshell flight_mode_manager start
 # Start all of the processing modules on the applications processor
 dataman start
 navigator start
+load_mon start
 
 # Start microdds_client for ros2 offboard messages from agent over localhost
 microdds_client start -t udp -h 127.0.0.1 -p 8888

boards/modalai/voxl2/target/voxl-px4-start

@@ -207,6 +207,7 @@ qshell flight_mode_manager start
 # Start all of the processing modules on the applications processor
 dataman start
 navigator start
+load_mon start
 
 # This bridge allows raw data packets to be sent over UART to the ESC
 voxl2_io_bridge start

boards/mro/ctrl-zero-classic/default.px4board

@@ -49,6 +49,7 @@ CONFIG_MODULES_FW_POS_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
+CONFIG_MODULES_GYRO_FFT=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y

boards/px4/fmu-v5/cyphal.px4board

@@ -1,4 +1,3 @@
-# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
 CONFIG_DRIVERS_HEATER=n
 CONFIG_DRIVERS_UAVCAN=n
 CONFIG_CYPHAL_BMS_SUBSCRIBER=y

boards/px4/fmu-v5/debug.px4board

@@ -1,4 +1,3 @@
-# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
 CONFIG_COMMON_BAROMETERS=n
 CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n
 CONFIG_COMMON_MAGNETOMETER=n

boards/px4/fmu-v5/default.px4board

@@ -40,7 +40,6 @@ CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_DRIVERS_UAVCAN=y
-CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=6
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_BATTERY_STATUS=y

boards/px4/fmu-v5/protected.px4board

@@ -1,4 +1,3 @@
-# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
 CONFIG_COMMON_BAROMETERS=n
 CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n
 CONFIG_COMMON_DISTANCE_SENSOR=n

boards/px4/fmu-v5/stackcheck.px4board

@@ -1,4 +1,3 @@
-# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
 CONFIG_COMMON_BAROMETERS=n
 CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n
 CONFIG_COMMON_DISTANCE_SENSOR=n

boards/px4/fmu-v5/test.px4board

@@ -1,4 +1,3 @@
-# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
 CONFIG_COMMON_HYGROMETERS=n
 CONFIG_COMMON_OSD=n
 CONFIG_DRIVERS_ADC_ADS1115=n

boards/px4/fmu-v5/uavcanv0periph.px4board

@@ -1,4 +1,3 @@
-# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
 CONFIG_COMMON_BAROMETERS=n
 CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n
 CONFIG_COMMON_HYGROMETERS=n

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,7 +102,9 @@ CONFIG_SYSTEMCMDS_NSHTERM=y
 CONFIG_SYSTEMCMDS_PARAM=y
 CONFIG_SYSTEMCMDS_PERF=y
 CONFIG_SYSTEMCMDS_REBOOT=y
+CONFIG_SYSTEMCMDS_REFLECT=y
 CONFIG_SYSTEMCMDS_SD_BENCH=y
+CONFIG_SYSTEMCMDS_SD_STRESS=y
 CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
 CONFIG_SYSTEMCMDS_TOP=y
 CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y

boards/px4/fmu-v6c/default.px4board

@@ -30,7 +30,6 @@ CONFIG_DRIVERS_PX4IO=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_DRIVERS_UAVCAN=y
-CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y

boards/px4/fmu-v6x/default.px4board

@@ -42,7 +42,6 @@ CONFIG_DRIVERS_RC_INPUT=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_DRIVERS_UAVCAN=y
-CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y

boards/px4/fmu-v6x/zenoh.px4board

@@ -1,4 +1,3 @@
-# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
 CONFIG_DRIVERS_UAVCAN=n
 CONFIG_MODULES_UXRCE_DDS_CLIENT=n
 CONFIG_MODULES_ZENOH=y

cmake/kconfig.cmake

@@ -333,10 +333,6 @@ if(EXISTS ${BOARD_DEFCONFIG})
 		set(config_uavcan_num_ifaces ${UAVCAN_INTERFACES} CACHE INTERNAL "UAVCAN interfaces" FORCE)
 	endif()
 
-	if(UAVCAN_TIMER_OVERRIDE)
-		set(config_uavcan_timer_override ${UAVCAN_TIMER_OVERRIDE} CACHE INTERNAL "UAVCAN TIMER OVERRIDE" FORCE)
-	endif()
-
 	# OPTIONS
 
 	if(CONSTRAINED_FLASH)

msg/CMakeLists.txt

@@ -58,7 +58,6 @@ set(msg_files
 	CameraCapture.msg
 	CameraStatus.msg
 	CameraTrigger.msg
-	CanInterfaceStatus.msg
 	CellularStatus.msg
 	CollisionConstraints.msg
 	CollisionReport.msg

msg/CanInterfaceStatus.msg

@@ -1,6 +0,0 @@
-uint64 timestamp		# time since system start (microseconds)
-uint8 interface
-
-uint64 io_errors
-uint64 frames_tx
-uint64 frames_rx

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/SerialCommon.hpp

@@ -1,70 +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
-
-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

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/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/stm/stm32_common/hrt/hrt.c

@@ -244,6 +244,8 @@
 # error HRT_TIMER_CHANNEL must be a value between 1 and 4
 #endif
 
+static volatile hrt_abstime base_time;
+
 /*
  * Queue of callout entries.
  */
@@ -276,6 +278,112 @@ static void		hrt_call_reschedule(void);
 static void		hrt_call_invoke(void);
 
 
+#define HRT_TIME_SYNC
+
+#if defined(HRT_TIME_SYNC)
+
+#include <math.h>
+
+static bool sync_set = false;
+static bool sync_locked = false;
+static uint32_t sync_jump_cnt = 0;
+
+static float sync_prev_adj = 0;
+static float sync_rel_rate_ppm = 0;
+static float sync_rel_rate_error_integral = 0;
+static int32_t sync_accumulated_correction_nsec = 0;
+static int32_t sync_correction_nsec_per_overflow = 0;
+static hrt_abstime prev_sync_adj_at;
+
+static const float sync_offset_p = 0.01f;                 ///< PPM per one usec error
+static const float sync_rate_i = 0.02f;                   ///< PPM per one PPM error for second
+static const float sync_rate_error_corner_freq = 0.01f;
+static const float sync_max_rate_correction_ppm = 300.0f;
+static const float sync_lock_thres_rate_ppm = 2.f;
+static const hrt_abstime sync_lock_thres_offset = 4000;   ///< usec
+static const hrt_abstime sync_min_jump = 10000;           ///< Min error to jump rather than change rate
+
+static float lowpass(float xold, float xnew, float corner, float dt)
+{
+	const float tau = 1.F / corner;
+	return (dt * xnew + tau * xold) / (dt + tau);
+}
+
+static void updateRatePID(float adj_usec)
+{
+	const hrt_abstime ts = hrt_absolute_time();
+	const float dt = (ts - prev_sync_adj_at) / 1e6f;
+	prev_sync_adj_at = ts;
+
+	/*
+	 * Target relative rate in PPM
+	 * Positive to go faster
+	 */
+	const float target_rel_rate_ppm = adj_usec * sync_offset_p;
+
+	/*
+	 * Current relative rate in PPM
+	 * Positive if the local clock is faster
+	 */
+	const float new_rel_rate_ppm = (sync_prev_adj - adj_usec) / dt; // rate error in [usec/sec], which is PPM
+	sync_prev_adj = adj_usec;
+	sync_rel_rate_ppm = lowpass(sync_rel_rate_ppm, new_rel_rate_ppm, sync_rate_error_corner_freq, dt);
+
+	const float rel_rate_error = target_rel_rate_ppm - sync_rel_rate_ppm;
+
+	if (dt > 10) {
+		sync_rel_rate_error_integral = 0;
+
+	} else {
+		sync_rel_rate_error_integral += rel_rate_error * dt * sync_rate_i;
+		sync_rel_rate_error_integral = fmaxf(sync_rel_rate_error_integral, -sync_max_rate_correction_ppm);
+		sync_rel_rate_error_integral = fminf(sync_rel_rate_error_integral, sync_max_rate_correction_ppm);
+	}
+
+	/* Rate controller */
+	float total_rate_correction_ppm = rel_rate_error + sync_rel_rate_error_integral;
+	total_rate_correction_ppm = fmaxf(total_rate_correction_ppm, -sync_max_rate_correction_ppm);
+	total_rate_correction_ppm = fminf(total_rate_correction_ppm, sync_max_rate_correction_ppm);
+
+	sync_correction_nsec_per_overflow = (HRT_COUNTER_PERIOD * 1000) * (total_rate_correction_ppm / 1e6f);
+
+	// syslog(LOG_INFO, "$ adj=%f   rel_rate=%f   rel_rate_eint=%f   tgt_rel_rate=%f   ppm=%f\n",
+	//        (double)adj_usec, (double)sync_rel_rate_ppm, (double)sync_rel_rate_error_integral, (double)target_rel_rate_ppm,
+	//        (double)total_rate_correction_ppm);
+}
+
+void hrt_absolute_time_adjust(int64_t adjustment)
+{
+	irqstate_t flags = px4_enter_critical_section();
+
+	if (fabsf((float)adjustment) > (float)sync_min_jump || !sync_set) {
+
+		const hrt_abstime time_prev = base_time;
+		base_time += adjustment;
+		syslog(LOG_NOTICE, "HRT: resetting %llu -> %llu\n", time_prev, base_time);
+
+		sync_set = true;
+		sync_locked = false;
+		sync_jump_cnt++;
+		sync_prev_adj = 0;
+		sync_rel_rate_ppm = 0;
+
+	} else {
+		updateRatePID(adjustment);
+
+		if (!sync_locked) {
+			sync_locked =
+				(fabsf(sync_rel_rate_ppm) < sync_lock_thres_rate_ppm) &&
+				(fabsf(sync_prev_adj) < sync_lock_thres_offset);
+		}
+	}
+
+	px4_leave_critical_section(flags);
+}
+#endif // HRT_TIME_SYNC
+
+
+
 int hrt_ioctl(unsigned int cmd, unsigned long arg);
 /*
  * Specific registers and bits used by PPM sub-functions
@@ -665,7 +773,6 @@ hrt_absolute_time(void)
 	 * pair.  Discourage the compiler from moving loads/stores
 	 * to these outside of the protected range.
 	 */
-	static volatile hrt_abstime base_time;
 	static volatile uint32_t last_count;
 
 	/* prevent re-entry */
@@ -683,12 +790,34 @@ hrt_absolute_time(void)
 	 */
 	if (count < last_count) {
 		base_time += HRT_COUNTER_PERIOD;
+
+#if defined(HRT_TIME_SYNC)
+
+		if (sync_set) {
+			sync_accumulated_correction_nsec += sync_correction_nsec_per_overflow;
+
+			if (abs(sync_accumulated_correction_nsec) >= 1000) {
+				base_time += sync_accumulated_correction_nsec / 1000;
+				sync_accumulated_correction_nsec %= 1000;
+			}
+
+			// Correction decay - 1 nsec per 65536 usec
+			if (sync_correction_nsec_per_overflow > 0) {
+				sync_correction_nsec_per_overflow--;
+
+			} else if (sync_correction_nsec_per_overflow < 0) {
+				sync_correction_nsec_per_overflow++;
+			}
+		}
+
+#endif /* HRT_TIME_SYNC */
 	}
 
 	/* save the count for next time */
 	last_count = count;
 
 	/* compute the current time */
+
 	abstime = HRT_COUNTER_SCALE(base_time + count);
 
 	px4_leave_critical_section(flags);

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_hrt.h

@@ -131,6 +131,11 @@ typedef struct latency_boardctl {
  */
 __EXPORT extern hrt_abstime hrt_absolute_time(void);
 
+/**
+ * Adjust the synchronized clock.
+ */
+__EXPORT extern void hrt_absolute_time_adjust(int64_t adjustment);
+
 /**
  * Convert a timespec to absolute time.
  */
@@ -162,16 +167,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/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/drivers/uavcan/CMakeLists.txt

@@ -42,22 +42,12 @@ set(UAVCAN_PLATFORM "generic")
 if(CONFIG_ARCH_CHIP)
 	if(${CONFIG_NET_CAN} MATCHES "y")
 		set(UAVCAN_DRIVER "socketcan")
-		set(UAVCAN_TIMER 1)
 	elseif(${CONFIG_ARCH_CHIP} MATCHES "kinetis")
 		set(UAVCAN_DRIVER "kinetis")
-		set(UAVCAN_TIMER 1)
 	elseif(${CONFIG_ARCH_CHIP} MATCHES "stm32h7")
 		set(UAVCAN_DRIVER "stm32h7")
-		set(UAVCAN_TIMER 5) # The default timer is TIM5
-		if (DEFINED config_uavcan_timer_override)
-			set (UAVCAN_TIMER  ${config_uavcan_timer_override})
-		endif()
 	elseif(${CONFIG_ARCH_CHIP} MATCHES "stm32")
 		set(UAVCAN_DRIVER "stm32")
-		set(UAVCAN_TIMER 6) # The default timer is TIM6
-		if (DEFINED config_uavcan_timer_override)
-			set (UAVCAN_TIMER  ${config_uavcan_timer_override})
-		endif()
 	endif()
 endif()
 
@@ -74,8 +64,6 @@ string(TOUPPER "${UAVCAN_DRIVER}" UAVCAN_DRIVER_UPPER)
 add_definitions(
 	-DUAVCAN_${UAVCAN_DRIVER_UPPER}_${OS_UPPER}=1
 	-DUAVCAN_${UAVCAN_DRIVER_UPPER}_NUM_IFACES=${config_uavcan_num_ifaces}
-	-DUAVCAN_${UAVCAN_DRIVER_UPPER}_TIMER_NUMBER=${UAVCAN_TIMER}
-	-DUAVCAN_NUM_IFACES=${config_uavcan_num_ifaces}
 	-DUAVCAN_CPP_VERSION=UAVCAN_CPP03
 	-DUAVCAN_DRIVER=uavcan_${UAVCAN_DRIVER}
 	-DUAVCAN_IMPLEMENT_PLACEMENT_NEW=1

src/drivers/uavcan/Kconfig

@@ -96,8 +96,3 @@ depends on DRIVERS_UAVCAN
     string "UAVCAN peripheral firmware"
     help
         list of UAVCAN peripheral firmware to build and embed
-
-menuconfig BOARD_UAVCAN_TIMER_OVERRIDE
-depends on DRIVERS_UAVCAN
-    int "UAVCAN timer override"
-    default 0

src/drivers/uavcan/uavcan_drivers/kinetis/README.md

@@ -15,7 +15,6 @@ and the following commandline defines:
 |      Setting      |                             Description                                           |
 |-------------------|-----------------------------------------------------------------------------------|
 |UAVCAN_KINETIS_NUM_IFACES   | - {1..2}  Sets the number of CAN interfaces the SW will support |
-|UAVCAN_KINETIS_TIMER_NUMBER | - {0..3}  Sets the Periodic Interrupt Timer (PITn) channel |
 
 Things that could be improved:
 1. Build time command line configuration of Mailbox/FIFO and filters

src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/build_config.hpp

@@ -18,12 +18,3 @@
 #if !defined(UAVCAN_KINETIS_NUM_IFACES) || (UAVCAN_KINETIS_NUM_IFACES != 1 && UAVCAN_KINETIS_NUM_IFACES != 2)
 # error "UAVCAN_KINETIS_NUM_IFACES must be set to either 1 or 2"
 #endif
-
-/**
- * Any PIT timer channel (PIT0-PIT3)
- * e.g. -DUAVCAN_KINETIS_TIMER_NUMBER=2
- */
-#ifndef UAVCAN_KINETIS_TIMER_NUMBER
-// In this case the clock driver should be implemented by the application
-# define UAVCAN_KINETIS_TIMER_NUMBER 0
-#endif

src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/clock.hpp

@@ -46,57 +46,6 @@ uavcan::UtcTime getUtc();
  */
 void adjustUtc(uavcan::UtcDuration adjustment);
 
-/**
- * UTC clock synchronization parameters
- */
-struct UtcSyncParams {
-	float offset_p;                        ///< PPM per one usec error
-	float rate_i;                          ///< PPM per one PPM error for second
-	float rate_error_corner_freq;
-	float max_rate_correction_ppm;
-	float lock_thres_rate_ppm;
-	uavcan::UtcDuration lock_thres_offset;
-	uavcan::UtcDuration min_jump;          ///< Min error to jump rather than change rate
-
-	UtcSyncParams()
-		: offset_p(0.01F),
-		  rate_i(0.02F),
-		  rate_error_corner_freq(0.01F),
-		  max_rate_correction_ppm(300.0F),
-		  lock_thres_rate_ppm(2.0F),
-		  lock_thres_offset(uavcan::UtcDuration::fromMSec(4)),
-		  min_jump(uavcan::UtcDuration::fromMSec(10))
-	{
-	}
-};
-
-/**
- * Clock rate error.
- * Positive if the hardware timer is slower than reference time.
- * This function is thread safe.
- */
-float getUtcRateCorrectionPPM();
-
-/**
- * Number of non-gradual adjustments performed so far.
- * Ideally should be zero.
- * This function is thread safe.
- */
-uavcan::uint32_t getUtcJumpCount();
-
-/**
- * Whether UTC is synchronized and locked.
- * This function is thread safe.
- */
-bool isUtcLocked();
-
-/**
- * UTC sync params get/set.
- * Both functions are thread safe.
- */
-UtcSyncParams getUtcSyncParams();
-void setUtcSyncParams(const UtcSyncParams &params);
-
 }
 
 /**

src/drivers/uavcan/uavcan_drivers/kinetis/driver/src/uc_kinetis_clock.cpp

@@ -7,31 +7,7 @@
 #include <uavcan_kinetis/thread.hpp>
 #include "internal.hpp"
 
-#if UAVCAN_KINETIS_TIMER_NUMBER
-# include <cassert>
-# include <math.h>
-
-/*
- * Timer instance
- * todo:Consider using Lifetime Timer support
- */
-# define TIMX_IRQHandler         UAVCAN_KINETIS_GLUE3(PIT, UAVCAN_KINETIS_TIMER_NUMBER, _IRQHandler)
-# define TIMX                    (KINETIS_PIT_BASE + (UAVCAN_KINETIS_TIMER_NUMBER << 4))
-# define TMR_REG(o)              (TIMX + (o))
-# define TIMX_INPUT_CLOCK        BOARD_BUS_FREQ
-# define TIMX_INTERRUPT_FREQ     16
-# define TIMX_IRQn               UAVCAN_KINETIS_GLUE2(KINETIS_IRQ_PITCH, UAVCAN_KINETIS_TIMER_NUMBER)
-
-# if UAVCAN_KINETIS_TIMER_NUMBER >= 0 && UAVCAN_KINETIS_TIMER_NUMBER <= 3
-#  define KINETIS_PIT_LDVAL_OFFSET KINETIS_PIT_LDVAL0_OFFSET
-#  define KINETIS_PIT_CVAL_OFFSET  KINETIS_PIT_CVAL0_OFFSET
-#  define KINETIS_PIT_TCTRL_OFFSET KINETIS_PIT_TCTRL0_OFFSET
-#  define KINETIS_PIT_TFLG_OFFSET  KINETIS_PIT_TFLG0_OFFSET
-# else
-#  error "This UAVCAN_KINETIS_TIMER_NUMBER is not supported yet"
-# endif
-
-extern "C" UAVCAN_KINETIS_IRQ_HANDLER(TIMX_IRQHandler);
+#include <drivers/drv_hrt.h>
 
 namespace uavcan_kinetis
 {
@@ -40,30 +16,12 @@ namespace clock
 namespace
 {
 
-const uavcan::uint32_t CountsPerPeriod = (TIMX_INPUT_CLOCK / TIMX_INTERRUPT_FREQ);
-const uavcan::uint32_t CountsPerUs = (TIMX_INPUT_CLOCK / 1000000);
-const uavcan::uint32_t USecPerOverflow = (1000000 / TIMX_INTERRUPT_FREQ);
-
 Mutex mutex;
 
 bool initialized = false;
-
-bool utc_set = false;
-bool utc_locked = false;
-uavcan::uint32_t utc_jump_cnt = 0;
-UtcSyncParams utc_sync_params;
-float utc_prev_adj = 0;
-float utc_rel_rate_ppm = 0;
-float utc_rel_rate_error_integral = 0;
-uavcan::int32_t utc_accumulated_correction_nsec = 0;
-uavcan::int32_t utc_correction_nsec_per_overflow = 0;
-uavcan::MonotonicTime prev_utc_adj_at;
-
-uavcan::uint64_t time_mono = 0;
-uavcan::uint64_t time_utc = 0;
-
 }
 
+
 void init()
 {
 	CriticalSectionLocker lock;
@@ -73,231 +31,34 @@ void init()
 	}
 
 	initialized = true;
-
-	// Attach IRQ
-	irq_attach(TIMX_IRQn, &TIMX_IRQHandler, NULL);
-
-	// Power-on Clock
-	modifyreg32(KINETIS_SIM_SCGC6, 0, SIM_SCGC6_PIT);
-
-	// Enable module
-	putreg32(0, KINETIS_PIT_MCR);
-
-	// Start the timer
-
-	putreg32(CountsPerPeriod - 1, TMR_REG(KINETIS_PIT_LDVAL_OFFSET));
-	putreg32(PIT_TCTRL_TEN | PIT_TCTRL_TIE, TMR_REG(KINETIS_PIT_TCTRL_OFFSET));     // Start
-
-	// Prioritize and Enable  IRQ
-
-#if 0
-	// This has to be off or uses the default priority
-	// Without the ability to point the vector
-	// Directly to this ISR this will reenter the
-	// exception_common and cause the interrupt
-	// Stack pointer to be reset
-	up_prioritize_irq(TIMX_IRQn, NVIC_SYSH_HIGH_PRIORITY);
-#endif
-	up_enable_irq(TIMX_IRQn);
 }
 
 void setUtc(uavcan::UtcTime time)
 {
-	MutexLocker mlocker(mutex);
-	UAVCAN_ASSERT(initialized);
-
-	{
-		CriticalSectionLocker locker;
-		time_utc = time.toUSec();
-	}
-
-	utc_set = true;
-	utc_locked = false;
-	utc_jump_cnt++;
-	utc_prev_adj = 0;
-	utc_rel_rate_ppm = 0;
-}
-
-static uavcan::uint64_t sampleUtcFromCriticalSection()
-{
-	UAVCAN_ASSERT(initialized);
-	UAVCAN_ASSERT(getreg32(TMR_REG(KINETIS_PIT_TCTRL_OFFSET)) & PIT_TCTRL_TIE);
-
-	volatile uavcan::uint64_t time = time_utc;
-	volatile uavcan::uint32_t cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET));
-
-	if (getreg32(TMR_REG(KINETIS_PIT_TFLG_OFFSET)) & PIT_TFLG_TIF) {
-		cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET));
-		const uavcan::int32_t add = uavcan::int32_t(USecPerOverflow) +
-					    (utc_accumulated_correction_nsec + utc_correction_nsec_per_overflow) / 1000;
-		time = uavcan::uint64_t(uavcan::int64_t(time) + add);
-	}
-
-	return time + (cnt / CountsPerUs);
+	// DO NOTHING
 }
 
 uavcan::uint64_t getUtcUSecFromCanInterrupt()
 {
-	return utc_set ? sampleUtcFromCriticalSection() : 0;
+	return hrt_absolute_time();
 }
 
 uavcan::MonotonicTime getMonotonic()
 {
-	uavcan::uint64_t usec = 0;
-	// Scope Critical section
-	{
-		CriticalSectionLocker locker;
-
-		volatile uavcan::uint64_t time = time_mono;
-		volatile uavcan::uint32_t cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET));
-
-		if (getreg32(TMR_REG(KINETIS_PIT_TFLG_OFFSET)) & PIT_TFLG_TIF) {
-			cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET));
-			time += USecPerOverflow;
-		}
-
-		usec = time + (cnt / CountsPerUs);
-
-	}     // End Scope Critical section
-
+	uavcan::uint64_t usec = hrt_absolute_time();
 	return uavcan::MonotonicTime::fromUSec(usec);
 }
 
-
 uavcan::UtcTime getUtc()
 {
-	if (utc_set) {
-		uavcan::uint64_t usec = 0;
-		{
-			CriticalSectionLocker locker;
-			usec = sampleUtcFromCriticalSection();
-		}
-		return uavcan::UtcTime::fromUSec(usec);
-	}
-
-	return uavcan::UtcTime();
-}
-
-static float lowpass(float xold, float xnew, float corner, float dt)
-{
-	const float tau = 1.F / corner;
-	return (dt * xnew + tau * xold) / (dt + tau);
-}
-
-static void updateRatePID(uavcan::UtcDuration adjustment)
-{
-	const uavcan::MonotonicTime ts = getMonotonic();
-	const float dt = float((ts - prev_utc_adj_at).toUSec()) / 1e6F;
-	prev_utc_adj_at = ts;
-	const float adj_usec = float(adjustment.toUSec());
-
-	/*
-	 * Target relative rate in PPM
-	 * Positive to go faster
-	 */
-	const float target_rel_rate_ppm = adj_usec * utc_sync_params.offset_p;
-
-	/*
-	 * Current relative rate in PPM
-	 * Positive if the local clock is faster
-	 */
-	const float new_rel_rate_ppm = (utc_prev_adj - adj_usec) / dt;     // rate error in [usec/sec], which is PPM
-	utc_prev_adj = adj_usec;
-	utc_rel_rate_ppm = lowpass(utc_rel_rate_ppm, new_rel_rate_ppm, utc_sync_params.rate_error_corner_freq, dt);
-
-	const float rel_rate_error = target_rel_rate_ppm - utc_rel_rate_ppm;
-
-	if (dt > 10) {
-		utc_rel_rate_error_integral = 0;
-
-	} else {
-		utc_rel_rate_error_integral += rel_rate_error * dt * utc_sync_params.rate_i;
-		utc_rel_rate_error_integral =
-			uavcan::max(utc_rel_rate_error_integral, -utc_sync_params.max_rate_correction_ppm);
-		utc_rel_rate_error_integral =
-			uavcan::min(utc_rel_rate_error_integral, utc_sync_params.max_rate_correction_ppm);
-	}
-
-	/*
-	 * Rate controller
-	 */
-	float total_rate_correction_ppm = rel_rate_error + utc_rel_rate_error_integral;
-	total_rate_correction_ppm = uavcan::max(total_rate_correction_ppm, -utc_sync_params.max_rate_correction_ppm);
-	total_rate_correction_ppm = uavcan::min(total_rate_correction_ppm, utc_sync_params.max_rate_correction_ppm);
-
-	utc_correction_nsec_per_overflow = uavcan::int32_t((USecPerOverflow * 1000) * (total_rate_correction_ppm / 1e6F));
-
-//    syslog("$ adj=%f   rel_rate=%f   rel_rate_eint=%f   tgt_rel_rate=%f   ppm=%f\n",
-//              adj_usec, utc_rel_rate_ppm, utc_rel_rate_error_integral, target_rel_rate_ppm,
-// total_rate_correction_ppm);
+	uavcan::uint64_t usec = hrt_absolute_time();
+	return uavcan::UtcTime::fromUSec(usec);
 }
 
 void adjustUtc(uavcan::UtcDuration adjustment)
 {
-	MutexLocker mlocker(mutex);
-	UAVCAN_ASSERT(initialized);
-
-	if (adjustment.getAbs() > utc_sync_params.min_jump || !utc_set) {
-		const uavcan::int64_t adj_usec = adjustment.toUSec();
-
-		{
-			CriticalSectionLocker locker;
-
-			if ((adj_usec < 0) && uavcan::uint64_t(-adj_usec) > time_utc) {
-				time_utc = 1;
-
-			} else {
-				time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + adj_usec);
-			}
-		}
-
-		utc_set = true;
-		utc_locked = false;
-		utc_jump_cnt++;
-		utc_prev_adj = 0;
-		utc_rel_rate_ppm = 0;
-
-	} else {
-		updateRatePID(adjustment);
-
-		if (!utc_locked) {
-			utc_locked =
-				(std::abs(utc_rel_rate_ppm) < utc_sync_params.lock_thres_rate_ppm) &&
-				(std::abs(utc_prev_adj) < float(utc_sync_params.lock_thres_offset.toUSec()));
-		}
-	}
-}
-
-float getUtcRateCorrectionPPM()
-{
-	MutexLocker mlocker(mutex);
-	const float rate_correction_mult = float(utc_correction_nsec_per_overflow) / float(USecPerOverflow * 1000);
-	return 1e6F * rate_correction_mult;
-}
-
-uavcan::uint32_t getUtcJumpCount()
-{
-	MutexLocker mlocker(mutex);
-	return utc_jump_cnt;
-}
-
-bool isUtcLocked()
-{
-	MutexLocker mlocker(mutex);
-	return utc_locked;
-}
-
-UtcSyncParams getUtcSyncParams()
-{
-	MutexLocker mlocker(mutex);
-	return utc_sync_params;
-}
-
-void setUtcSyncParams(const UtcSyncParams &params)
-{
-	MutexLocker mlocker(mutex);
-	// Add some sanity check
-	utc_sync_params = params;
+	const float adj_usec = float(adjustment.toUSec());
+	hrt_absolute_time_adjust(adj_usec);
 }
 
 } // namespace clock
@@ -321,45 +82,5 @@ SystemClock &SystemClock::instance()
 	return *ptr;
 }
 
-} // namespace uavcan_kinetis
+} // namespace uavcan_stm32
 
-
-/**
- * Timer interrupt handler
- */
-
-extern "C"
-UAVCAN_KINETIS_IRQ_HANDLER(TIMX_IRQHandler)
-{
-	putreg32(PIT_TFLG_TIF, TMR_REG(KINETIS_PIT_TFLG_OFFSET));
-
-	using namespace uavcan_kinetis::clock;
-	UAVCAN_ASSERT(initialized);
-
-	time_mono += USecPerOverflow;
-
-	if (utc_set) {
-		time_utc += USecPerOverflow;
-		utc_accumulated_correction_nsec += utc_correction_nsec_per_overflow;
-
-		if (std::abs(utc_accumulated_correction_nsec) >= 1000) {
-			time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + utc_accumulated_correction_nsec / 1000);
-			utc_accumulated_correction_nsec %= 1000;
-		}
-
-		// Correction decay - 1 nsec per 65536 usec
-		if (utc_correction_nsec_per_overflow > 0) {
-			utc_correction_nsec_per_overflow--;
-
-		} else if (utc_correction_nsec_per_overflow < 0) {
-			utc_correction_nsec_per_overflow++;
-
-		} else {
-			;             // Zero
-		}
-	}
-
-	return 0;
-}
-
-#endif

src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/build_config.hpp

@@ -17,12 +17,3 @@
 #if !defined(UAVCAN_STM32_NUM_IFACES) || (UAVCAN_STM32_NUM_IFACES != 1 && UAVCAN_STM32_NUM_IFACES != 2)
 # error "UAVCAN_STM32_NUM_IFACES must be set to either 1 or 2"
 #endif
-
-/**
- * Any General-Purpose timer (TIM2, TIM3, TIM4, TIM5)
- * e.g. -DUAVCAN_STM32_TIMER_NUMBER=2
- */
-#ifndef UAVCAN_STM32_TIMER_NUMBER
-// In this case the clock driver should be implemented by the application
-# define UAVCAN_STM32_TIMER_NUMBER 0
-#endif

src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/clock.hpp

@@ -45,56 +45,6 @@ uavcan::UtcTime getUtc();
  */
 void adjustUtc(uavcan::UtcDuration adjustment);
 
-/**
- * UTC clock synchronization parameters
- */
-struct UtcSyncParams {
-	float offset_p;                        ///< PPM per one usec error
-	float rate_i;                          ///< PPM per one PPM error for second
-	float rate_error_corner_freq;
-	float max_rate_correction_ppm;
-	float lock_thres_rate_ppm;
-	uavcan::UtcDuration lock_thres_offset;
-	uavcan::UtcDuration min_jump;          ///< Min error to jump rather than change rate
-
-	UtcSyncParams()
-		: offset_p(0.01F)
-		, rate_i(0.02F)
-		, rate_error_corner_freq(0.01F)
-		, max_rate_correction_ppm(300.0F)
-		, lock_thres_rate_ppm(2.0F)
-		, lock_thres_offset(uavcan::UtcDuration::fromMSec(4))
-		, min_jump(uavcan::UtcDuration::fromMSec(10))
-	{ }
-};
-
-/**
- * Clock rate error.
- * Positive if the hardware timer is slower than reference time.
- * This function is thread safe.
- */
-float getUtcRateCorrectionPPM();
-
-/**
- * Number of non-gradual adjustments performed so far.
- * Ideally should be zero.
- * This function is thread safe.
- */
-uavcan::uint32_t getUtcJumpCount();
-
-/**
- * Whether UTC is synchronized and locked.
- * This function is thread safe.
- */
-bool isUtcLocked();
-
-/**
- * UTC sync params get/set.
- * Both functions are thread safe.
- */
-UtcSyncParams getUtcSyncParams();
-void setUtcSyncParams(const UtcSyncParams &params);
-
 }
 
 /**

src/drivers/uavcan/uavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp

@@ -6,49 +6,7 @@
 #include <uavcan_stm32/thread.hpp>
 #include "internal.hpp"
 
-#if UAVCAN_STM32_TIMER_NUMBER
-
-#include <cassert>
-#include <math.h>
-
-/*
- * Timer instance
- */
-# if UAVCAN_STM32_NUTTX
-#  define TIMX                    UAVCAN_STM32_GLUE3(STM32_TIM, UAVCAN_STM32_TIMER_NUMBER, _BASE)
-#  define  TMR_REG(o)              (TIMX + (o))
-#  define TIMX_INPUT_CLOCK         UAVCAN_STM32_GLUE3(STM32_APB1_TIM, UAVCAN_STM32_TIMER_NUMBER, _CLKIN)
-
-#  define TIMX_IRQn                UAVCAN_STM32_GLUE2(STM32_IRQ_TIM, UAVCAN_STM32_TIMER_NUMBER)
-# endif
-
-# if UAVCAN_STM32_TIMER_NUMBER >= 2 && UAVCAN_STM32_TIMER_NUMBER <= 7
-#  define TIMX_RCC_ENR           RCC->APB1ENR
-#  define TIMX_RCC_RSTR          RCC->APB1RSTR
-#  define TIMX_RCC_ENR_MASK      UAVCAN_STM32_GLUE3(RCC_APB1ENR_TIM,  UAVCAN_STM32_TIMER_NUMBER, EN)
-#  define TIMX_RCC_RSTR_MASK     UAVCAN_STM32_GLUE3(RCC_APB1RSTR_TIM, UAVCAN_STM32_TIMER_NUMBER, RST)
-# else
-#  error "This UAVCAN_STM32_TIMER_NUMBER is not supported yet"
-# endif
-
-/**
- * UAVCAN_STM32_TIMX_INPUT_CLOCK can be used to manually override the auto-detected timer clock speed.
- * This is useful at least with certain versions of ChibiOS which do not support the bit
- * RCC_DKCFGR.TIMPRE that is available in newer models of STM32. In that case, if TIMPRE is active,
- * the auto-detected value of TIMX_INPUT_CLOCK will be twice lower than the actual clock speed.
- * Read this for additional context: http://www.chibios.com/forum/viewtopic.php?f=35&t=3870
- * A normal way to use the override feature is to provide an alternative macro, e.g.:
- *
- *      -DUAVCAN_STM32_TIMX_INPUT_CLOCK=STM32_HCLK
- *
- * Alternatively, the new clock rate can be specified directly.
- */
-# ifdef UAVCAN_STM32_TIMX_INPUT_CLOCK
-#  undef TIMX_INPUT_CLOCK
-#  define TIMX_INPUT_CLOCK      UAVCAN_STM32_TIMX_INPUT_CLOCK
-# endif
-
-extern "C" UAVCAN_STM32_IRQ_HANDLER(TIMX_IRQHandler);
+#include <drivers/drv_hrt.h>
 
 namespace uavcan_stm32
 {
@@ -57,26 +15,9 @@ namespace clock
 namespace
 {
 
-const uavcan::uint32_t USecPerOverflow = 65536;
-
 Mutex mutex;
 
 bool initialized = false;
-
-bool utc_set = false;
-bool utc_locked = false;
-uavcan::uint32_t utc_jump_cnt = 0;
-UtcSyncParams utc_sync_params;
-float utc_prev_adj = 0;
-float utc_rel_rate_ppm = 0;
-float utc_rel_rate_error_integral = 0;
-uavcan::int32_t utc_accumulated_correction_nsec = 0;
-uavcan::int32_t utc_correction_nsec_per_overflow = 0;
-uavcan::MonotonicTime prev_utc_adj_at;
-
-uavcan::uint64_t time_mono = 0;
-uavcan::uint64_t time_utc = 0;
-
 }
 
 
@@ -89,246 +30,34 @@ void init()
 	}
 
 	initialized = true;
-
-
-# if UAVCAN_STM32_NUTTX
-
-	// Attach IRQ
-	irq_attach(TIMX_IRQn, &TIMX_IRQHandler, NULL);
-
-	// Power-on and reset
-	modifyreg32(STM32_RCC_APB1ENR, 0, TIMX_RCC_ENR_MASK);
-	modifyreg32(STM32_RCC_APB1RSTR, 0, TIMX_RCC_RSTR_MASK);
-	modifyreg32(STM32_RCC_APB1RSTR, TIMX_RCC_RSTR_MASK, 0);
-
-
-	// Start the timer
-	putreg32(0xFFFF, TMR_REG(STM32_BTIM_ARR_OFFSET));
-	putreg16(((TIMX_INPUT_CLOCK / 1000000) - 1), TMR_REG(STM32_BTIM_PSC_OFFSET));
-	putreg16(BTIM_CR1_URS, TMR_REG(STM32_BTIM_CR1_OFFSET));
-	putreg16(0, TMR_REG(STM32_BTIM_SR_OFFSET));
-	putreg16(BTIM_EGR_UG, TMR_REG(STM32_BTIM_EGR_OFFSET)); // Reload immediately
-	putreg16(BTIM_DIER_UIE, TMR_REG(STM32_BTIM_DIER_OFFSET));
-	putreg16(BTIM_CR1_CEN, TMR_REG(STM32_BTIM_CR1_OFFSET)); // Start
-
-	// Prioritize and Enable  IRQ
-// todo: Currently changing the NVIC_SYSH_HIGH_PRIORITY is HARD faulting
-// need to investigate
-//    up_prioritize_irq(TIMX_IRQn, NVIC_SYSH_HIGH_PRIORITY);
-	up_enable_irq(TIMX_IRQn);
-
-# endif
 }
 
 void setUtc(uavcan::UtcTime time)
 {
-	MutexLocker mlocker(mutex);
-	UAVCAN_ASSERT(initialized);
-
-	{
-		CriticalSectionLocker locker;
-		time_utc = time.toUSec();
-	}
-
-	utc_set = true;
-	utc_locked = false;
-	utc_jump_cnt++;
-	utc_prev_adj = 0;
-	utc_rel_rate_ppm = 0;
-}
-
-static uavcan::uint64_t sampleUtcFromCriticalSection()
-{
-# if UAVCAN_STM32_NUTTX
-
-	UAVCAN_ASSERT(initialized);
-	UAVCAN_ASSERT(getreg16(TMR_REG(STM32_BTIM_DIER_OFFSET)) & BTIM_DIER_UIE);
-
-	volatile uavcan::uint64_t time = time_utc;
-	volatile uavcan::uint32_t cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
-
-	if (getreg16(TMR_REG(STM32_BTIM_SR_OFFSET)) & BTIM_SR_UIF) {
-		cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
-		const uavcan::int32_t add = uavcan::int32_t(USecPerOverflow) +
-					    (utc_accumulated_correction_nsec + utc_correction_nsec_per_overflow) / 1000;
-		time = uavcan::uint64_t(uavcan::int64_t(time) + add);
-	}
-
-	return time + cnt;
-# endif
+	// DO NOTHING
 }
 
 uavcan::uint64_t getUtcUSecFromCanInterrupt()
 {
-	return utc_set ? sampleUtcFromCriticalSection() : 0;
+	return hrt_absolute_time();
 }
 
 uavcan::MonotonicTime getMonotonic()
 {
-	uavcan::uint64_t usec = 0;
-	// Scope Critical section
-	{
-		CriticalSectionLocker locker;
-
-		volatile uavcan::uint64_t time = time_mono;
-
-# if UAVCAN_STM32_NUTTX
-
-		volatile uavcan::uint32_t cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
-
-		if (getreg16(TMR_REG(STM32_BTIM_SR_OFFSET)) & BTIM_SR_UIF) {
-			cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
-# endif
-			time += USecPerOverflow;
-		}
-
-		usec = time + cnt;
-
-# ifndef NDEBUG
-		static uavcan::uint64_t prev_usec = 0;      // Self-test
-		UAVCAN_ASSERT(prev_usec <= usec);
-		(void)prev_usec;
-		prev_usec = usec;
-# endif
-	} // End Scope Critical section
-
+	uavcan::uint64_t usec = hrt_absolute_time();
 	return uavcan::MonotonicTime::fromUSec(usec);
 }
 
 uavcan::UtcTime getUtc()
 {
-	if (utc_set) {
-		uavcan::uint64_t usec = 0;
-		{
-			CriticalSectionLocker locker;
-			usec = sampleUtcFromCriticalSection();
-		}
-		return uavcan::UtcTime::fromUSec(usec);
-	}
-
-	return uavcan::UtcTime();
-}
-
-static float lowpass(float xold, float xnew, float corner, float dt)
-{
-	const float tau = 1.F / corner;
-	return (dt * xnew + tau * xold) / (dt + tau);
-}
-
-static void updateRatePID(uavcan::UtcDuration adjustment)
-{
-	const uavcan::MonotonicTime ts = getMonotonic();
-	const float dt = float((ts - prev_utc_adj_at).toUSec()) / 1e6F;
-	prev_utc_adj_at = ts;
-	const float adj_usec = float(adjustment.toUSec());
-
-	/*
-	 * Target relative rate in PPM
-	 * Positive to go faster
-	 */
-	const float target_rel_rate_ppm = adj_usec * utc_sync_params.offset_p;
-
-	/*
-	 * Current relative rate in PPM
-	 * Positive if the local clock is faster
-	 */
-	const float new_rel_rate_ppm = (utc_prev_adj - adj_usec) / dt; // rate error in [usec/sec], which is PPM
-	utc_prev_adj = adj_usec;
-	utc_rel_rate_ppm = lowpass(utc_rel_rate_ppm, new_rel_rate_ppm, utc_sync_params.rate_error_corner_freq, dt);
-
-	const float rel_rate_error = target_rel_rate_ppm - utc_rel_rate_ppm;
-
-	if (dt > 10) {
-		utc_rel_rate_error_integral = 0;
-
-	} else {
-		utc_rel_rate_error_integral += rel_rate_error * dt * utc_sync_params.rate_i;
-		utc_rel_rate_error_integral =
-			uavcan::max(utc_rel_rate_error_integral, -utc_sync_params.max_rate_correction_ppm);
-		utc_rel_rate_error_integral =
-			uavcan::min(utc_rel_rate_error_integral, utc_sync_params.max_rate_correction_ppm);
-	}
-
-	/*
-	 * Rate controller
-	 */
-	float total_rate_correction_ppm = rel_rate_error + utc_rel_rate_error_integral;
-	total_rate_correction_ppm = uavcan::max(total_rate_correction_ppm, -utc_sync_params.max_rate_correction_ppm);
-	total_rate_correction_ppm = uavcan::min(total_rate_correction_ppm, utc_sync_params.max_rate_correction_ppm);
-
-	utc_correction_nsec_per_overflow = uavcan::int32_t((USecPerOverflow * 1000) * (total_rate_correction_ppm / 1e6F));
-
-//    syslog("$ adj=%f   rel_rate=%f   rel_rate_eint=%f   tgt_rel_rate=%f   ppm=%f\n",
-//              adj_usec, utc_rel_rate_ppm, utc_rel_rate_error_integral, target_rel_rate_ppm,
-// total_rate_correction_ppm);
+	uavcan::uint64_t usec = hrt_absolute_time();
+	return uavcan::UtcTime::fromUSec(usec);
 }
 
 void adjustUtc(uavcan::UtcDuration adjustment)
 {
-	MutexLocker mlocker(mutex);
-	UAVCAN_ASSERT(initialized);
-
-	if (adjustment.getAbs() > utc_sync_params.min_jump || !utc_set) {
-		const uavcan::int64_t adj_usec = adjustment.toUSec();
-
-		{
-			CriticalSectionLocker locker;
-
-			if ((adj_usec < 0) && uavcan::uint64_t(-adj_usec) > time_utc) {
-				time_utc = 1;
-
-			} else {
-				time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + adj_usec);
-			}
-		}
-
-		utc_set = true;
-		utc_locked = false;
-		utc_jump_cnt++;
-		utc_prev_adj = 0;
-		utc_rel_rate_ppm = 0;
-
-	} else {
-		updateRatePID(adjustment);
-
-		if (!utc_locked) {
-			utc_locked =
-				(std::abs(utc_rel_rate_ppm) < utc_sync_params.lock_thres_rate_ppm) &&
-				(std::abs(utc_prev_adj) < float(utc_sync_params.lock_thres_offset.toUSec()));
-		}
-	}
-}
-
-float getUtcRateCorrectionPPM()
-{
-	MutexLocker mlocker(mutex);
-	const float rate_correction_mult = float(utc_correction_nsec_per_overflow) / float(USecPerOverflow * 1000);
-	return 1e6F * rate_correction_mult;
-}
-
-uavcan::uint32_t getUtcJumpCount()
-{
-	MutexLocker mlocker(mutex);
-	return utc_jump_cnt;
-}
-
-bool isUtcLocked()
-{
-	MutexLocker mlocker(mutex);
-	return utc_locked;
-}
-
-UtcSyncParams getUtcSyncParams()
-{
-	MutexLocker mlocker(mutex);
-	return utc_sync_params;
-}
-
-void setUtcSyncParams(const UtcSyncParams &params)
-{
-	MutexLocker mlocker(mutex);
-	// Add some sanity check
-	utc_sync_params = params;
+	const float adj_usec = float(adjustment.toUSec());
+	hrt_absolute_time_adjust(adj_usec);
 }
 
 } // namespace clock
@@ -354,47 +83,3 @@ SystemClock &SystemClock::instance()
 
 } // namespace uavcan_stm32
 
-
-/**
- * Timer interrupt handler
- */
-
-extern "C"
-UAVCAN_STM32_IRQ_HANDLER(TIMX_IRQHandler)
-{
-	UAVCAN_STM32_IRQ_PROLOGUE();
-
-# if UAVCAN_STM32_NUTTX
-	putreg16(0, TMR_REG(STM32_BTIM_SR_OFFSET));
-# endif
-
-	using namespace uavcan_stm32::clock;
-	UAVCAN_ASSERT(initialized);
-
-	time_mono += USecPerOverflow;
-
-	if (utc_set) {
-		time_utc += USecPerOverflow;
-		utc_accumulated_correction_nsec += utc_correction_nsec_per_overflow;
-
-		if (std::abs(utc_accumulated_correction_nsec) >= 1000) {
-			time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + utc_accumulated_correction_nsec / 1000);
-			utc_accumulated_correction_nsec %= 1000;
-		}
-
-		// Correction decay - 1 nsec per 65536 usec
-		if (utc_correction_nsec_per_overflow > 0) {
-			utc_correction_nsec_per_overflow--;
-
-		} else if (utc_correction_nsec_per_overflow < 0) {
-			utc_correction_nsec_per_overflow++;
-
-		} else {
-			; // Zero
-		}
-	}
-
-	UAVCAN_STM32_IRQ_EPILOGUE();
-}
-
-#endif

src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/build_config.hpp

@@ -17,12 +17,3 @@
 #if !defined(UAVCAN_STM32H7_NUM_IFACES) || (UAVCAN_STM32H7_NUM_IFACES != 1 && UAVCAN_STM32H7_NUM_IFACES != 2)
 # error "UAVCAN_STM32H7_NUM_IFACES must be set to either 1 or 2"
 #endif
-
-/**
- * Any General-Purpose timer (TIM2, TIM3, TIM4, TIM5)
- * e.g. -DUAVCAN_STM32H7_TIMER_NUMBER=2
- */
-#ifndef UAVCAN_STM32H7_TIMER_NUMBER
-// In this case the clock driver should be implemented by the application
-# define UAVCAN_STM32H7_TIMER_NUMBER 0
-#endif

src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/clock.hpp

@@ -45,56 +45,6 @@ uavcan::UtcTime getUtc();
  */
 void adjustUtc(uavcan::UtcDuration adjustment);
 
-/**
- * UTC clock synchronization parameters
- */
-struct UtcSyncParams {
-	float offset_p;                        ///< PPM per one usec error
-	float rate_i;                          ///< PPM per one PPM error for second
-	float rate_error_corner_freq;
-	float max_rate_correction_ppm;
-	float lock_thres_rate_ppm;
-	uavcan::UtcDuration lock_thres_offset;
-	uavcan::UtcDuration min_jump;          ///< Min error to jump rather than change rate
-
-	UtcSyncParams()
-		: offset_p(0.01F)
-		, rate_i(0.02F)
-		, rate_error_corner_freq(0.01F)
-		, max_rate_correction_ppm(300.0F)
-		, lock_thres_rate_ppm(2.0F)
-		, lock_thres_offset(uavcan::UtcDuration::fromMSec(4))
-		, min_jump(uavcan::UtcDuration::fromMSec(10))
-	{ }
-};
-
-/**
- * Clock rate error.
- * Positive if the hardware timer is slower than reference time.
- * This function is thread safe.
- */
-float getUtcRateCorrectionPPM();
-
-/**
- * Number of non-gradual adjustments performed so far.
- * Ideally should be zero.
- * This function is thread safe.
- */
-uavcan::uint32_t getUtcJumpCount();
-
-/**
- * Whether UTC is synchronized and locked.
- * This function is thread safe.
- */
-bool isUtcLocked();
-
-/**
- * UTC sync params get/set.
- * Both functions are thread safe.
- */
-UtcSyncParams getUtcSyncParams();
-void setUtcSyncParams(const UtcSyncParams &params);
-
 }
 
 /**

src/drivers/uavcan/uavcan_drivers/stm32h7/driver/src/uc_stm32h7_clock.cpp

@@ -6,49 +6,7 @@
 #include <uavcan_stm32h7/thread.hpp>
 #include "internal.hpp"
 
-#if UAVCAN_STM32H7_TIMER_NUMBER
-
-#include <cassert>
-#include <math.h>
-
-/*
- * Timer instance
- */
-# if UAVCAN_STM32H7_NUTTX
-#  define TIMX                    UAVCAN_STM32H7_GLUE3(STM32_TIM, UAVCAN_STM32H7_TIMER_NUMBER, _BASE)
-#  define  TMR_REG(o)              (TIMX + (o))
-#  define TIMX_INPUT_CLOCK         UAVCAN_STM32H7_GLUE3(STM32_APB1_TIM, UAVCAN_STM32H7_TIMER_NUMBER, _CLKIN)
-
-#  define TIMX_IRQn                UAVCAN_STM32H7_GLUE2(STM32_IRQ_TIM, UAVCAN_STM32H7_TIMER_NUMBER)
-# endif
-
-# if UAVCAN_STM32H7_TIMER_NUMBER >= 2 && UAVCAN_STM32H7_TIMER_NUMBER <= 7
-#  define TIMX_RCC_ENR           RCC->APB1ENR
-#  define TIMX_RCC_RSTR          RCC->APB1RSTR
-#  define TIMX_RCC_ENR_MASK      UAVCAN_STM32H7_GLUE3(RCC_APB1ENR_TIM,  UAVCAN_STM32H7_TIMER_NUMBER, EN)
-#  define TIMX_RCC_RSTR_MASK     UAVCAN_STM32H7_GLUE3(RCC_APB1RSTR_TIM, UAVCAN_STM32H7_TIMER_NUMBER, RST)
-# else
-#  error "This UAVCAN_STM32H7_TIMER_NUMBER is not supported yet"
-# endif
-
-/**
- * UAVCAN_STM32H7_TIMX_INPUT_CLOCK can be used to manually override the auto-detected timer clock speed.
- * This is useful at least with certain versions of ChibiOS which do not support the bit
- * RCC_DKCFGR.TIMPRE that is available in newer models of STM32. In that case, if TIMPRE is active,
- * the auto-detected value of TIMX_INPUT_CLOCK will be twice lower than the actual clock speed.
- * Read this for additional context: http://www.chibios.com/forum/viewtopic.php?f=35&t=3870
- * A normal way to use the override feature is to provide an alternative macro, e.g.:
- *
- *      -DUAVCAN_STM32H7_TIMX_INPUT_CLOCK=STM32_HCLK
- *
- * Alternatively, the new clock rate can be specified directly.
- */
-# ifdef UAVCAN_STM32H7_TIMX_INPUT_CLOCK
-#  undef TIMX_INPUT_CLOCK
-#  define TIMX_INPUT_CLOCK      UAVCAN_STM32H7_TIMX_INPUT_CLOCK
-# endif
-
-extern "C" UAVCAN_STM32H7_IRQ_HANDLER(TIMX_IRQHandler);
+#include <drivers/drv_hrt.h>
 
 namespace uavcan_stm32h7
 {
@@ -57,26 +15,9 @@ namespace clock
 namespace
 {
 
-const uavcan::uint32_t USecPerOverflow = 65536;
-
 Mutex mutex;
 
 bool initialized = false;
-
-bool utc_set = false;
-bool utc_locked = false;
-uavcan::uint32_t utc_jump_cnt = 0;
-UtcSyncParams utc_sync_params;
-float utc_prev_adj = 0;
-float utc_rel_rate_ppm = 0;
-float utc_rel_rate_error_integral = 0;
-uavcan::int32_t utc_accumulated_correction_nsec = 0;
-uavcan::int32_t utc_correction_nsec_per_overflow = 0;
-uavcan::MonotonicTime prev_utc_adj_at;
-
-uavcan::uint64_t time_mono = 0;
-uavcan::uint64_t time_utc = 0;
-
 }
 
 
@@ -89,246 +30,34 @@ void init()
 	}
 
 	initialized = true;
-
-
-# if UAVCAN_STM32H7_NUTTX
-
-	// Attach IRQ
-	irq_attach(TIMX_IRQn, &TIMX_IRQHandler, NULL);
-
-	// Power-on and reset
-	modifyreg32(STM32_RCC_APB1ENR, 0, TIMX_RCC_ENR_MASK);
-	modifyreg32(STM32_RCC_APB1RSTR, 0, TIMX_RCC_RSTR_MASK);
-	modifyreg32(STM32_RCC_APB1RSTR, TIMX_RCC_RSTR_MASK, 0);
-
-
-	// Start the timer
-	putreg32(0xFFFF, TMR_REG(STM32_BTIM_ARR_OFFSET));
-	putreg16(((TIMX_INPUT_CLOCK / 1000000) - 1), TMR_REG(STM32_BTIM_PSC_OFFSET));
-	putreg16(BTIM_CR1_URS, TMR_REG(STM32_BTIM_CR1_OFFSET));
-	putreg16(0, TMR_REG(STM32_BTIM_SR_OFFSET));
-	putreg16(BTIM_EGR_UG, TMR_REG(STM32_BTIM_EGR_OFFSET)); // Reload immediately
-	putreg16(BTIM_DIER_UIE, TMR_REG(STM32_BTIM_DIER_OFFSET));
-	putreg16(BTIM_CR1_CEN, TMR_REG(STM32_BTIM_CR1_OFFSET)); // Start
-
-	// Prioritize and Enable  IRQ
-// todo: Currently changing the NVIC_SYSH_HIGH_PRIORITY is HARD faulting
-// need to investigate
-//    up_prioritize_irq(TIMX_IRQn, NVIC_SYSH_HIGH_PRIORITY);
-	up_enable_irq(TIMX_IRQn);
-
-# endif
 }
 
 void setUtc(uavcan::UtcTime time)
 {
-	MutexLocker mlocker(mutex);
-	UAVCAN_ASSERT(initialized);
-
-	{
-		CriticalSectionLocker locker;
-		time_utc = time.toUSec();
-	}
-
-	utc_set = true;
-	utc_locked = false;
-	utc_jump_cnt++;
-	utc_prev_adj = 0;
-	utc_rel_rate_ppm = 0;
-}
-
-static uavcan::uint64_t sampleUtcFromCriticalSection()
-{
-# if UAVCAN_STM32H7_NUTTX
-
-	UAVCAN_ASSERT(initialized);
-	UAVCAN_ASSERT(getreg16(TMR_REG(STM32_BTIM_DIER_OFFSET)) & BTIM_DIER_UIE);
-
-	volatile uavcan::uint64_t time = time_utc;
-	volatile uavcan::uint32_t cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
-
-	if (getreg16(TMR_REG(STM32_BTIM_SR_OFFSET)) & BTIM_SR_UIF) {
-		cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
-		const uavcan::int32_t add = uavcan::int32_t(USecPerOverflow) +
-					    (utc_accumulated_correction_nsec + utc_correction_nsec_per_overflow) / 1000;
-		time = uavcan::uint64_t(uavcan::int64_t(time) + add);
-	}
-
-	return time + cnt;
-# endif
+	// DO NOTHING
 }
 
 uavcan::uint64_t getUtcUSecFromCanInterrupt()
 {
-	return utc_set ? sampleUtcFromCriticalSection() : 0;
+	return hrt_absolute_time();
 }
 
 uavcan::MonotonicTime getMonotonic()
 {
-	uavcan::uint64_t usec = 0;
-	// Scope Critical section
-	{
-		CriticalSectionLocker locker;
-
-		volatile uavcan::uint64_t time = time_mono;
-
-# if UAVCAN_STM32H7_NUTTX
-
-		volatile uavcan::uint32_t cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
-
-		if (getreg16(TMR_REG(STM32_BTIM_SR_OFFSET)) & BTIM_SR_UIF) {
-			cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
-# endif
-			time += USecPerOverflow;
-		}
-
-		usec = time + cnt;
-
-# ifndef NDEBUG
-		static uavcan::uint64_t prev_usec = 0;      // Self-test
-		UAVCAN_ASSERT(prev_usec <= usec);
-		(void)prev_usec;
-		prev_usec = usec;
-# endif
-	} // End Scope Critical section
-
+	uavcan::uint64_t usec = hrt_absolute_time();
 	return uavcan::MonotonicTime::fromUSec(usec);
 }
 
 uavcan::UtcTime getUtc()
 {
-	if (utc_set) {
-		uavcan::uint64_t usec = 0;
-		{
-			CriticalSectionLocker locker;
-			usec = sampleUtcFromCriticalSection();
-		}
-		return uavcan::UtcTime::fromUSec(usec);
-	}
-
-	return uavcan::UtcTime();
-}
-
-static float lowpass(float xold, float xnew, float corner, float dt)
-{
-	const float tau = 1.F / corner;
-	return (dt * xnew + tau * xold) / (dt + tau);
-}
-
-static void updateRatePID(uavcan::UtcDuration adjustment)
-{
-	const uavcan::MonotonicTime ts = getMonotonic();
-	const float dt = float((ts - prev_utc_adj_at).toUSec()) / 1e6F;
-	prev_utc_adj_at = ts;
-	const float adj_usec = float(adjustment.toUSec());
-
-	/*
-	 * Target relative rate in PPM
-	 * Positive to go faster
-	 */
-	const float target_rel_rate_ppm = adj_usec * utc_sync_params.offset_p;
-
-	/*
-	 * Current relative rate in PPM
-	 * Positive if the local clock is faster
-	 */
-	const float new_rel_rate_ppm = (utc_prev_adj - adj_usec) / dt; // rate error in [usec/sec], which is PPM
-	utc_prev_adj = adj_usec;
-	utc_rel_rate_ppm = lowpass(utc_rel_rate_ppm, new_rel_rate_ppm, utc_sync_params.rate_error_corner_freq, dt);
-
-	const float rel_rate_error = target_rel_rate_ppm - utc_rel_rate_ppm;
-
-	if (dt > 10) {
-		utc_rel_rate_error_integral = 0;
-
-	} else {
-		utc_rel_rate_error_integral += rel_rate_error * dt * utc_sync_params.rate_i;
-		utc_rel_rate_error_integral =
-			uavcan::max(utc_rel_rate_error_integral, -utc_sync_params.max_rate_correction_ppm);
-		utc_rel_rate_error_integral =
-			uavcan::min(utc_rel_rate_error_integral, utc_sync_params.max_rate_correction_ppm);
-	}
-
-	/*
-	 * Rate controller
-	 */
-	float total_rate_correction_ppm = rel_rate_error + utc_rel_rate_error_integral;
-	total_rate_correction_ppm = uavcan::max(total_rate_correction_ppm, -utc_sync_params.max_rate_correction_ppm);
-	total_rate_correction_ppm = uavcan::min(total_rate_correction_ppm, utc_sync_params.max_rate_correction_ppm);
-
-	utc_correction_nsec_per_overflow = uavcan::int32_t((USecPerOverflow * 1000) * (total_rate_correction_ppm / 1e6F));
-
-//    syslog("$ adj=%f   rel_rate=%f   rel_rate_eint=%f   tgt_rel_rate=%f   ppm=%f\n",
-//              adj_usec, utc_rel_rate_ppm, utc_rel_rate_error_integral, target_rel_rate_ppm,
-// total_rate_correction_ppm);
+	uavcan::uint64_t usec = hrt_absolute_time();
+	return uavcan::UtcTime::fromUSec(usec);
 }
 
 void adjustUtc(uavcan::UtcDuration adjustment)
 {
-	MutexLocker mlocker(mutex);
-	UAVCAN_ASSERT(initialized);
-
-	if (adjustment.getAbs() > utc_sync_params.min_jump || !utc_set) {
-		const uavcan::int64_t adj_usec = adjustment.toUSec();
-
-		{
-			CriticalSectionLocker locker;
-
-			if ((adj_usec < 0) && uavcan::uint64_t(-adj_usec) > time_utc) {
-				time_utc = 1;
-
-			} else {
-				time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + adj_usec);
-			}
-		}
-
-		utc_set = true;
-		utc_locked = false;
-		utc_jump_cnt++;
-		utc_prev_adj = 0;
-		utc_rel_rate_ppm = 0;
-
-	} else {
-		updateRatePID(adjustment);
-
-		if (!utc_locked) {
-			utc_locked =
-				(std::abs(utc_rel_rate_ppm) < utc_sync_params.lock_thres_rate_ppm) &&
-				(std::abs(utc_prev_adj) < float(utc_sync_params.lock_thres_offset.toUSec()));
-		}
-	}
-}
-
-float getUtcRateCorrectionPPM()
-{
-	MutexLocker mlocker(mutex);
-	const float rate_correction_mult = float(utc_correction_nsec_per_overflow) / float(USecPerOverflow * 1000);
-	return 1e6F * rate_correction_mult;
-}
-
-uavcan::uint32_t getUtcJumpCount()
-{
-	MutexLocker mlocker(mutex);
-	return utc_jump_cnt;
-}
-
-bool isUtcLocked()
-{
-	MutexLocker mlocker(mutex);
-	return utc_locked;
-}
-
-UtcSyncParams getUtcSyncParams()
-{
-	MutexLocker mlocker(mutex);
-	return utc_sync_params;
-}
-
-void setUtcSyncParams(const UtcSyncParams &params)
-{
-	MutexLocker mlocker(mutex);
-	// Add some sanity check
-	utc_sync_params = params;
+	const float adj_usec = float(adjustment.toUSec());
+	hrt_absolute_time_adjust(adj_usec);
 }
 
 } // namespace clock
@@ -354,47 +83,3 @@ SystemClock &SystemClock::instance()
 
 } // namespace uavcan_stm32h7
 
-
-/**
- * Timer interrupt handler
- */
-
-extern "C"
-UAVCAN_STM32H7_IRQ_HANDLER(TIMX_IRQHandler)
-{
-	UAVCAN_STM32H7_IRQ_PROLOGUE();
-
-# if UAVCAN_STM32H7_NUTTX
-	putreg16(0, TMR_REG(STM32_BTIM_SR_OFFSET));
-# endif
-
-	using namespace uavcan_stm32h7::clock;
-	UAVCAN_ASSERT(initialized);
-
-	time_mono += USecPerOverflow;
-
-	if (utc_set) {
-		time_utc += USecPerOverflow;
-		utc_accumulated_correction_nsec += utc_correction_nsec_per_overflow;
-
-		if (std::abs(utc_accumulated_correction_nsec) >= 1000) {
-			time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + utc_accumulated_correction_nsec / 1000);
-			utc_accumulated_correction_nsec %= 1000;
-		}
-
-		// Correction decay - 1 nsec per 65536 usec
-		if (utc_correction_nsec_per_overflow > 0) {
-			utc_correction_nsec_per_overflow--;
-
-		} else if (utc_correction_nsec_per_overflow < 0) {
-			utc_correction_nsec_per_overflow++;
-
-		} else {
-			; // Zero
-		}
-	}
-
-	UAVCAN_STM32H7_IRQ_EPILOGUE();
-}
-
-#endif

src/drivers/uavcan/uavcan_main.cpp

@@ -744,41 +744,6 @@ UavcanNode::Run()
 
 	_node.spinOnce(); // expected to be non-blocking
 
-	// Publish status
-	constexpr hrt_abstime status_pub_interval = 100_ms;
-
-	if (hrt_absolute_time() - _last_can_status_pub >= status_pub_interval) {
-		_last_can_status_pub = hrt_absolute_time();
-
-		for (int i = 0; i < _node.getDispatcher().getCanIOManager().getCanDriver().getNumIfaces(); i++) {
-			if (i > UAVCAN_NUM_IFACES) {
-				break;
-			}
-
-			auto iface = _node.getDispatcher().getCanIOManager().getCanDriver().getIface(i);
-
-			if (!iface) {
-				continue;
-			}
-
-			auto iface_perf_cnt = _node.getDispatcher().getCanIOManager().getIfacePerfCounters(i);
-			can_interface_status_s status{
-				.timestamp = hrt_absolute_time(),
-				.io_errors = iface_perf_cnt.errors,
-				.frames_tx = iface_perf_cnt.frames_tx,
-				.frames_rx = iface_perf_cnt.frames_rx,
-				.interface = static_cast<uint8_t>(i),
-			};
-
-			if (_can_status_pub_handles[i] == nullptr) {
-				int instance{0};
-				_can_status_pub_handles[i] = orb_advertise_multi(ORB_ID(can_interface_status), nullptr, &instance);
-			}
-
-			(void)orb_publish(ORB_ID(can_interface_status), _can_status_pub_handles[i], &status);
-		}
-	}
-
 	// check for parameter updates
 	if (_parameter_update_sub.updated()) {
 		// clear update

src/drivers/uavcan/uavcan_main.hpp

@@ -96,7 +96,6 @@
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionInterval.hpp>
-#include <uORB/topics/can_interface_status.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/uavcan_parameter_request.h>
 #include <uORB/topics/uavcan_parameter_value.h>
@@ -310,10 +309,6 @@ private:
 
 	uORB::Publication<uavcan_parameter_value_s> _param_response_pub{ORB_ID(uavcan_parameter_value)};
 	uORB::Publication<vehicle_command_ack_s>	_command_ack_pub{ORB_ID(vehicle_command_ack)};
-	uORB::PublicationMulti<can_interface_status_s> _can_status_pub{ORB_ID(can_interface_status)};
-
-	hrt_abstime _last_can_status_pub{0};
-	orb_advert_t _can_status_pub_handles[UAVCAN_NUM_IFACES] = {nullptr};
 
 	/*
 	 * The MAVLink parameter bridge needs to know the maximum parameter index

src/drivers/uavcannode/CMakeLists.txt

@@ -42,22 +42,12 @@ set(UAVCAN_PLATFORM "generic")
 if(CONFIG_ARCH_CHIP)
 	if(${CONFIG_NET_CAN} MATCHES "y")
 		set(UAVCAN_DRIVER "socketcan")
-		set(UAVCAN_TIMER 1)
 	elseif(${CONFIG_ARCH_CHIP} MATCHES "kinetis")
 		set(UAVCAN_DRIVER "kinetis")
-		set(UAVCAN_TIMER 1)
 	elseif(${CONFIG_ARCH_CHIP} MATCHES "stm32h7")
 		set(UAVCAN_DRIVER "stm32h7")
-		set(UAVCAN_TIMER 5) # The default timer is TIM5
-		if (DEFINED config_uavcan_timer_override)
-			set (UAVCAN_TIMER  ${config_uavcan_timer_override})
-		endif()
 	elseif(${CONFIG_ARCH_CHIP} MATCHES "stm32")
 		set(UAVCAN_DRIVER "stm32")
-		set(UAVCAN_TIMER 5) # The default timer is TIM5
-		if (DEFINED config_uavcan_timer_override)
-			set (UAVCAN_TIMER  ${config_uavcan_timer_override})
-		endif()
 	endif()
 endif()
 
@@ -74,7 +64,6 @@ string(TOUPPER "${UAVCAN_DRIVER}" UAVCAN_DRIVER_UPPER)
 add_definitions(
 	-DUAVCAN_${UAVCAN_DRIVER_UPPER}_${OS_UPPER}=1
 	-DUAVCAN_${UAVCAN_DRIVER_UPPER}_NUM_IFACES=${config_uavcan_num_ifaces}
-	-DUAVCAN_${UAVCAN_DRIVER_UPPER}_TIMER_NUMBER=${UAVCAN_TIMER}
 	-DUAVCAN_CPP_VERSION=UAVCAN_CPP03
 	-DUAVCAN_DRIVER=uavcan_${UAVCAN_DRIVER}
 	-DUAVCAN_IMPLEMENT_PLACEMENT_NEW=1

src/lib/events/enums.json

@@ -364,7 +364,7 @@
                         },
                         "1": {
                             "name": "manual_control_loss",
-                            "description": "Manual control loss"
+                            "description": "manual control loss"
                         },
                         "2": {
                             "name": "gcs_connection_loss",
@@ -372,19 +372,19 @@
                         },
                         "3": {
                             "name": "low_battery_level",
-                            "description": "Low battery level"
+                            "description": "low battery level"
                         },
                         "4": {
                             "name": "critical_battery_level",
-                            "description": "Critical battery level"
+                            "description": "critical battery level"
                         },
                         "5": {
                             "name": "emergency_battery_level",
-                            "description": "Emergency battery level"
+                            "description": "emergency battery level"
                         },
                         "6": {
                             "name": "low_remaining_flight_time",
-                            "description": "Remaining flight time low"
+                            "description": "low remaining flight time"
                         }
                     }
                 },
@@ -402,19 +402,19 @@
                         },
                         "2": {
                             "name": "fallback_posctrl",
-                            "description": "Position mode"
+                            "description": "fallback to position control"
                         },
                         "3": {
                             "name": "fallback_altctrl",
-                            "description": "Altitude mode"
+                            "description": "fallback to altitude control"
                         },
                         "4": {
                             "name": "fallback_stabilized",
-                            "description": "Stabilized mode"
+                            "description": "fallback to stabilized"
                         },
                         "5": {
                             "name": "hold",
-                            "description": "Hold"
+                            "description": "hold"
                         },
                         "6": {
                             "name": "rtl",
@@ -422,11 +422,11 @@
                         },
                         "7": {
                             "name": "land",
-                            "description": "Land"
+                            "description": "land"
                         },
                         "8": {
                             "name": "descend",
-                            "description": "Descend"
+                            "description": "descend"
                         },
                         "9": {
                             "name": "disarm",

src/lib/mathlib/math/Utilities.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2024 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 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
@@ -51,18 +51,17 @@ static constexpr float sq(float var) { return var * var; }
 // rot312(1) - Second rotation is a RH rotation about the X axis (rad)
 // rot312(2) - Third rotation is a RH rotation about the Y axis (rad)
 // See http://www.atacolorado.com/eulersequences.doc
-template<typename T = float>
-inline matrix::Dcm<T> taitBryan312ToRotMat(const matrix::Vector3<T> &rot312)
+inline matrix::Dcmf taitBryan312ToRotMat(const matrix::Vector3f &rot312)
 {
 	// Calculate the frame2 to frame 1 rotation matrix from a 312 Tait-Bryan rotation sequence
-	const T c2 = cosf(rot312(2)); // third rotation is pitch
-	const T s2 = sinf(rot312(2));
-	const T s1 = sinf(rot312(1)); // second rotation is roll
-	const T c1 = cosf(rot312(1));
-	const T s0 = sinf(rot312(0)); // first rotation is yaw
-	const T c0 = cosf(rot312(0));
+	const float c2 = cosf(rot312(2)); // third rotation is pitch
+	const float s2 = sinf(rot312(2));
+	const float s1 = sinf(rot312(1)); // second rotation is roll
+	const float c1 = cosf(rot312(1));
+	const float s0 = sinf(rot312(0)); // first rotation is yaw
+	const float c0 = cosf(rot312(0));
 
-	matrix::Dcm<T> R;
+	matrix::Dcmf R;
 	R(0, 0) = c0 * c2 - s0 * s1 * s2;
 	R(1, 1) = c0 * c1;
 	R(2, 2) = c2 * c1;
@@ -76,21 +75,20 @@ inline matrix::Dcm<T> taitBryan312ToRotMat(const matrix::Vector3<T> &rot312)
 	return R;
 }
 
-template<typename T = float>
-inline matrix::Dcm<T> quatToInverseRotMat(const matrix::Quaternion<T> &quat)
+inline matrix::Dcmf quatToInverseRotMat(const matrix::Quatf &quat)
 {
-	const T q00 = quat(0) * quat(0);
-	const T q11 = quat(1) * quat(1);
-	const T q22 = quat(2) * quat(2);
-	const T q33 = quat(3) * quat(3);
-	const T q01 = quat(0) * quat(1);
-	const T q02 = quat(0) * quat(2);
-	const T q03 = quat(0) * quat(3);
-	const T q12 = quat(1) * quat(2);
-	const T q13 = quat(1) * quat(3);
-	const T q23 = quat(2) * quat(3);
+	const float q00 = quat(0) * quat(0);
+	const float q11 = quat(1) * quat(1);
+	const float q22 = quat(2) * quat(2);
+	const float q33 = quat(3) * quat(3);
+	const float q01 = quat(0) * quat(1);
+	const float q02 = quat(0) * quat(2);
+	const float q03 = quat(0) * quat(3);
+	const float q12 = quat(1) * quat(2);
+	const float q13 = quat(1) * quat(3);
+	const float q23 = quat(2) * quat(3);
 
-	matrix::Dcm<T> dcm;
+	matrix::Dcmf dcm;
 	dcm(0, 0) = q00 + q11 - q22 - q33;
 	dcm(1, 1) = q00 - q11 + q22 - q33;
 	dcm(2, 2) = q00 - q11 - q22 + q33;
@@ -107,46 +105,40 @@ inline matrix::Dcm<T> quatToInverseRotMat(const matrix::Quaternion<T> &quat)
 // We should use a 3-2-1 Tait-Bryan (yaw-pitch-roll) rotation sequence
 // when there is more roll than pitch tilt and a 3-1-2 rotation sequence
 // when there is more pitch than roll tilt to avoid gimbal lock.
-template<typename T = float>
-inline bool shouldUse321RotationSequence(const matrix::Dcm<T> &R)
+inline bool shouldUse321RotationSequence(const matrix::Dcmf &R)
 {
 	return fabsf(R(2, 0)) < fabsf(R(2, 1));
 }
 
-template<typename T = float>
-inline float getEuler321Yaw(const matrix::Dcm<T> &R)
+inline float getEuler321Yaw(const matrix::Dcmf &R)
 {
 	return atan2f(R(1, 0), R(0, 0));
 }
 
-template<typename T = float>
-inline float getEuler312Yaw(const matrix::Dcm<T> &R)
+inline float getEuler312Yaw(const matrix::Dcmf &R)
 {
 	return atan2f(-R(0, 1), R(1, 1));
 }
 
-template<typename T = float>
-inline T getEuler321Yaw(const matrix::Quaternion<T> &q)
+inline float getEuler321Yaw(const matrix::Quatf &q)
 {
 	// Values from yaw_input_321.c file produced by
 	// https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw321.m
-	const T a = static_cast<T>(2.) * (q(0) * q(3) + q(1) * q(2));
-	const T b = sq(q(0)) + sq(q(1)) - sq(q(2)) - sq(q(3));
+	const float a = 2.f * (q(0) * q(3) + q(1) * q(2));
+	const float b = sq(q(0)) + sq(q(1)) - sq(q(2)) - sq(q(3));
 	return atan2f(a, b);
 }
 
-template<typename T = float>
-inline T getEuler312Yaw(const matrix::Quaternion<T> &q)
+inline float getEuler312Yaw(const matrix::Quatf &q)
 {
 	// Values from yaw_input_312.c file produced by
 	// https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw312.m
-	const T a = static_cast<T>(2.) * (q(0) * q(3) - q(1) * q(2));
-	const T b = sq(q(0)) - sq(q(1)) + sq(q(2)) - sq(q(3));
+	const float a = 2.f * (q(0) * q(3) - q(1) * q(2));
+	const float b = sq(q(0)) - sq(q(1)) + sq(q(2)) - sq(q(3));
 	return atan2f(a, b);
 }
 
-template<typename T = float>
-inline T getEulerYaw(const matrix::Dcm<T> &R)
+inline float getEulerYaw(const matrix::Dcmf &R)
 {
 	if (shouldUse321RotationSequence(R)) {
 		return getEuler321Yaw(R);
@@ -156,32 +148,23 @@ inline T getEulerYaw(const matrix::Dcm<T> &R)
 	}
 }
 
-template<typename T = float>
-inline T getEulerYaw(const matrix::Quaternion<T> &q)
+inline matrix::Dcmf updateEuler321YawInRotMat(float yaw, const matrix::Dcmf &rot_in)
 {
-	return getEulerYaw(matrix::Dcm<T>(q));
-}
-
-template<typename T = float>
-inline matrix::Dcm<T> updateEuler321YawInRotMat(T yaw, const matrix::Dcm<T> &rot_in)
-{
-	matrix::Euler<T> euler321(rot_in);
+	matrix::Eulerf euler321(rot_in);
 	euler321(2) = yaw;
-	return matrix::Dcm<T>(euler321);
+	return matrix::Dcmf(euler321);
 }
 
-template<typename T = float>
-inline matrix::Dcm<T> updateEuler312YawInRotMat(T yaw, const matrix::Dcm<T> &rot_in)
+inline matrix::Dcmf updateEuler312YawInRotMat(float yaw, const matrix::Dcmf &rot_in)
 {
-	const matrix::Vector3<T> rotVec312(yaw,                                  // yaw
-					   asinf(rot_in(2, 1)),                  // roll
-					   atan2f(-rot_in(2, 0), rot_in(2, 2))); // pitch
+	const matrix::Vector3f rotVec312(yaw,  // yaw
+					 asinf(rot_in(2, 1)),  // roll
+					 atan2f(-rot_in(2, 0), rot_in(2, 2)));  // pitch
 	return taitBryan312ToRotMat(rotVec312);
 }
 
 // Checks which euler rotation sequence to use and update yaw in rotation matrix
-template<typename T = float>
-inline matrix::Dcm<T> updateYawInRotMat(T yaw, const matrix::Dcm<T> &rot_in)
+inline matrix::Dcmf updateYawInRotMat(float yaw, const matrix::Dcmf &rot_in)
 {
 	if (shouldUse321RotationSequence(rot_in)) {
 		return updateEuler321YawInRotMat(yaw, rot_in);

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/commander/Commander.cpp

@@ -598,9 +598,6 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_
 
 		if (!_health_and_arming_checks.canArm(_vehicle_status.nav_state)) {
 			tune_negative(true);
-			mavlink_log_critical(&_mavlink_log_pub, "Arming denied: Resolve system health failures first\t");
-			events::send(events::ID("commander_arm_denied_resolve_failures"), {events::Log::Critical, events::LogInternal::Info},
-				     "Arming denied: Resolve system health failures first");
 			return TRANSITION_DENIED;
 		}
 	}
@@ -2422,14 +2419,7 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
 		if (_cpuload_sub.copy(&cpuload)) {
 			const float cpuload_percent = cpuload.load * 100.f;
 
-			bool overload = false;
-
-			// Only check CPU load if it hasn't been disabled
-			if (!(_param_com_cpu_max.get() < FLT_EPSILON)) {
-				overload = (cpuload_percent > _param_com_cpu_max.get());
-			}
-
-			overload = overload || (cpuload.ram_usage > 0.99f);
+			bool overload = (cpuload_percent > _param_com_cpu_max.get()) || (cpuload.ram_usage > 0.99f);
 
 			if (_overload_start == 0 && overload) {
 				_overload_start = time_now_us;

src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp

@@ -293,9 +293,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
 					mavlink_log_warning(reporter.mavlink_log_pub(), "GNSS data fusion stopped\t");
 				}
 
-				// only report this failure as critical if not already in a local position invalid state
-				events::Log log_level = reporter.failsafeFlags().local_position_invalid ? events::Log::Info : events::Log::Error;
-				events::send(events::ID("check_estimator_gnss_fusion_stopped"), {log_level, events::LogInternal::Info},
+				events::send(events::ID("check_estimator_gnss_fusion_stopped"), {events::Log::Error, events::LogInternal::Info},
 					     "GNSS data fusion stopped");
 
 			} else if (!_gps_was_fused && ekf_gps_fusion) {

src/modules/commander/failsafe/failsafe.cpp

@@ -369,7 +369,7 @@ FailsafeBase::ActionOptions Failsafe::fromRemainingFlightTimeLowActParam(int par
 {
 	ActionOptions options{};
 
-	options.allow_user_takeover = UserTakeoverAllowed::Auto;
+	options.allow_user_takeover = UserTakeoverAllowed::Always;
 	options.cause = Cause::RemainingFlightTimeLow;
 
 	switch (command_after_remaining_flight_time_low(param_value)) {

src/modules/commander/failsafe/framework.cpp

@@ -195,7 +195,7 @@ void FailsafeBase::notifyUser(uint8_t user_intended_mode, Action action, Action
 			events::send<uint32_t, events::px4::enums::failsafe_action_t, uint16_t>(
 				events::ID("commander_failsafe_enter_generic_hold"),
 			{events::Log::Critical, events::LogInternal::Warning},
-			"Failsafe, triggering {2} in {3} seconds", mavlink_mode, failsafe_action,
+			"Failsafe activated, triggering {2} in {3} seconds", mavlink_mode, failsafe_action,
 			(uint16_t) delay_s);
 
 		} else {
@@ -204,7 +204,7 @@ void FailsafeBase::notifyUser(uint8_t user_intended_mode, Action action, Action
 			events::send<uint32_t, events::px4::enums::failsafe_action_t, uint16_t, events::px4::enums::failsafe_cause_t>(
 				events::ID("commander_failsafe_enter_hold"),
 			{events::Log::Critical, events::LogInternal::Warning},
-			"{4}, triggering {2} in {3} seconds", mavlink_mode, failsafe_action,
+			"Failsafe activated due to {4}, triggering {2} in {3} seconds", mavlink_mode, failsafe_action,
 			(uint16_t) delay_s, failsafe_cause);
 		}
 
@@ -231,7 +231,7 @@ void FailsafeBase::notifyUser(uint8_t user_intended_mode, Action action, Action
 				events::send<uint32_t, events::px4::enums::failsafe_action_t>(
 					events::ID("commander_failsafe_enter_generic"),
 				{events::Log::Critical, events::LogInternal::Warning},
-				"Failsafe, triggering {2}", mavlink_mode, failsafe_action);
+				"Failsafe activated, triggering {2}", mavlink_mode, failsafe_action);
 			}
 
 		} else {
@@ -272,7 +272,7 @@ void FailsafeBase::notifyUser(uint8_t user_intended_mode, Action action, Action
 				events::send<uint32_t, events::px4::enums::failsafe_action_t, events::px4::enums::failsafe_cause_t>(
 					events::ID("commander_failsafe_enter"),
 				{events::Log::Critical, events::LogInternal::Warning},
-				"{3}, triggering {2}", mavlink_mode, failsafe_action, failsafe_cause);
+				"Failsafe activated due to {3}, triggering {2}", mavlink_mode, failsafe_action, failsafe_cause);
 			}
 		}
 
@@ -517,8 +517,6 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s
 			break;
 		}
 
-		returned_state.cause = Cause::Generic;
-
 	// fallthrough
 	case Action::FallbackAltCtrl:
 		if (modeCanRun(status_flags, vehicle_status_s::NAVIGATION_STATE_ALTCTL)) {
@@ -526,8 +524,6 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s
 			break;
 		}
 
-		returned_state.cause = Cause::Generic;
-
 	// fallthrough
 	case Action::FallbackStab:
 		if (modeCanRun(status_flags, vehicle_status_s::NAVIGATION_STATE_STAB)) {
@@ -535,8 +531,6 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s
 			break;
 		} // else: fall through here as well. If stabilized isn't available, we most certainly end up in Terminate
 
-		returned_state.cause = Cause::Generic;
-
 	// fallthrough
 	case Action::Hold:
 		if (modeCanRun(status_flags, vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER)) {
@@ -544,8 +538,6 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s
 			break;
 		}
 
-		returned_state.cause = Cause::Generic;
-
 	// fallthrough
 	case Action::RTL:
 		if (modeCanRun(status_flags, vehicle_status_s::NAVIGATION_STATE_AUTO_RTL)) {
@@ -553,8 +545,6 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s
 			break;
 		}
 
-		returned_state.cause = Cause::Generic;
-
 	// fallthrough
 	case Action::Land:
 		if (modeCanRun(status_flags, vehicle_status_s::NAVIGATION_STATE_AUTO_LAND)) {
@@ -562,8 +552,6 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s
 			break;
 		}
 
-		returned_state.cause = Cause::Generic;
-
 	// fallthrough
 	case Action::Descend:
 		if (modeCanRun(status_flags, vehicle_status_s::NAVIGATION_STATE_DESCEND)) {
@@ -571,8 +559,6 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s
 			break;
 		}
 
-		returned_state.cause = Cause::Generic;
-
 	// fallthrough
 	case Action::Terminate:
 		selected_action = Action::Terminate;

src/modules/commander/failsafe/framework.h

@@ -147,7 +147,7 @@ public:
 		       bool rc_sticks_takeover_request,
 		       const failsafe_flags_s &status_flags);
 
-	bool inFailsafe() const { return (_selected_action != Action::None && _selected_action != Action::Warn); }
+	bool inFailsafe() const { return _selected_action != Action::None; }
 
 	Action selectedAction() const { return _selected_action; }
 

src/modules/ekf2/EKF/ekf.h

@@ -485,14 +485,9 @@ public:
 #else
 		// Efficient implementation of the Joseph stabilized covariance update
 		// Based on "G. J. Bierman. Factorization Methods for Discrete Sequential Estimation. Academic Press, Dover Publications, New York, 1977, 2006"
-		// P = (I - K * H) * P * (I - K * H).T   + K * R * K.T
-		//   =      P_temp     * (I - H.T * K.T) + K * R * K.T
-		//   =      P_temp - P_temp * H.T * K.T  + K * R * K.T
 
 		// Step 1: conventional update
-		// Compute P_temp and store it in P to avoid allocating more memory
-		// P is symmetric, so PH == H.T * P.T == H.T * P. Taking the row is faster as matrices are row-major
-		VectorState PH = P * H; // H is stored as a column vector. H is in fact H.T
+		VectorState PH = P * H;
 
 		for (unsigned i = 0; i < State::size; i++) {
 			for (unsigned j = 0; j < State::size; j++) {
@@ -501,7 +496,7 @@ public:
 		}
 
 		// Step 2: stabilized update
-		PH = P * H; // H is stored as a column vector. H is in fact H.T
+		PH = P * H;
 
 		for (unsigned i = 0; i < State::size; i++) {
 			for (unsigned j = 0; j <= i; j++) {