[RFC] broadcast parameter changes and ModuleParams skip full updateParams() when possible

This checks was introduced to catch the (unlikely) case of the driver publishing
a stale value that is not actually from a current measurement right after boot.
This was done by declaring it invalid if there is no update of the measurement
within the first 10s after boot.
Practice though has shown that around 0, many airspeed sensors have such a low
resolution that the reported airspeed value is often at the exact same value
for longer periods of time on totally healthy sensors, and thus we trigger
some false positive failure detections.
Given that this failure mode (driver publishing stale data) is quite rare (if
it doesn't receive new data it should stop publishing), I remove this check
here again.
When in aerodynamic flight mode and armed, there is still the data stuck
check that can trigger if there is no update for 2s.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

.github/workflows/sitl_tests.yml

@@ -16,7 +16,7 @@ jobs:
       matrix:
         config:
           - {model: "iris",          latitude:  "59.617693", longitude: "-151.145316", altitude:  "48", build_type: "RelWithDebInfo" } # Alaska
-          - {model: "standard_vtol", latitude: "-38.071235", longitude:  "145.281220", altitude:  "31", build_type: "AddressSanitizer" } # Australia
+          # - {model: "standard_vtol", latitude: "-38.071235", longitude:  "145.281220", altitude:  "31", build_type: "AddressSanitizer" } # Australia
           - {model: "tailsitter" ,   latitude:  "29.660316", longitude:  "-82.316658", altitude:  "30", build_type: "RelWithDebInfo" } # Florida
           - {model: "standard_vtol", latitude:  "47.397742", longitude:    "8.545594", altitude: "488", build_type: "Coverage" } # Zurich
 

Kconfig

@@ -40,8 +40,8 @@ menu "Toolchain"
         help
             forces nolockstep behaviour, despite REPLAY env variable
 
-    config BOARD_LINUX
-        bool "Linux OS"
+    config BOARD_LINUX_TARGET
+        bool "Linux OS Target"
         depends on PLATFORM_POSIX
         help
             Board Platform is running the Linux operating system

ROMFS/px4fmu_common/init.d-posix/rcS

@@ -142,7 +142,9 @@ then
 	param set CAL_GYRO2_ID 1311004 # 1311004: DRV_IMU_DEVTYPE_SIM, BUS: 3, ADDR: 1, TYPE: SIMULATION
 
 	param set CAL_MAG0_ID 197388
+	param set CAL_MAG0_PRIO 50
 	param set CAL_MAG1_ID 197644
+	param set CAL_MAG1_PRIO 50
 
 	param set SENS_BOARD_X_OFF 0.000001
 	param set SENS_DPRES_OFF 0.001

Tools/generate_board_targets_json.py

@@ -28,7 +28,7 @@ args = parser.parse_args()
 verbose = args.verbose
 
 build_configs = []
-excluded_boards = ['modalai_voxl2']  # TODO: fix and enable
+excluded_boards = ['modalai_voxl2', 'px4_ros2']  # TODO: fix and enable
 excluded_manufacturers = ['atlflight']
 excluded_platforms = ['qurt']
 excluded_labels = [

Tools/msg/px_generate_uorb_topic_files.py

@@ -122,21 +122,13 @@ def generate_output_from_file(format_idx, filename, outputdir, package, template
     for field in spec.parsed_fields():
         field_name_and_type.update({field.name: field.type})
 
-    # assert if the timestamp field exists
-    try:
-        assert 'timestamp' in field_name_and_type
-    except AssertionError:
-        print("[ERROR] uORB topic files generator:\n\tgenerate_output_from_file:\tNo 'timestamp' field found in " +
-              spec.short_name + " msg definition!")
-        exit(1)
-
     # assert if the timestamp field is of type uint64
-    try:
-        assert field_name_and_type.get('timestamp') == 'uint64'
-    except AssertionError:
-        print("[ERROR] uORB topic files generator:\n\tgenerate_output_from_file:\t'timestamp' field in " + spec.short_name +
-              " msg definition is not of type uint64 but rather of type " + field_name_and_type.get('timestamp') + "!")
-        exit(1)
+    # try:
+    #     assert field_name_and_type.get('timestamp') == 'uint64'
+    # except AssertionError:
+    #     print("[ERROR] uORB topic files generator:\n\tgenerate_output_from_file:\t'timestamp' field in " + spec.short_name +
+    #           " msg definition is not of type uint64 but rather of type " + field_name_and_type.get('timestamp') + "!")
+    #     exit(1)
 
     topics = get_topics(filename)
 

Tools/serial/generate_config.py

@@ -136,9 +136,9 @@ serial_ports = {
         "default_baudrate": 1, # set default to an unusable value to detect that this serial port has not been configured
         },
 
-    # Pixhawk Payload Bus
-    "PPB": {
-        "label": "Pixhawk Payload Bus",
+    # EXT2
+    "EXT2": {
+        "label": "EXT2",
         "index": 401,
         "default_baudrate": 57600,
         },

boards/ark/fmu-v6x/nuttx-config/nsh/defconfig

@@ -159,6 +159,7 @@ CONFIG_NET_BROADCAST=y
 CONFIG_NET_ETH_PKTSIZE=1518
 CONFIG_NET_ICMP=y
 CONFIG_NET_ICMP_SOCKET=y
+CONFIG_NET_NACTIVESOCKETS=16
 CONFIG_NET_SOLINGER=y
 CONFIG_NET_TCP=y
 CONFIG_NET_TCPBACKLOG=y

boards/av/x-v1/nuttx-config/nsh/defconfig

@@ -138,6 +138,7 @@ CONFIG_NET_ARP_SEND=y
 CONFIG_NET_BROADCAST=y
 CONFIG_NET_ICMP=y
 CONFIG_NET_ICMP_SOCKET=y
+CONFIG_NET_NACTIVESOCKETS=16
 CONFIG_NET_SOLINGER=y
 CONFIG_NET_TCP=y
 CONFIG_NET_TCPBACKLOG=y

boards/beaglebone/blue/cmake/init.cmake

@@ -35,8 +35,8 @@ add_compile_options(-Wno-cast-align)
 
 include(ExternalProject)
 ExternalProject_Add(librobotcontrol
-	GIT_REPOSITORY https://github.com/dagar/librobotcontrol.git
-	GIT_TAG 1abcb0a # latest as of 2019-12-29
+        GIT_REPOSITORY https://github.com/beagleboard/librobotcontrol.git
+        GIT_TAG 290e14f
 	CMAKE_CACHE_ARGS -DCMAKE_TOOLCHAIN_FILE:STRING=${CMAKE_TOOLCHAIN_FILE}
 	INSTALL_COMMAND ""
 	TEST_COMMAND ""

boards/beaglebone/blue/default.px4board

@@ -1,5 +1,5 @@
 CONFIG_PLATFORM_POSIX=y
-CONFIG_BOARD_LINUX=y
+CONFIG_BOARD_LINUX_TARGET=y
 CONFIG_BOARD_TOOLCHAIN="arm-linux-gnueabihf"
 CONFIG_BOARD_ARCHITECTURE="cortex-a8"
 CONFIG_BOARD_TESTING=y

boards/emlid/navio2/default.px4board

@@ -1,5 +1,5 @@
 CONFIG_PLATFORM_POSIX=y
-CONFIG_BOARD_LINUX=y
+CONFIG_BOARD_LINUX_TARGET=y
 CONFIG_BOARD_TOOLCHAIN="arm-linux-gnueabihf"
 CONFIG_BOARD_ARCHITECTURE="cortex-a53"
 CONFIG_BOARD_TESTING=y

boards/modalai/voxl2-slpi/default.px4board

@@ -4,3 +4,4 @@ CONFIG_MODULES_MUORB_SLPI=y
 CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_ORB_COMMUNICATOR=y
 CONFIG_SYSTEMCMDS_PARAM=y
+CONFIG_DRIVERS_QSHELL_QURT=y

boards/modalai/voxl2-slpi/src/CMakeLists.txt

@@ -34,4 +34,8 @@
 add_library(drivers_board
 	board_config.h
 	init.c
+	spi.cpp
     )
+
+# Add custom drivers for SLPI
+add_subdirectory(${PX4_BOARD_DIR}/src/drivers/icm42688p)

boards/modalai/voxl2-slpi/src/board_config.h

@@ -46,5 +46,14 @@
  */
 #define PX4_NUMBER_I2C_BUSES    3
 
+/*
+ * SPI buses
+ */
+#define CONFIG_SPI 1
+#define BOARD_SPI_BUS_MAX_BUS_ITEMS 1
+
+/*
+ * Include these last to make use of the definitions above
+ */
 #include <system_config.h>
 #include <px4_platform_common/board_common.h>

boards/modalai/voxl2-slpi/src/drivers/icm42688p/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2020 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -30,20 +30,19 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 ############################################################################
-
-add_subdirectory(AngularVelocityControl)
-
 px4_add_module(
-	MODULE modules__angular_velocity_control
-	MAIN angular_velocity_controller
+	MODULE drivers__imu__invensense__icm42688p
+	MAIN icm42688p
 	COMPILE_FLAGS
 		${MAX_CUSTOM_OPT_LEVEL}
 	SRCS
-		AngularVelocityController.cpp
-		AngularVelocityController.hpp
+		icm42688p_main.cpp
+		ICM42688P.cpp
+		ICM42688P.hpp
+		InvenSense_ICM42688P_registers.hpp
 	DEPENDS
-		circuit_breaker
-		mathlib
-		AngularVelocityControl
 		px4_work_queue
+		drivers_accelerometer
+		drivers_gyroscope
+		drivers__device
 	)

boards/modalai/voxl2-slpi/src/drivers/icm42688p/ICM42688P.cpp

@@ -0,0 +1,985 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "ICM42688P.hpp"
+
+bool hitl_mode = false;
+
+using namespace time_literals;
+
+static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
+{
+	return (msb << 8u) | lsb;
+}
+
+ICM42688P::ICM42688P(const I2CSPIDriverConfig &config) :
+	// SPI(DRV_IMU_DEVTYPE_ICM42688P, MODULE_NAME, bus, device, spi_mode, bus_frequency),
+	// I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus),
+	// _drdy_gpio(drdy_gpio)
+	SPI(config),
+	I2CSPIDriver(config),
+	_drdy_gpio(config.drdy_gpio),
+	_px4_accel(get_device_id(), config.rotation),
+	_px4_gyro(get_device_id(), config.rotation)
+{
+	if (config.drdy_gpio != 0) {
+		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
+	}
+
+	if (!hitl_mode) {
+		// _px4_accel = std::make_shared<PX4Accelerometer>(get_device_id(), rotation);
+		// _px4_gyro = std::make_shared<PX4Gyroscope>(get_device_id(), rotation);
+		ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
+		// _imu_server_pub.advertise();
+
+	} else {
+		ConfigureSampleRate(0);
+	}
+}
+
+ICM42688P::~ICM42688P()
+{
+	perf_free(_bad_register_perf);
+	perf_free(_bad_transfer_perf);
+	perf_free(_fifo_empty_perf);
+	perf_free(_fifo_overflow_perf);
+	perf_free(_fifo_reset_perf);
+	perf_free(_drdy_missed_perf);
+
+	// if (!hitl_mode){
+	//  		_imu_server_pub.unadvertise();
+	// }
+}
+
+int ICM42688P::init()
+{
+	int ret = SPI::init();
+
+	if (ret != PX4_OK) {
+		DEVICE_DEBUG("SPI::init failed (%i)", ret);
+		return ret;
+	}
+
+	return Reset() ? 0 : -1;
+}
+
+bool ICM42688P::Reset()
+{
+	_state = STATE::RESET;
+	DataReadyInterruptDisable();
+	ScheduleClear();
+	ScheduleNow();
+	return true;
+}
+
+void ICM42688P::exit_and_cleanup()
+{
+	DataReadyInterruptDisable();
+	I2CSPIDriverBase::exit_and_cleanup();
+}
+
+void ICM42688P::print_status()
+{
+	I2CSPIDriverBase::print_status();
+
+	PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us);
+
+	perf_print_counter(_bad_register_perf);
+	perf_print_counter(_bad_transfer_perf);
+	perf_print_counter(_fifo_empty_perf);
+	perf_print_counter(_fifo_overflow_perf);
+	perf_print_counter(_fifo_reset_perf);
+	perf_print_counter(_drdy_missed_perf);
+}
+
+int ICM42688P::probe()
+{
+	for (int i = 0; i < 3; i++) {
+		uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I);
+
+		if (whoami == WHOAMI) {
+			PX4_INFO("ICM42688P::probe successful!");
+			return PX4_OK;
+
+		} else {
+			DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
+
+			uint8_t reg_bank_sel = RegisterRead(Register::BANK_0::REG_BANK_SEL);
+			int bank = reg_bank_sel >> 4;
+
+			if (bank >= 1 && bank <= 3) {
+				DEVICE_DEBUG("incorrect register bank for WHO_AM_I REG_BANK_SEL:0x%02x, bank:%d", reg_bank_sel, bank);
+				// force bank selection and retry
+				SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0, true);
+			}
+		}
+	}
+
+	return PX4_ERROR;
+}
+
+void ICM42688P::RunImpl()
+{
+	PX4_INFO(">>> ICM42688P this: %p", this);
+
+	const hrt_abstime now = hrt_absolute_time();
+
+	switch (_state) {
+	case STATE::RESET:
+		// DEVICE_CONFIG: Software reset configuration
+		RegisterWrite(Register::BANK_0::DEVICE_CONFIG, DEVICE_CONFIG_BIT::SOFT_RESET_CONFIG);
+		_reset_timestamp = now;
+		_failure_count = 0;
+		_state = STATE::WAIT_FOR_RESET;
+		ScheduleDelayed(2_ms); // to be safe wait 2 ms for soft reset to be effective
+		break;
+
+	case STATE::WAIT_FOR_RESET:
+		if ((RegisterRead(Register::BANK_0::WHO_AM_I) == WHOAMI)
+		    && (RegisterRead(Register::BANK_0::DEVICE_CONFIG) == 0x00)
+		    && (RegisterRead(Register::BANK_0::INT_STATUS) & INT_STATUS_BIT::RESET_DONE_INT)) {
+
+			_state = STATE::CONFIGURE;
+			ScheduleDelayed(10_ms); // 30 ms gyro startup time, 10 ms accel from sleep to valid data
+
+		} else {
+			// RESET not complete
+			if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
+				PX4_DEBUG("Reset failed, retrying");
+				_state = STATE::RESET;
+				ScheduleDelayed(100_ms);
+
+			} else {
+				PX4_DEBUG("Reset not complete, check again in 10 ms");
+				ScheduleDelayed(10_ms);
+			}
+		}
+
+		break;
+
+	case STATE::CONFIGURE:
+
+		if (Configure()) {
+
+			// Wakeup accel and gyro after configuring registers
+			ScheduleDelayed(1_ms); // add a delay here to be safe
+			RegisterWrite(Register::BANK_0::PWR_MGMT0, PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE);
+			ScheduleDelayed(30_ms); // 30 ms gyro startup time, 10 ms accel from sleep to valid data
+
+			// if configure succeeded then start reading from FIFO
+			_state = STATE::FIFO_READ;
+
+			if (DataReadyInterruptConfigure()) {
+				_data_ready_interrupt_enabled = true;
+
+				// backup schedule as a watchdog timeout
+				ScheduleDelayed(100_ms);
+
+			} else {
+				PX4_ERR("ICM42688P::RunImpl interrupt configuration failed");
+
+				_data_ready_interrupt_enabled = false;
+				ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
+			}
+
+			FIFOReset();
+
+		} else {
+
+			PX4_ERR("ICM42688P::RunImpl configuration failed");
+
+			// CONFIGURE not complete
+			if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
+				PX4_DEBUG("Configure failed, resetting");
+				_state = STATE::RESET;
+
+			} else {
+				PX4_DEBUG("Configure failed, retrying");
+			}
+
+			ScheduleDelayed(100_ms);
+		}
+
+		break;
+
+	case STATE::FIFO_READ: {
+#ifndef __PX4_QURT
+			uint32_t samples = 0;
+
+			if (_data_ready_interrupt_enabled) {
+				// scheduled from interrupt if _drdy_fifo_read_samples was set as expected
+				if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
+					perf_count(_drdy_missed_perf);
+
+				} else {
+					samples = _fifo_gyro_samples;
+				}
+
+				// push backup schedule back
+				ScheduleDelayed(_fifo_empty_interval_us * 2);
+			}
+
+			if (samples == 0) {
+				// check current FIFO count
+				const uint16_t fifo_count = FIFOReadCount();
+
+				if (fifo_count >= FIFO::SIZE) {
+					FIFOReset();
+					perf_count(_fifo_overflow_perf);
+
+				} else if (fifo_count == 0) {
+					perf_count(_fifo_empty_perf);
+
+				} else {
+					// FIFO count (size in bytes)
+					samples = (fifo_count / sizeof(FIFO::DATA));
+
+					if (samples > FIFO_MAX_SAMPLES) {
+						// not technically an overflow, but more samples than we expected or can publish
+						FIFOReset();
+						perf_count(_fifo_overflow_perf);
+						samples = 0;
+					}
+				}
+			}
+
+			bool success = false;
+
+			if (samples >= 1) {
+				if (FIFORead(now, samples)) {
+					success = true;
+
+					if (_failure_count > 0) {
+						_failure_count--;
+					}
+				}
+			}
+
+			if (!success) {
+				_failure_count++;
+
+				// full reset if things are failing consistently
+				if (_failure_count > 10) {
+					Reset();
+					return;
+				}
+			}
+
+			// check configuration registers periodically or immediately following any failure
+			if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0])
+			    && RegisterCheck(_register_bank1_cfg[_checked_register_bank1])
+			    && RegisterCheck(_register_bank2_cfg[_checked_register_bank2])
+			   ) {
+				_last_config_check_timestamp = now;
+				_checked_register_bank0 = (_checked_register_bank0 + 1) % size_register_bank0_cfg;
+				_checked_register_bank1 = (_checked_register_bank1 + 1) % size_register_bank1_cfg;
+				_checked_register_bank2 = (_checked_register_bank2 + 1) % size_register_bank2_cfg;
+
+			} else {
+				// register check failed, force reset
+				perf_count(_bad_register_perf);
+				Reset();
+			}
+
+#endif
+		}
+
+		break;
+	}
+}
+
+void ICM42688P::ConfigureSampleRate(int sample_rate)
+{
+	if (sample_rate == 0) {
+		sample_rate = 800; // default to 800 Hz
+	}
+
+	// round down to nearest FIFO sample dt
+	const float min_interval = FIFO_SAMPLE_DT;
+	_fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval);
+
+	_fifo_gyro_samples = roundf(math::min((float)_fifo_empty_interval_us / (1e6f / GYRO_RATE), (float)FIFO_MAX_SAMPLES));
+
+	// recompute FIFO empty interval (us) with actual gyro sample limit
+	_fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE);
+
+	ConfigureFIFOWatermark(_fifo_gyro_samples);
+}
+
+void ICM42688P::ConfigureFIFOWatermark(uint8_t samples)
+{
+	// FIFO watermark threshold in number of bytes
+	const uint16_t fifo_watermark_threshold = samples * sizeof(FIFO::DATA);
+
+	for (auto &r : _register_bank0_cfg) {
+		if (r.reg == Register::BANK_0::FIFO_CONFIG2) {
+			// FIFO_WM[7:0]  FIFO_CONFIG2
+			r.set_bits = fifo_watermark_threshold & 0xFF;
+
+		} else if (r.reg == Register::BANK_0::FIFO_CONFIG3) {
+			// FIFO_WM[11:8] FIFO_CONFIG3
+			r.set_bits = (fifo_watermark_threshold >> 8) & 0x0F;
+		}
+	}
+}
+
+void ICM42688P::SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force)
+{
+	if (bank != _last_register_bank || force) {
+		// select BANK_0
+		uint8_t cmd_bank_sel[2] {};
+		cmd_bank_sel[0] = static_cast<uint8_t>(Register::BANK_0::REG_BANK_SEL);
+		cmd_bank_sel[1] = bank;
+		transfer(cmd_bank_sel, cmd_bank_sel, sizeof(cmd_bank_sel));
+
+		_last_register_bank = bank;
+	}
+}
+
+bool ICM42688P::Configure()
+{
+	// first set and clear all configured register bits
+	for (const auto &reg_cfg : _register_bank0_cfg) {
+		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
+	}
+
+	for (const auto &reg_cfg : _register_bank1_cfg) {
+		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
+	}
+
+	for (const auto &reg_cfg : _register_bank2_cfg) {
+		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
+	}
+
+	// now check that all are configured
+	bool success = true;
+
+	for (const auto &reg_cfg : _register_bank0_cfg) {
+		if (!RegisterCheck(reg_cfg)) {
+			success = false;
+		}
+	}
+
+	for (const auto &reg_cfg : _register_bank1_cfg) {
+		if (!RegisterCheck(reg_cfg)) {
+			success = false;
+		}
+	}
+
+	for (const auto &reg_cfg : _register_bank2_cfg) {
+		if (!RegisterCheck(reg_cfg)) {
+			success = false;
+		}
+	}
+
+	// // 20-bits data format used
+	// //  the only FSR settings that are operational are ±2000dps for gyroscope and ±16g for accelerometer
+	if (!hitl_mode) {
+		_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
+		_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
+		_px4_gyro.set_range(math::radians(2000.f));
+		_px4_gyro.set_scale(math::radians(1.f / 131.f));
+	}
+
+	return success;
+}
+
+static bool interrupt_debug = true;
+static uint32_t interrupt_debug_count = 0;
+static const uint32_t interrupt_debug_trigger = 800;
+static hrt_abstime last_interrupt_time = 0;
+static hrt_abstime avg_interrupt_delta = 0;
+static hrt_abstime max_interrupt_delta = 0;
+static hrt_abstime min_interrupt_delta = 60 * 1000 * 1000;
+static hrt_abstime cumulative_interrupt_delta = 0;
+
+int ICM42688P::DataReadyInterruptCallback(int irq, void *context, void *arg)
+{
+	hrt_abstime current_interrupt_time = hrt_absolute_time();
+
+	if (interrupt_debug) {
+		if (last_interrupt_time) {
+			hrt_abstime interrupt_delta_time = current_interrupt_time - last_interrupt_time;
+
+			if (interrupt_delta_time > max_interrupt_delta) { max_interrupt_delta = interrupt_delta_time; }
+
+			if (interrupt_delta_time < min_interrupt_delta) { min_interrupt_delta = interrupt_delta_time; }
+
+			cumulative_interrupt_delta += interrupt_delta_time;
+		}
+
+		last_interrupt_time = current_interrupt_time;
+
+		interrupt_debug_count++;
+
+		if (interrupt_debug_count == interrupt_debug_trigger) {
+			avg_interrupt_delta = cumulative_interrupt_delta / interrupt_debug_trigger;
+			PX4_INFO(">>> Max: %llu, Min: %llu, Avg: %llu", max_interrupt_delta,
+				 min_interrupt_delta, avg_interrupt_delta);
+			interrupt_debug_count = 0;
+			cumulative_interrupt_delta = 0;
+		}
+	}
+
+	static_cast<ICM42688P *>(arg)->DataReady();
+
+	return 0;
+}
+
+void ICM42688P::DataReady()
+{
+#ifndef __PX4_QURT
+	uint32_t expected = 0;
+
+	if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
+		ScheduleNow();
+	}
+
+#else
+	uint16_t fifo_byte_count = FIFOReadCount();
+
+	FIFORead(hrt_absolute_time(), fifo_byte_count / sizeof(FIFO::DATA));
+#endif
+}
+
+bool ICM42688P::DataReadyInterruptConfigure()
+{
+	if (_drdy_gpio == 0) {
+		return false;
+	}
+
+	// Setup data ready on falling edge
+	return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0;
+}
+
+bool ICM42688P::DataReadyInterruptDisable()
+{
+	if (_drdy_gpio == 0) {
+		return false;
+	}
+
+	return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
+}
+
+template <typename T>
+bool ICM42688P::RegisterCheck(const T &reg_cfg)
+{
+	bool success = true;
+
+	const uint8_t reg_value = RegisterRead(reg_cfg.reg);
+
+	if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) {
+		PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
+		success = false;
+	}
+
+	if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) {
+		PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
+		success = false;
+	}
+
+	return success;
+}
+
+template <typename T>
+uint8_t ICM42688P::RegisterRead(T reg)
+{
+	uint8_t cmd[2] {};
+	cmd[0] = static_cast<uint8_t>(reg) | DIR_READ;
+	SelectRegisterBank(reg);
+	transfer(cmd, cmd, sizeof(cmd));
+	return cmd[1];
+}
+
+template <typename T>
+void ICM42688P::RegisterWrite(T reg, uint8_t value)
+{
+	uint8_t cmd[2] { (uint8_t)reg, value };
+	SelectRegisterBank(reg);
+	transfer(cmd, cmd, sizeof(cmd));
+}
+
+template <typename T>
+void ICM42688P::RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits)
+{
+	const uint8_t orig_val = RegisterRead(reg);
+
+	uint8_t val = (orig_val & ~clearbits) | setbits;
+
+	if (orig_val != val) {
+		RegisterWrite(reg, val);
+	}
+}
+
+uint16_t ICM42688P::FIFOReadCount()
+{
+	// read FIFO count
+	uint8_t fifo_count_buf[3] {};
+	fifo_count_buf[0] = static_cast<uint8_t>(Register::BANK_0::FIFO_COUNTH) | DIR_READ;
+	SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
+
+	if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) {
+		perf_count(_bad_transfer_perf);
+		return 0;
+	}
+
+	return combine(fifo_count_buf[1], fifo_count_buf[2]);
+}
+
+// static uint32_t debug_decimator = 0;
+// static hrt_abstime last_sample_time = 0;
+// static bool imu_debug = true;
+
+bool ICM42688P::FIFORead(const hrt_abstime &timestamp_sample, uint16_t samples)
+{
+	FIFOTransferBuffer buffer{};
+	const size_t max_transfer_size = 10 * sizeof(FIFO::DATA) + 4;
+	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 4, max_transfer_size);
+	SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
+
+	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
+		perf_count(_bad_transfer_perf);
+		return false;
+	}
+
+	if (buffer.INT_STATUS & INT_STATUS_BIT::FIFO_FULL_INT) {
+		perf_count(_fifo_overflow_perf);
+		FIFOReset();
+		return false;
+	}
+
+	const uint16_t fifo_count_bytes = combine(buffer.FIFO_COUNTH, buffer.FIFO_COUNTL);
+
+	if (fifo_count_bytes >= FIFO::SIZE) {
+		perf_count(_fifo_overflow_perf);
+		FIFOReset();
+		return false;
+	}
+
+	const uint16_t fifo_count_samples = fifo_count_bytes / sizeof(FIFO::DATA);
+
+	if (fifo_count_samples == 0) {
+		perf_count(_fifo_empty_perf);
+		return false;
+	}
+
+	// check FIFO header in every sample
+	uint16_t valid_samples = 0;
+
+	// for (int i = 0; i < math::min(samples, fifo_count_samples); i++) {
+	for (int i = 0; i < math::min(samples, (uint16_t) 10); i++) {
+		bool valid = true;
+
+		// With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx
+		const uint8_t FIFO_HEADER = buffer.f[i].FIFO_Header;
+
+		if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_MSG) {
+			// FIFO sample empty if HEADER_MSG set
+			valid = false;
+
+		} else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ACCEL)) {
+			// accel bit not set
+			valid = false;
+
+		} else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_GYRO)) {
+			// gyro bit not set
+			valid = false;
+
+		} else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_20)) {
+			// Packet does not contain a new and valid extended 20-bit data
+			valid = false;
+
+		} else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_ACCEL) {
+			// accel ODR changed
+			valid = false;
+
+		} else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_GYRO) {
+			// gyro ODR changed
+			valid = false;
+		}
+
+		if (valid) {
+			valid_samples++;
+
+		} else {
+			perf_count(_bad_transfer_perf);
+			break;
+		}
+	}
+
+	// if (imu_debug) {
+	// 	debug_decimator++;
+	// 	if (debug_decimator == 801) {
+	// 		debug_decimator = 0;
+	// 		PX4_INFO("Initial: %u Next: %u Valid: %u Delta: %llu", samples, fifo_count_samples, valid_samples, timestamp_sample - last_sample_time);
+	// 	}
+	// 	last_sample_time = timestamp_sample;
+	// }
+
+	if (valid_samples > 0) {
+		if (ProcessTemperature(buffer.f, valid_samples)) {
+			ProcessGyro(timestamp_sample, buffer.f, valid_samples);
+			ProcessAccel(timestamp_sample, buffer.f, valid_samples);
+			// Pass only most recent valid sample to IMU server
+			// ProcessIMU(timestamp_sample, buffer.f[valid_samples - 1]);
+			return true;
+		}
+	}
+
+	return false;
+}
+
+void ICM42688P::FIFOReset()
+{
+	perf_count(_fifo_reset_perf);
+
+	// SIGNAL_PATH_RESET: FIFO flush
+	RegisterSetBits(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::FIFO_FLUSH);
+
+	// reset while FIFO is disabled
+	_drdy_fifo_read_samples.store(0);
+}
+
+static constexpr int32_t reassemble_20bit(const uint32_t a, const uint32_t b, const uint32_t c)
+{
+	// 0xXXXAABBC
+	uint32_t high   = ((a << 12) & 0x000FF000);
+	uint32_t low    = ((b << 4)  & 0x00000FF0);
+	uint32_t lowest = (c         & 0x0000000F);
+
+	uint32_t x = high | low | lowest;
+
+	if (a & Bit7) {
+		// sign extend
+		x |= 0xFFF00000u;
+	}
+
+	return static_cast<int32_t>(x);
+}
+
+void ICM42688P::ProcessIMU(const hrt_abstime &timestamp_sample, const FIFO::DATA &fifo)
+{
+	// float accel_x = 0.0, accel_y = 0.0, accel_z = 0.0;
+	// float gyro_x = 0.0,  gyro_y = 0.0,  gyro_z = 0.0;
+	//
+	// // 20 bit hires mode
+	//
+	// // Sign extension + Accel [19:12] + Accel [11:4] + Accel [3:2] (20 bit extension byte)
+	// // Accel data is 18 bit
+	// int32_t temp_accel_x = reassemble_20bit(fifo.ACCEL_DATA_X1, fifo.ACCEL_DATA_X0,
+	// 				   fifo.Ext_Accel_X_Gyro_X & 0xF0 >> 4);
+	// int32_t temp_accel_y = reassemble_20bit(fifo.ACCEL_DATA_Y1, fifo.ACCEL_DATA_Y0,
+	// 				   fifo.Ext_Accel_Y_Gyro_Y & 0xF0 >> 4);
+	// int32_t temp_accel_z = reassemble_20bit(fifo.ACCEL_DATA_Z1, fifo.ACCEL_DATA_Z0,
+	// 				   fifo.Ext_Accel_Z_Gyro_Z & 0xF0 >> 4);
+	//
+	// // Gyro [19:12] + Gyro [11:4] + Gyro [3:0] (bottom 4 bits of 20 bit extension byte)
+	// int32_t temp_gyro_x = reassemble_20bit(fifo.GYRO_DATA_X1, fifo.GYRO_DATA_X0,
+	//                    fifo.Ext_Accel_X_Gyro_X & 0x0F);
+	// int32_t temp_gyro_y = reassemble_20bit(fifo.GYRO_DATA_Y1, fifo.GYRO_DATA_Y0,
+	//                    fifo.Ext_Accel_Y_Gyro_Y & 0x0F);
+	// int32_t temp_gyro_z = reassemble_20bit(fifo.GYRO_DATA_Z1, fifo.GYRO_DATA_Z0,
+	//                    fifo.Ext_Accel_Z_Gyro_Z & 0x0F);
+
+	// // accel samples invalid if -524288
+	// if (temp_accel_x != -524288 && temp_accel_y != -524288 && temp_accel_z != -524288) {
+	// 	// shift accel by 2 (2 least significant bits are always 0)
+	// 	accel_x = (float) temp_accel_x / 4.f;
+	// 	accel_y = (float) temp_accel_y / 4.f;
+	// 	accel_z = (float) temp_accel_z / 4.f;
+	//
+	// 	// shift gyro by 1 (least significant bit is always 0)
+	// 	gyro_x = (float) temp_gyro_x / 2.f;
+	// 	gyro_y = (float) temp_gyro_y / 2.f;
+	// 	gyro_z = (float) temp_gyro_z / 2.f;
+	//
+	// 	// correct frame for publication
+	// 	// sensor's frame is +x forward, +y left, +z up
+	// 	// flip y & z to publish right handed with z down (x forward, y right, z down)
+	// 	accel_y = -accel_y;
+	// 	accel_z = -accel_z;
+	// 	gyro_y  = -gyro_y;
+	// 	gyro_z  = -gyro_z;
+	//
+	// 	// Scale everything appropriately
+	// 	float accel_scale_factor = (CONSTANTS_ONE_G / 8192.f);
+	// 	accel_x *= accel_scale_factor;
+	// 	accel_y *= accel_scale_factor;
+	// 	accel_z *= accel_scale_factor;
+	//
+	// 	float gyro_scale_factor = math::radians(1.f / 131.f);
+	// 	gyro_x *= gyro_scale_factor;
+	// 	gyro_y *= gyro_scale_factor;
+	// 	gyro_z *= gyro_scale_factor;
+	//
+	// 	// Store the data in our array
+	// 	_imu_server_data.accel_x[_imu_server_index] = accel_x;
+	// 	_imu_server_data.accel_y[_imu_server_index] = accel_y;
+	// 	_imu_server_data.accel_z[_imu_server_index] = accel_z;
+	// 	_imu_server_data.gyro_x[_imu_server_index]  = gyro_x;
+	// 	_imu_server_data.gyro_y[_imu_server_index]  = gyro_y;
+	// 	_imu_server_data.gyro_z[_imu_server_index]  = gyro_z;
+	// 	_imu_server_data.ts[_imu_server_index]      = timestamp_sample;
+	// 	_imu_server_index++;
+	//
+	// 	// If array is full, publish the data
+	// 	if (_imu_server_index == 10) {
+	// 		_imu_server_index = 0;
+	// 		_imu_server_data.timestamp = hrt_absolute_time();
+	// 		_imu_server_data.temperature = 0; // Not used right now
+	// 		_imu_server_pub.publish(_imu_server_data);
+	// 	}
+	// }
+}
+
+void ICM42688P::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
+{
+	sensor_accel_fifo_s accel{};
+	accel.timestamp_sample = timestamp_sample;
+	accel.samples = 0;
+	accel.dt = FIFO_SAMPLE_DT;
+
+	// 18-bits of accelerometer data
+	bool scale_20bit = false;
+
+	// first pass
+	for (int i = 0; i < samples; i++) {
+		// 20 bit hires mode
+		// Sign extension + Accel [19:12] + Accel [11:4] + Accel [3:2] (20 bit extension byte)
+		// Accel data is 18 bit ()
+		int32_t accel_x = reassemble_20bit(fifo[i].ACCEL_DATA_X1, fifo[i].ACCEL_DATA_X0,
+						   fifo[i].Ext_Accel_X_Gyro_X & 0xF0 >> 4);
+		int32_t accel_y = reassemble_20bit(fifo[i].ACCEL_DATA_Y1, fifo[i].ACCEL_DATA_Y0,
+						   fifo[i].Ext_Accel_Y_Gyro_Y & 0xF0 >> 4);
+		int32_t accel_z = reassemble_20bit(fifo[i].ACCEL_DATA_Z1, fifo[i].ACCEL_DATA_Z0,
+						   fifo[i].Ext_Accel_Z_Gyro_Z & 0xF0 >> 4);
+
+		// sample invalid if -524288
+		if (accel_x != -524288 && accel_y != -524288 && accel_z != -524288) {
+			// check if any values are going to exceed int16 limits
+			static constexpr int16_t max_accel = INT16_MAX;
+			static constexpr int16_t min_accel = INT16_MIN;
+
+			if (accel_x >= max_accel || accel_x <= min_accel) {
+				scale_20bit = true;
+			}
+
+			if (accel_y >= max_accel || accel_y <= min_accel) {
+				scale_20bit = true;
+			}
+
+			if (accel_z >= max_accel || accel_z <= min_accel) {
+				scale_20bit = true;
+			}
+
+			// shift by 2 (2 least significant bits are always 0)
+			accel.x[accel.samples] = accel_x / 4;
+			accel.y[accel.samples] = accel_y / 4;
+			accel.z[accel.samples] = accel_z / 4;
+			accel.samples++;
+		}
+	}
+
+	if (!scale_20bit) {
+		// if highres enabled accel data is always 8192 LSB/g
+		if (!hitl_mode) {
+			_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
+		}
+
+	} else {
+		// 20 bit data scaled to 16 bit (2^4)
+		for (int i = 0; i < samples; i++) {
+			// 20 bit hires mode
+			// Sign extension + Accel [19:12] + Accel [11:4] + Accel [3:2] (20 bit extension byte)
+			// Accel data is 18 bit ()
+			int16_t accel_x = combine(fifo[i].ACCEL_DATA_X1, fifo[i].ACCEL_DATA_X0);
+			int16_t accel_y = combine(fifo[i].ACCEL_DATA_Y1, fifo[i].ACCEL_DATA_Y0);
+			int16_t accel_z = combine(fifo[i].ACCEL_DATA_Z1, fifo[i].ACCEL_DATA_Z0);
+
+			accel.x[i] = accel_x;
+			accel.y[i] = accel_y;
+			accel.z[i] = accel_z;
+		}
+
+		if (!hitl_mode) {
+			_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
+		}
+	}
+
+	// correct frame for publication
+	for (int i = 0; i < accel.samples; i++) {
+		// sensor's frame is +x forward, +y left, +z up
+		//  flip y & z to publish right handed with z down (x forward, y right, z down)
+		accel.x[i] = accel.x[i];
+		accel.y[i] = (accel.y[i] == INT16_MIN) ? INT16_MAX : -accel.y[i];
+		accel.z[i] = (accel.z[i] == INT16_MIN) ? INT16_MAX : -accel.z[i];
+	}
+
+	if (!hitl_mode) {
+		_px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
+					   perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
+	}
+
+	if (accel.samples > 0) {
+		if (!hitl_mode) {
+			_px4_accel.updateFIFO(accel);
+		}
+	}
+}
+
+void ICM42688P::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
+{
+	sensor_gyro_fifo_s gyro{};
+	gyro.timestamp_sample = timestamp_sample;
+	gyro.samples = 0;
+	gyro.dt = FIFO_SAMPLE_DT;
+
+	// 20-bits of gyroscope data
+	bool scale_20bit = false;
+
+	// first pass
+	for (int i = 0; i < samples; i++) {
+		// 20 bit hires mode
+		// Gyro [19:12] + Gyro [11:4] + Gyro [3:0] (bottom 4 bits of 20 bit extension byte)
+		int32_t gyro_x = reassemble_20bit(fifo[i].GYRO_DATA_X1, fifo[i].GYRO_DATA_X0, fifo[i].Ext_Accel_X_Gyro_X & 0x0F);
+		int32_t gyro_y = reassemble_20bit(fifo[i].GYRO_DATA_Y1, fifo[i].GYRO_DATA_Y0, fifo[i].Ext_Accel_Y_Gyro_Y & 0x0F);
+		int32_t gyro_z = reassemble_20bit(fifo[i].GYRO_DATA_Z1, fifo[i].GYRO_DATA_Z0, fifo[i].Ext_Accel_Z_Gyro_Z & 0x0F);
+
+		// check if any values are going to exceed int16 limits
+		static constexpr int16_t max_gyro = INT16_MAX;
+		static constexpr int16_t min_gyro = INT16_MIN;
+
+		if (gyro_x >= max_gyro || gyro_x <= min_gyro) {
+			scale_20bit = true;
+		}
+
+		if (gyro_y >= max_gyro || gyro_y <= min_gyro) {
+			scale_20bit = true;
+		}
+
+		if (gyro_z >= max_gyro || gyro_z <= min_gyro) {
+			scale_20bit = true;
+		}
+
+		gyro.x[gyro.samples] = gyro_x / 2;
+		gyro.y[gyro.samples] = gyro_y / 2;
+		gyro.z[gyro.samples] = gyro_z / 2;
+		gyro.samples++;
+	}
+
+	if (!scale_20bit) {
+		// if highres enabled gyro data is always 131 LSB/dps
+		if (!hitl_mode) {
+			_px4_gyro.set_scale(math::radians(1.f / 131.f));
+		}
+
+	} else {
+		// 20 bit data scaled to 16 bit (2^4)
+		for (int i = 0; i < samples; i++) {
+			gyro.x[i] = combine(fifo[i].GYRO_DATA_X1, fifo[i].GYRO_DATA_X0);
+			gyro.y[i] = combine(fifo[i].GYRO_DATA_Y1, fifo[i].GYRO_DATA_Y0);
+			gyro.z[i] = combine(fifo[i].GYRO_DATA_Z1, fifo[i].GYRO_DATA_Z0);
+		}
+
+		if (!hitl_mode) {
+			_px4_gyro.set_scale(math::radians(2000.f / 32768.f));
+		}
+	}
+
+	// correct frame for publication
+	for (int i = 0; i < gyro.samples; i++) {
+		// sensor's frame is +x forward, +y left, +z up
+		//  flip y & z to publish right handed with z down (x forward, y right, z down)
+		gyro.x[i] = gyro.x[i];
+		gyro.y[i] = (gyro.y[i] == INT16_MIN) ? INT16_MAX : -gyro.y[i];
+		gyro.z[i] = (gyro.z[i] == INT16_MIN) ? INT16_MAX : -gyro.z[i];
+	}
+
+	if (!hitl_mode) {
+		_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
+					  perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
+	}
+
+	if (gyro.samples > 0) {
+		if (!hitl_mode) {
+			_px4_gyro.updateFIFO(gyro);
+		}
+	}
+}
+
+bool ICM42688P::ProcessTemperature(const FIFO::DATA fifo[], const uint8_t samples)
+{
+	int16_t temperature[FIFO_MAX_SAMPLES];
+	float temperature_sum{0};
+
+	int valid_samples = 0;
+
+	for (int i = 0; i < samples; i++) {
+		const int16_t t = combine(fifo[i].TEMP_DATA1, fifo[i].TEMP_DATA0);
+
+		// sample invalid if -32768
+		if (t != -32768) {
+			temperature_sum += t;
+			temperature[valid_samples] = t;
+			valid_samples++;
+		}
+	}
+
+	if (valid_samples > 0) {
+		const float temperature_avg = temperature_sum / valid_samples;
+
+		for (int i = 0; i < valid_samples; i++) {
+			// temperature changing wildly is an indication of a transfer error
+			if (fabsf(temperature[i] - temperature_avg) > 1000) {
+				perf_count(_bad_transfer_perf);
+				return false;
+			}
+		}
+
+		// use average temperature reading
+		const float TEMP_degC = (temperature_avg / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET;
+
+		if (PX4_ISFINITE(TEMP_degC)) {
+			if (!hitl_mode) {
+				_px4_accel.set_temperature(TEMP_degC);
+				_px4_gyro.set_temperature(TEMP_degC);
+				return true;
+			}
+
+		} else {
+			perf_count(_bad_transfer_perf);
+		}
+	}
+
+	return false;
+}

boards/modalai/voxl2-slpi/src/drivers/icm42688p/ICM42688P.hpp

@@ -0,0 +1,233 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ICM42688P.hpp
+ *
+ * Driver for the Invensense ICM42688P connected via SPI.
+ *
+ */
+
+#pragma once
+
+#include "InvenSense_ICM42688P_registers.hpp"
+
+#include <drivers/drv_hrt.h>
+#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
+#include <lib/drivers/device/spi.h>
+#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
+#include <lib/geo/geo.h>
+#include <lib/perf/perf_counter.h>
+#include <px4_platform_common/atomic.h>
+#include <px4_platform_common/i2c_spi_buses.h>
+#include <uORB/topics/sensor_accel_fifo.h>
+#include <uORB/topics/sensor_gyro_fifo.h>
+#include <memory>
+
+using namespace InvenSense_ICM42688P;
+
+extern bool hitl_mode;
+
+class ICM42688P : public device::SPI, public I2CSPIDriver<ICM42688P>
+{
+public:
+	// ICM42688P(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency,
+	// 	  spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio);
+	ICM42688P(const I2CSPIDriverConfig &config);
+	~ICM42688P() override;
+
+	// static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
+	// 				     int runtime_instance);
+	static void print_usage();
+
+	void RunImpl();
+
+	int init() override;
+	void print_status() override;
+
+private:
+	void exit_and_cleanup() override;
+
+	// Sensor Configuration
+	static constexpr float IMU_ODR{8000.f}; // 8kHz accel & gyro ODR configured
+	static constexpr float FIFO_SAMPLE_DT{1e6f / IMU_ODR};
+	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};
+	static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT};
+
+	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
+	// static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
+	static constexpr uint32_t FIFO_MAX_SAMPLES{10};
+
+	// Transfer data
+	struct FIFOTransferBuffer {
+		uint8_t cmd{static_cast<uint8_t>(Register::BANK_0::INT_STATUS) | DIR_READ};
+		uint8_t INT_STATUS{0};
+		uint8_t FIFO_COUNTH{0};
+		uint8_t FIFO_COUNTL{0};
+		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
+	};
+	// ensure no struct padding
+	static_assert(sizeof(FIFOTransferBuffer) == (4 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)),
+		      "Invalid FIFOTransferBuffer size");
+
+	struct register_bank0_config_t {
+		Register::BANK_0 reg;
+		uint8_t set_bits{0};
+		uint8_t clear_bits{0};
+	};
+
+	struct register_bank1_config_t {
+		Register::BANK_1 reg;
+		uint8_t set_bits{0};
+		uint8_t clear_bits{0};
+	};
+
+	struct register_bank2_config_t {
+		Register::BANK_2 reg;
+		uint8_t set_bits{0};
+		uint8_t clear_bits{0};
+	};
+
+	int probe() override;
+
+	bool Reset();
+
+	bool Configure();
+	void ConfigureSampleRate(int sample_rate);
+	void ConfigureFIFOWatermark(uint8_t samples);
+
+	void SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force = false);
+	void SelectRegisterBank(Register::BANK_0 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); }
+	void SelectRegisterBank(Register::BANK_1 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_1); }
+	void SelectRegisterBank(Register::BANK_2 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_2); }
+
+	static int DataReadyInterruptCallback(int irq, void *context, void *arg);
+	void DataReady();
+	bool DataReadyInterruptConfigure();
+	bool DataReadyInterruptDisable();
+
+	template <typename T> bool RegisterCheck(const T &reg_cfg);
+	template <typename T> uint8_t RegisterRead(T reg);
+	template <typename T> void RegisterWrite(T reg, uint8_t value);
+	template <typename T> void RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits);
+	template <typename T> void RegisterSetBits(T reg, uint8_t setbits) { RegisterSetAndClearBits(reg, setbits, 0); }
+	template <typename T> void RegisterClearBits(T reg, uint8_t clearbits) { RegisterSetAndClearBits(reg, 0, clearbits); }
+
+	uint16_t FIFOReadCount();
+	bool FIFORead(const hrt_abstime &timestamp_sample, uint16_t samples);
+	void FIFOReset();
+
+	void ProcessIMU(const hrt_abstime &timestamp_sample, const FIFO::DATA &fifo);
+	void ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
+	void ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
+	bool ProcessTemperature(const FIFO::DATA fifo[], const uint8_t samples);
+
+	const spi_drdy_gpio_t _drdy_gpio;
+
+	// std::shared_ptr<PX4Accelerometer> _px4_accel;
+	// std::shared_ptr<PX4Gyroscope> _px4_gyro;
+	PX4Accelerometer _px4_accel;
+	PX4Gyroscope _px4_gyro;
+
+	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")};
+	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")};
+	perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO empty")};
+	perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO overflow")};
+	perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO reset")};
+	perf_counter_t _drdy_missed_perf{nullptr};
+
+	hrt_abstime _reset_timestamp{0};
+	hrt_abstime _last_config_check_timestamp{0};
+	hrt_abstime _temperature_update_timestamp{0};
+	int _failure_count{0};
+
+	enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0};
+
+	px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
+	bool _data_ready_interrupt_enabled{false};
+
+	enum class STATE : uint8_t {
+		RESET,
+		WAIT_FOR_RESET,
+		CONFIGURE,
+		FIFO_READ,
+	} _state{STATE::RESET};
+
+	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
+	uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
+
+	uint8_t _checked_register_bank0{0};
+	static constexpr uint8_t size_register_bank0_cfg{12};
+	register_bank0_config_t _register_bank0_cfg[size_register_bank0_cfg] {
+		// Register                              | Set bits, Clear bits
+		{ Register::BANK_0::INT_CONFIG,           INT_CONFIG_BIT::INT1_MODE | INT_CONFIG_BIT::INT1_DRIVE_CIRCUIT, INT_CONFIG_BIT::INT1_POLARITY },
+		{ Register::BANK_0::FIFO_CONFIG,          FIFO_CONFIG_BIT::FIFO_MODE_STOP_ON_FULL, 0 },
+		{ Register::BANK_0::GYRO_CONFIG0,         GYRO_CONFIG0_BIT::GYRO_FS_SEL_2000_DPS | GYRO_CONFIG0_BIT::GYRO_ODR_8KHZ_SET, GYRO_CONFIG0_BIT::GYRO_ODR_8KHZ_CLEAR },
+		{ Register::BANK_0::ACCEL_CONFIG0,        ACCEL_CONFIG0_BIT::ACCEL_FS_SEL_16G | ACCEL_CONFIG0_BIT::ACCEL_ODR_8KHZ_SET, ACCEL_CONFIG0_BIT::ACCEL_ODR_8KHZ_CLEAR },
+		{ Register::BANK_0::GYRO_CONFIG1,         0, GYRO_CONFIG1_BIT::GYRO_UI_FILT_ORD },
+		{ Register::BANK_0::GYRO_ACCEL_CONFIG0,   0, GYRO_ACCEL_CONFIG0_BIT::ACCEL_UI_FILT_BW | GYRO_ACCEL_CONFIG0_BIT::GYRO_UI_FILT_BW },
+		{ Register::BANK_0::ACCEL_CONFIG1,        0, ACCEL_CONFIG1_BIT::ACCEL_UI_FILT_ORD },
+		{ Register::BANK_0::FIFO_CONFIG1,         FIFO_CONFIG1_BIT::FIFO_WM_GT_TH | FIFO_CONFIG1_BIT::FIFO_HIRES_EN | FIFO_CONFIG1_BIT::FIFO_TEMP_EN | FIFO_CONFIG1_BIT::FIFO_GYRO_EN | FIFO_CONFIG1_BIT::FIFO_ACCEL_EN, 0 },
+		{ Register::BANK_0::FIFO_CONFIG2,         0, 0 }, // FIFO_WM[7:0] set at runtime
+		{ Register::BANK_0::FIFO_CONFIG3,         0, 0 }, // FIFO_WM[11:8] set at runtime
+		{ Register::BANK_0::INT_CONFIG0,          INT_CONFIG0_BIT::CLEAR_ON_FIFO_READ, 0 },
+		{ Register::BANK_0::INT_SOURCE0,          INT_SOURCE0_BIT::FIFO_THS_INT1_EN, 0 },
+	};
+
+	uint8_t _checked_register_bank1{0};
+	static constexpr uint8_t size_register_bank1_cfg{4};
+	register_bank1_config_t _register_bank1_cfg[size_register_bank1_cfg] {
+		// Register                              | Set bits, Clear bits
+		{ Register::BANK_1::GYRO_CONFIG_STATIC2,  0, GYRO_CONFIG_STATIC2_BIT::GYRO_NF_DIS | GYRO_CONFIG_STATIC2_BIT::GYRO_AAF_DIS },
+		{ Register::BANK_1::GYRO_CONFIG_STATIC3,  GYRO_CONFIG_STATIC3_BIT::GYRO_AAF_DELT_SET, GYRO_CONFIG_STATIC3_BIT::GYRO_AAF_DELT_CLEAR},
+		{ Register::BANK_1::GYRO_CONFIG_STATIC4,  GYRO_CONFIG_STATIC4_BIT::GYRO_AAF_DELTSQR_LOW_SET, GYRO_CONFIG_STATIC4_BIT::GYRO_AAF_DELTSQR_LOW_CLEAR},
+		{ Register::BANK_1::GYRO_CONFIG_STATIC5,  GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_BITSHIFT_SET | GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_DELTSQR_HIGH_SET, GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_BITSHIFT_CLEAR | GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_DELTSQR_HIGH_CLEAR},
+	};
+
+	uint8_t _checked_register_bank2{0};
+	static constexpr uint8_t size_register_bank2_cfg{3};
+	register_bank2_config_t _register_bank2_cfg[size_register_bank2_cfg] {
+		// Register                              | Set bits, Clear bits
+		{ Register::BANK_2::ACCEL_CONFIG_STATIC2, ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DELT_SET, ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DELT_CLEAR | ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DIS },
+		{ Register::BANK_2::ACCEL_CONFIG_STATIC3, ACCEL_CONFIG_STATIC3_BIT::ACCEL_AAF_DELTSQR_LOW_SET, ACCEL_CONFIG_STATIC3_BIT::ACCEL_AAF_DELTSQR_LOW_CLEAR },
+		{ Register::BANK_2::ACCEL_CONFIG_STATIC4, ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_BITSHIFT_SET | ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_DELTSQR_HIGH_SET, ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_BITSHIFT_CLEAR | ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_DELTSQR_HIGH_CLEAR },
+	};
+
+	uint32_t _temperature_samples{0};
+
+	// Support for the IMU server
+	// uint32_t _imu_server_index{0};
+	// imu_server_s _imu_server_data;
+	// uORB::Publication<imu_server_s> _imu_server_pub{ORB_ID(imu_server)};
+
+};

boards/modalai/voxl2-slpi/src/drivers/icm42688p/InvenSense_ICM42688P_registers.hpp

@@ -0,0 +1,430 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file InvenSense_ICM42688P_registers.hpp
+ *
+ * Invensense ICM-42688-P registers.
+ *
+ */
+
+#pragma once
+
+#include <cstdint>
+
+namespace InvenSense_ICM42688P
+{
+// TODO: move to a central header
+static constexpr uint8_t Bit0 = (1 << 0);
+static constexpr uint8_t Bit1 = (1 << 1);
+static constexpr uint8_t Bit2 = (1 << 2);
+static constexpr uint8_t Bit3 = (1 << 3);
+static constexpr uint8_t Bit4 = (1 << 4);
+static constexpr uint8_t Bit5 = (1 << 5);
+static constexpr uint8_t Bit6 = (1 << 6);
+static constexpr uint8_t Bit7 = (1 << 7);
+
+static constexpr uint32_t SPI_SPEED = 24 * 1000 * 1000; // 24 MHz SPI
+static constexpr uint8_t DIR_READ = 0x80;
+
+static constexpr uint8_t WHOAMI = 0x47;
+
+static constexpr float TEMPERATURE_SENSITIVITY = 132.48f; // LSB/C
+static constexpr float TEMPERATURE_OFFSET = 25.f; // C
+
+namespace Register
+{
+
+enum class BANK_0 : uint8_t {
+	DEVICE_CONFIG      = 0x11,
+
+	INT_CONFIG         = 0x14,
+
+	FIFO_CONFIG        = 0x16,
+
+	TEMP_DATA1         = 0x1D,
+	TEMP_DATA0         = 0x1E,
+
+	INT_STATUS         = 0x2D,
+	FIFO_COUNTH        = 0x2E,
+	FIFO_COUNTL        = 0x2F,
+	FIFO_DATA          = 0x30,
+
+	SIGNAL_PATH_RESET  = 0x4B,
+	INTF_CONFIG0       = 0x4C,
+	INTF_CONFIG1       = 0x4D,
+	PWR_MGMT0          = 0x4E,
+	GYRO_CONFIG0       = 0x4F,
+	ACCEL_CONFIG0      = 0x50,
+	GYRO_CONFIG1       = 0x51,
+	GYRO_ACCEL_CONFIG0 = 0x52,
+	ACCEL_CONFIG1      = 0x53,
+
+	FIFO_CONFIG1       = 0x5F,
+	FIFO_CONFIG2       = 0x60,
+	FIFO_CONFIG3       = 0x61,
+
+	INT_CONFIG0        = 0x63,
+
+	INT_SOURCE0        = 0x65,
+
+	SELF_TEST_CONFIG   = 0x70,
+
+	WHO_AM_I           = 0x75,
+	REG_BANK_SEL       = 0x76,
+};
+
+enum class BANK_1 : uint8_t {
+	GYRO_CONFIG_STATIC2 = 0x0B,
+	GYRO_CONFIG_STATIC3 = 0x0C,
+	GYRO_CONFIG_STATIC4 = 0x0D,
+	GYRO_CONFIG_STATIC5 = 0x0E,
+	INTF_CONFIG5        = 0x7B,
+};
+enum class BANK_2 : uint8_t {
+	ACCEL_CONFIG_STATIC2 = 0x03,
+	ACCEL_CONFIG_STATIC3 = 0x04,
+	ACCEL_CONFIG_STATIC4 = 0x05,
+};
+
+};
+
+//---------------- BANK0 Register bits
+
+// DEVICE_CONFIG
+enum DEVICE_CONFIG_BIT : uint8_t {
+	SOFT_RESET_CONFIG = Bit0, //
+};
+
+// INT_CONFIG
+enum INT_CONFIG_BIT : uint8_t {
+	INT1_MODE           = Bit2,
+	INT1_DRIVE_CIRCUIT  = Bit1,
+	INT1_POLARITY       = Bit0,
+};
+
+// FIFO_CONFIG
+enum FIFO_CONFIG_BIT : uint8_t {
+	// 7:6 FIFO_MODE
+	FIFO_MODE_STOP_ON_FULL = Bit7 | Bit6, // 11: STOP-on-FULL Mode
+};
+
+// INT_STATUS
+enum INT_STATUS_BIT : uint8_t {
+	RESET_DONE_INT = Bit4,
+	DATA_RDY_INT   = Bit3,
+	FIFO_THS_INT   = Bit2,
+	FIFO_FULL_INT  = Bit1,
+};
+
+// SIGNAL_PATH_RESET
+enum SIGNAL_PATH_RESET_BIT : uint8_t {
+	ABORT_AND_RESET = Bit3,
+	FIFO_FLUSH      = Bit1,
+};
+
+// PWR_MGMT0
+enum PWR_MGMT0_BIT : uint8_t {
+	GYRO_MODE_LOW_NOISE  = Bit3 | Bit2, // 11: Places gyroscope in Low Noise (LN) Mode
+	ACCEL_MODE_LOW_NOISE = Bit1 | Bit0, // 11: Places accelerometer in Low Noise (LN) Mode
+};
+
+// GYRO_CONFIG0
+enum GYRO_CONFIG0_BIT : uint8_t {
+	// 7:5 GYRO_FS_SEL
+	GYRO_FS_SEL_2000_DPS = 0, // 0b000 = ±2000dps (default)
+	GYRO_FS_SEL_1000_DPS = Bit5,
+	GYRO_FS_SEL_500_DPS  = Bit6,
+	GYRO_FS_SEL_250_DPS  = Bit6 | Bit5,
+	GYRO_FS_SEL_125_DPS  = Bit7,
+
+
+	// 3:0 GYRO_ODR
+	//  0001: 32kHz
+	GYRO_ODR_32KHZ_SET   = Bit0,
+	GYRO_ODR_32KHZ_CLEAR = Bit3 | Bit2 | Bit0,
+	//  0010: 16kHz
+	GYRO_ODR_16KHZ_SET   = Bit1,
+	GYRO_ODR_16KHZ_CLEAR = Bit3 | Bit2 | Bit0,
+	//  0011: 8kHz
+	GYRO_ODR_8KHZ_SET    = Bit1 | Bit0,
+	GYRO_ODR_8KHZ_CLEAR  = Bit3 | Bit2,
+	//  0110: 1kHz (default)
+	GYRO_ODR_1KHZ_SET    = Bit2 | Bit1,
+	GYRO_ODR_1KHZ_CLEAR  = Bit3 | Bit0,
+};
+
+// ACCEL_CONFIG0
+enum ACCEL_CONFIG0_BIT : uint8_t {
+	// 7:5 ACCEL_FS_SEL
+	ACCEL_FS_SEL_16G = 0, // 000: ±16g (default)
+	ACCEL_FS_SEL_8G  = Bit5,
+	ACCEL_FS_SEL_4G  = Bit6,
+	ACCEL_FS_SEL_2G  = Bit6 | Bit5,
+
+
+	// 3:0 ACCEL_ODR
+	//  0001: 32kHz
+	ACCEL_ODR_32KHZ_SET   = Bit0,
+	ACCEL_ODR_32KHZ_CLEAR = Bit3 | Bit2 | Bit0,
+	//  0010: 16kHz
+	ACCEL_ODR_16KHZ_SET   = Bit1,
+	ACCEL_ODR_16KHZ_CLEAR = Bit3 | Bit2 | Bit0,
+	//  0011: 8kHz
+	ACCEL_ODR_8KHZ_SET    = Bit1 | Bit0,
+	ACCEL_ODR_8KHZ_CLEAR  = Bit3 | Bit2,
+	//  0110: 1kHz (default)
+	ACCEL_ODR_1KHZ_SET    = Bit2 | Bit1,
+	ACCEL_ODR_1KHZ_CLEAR  = Bit3 | Bit0,
+};
+
+// GYRO_CONFIG1
+enum GYRO_CONFIG1_BIT : uint8_t {
+	GYRO_UI_FILT_ORD = Bit3 | Bit2, // 00: 1st Order
+};
+
+// GYRO_ACCEL_CONFIG0
+enum GYRO_ACCEL_CONFIG0_BIT : uint8_t {
+	// 7:4 ACCEL_UI_FILT_BW
+	ACCEL_UI_FILT_BW = Bit7 | Bit6 | Bit5 | Bit4, // 0: BW=ODR/2
+
+	// 3:0 GYRO_UI_FILT_BW
+	GYRO_UI_FILT_BW  = Bit3 | Bit2 | Bit1 | Bit0, // 0: BW=ODR/2
+};
+
+// ACCEL_CONFIG1
+enum ACCEL_CONFIG1_BIT : uint8_t {
+	ACCEL_UI_FILT_ORD = Bit4 | Bit3, // 00: 1st Order
+};
+
+// FIFO_CONFIG1
+enum FIFO_CONFIG1_BIT : uint8_t {
+	FIFO_RESUME_PARTIAL_RD = Bit6,
+	FIFO_WM_GT_TH          = Bit5,
+	FIFO_HIRES_EN          = Bit4,
+	FIFO_TEMP_EN           = Bit2,
+	FIFO_GYRO_EN           = Bit1,
+	FIFO_ACCEL_EN          = Bit0,
+};
+
+// INT_CONFIG0
+enum INT_CONFIG0_BIT : uint8_t {
+	// 3:2 FIFO_THS_INT_CLEAR
+	CLEAR_ON_FIFO_READ = Bit3,
+};
+
+// INT_SOURCE0
+enum INT_SOURCE0_BIT : uint8_t {
+	UI_FSYNC_INT1_EN   = Bit6,
+	PLL_RDY_INT1_EN    = Bit5,
+	RESET_DONE_INT1_EN = Bit4,
+	UI_DRDY_INT1_EN    = Bit3,
+	FIFO_THS_INT1_EN   = Bit2, // FIFO threshold interrupt routed to INT1
+	FIFO_FULL_INT1_EN  = Bit1,
+	UI_AGC_RDY_INT1_EN = Bit0,
+};
+
+// REG_BANK_SEL
+enum REG_BANK_SEL_BIT : uint8_t {
+	USER_BANK_0 = 0,           // 0: Select USER BANK 0.
+	USER_BANK_1 = Bit0,        // 1: Select USER BANK 1.
+	USER_BANK_2 = Bit1,        // 2: Select USER BANK 2.
+	USER_BANK_3 = Bit1 | Bit0, // 3: Select USER BANK 3.
+};
+
+
+//---------------- BANK1 Register bits
+
+// GYRO_CONFIG_STATIC2
+enum GYRO_CONFIG_STATIC2_BIT : uint8_t {
+	GYRO_AAF_DIS = Bit1,
+	GYRO_NF_DIS  = Bit0,
+};
+
+// GYRO_CONFIG_STATIC3
+enum GYRO_CONFIG_STATIC3_BIT : uint8_t {
+
+	// 585 Hz
+	GYRO_AAF_DELT_SET = Bit3 | Bit2 | Bit0, //13
+	GYRO_AAF_DELT_CLEAR = Bit5 | Bit4 | Bit1,
+
+	// 213 Hz
+	// GYRO_AAF_DELT_SET = Bit2 | Bit0, //5
+	// GYRO_AAF_DELT_CLEAR = Bit5 | Bit4 | Bit3 | Bit1,
+
+	// 126 Hz
+	//GYRO_AAF_DELT_SET = Bit1 | Bit0, //3
+	//GYRO_AAF_DELT_CLEAR = Bit5 | Bit4 | Bit3 | Bit2,
+
+	// 42 Hz
+	// GYRO_AAF_DELT_SET = Bit0, //1
+	// GYRO_AAF_DELT_CLEAR = Bit5 | Bit4 | Bit3 | Bit2 | Bit1,
+
+};
+
+// GYRO_CONFIG_STATIC4
+enum GYRO_CONFIG_STATIC4_BIT : uint8_t {
+
+	// 585 Hz
+	GYRO_AAF_DELTSQR_LOW_SET = Bit7 | Bit5 | Bit3 | Bit1, //170
+	GYRO_AAF_DELTSQR_LOW_CLEAR = Bit6 | Bit4 | Bit2 | Bit0,
+
+	// 213 Hz
+	// GYRO_AAF_DELTSQR_LOW_SET = Bit4 | Bit3 | Bit0, //25
+	// GYRO_AAF_DELTSQR_LOW_CLEAR = Bit7 | Bit6 | Bit5 | Bit2 | Bit1,
+
+	// 126 Hz
+	//GYRO_AAF_DELTSQR_LOW_SET = Bit3 | Bit0, //9
+	//GYRO_AAF_DELTSQR_LOW_CLEAR = Bit7 | Bit6 | Bit5 | Bit4 | Bit2 | Bit1,
+
+	// 42 Hz
+	// GYRO_AAF_DELTSQR_LOW_SET = Bit0, //1
+	// GYRO_AAF_DELTSQR_LOW_CLEAR = Bit7 | Bit6 | Bit5 | Bit4 | Bit3 | Bit2 | Bit1,
+};
+
+// GYRO_CONFIG_STATIC5
+enum GYRO_CONFIG_STATIC5_BIT : uint8_t {
+
+	// 585 Hz
+	GYRO_AAF_DELTSQR_HIGH_SET = 0,
+	GYRO_AAF_DELTSQR_HIGH_CLEAR = Bit3 | Bit2 | Bit1 | Bit0,
+	GYRO_AAF_BITSHIFT_SET = Bit7, // 8 << 4
+	GYRO_AAF_BITSHIFT_CLEAR = Bit6 | Bit5 | Bit4,
+
+	// 213 Hz
+	// GYRO_AAF_DELTSQR_HIGH_SET = 0,
+	// GYRO_AAF_DELTSQR_HIGH_CLEAR = Bit3 | Bit2 | Bit1 | Bit0,
+	// GYRO_AAF_BITSHIFT_SET = Bit7 | Bit5, //10
+	// GYRO_AAF_BITSHIFT_CLEAR = Bit6 | Bit4,
+
+	// 126 Hz
+	// GYRO_AAF_BITSHIFT_SET = Bit7 | Bit6, //12
+	// GYRO_AAF_BITSHIFT_CLEAR = Bit5 | Bit4,
+
+	// 42 Hz
+	// GYRO_AAF_BITSHIFT_SET = Bit7 | Bit6 | Bit5 | Bit4, //15
+	// GYRO_AAF_BITSHIFT_CLEAR = 0,
+
+
+};
+
+
+//---------------- BANK2 Register bits
+
+// ACCEL_CONFIG_STATIC2
+enum ACCEL_CONFIG_STATIC2_BIT : uint8_t {
+	ACCEL_AAF_DIS  = Bit0,
+	ACCEL_AAF_DELT = Bit3 | Bit1,
+
+	// 213 Hz
+	ACCEL_AAF_DELT_SET = Bit3 | Bit1, //5
+	ACCEL_AAF_DELT_CLEAR = Bit6 | Bit5 | Bit4 | Bit2,
+
+	// 42 Hz
+	// ACCEL_AAF_DELT_SET = Bit1, //1
+	// ACCEL_AAF_DELT_CLEAR = Bit6 | Bit5 | Bit4 | Bit3 | Bit2,
+};
+
+// ACCEL_CONFIG_STATIC3
+enum ACCEL_CONFIG_STATIC3_BIT : uint8_t {
+	ACCEL_AAF_DELTSQR_LOW = Bit4 | Bit3 | Bit0,
+	// 213 Hz
+	ACCEL_AAF_DELTSQR_LOW_SET = Bit4 | Bit3 | Bit0, //25
+	ACCEL_AAF_DELTSQR_LOW_CLEAR = Bit7 | Bit6 | Bit5 | Bit2 | Bit1,
+
+	// 42 Hz
+	// ACCEL_AAF_DELTSQR_LOW_SET = Bit0, //1
+	// ACCEL_AAF_DELTSQR_LOW_CLEAR = Bit7 | Bit6 | Bit5 | Bit4 | Bit3 | Bit2 | Bit1,
+
+};
+
+// ACCEL_CONFIG_STATIC4
+enum ACCEL_CONFIG_STATIC4_BIT : uint8_t {
+	ACCEL_AAF_BITSHIFT     = Bit7 | Bit5,
+	ACCEL_AAF_DELTSQR_HIGH = 0,
+	// 213 Hz
+	ACCEL_AAF_BITSHIFT_SET = Bit7 | Bit5, //10
+	ACCEL_AAF_BITSHIFT_CLEAR = Bit6 | Bit4,
+
+	// 42 Hz
+	// ACCEL_AAF_BITSHIFT_SET = Bit7 | Bit6 | Bit5 | Bit4, //15
+	// ACCEL_AAF_BITSHIFT_CLEAR = 0,
+
+	ACCEL_AAF_DELTSQR_HIGH_SET = 0,
+	ACCEL_AAF_DELTSQR_HIGH_CLEAR = Bit3 | Bit2 | Bit1 | Bit0,
+};
+
+
+namespace FIFO
+{
+static constexpr size_t SIZE = 2048;
+
+// FIFO_DATA layout when FIFO_CONFIG1 has FIFO_GYRO_EN and FIFO_ACCEL_EN set
+
+// Packet 4
+struct DATA {
+	uint8_t FIFO_Header;
+	uint8_t ACCEL_DATA_X1; // Accel X [19:12]
+	uint8_t ACCEL_DATA_X0; // Accel X [11:4]
+	uint8_t ACCEL_DATA_Y1; // Accel Y [19:12]
+	uint8_t ACCEL_DATA_Y0; // Accel Y [11:4]
+	uint8_t ACCEL_DATA_Z1; // Accel Z [19:12]
+	uint8_t ACCEL_DATA_Z0; // Accel Z [11:4]
+	uint8_t GYRO_DATA_X1;  // Gyro X [19:12]
+	uint8_t GYRO_DATA_X0;  // Gyro X [11:4]
+	uint8_t GYRO_DATA_Y1;  // Gyro Y [19:12]
+	uint8_t GYRO_DATA_Y0;  // Gyro Y [11:4]
+	uint8_t GYRO_DATA_Z1;  // Gyro Z [19:12]
+	uint8_t GYRO_DATA_Z0;  // Gyro Z [11:4]
+	uint8_t TEMP_DATA1;    // Temperature[15:8]
+	uint8_t TEMP_DATA0;    // Temperature[7:0]
+	uint8_t TimeStamp_h;   // TimeStamp[15:8]
+	uint8_t TimeStamp_l;   // TimeStamp[7:0]
+	uint8_t Ext_Accel_X_Gyro_X; // Accel X [3:0] Gyro X [3:0]
+	uint8_t Ext_Accel_Y_Gyro_Y; // Accel Y [3:0] Gyro Y [3:0]
+	uint8_t Ext_Accel_Z_Gyro_Z; // Accel Z [3:0] Gyro Z [3:0]
+};
+
+// With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx
+enum FIFO_HEADER_BIT : uint8_t {
+	HEADER_MSG             = Bit7, // 1: FIFO is empty
+	HEADER_ACCEL           = Bit6, // 1: Packet is sized so that accel data have location in the packet, FIFO_ACCEL_EN must be 1
+	HEADER_GYRO            = Bit5, // 1: Packet is sized so that gyro data have location in the packet, FIFO_GYRO_EN must be1
+	HEADER_20              = Bit4, // 1: Packet has a new and valid sample of extended 20-bit data for gyro and/or accel
+	HEADER_TIMESTAMP_FSYNC = Bit3 | Bit2,
+	HEADER_ODR_ACCEL       = Bit1, // 1: The ODR for accel is different for this accel data packet compared to the previous accel packet
+	HEADER_ODR_GYRO        = Bit0, // 1: The ODR for gyro is different for this gyro data packet compared to the previous gyro packet
+};
+
+}
+} // namespace InvenSense_ICM42688P

boards/modalai/voxl2-slpi/src/drivers/icm42688p/icm42688p_main.cpp

@@ -0,0 +1,116 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "ICM42688P.hpp"
+
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/module.h>
+#include <string>
+
+void ICM42688P::print_usage()
+{
+	PRINT_MODULE_USAGE_NAME("icm42688p", "driver");
+	PRINT_MODULE_USAGE_SUBCATEGORY("imu");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
+	PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+}
+
+// I2CSPIDriverBase *ICM42688P::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
+// 		int runtime_instance)
+// {
+// 	ICM42688P *instance = new ICM42688P(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation,
+// 					    cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO());
+//
+// 	if (!instance) {
+// 		PX4_ERR("alloc failed");
+// 		return nullptr;
+// 	}
+//
+// 	if (OK != instance->init()) {
+// 		delete instance;
+// 		return nullptr;
+// 	}
+//
+// 	return instance;
+// }
+
+extern "C" int icm42688p_main(int argc, char *argv[])
+{
+
+	for (int i = 0; i <= argc - 1; i++) {
+		if (std::string(argv[i]) == "-h") {
+			argv[i] = 0;
+			hitl_mode = true;
+			break;
+		}
+	}
+
+	int ch;
+	using ThisDriver = ICM42688P;
+	BusCLIArguments cli{false, true};
+	cli.default_spi_frequency = SPI_SPEED;
+
+	while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) {
+		switch (ch) {
+		case 'R':
+			cli.rotation = (enum Rotation)atoi(cli.optArg());
+			break;
+		}
+	}
+
+	const char *verb = cli.optArg();
+
+	if (!verb) {
+		ThisDriver::print_usage();
+		return -1;
+	}
+
+	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_IMU_DEVTYPE_ICM42688P);
+
+	if (!strcmp(verb, "start")) {
+		return ThisDriver::module_start(cli, iterator);
+	}
+
+	if (!strcmp(verb, "stop")) {
+		return ThisDriver::module_stop(iterator);
+	}
+
+	if (!strcmp(verb, "status")) {
+		return ThisDriver::module_status(iterator);
+	}
+
+	ThisDriver::print_usage();
+	return -1;
+}

boards/modalai/voxl2-slpi/src/spi.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2019 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2020 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -31,15 +31,10 @@
  *
  ****************************************************************************/
 
-#include <gtest/gtest.h>
-#include <AngularVelocityControl.hpp>
+#include <px4_arch/spi_hw_description.h>
+#include <px4_platform_common/spi.h>
+#include <drivers/drv_sensor.h>
 
-using namespace matrix;
-
-TEST(AngularVelocityControlTest, AllZeroCase)
-{
-	AngularVelocityControl control;
-	control.update(Vector3f(), Vector3f(), Vector3f(), 0.f, false);
-	Vector3f torque = control.getTorqueSetpoint();
-	EXPECT_EQ(torque, Vector3f());
-}
+constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
+	initSPIBus(1, {initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P), }),
+};

boards/modalai/voxl2/default.px4board

@@ -1,8 +1,10 @@
 CONFIG_PLATFORM_POSIX=y
-CONFIG_BOARD_LINUX=y
+CONFIG_BOARD_LINUX_TARGET=y
 CONFIG_BOARD_TOOLCHAIN="aarch64-linux-gnu"
 CONFIG_MODULES_MUORB_APPS=y
 CONFIG_SYSTEMCMDS_PERF=y
 CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_ORB_COMMUNICATOR=y
 CONFIG_SYSTEMCMDS_PARAM=y
+CONFIG_DRIVERS_QSHELL_POSIX=y
+CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y

boards/modalai/voxl2/target/voxl-px4.config

@@ -17,3 +17,5 @@ if [ $TESTMODE = "ON" ]; then
 fi
 
 muorb start
+
+qshell icm42688p start -s

boards/nxp/mr-canhubk3/default.px4board

@@ -15,8 +15,6 @@ CONFIG_MODULES_SENSORS=y
 CONFIG_SYSTEMCMDS_I2CDETECT=y
 CONFIG_SYSTEMCMDS_LED_CONTROL=y
 CONFIG_SYSTEMCMDS_MFT=y
-CONFIG_SYSTEMCMDS_MIXER=y
-CONFIG_SYSTEMCMDS_MOTOR_TEST=y
 CONFIG_SYSTEMCMDS_MTD=y
 CONFIG_SYSTEMCMDS_PARAM=y
 CONFIG_SYSTEMCMDS_REBOOT=y

boards/nxp/mr-canhubk3/fmu.px4board

@@ -2,14 +2,14 @@
 CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS1"
 CONFIG_BOARD_SERIAL_RC="/dev/ttyS5"
 CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS2"
+CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_COMMON_LIGHT=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_IRLOCK=y
 CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y
 CONFIG_DRIVERS_MAGNETOMETER_LIS3MDL=y
-CONFIG_DRIVERS_OSD=y
 CONFIG_DRIVERS_RC_INPUT=y
-CONFIG_DRIVERS_RPM=y
+CONFIG_DRIVERS_UAVCAN=y
 CONFIG_EXAMPLES_FAKE_GPS=y
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
@@ -38,7 +38,6 @@ CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_ROVER_POS_CONTROL=y
-CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -47,7 +46,6 @@ CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_NETMAN=y
 CONFIG_SYSTEMCMDS_NSHTERM=y
 CONFIG_SYSTEMCMDS_PERF=y
-CONFIG_SYSTEMCMDS_PWM=y
 CONFIG_SYSTEMCMDS_REFLECT=y
 CONFIG_SYSTEMCMDS_SERIAL_TEST=y
 CONFIG_SYSTEMCMDS_TUNE_CONTROL=y

boards/nxp/mr-canhubk3/init/rc.board_defaults

@@ -15,3 +15,11 @@ param set-default MAV_1_REMOTE_PRT 14550
 param set-default MAV_1_UDP_PRT 14550
 
 param set-default SENS_EXT_I2C_PRB 0
+
+if param greater -s UAVCAN_ENABLE 0
+then
+	ifup can0
+	ifup can1
+	ifup can2
+	ifup can3
+fi

boards/nxp/mr-canhubk3/nuttx-config/Kconfig

@@ -3,6 +3,16 @@
 # see the file kconfig-language.txt in the NuttX tools repository.
 #
 
-if ARCH_BOARD_MR_CANHUBK3
+config BOARD_HAS_PROBES
+	bool "Board provides GPIO or other Hardware for signaling to timing analyze."
+	default y
+	---help---
+		This board provides GPIO PWM-CH0-7 as PROBE_1-8 to provide timing signals from selected drivers.
 
-endif # ARCH_BOARD_MR_CANHUBK3
+config BOARD_USE_PROBES
+	bool "Enable the use the board provided PWM-CH0-7 as PROBE_1-8"
+	default n
+	depends on BOARD_HAS_PROBES
+
+	---help---
+		Select to use GPIO PWM-CH0-7 to provide timing signals from selected drivers.

boards/nxp/mr-canhubk3/nuttx-config/include/board.h

@@ -321,4 +321,41 @@
 #define PIN_EMAC_MII_RMII_MDIO    PIN_EMAC_MII_RMII_MDIO_2     /* PTD16 */
 #define PIN_EMAC_MII_RMII_TX_CLK  PIN_EMAC_MII_RMII_TX_CLK_2   /* PTD6 */
 
+
+/* Board provides GPIO or other Hardware for signaling to timing analyzer */
+
+#if defined(CONFIG_BOARD_USE_PROBES)
+# include "s32k3xx_pin.h"
+# include "hardware/s32k3xx_pinmux.h"
+# define PROBE_N(n) (1<<((n)-1))
+# define PROBE_1    (PIN_PTB12 | GPIO_OUTPUT)  /* PWM-0 */
+# define PROBE_2    (PIN_PTB13 | GPIO_OUTPUT)  /* PWM-1 */
+# define PROBE_3    (PIN_PTB14 | GPIO_OUTPUT)  /* PWM-2 */
+# define PROBE_4    (PIN_PTB15 | GPIO_OUTPUT)  /* PWM-3 */
+# define PROBE_5    (PIN_PTB16 | GPIO_OUTPUT)  /* PWM-4 */
+# define PROBE_6    (PIN_PTB17 | GPIO_OUTPUT)  /* PWM-5 */
+# define PROBE_7    (PIN_PTA17 | GPIO_OUTPUT)  /* PWM-6 */
+# define PROBE_8    (PIN_PTE7  | GPIO_OUTPUT)  /* PWM-7 */
+
+# define PROBE_INIT(mask) \
+	do { \
+		if ((mask)& PROBE_N(1)) { s32k3xx_pinconfig(PROBE_1); } \
+		if ((mask)& PROBE_N(2)) { s32k3xx_pinconfig(PROBE_2); } \
+		if ((mask)& PROBE_N(3)) { s32k3xx_pinconfig(PROBE_3); } \
+		if ((mask)& PROBE_N(4)) { s32k3xx_pinconfig(PROBE_4); } \
+		if ((mask)& PROBE_N(5)) { s32k3xx_pinconfig(PROBE_5); } \
+		if ((mask)& PROBE_N(6)) { s32k3xx_pinconfig(PROBE_6); } \
+		if ((mask)& PROBE_N(7)) { s32k3xx_pinconfig(PROBE_7); } \
+		if ((mask)& PROBE_N(8)) { s32k3xx_pinconfig(PROBE_8); } \
+	} while(0)
+
+# define PROBE(n,s)  do {s32k3xx_gpiowrite(PROBE_##n,(s));}while(0)
+# define PROBE_MARK(n) PROBE(n,false);PROBE(n,true)
+#else
+# define PROBE_INIT(mask)
+# define PROBE(n,s)
+# define PROBE_MARK(n)
+#endif
+
+
 #endif  /* __BOARDS_ARM_S32K3XX_MR_CANHUBK3_INCLUDE_BOARD_H */

boards/nxp/mr-canhubk3/nuttx-config/nsh/defconfig

@@ -78,6 +78,7 @@ CONFIG_DEBUG_FEATURES=y
 CONFIG_DEBUG_FULLOPT=y
 CONFIG_DEBUG_HARDFAULT_ALERT=y
 CONFIG_DEBUG_SYMBOLS=y
+CONFIG_DEBUG_TCBINFO=y
 CONFIG_DEFAULT_SMALL=y
 CONFIG_DEV_FIFO_SIZE=0
 CONFIG_DEV_PIPE_MAXSIZE=1024
@@ -103,16 +104,17 @@ CONFIG_HAVE_CXX=y
 CONFIG_HAVE_CXXINITIALIZE=y
 CONFIG_I2C=y
 CONFIG_I2C_RESET=y
-CONFIG_IDLETHREAD_STACKSIZE=800
+CONFIG_IDLETHREAD_STACKSIZE=900
 CONFIG_INIT_ENTRYPOINT="nsh_main"
 CONFIG_INIT_STACKSIZE=2944
 CONFIG_IOB_NBUFFERS=24
 CONFIG_IOB_THROTTLE=0
 CONFIG_IPCFG_BINARY=y
 CONFIG_IPCFG_CHARDEV=y
-CONFIG_IPCFG_PATH="/fs/qspi/mtd_net"
+CONFIG_IPCFG_PATH="/mnt/qspi/mtd_net"
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
+CONFIG_LIBC_MAX_EXITFUNS=1
 CONFIG_LIBC_STRERROR=y
 CONFIG_LPI2C0_DMA=y
 CONFIG_LPI2C1_DMA=y
@@ -145,16 +147,21 @@ CONFIG_NET_ARP_IPIN=y
 CONFIG_NET_ARP_SEND=y
 CONFIG_NET_BROADCAST=y
 CONFIG_NET_CAN=y
+CONFIG_NET_CAN_EXTID=y
+CONFIG_NET_CAN_NOTIFIER=y
 CONFIG_NET_CAN_RAW_FILTER_MAX=1
+CONFIG_NET_CAN_RAW_TX_DEADLINE=y
 CONFIG_NET_CAN_SOCK_OPTS=y
 CONFIG_NET_ETH_PKTSIZE=1518
 CONFIG_NET_ICMP=y
 CONFIG_NET_ICMP_SOCKET=y
+CONFIG_NET_NACTIVESOCKETS=16
 CONFIG_NET_SOLINGER=y
 CONFIG_NET_TCP=y
 CONFIG_NET_TCPBACKLOG=y
 CONFIG_NET_TCP_DELAYED_ACK=y
 CONFIG_NET_TCP_WRITE_BUFFERS=y
+CONFIG_NET_TIMESTAMP=y
 CONFIG_NET_UDP=y
 CONFIG_NET_UDP_CHECKSUMS=y
 CONFIG_NET_UDP_WRITE_BUFFERS=y
@@ -177,6 +184,7 @@ CONFIG_NSH_VARS=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
+CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAM_SIZE=272000
 CONFIG_RAM_START=0x20400000
@@ -205,6 +213,7 @@ CONFIG_S32K3XX_LPSPI2_PINCFG=3
 CONFIG_S32K3XX_LPSPI3=y
 CONFIG_S32K3XX_LPSPI3_PINCFG=3
 CONFIG_S32K3XX_LPSPI4=y
+CONFIG_S32K3XX_LPSPI4_DMA=y
 CONFIG_S32K3XX_LPSPI4_PINCFG=3
 CONFIG_S32K3XX_LPSPI5=y
 CONFIG_S32K3XX_LPSPI5_DMA=y
@@ -220,6 +229,7 @@ CONFIG_S32K3XX_LPUART9=y
 CONFIG_S32K3XX_LPUART_INVERT=y
 CONFIG_S32K3XX_LPUART_SINGLEWIRE=y
 CONFIG_S32K3XX_QSPI=y
+CONFIG_S32K3XX_WKPUINTS=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
 CONFIG_SCHED_HPWORKSTACKSIZE=1280

boards/nxp/mr-canhubk3/src/board_config.h

@@ -82,10 +82,6 @@ __BEGIN_DECLS
 
 #define BOARD_NUMBER_I2C_BUSES  2
 
-/* Count of peripheral clock user configurations */
-
-#define NUM_OF_PERIPHERAL_CLOCKS_0 28
-
 /* Timer I/O PWM and capture */
 
 #define DIRECT_PWM_OUTPUT_CHANNELS  8
@@ -104,7 +100,7 @@ __BEGIN_DECLS
 
 /* User peripheral configuration structure 0 */
 
-extern const struct peripheral_clock_config_s g_peripheral_clockconfig0[NUM_OF_PERIPHERAL_CLOCKS_0];
+extern const struct peripheral_clock_config_s g_peripheral_clockconfig0[];
 
 /****************************************************************************
  * Public Function Prototypes

boards/nxp/mr-canhubk3/src/s32k3xx_clockconfig.c

@@ -156,7 +156,6 @@ const struct clock_configuration_s g_initial_clkconfig = {
 	},
 	.pcc                 =
 	{
-		.count             = NUM_OF_PERIPHERAL_CLOCKS_0,   /* Number of peripheral clock configurations */
 		.pclks             = g_peripheral_clockconfig0,    /* Peripheral clock configurations */
 	},
 };

boards/nxp/mr-canhubk3/src/s32k3xx_periphclocks.c

@@ -41,7 +41,7 @@
  * This is needed to establish the initial peripheral clocking.
  */
 
-const struct peripheral_clock_config_s g_peripheral_clockconfig0[NUM_OF_PERIPHERAL_CLOCKS_0] = {
+const struct peripheral_clock_config_s g_peripheral_clockconfig0[] = {
 	{
 		.clkname = FLEXCAN0_CLK,
 #ifdef CONFIG_S32K3XX_FLEXCAN0
@@ -260,6 +260,10 @@ const struct peripheral_clock_config_s g_peripheral_clockconfig0[NUM_OF_PERIPHER
 	},
 };
 
+unsigned int const num_of_peripheral_clocks_0 =
+	sizeof(g_peripheral_clockconfig0) /
+	sizeof(g_peripheral_clockconfig0[0]);
+
 /****************************************************************************
  * Public Functions
  ****************************************************************************/

boards/nxp/mr-canhubk3/src/spi.cpp

@@ -71,10 +71,10 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
 		initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin17})
 	}),
 	initSPIBus(SPI::Bus::SPI4, {
-		initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortA, GPIO::Pin16}, SPI::DRDY{GPIO::PortA, GPIO::Pin15, 551})
+		initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortA, GPIO::Pin16}, SPI::DRDY{PIN_WKPU20})
 	}),
 	initSPIBus(SPI::Bus::SPI5, {
-		initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortA, GPIO::Pin14}, SPI::DRDY{GPIO::PortA, GPIO::Pin13, 549})
+		initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortA, GPIO::Pin14}, SPI::DRDY{PIN_WKPU4})
 	}),
 };
 

boards/nxp/ucans32k146/nuttx-config/nsh/defconfig

@@ -26,9 +26,11 @@ CONFIG_BCH=y
 CONFIG_BOARDCTL_RESET=y
 CONFIG_BOARD_LOOPSPERMSEC=3997
 CONFIG_BUILTIN=y
+CONFIG_DEBUG_FEATURES=y
 CONFIG_DEBUG_FULLOPT=y
 CONFIG_DEBUG_HARDFAULT_ALERT=y
 CONFIG_DEBUG_SYMBOLS=y
+CONFIG_DEBUG_TCBINFO=y
 CONFIG_DISABLE_MQUEUE=y
 CONFIG_DISABLE_POSIX_TIMERS=y
 CONFIG_EXAMPLES_HELLO=y
@@ -44,7 +46,7 @@ CONFIG_I2CTOOL_MAXADDR=0x7f
 CONFIG_I2CTOOL_MAXBUS=0
 CONFIG_I2CTOOL_MINADDR=0x00
 CONFIG_INIT_ENTRYPOINT="nsh_main"
-CONFIG_INIT_STACKSIZE=2176
+CONFIG_INIT_STACKSIZE=2276
 CONFIG_INTELHEX_BINARY=y
 CONFIG_LIBC_STRERROR=y
 CONFIG_LPI2C0_DMA=y

boards/nxp/ucans32k146/src/board_config.h

@@ -88,11 +88,6 @@ __BEGIN_DECLS
 
 /* SPI chip selects */
 
-/* Count of peripheral clock user configurations */
-
-#define NUM_OF_PERIPHERAL_CLOCKS_0 18
-
-
 /* High-resolution timer */
 #define HRT_TIMER              5  /* FTM timer for the HRT */
 #define HRT_TIMER_CHANNEL      0  /* Use capture/compare channel 0 */

boards/nxp/ucans32k146/src/clockconfig.c

@@ -197,7 +197,6 @@ const struct clock_configuration_s g_initial_clkconfig = {
 	},
 	.pcc             =
 	{
-		.count         = NUM_OF_PERIPHERAL_CLOCKS_0,       /* Number peripheral clock configurations */
 		.pclks         = g_peripheral_clockconfig0,        /* Peripheral clock configurations */
 	},
 	.pmc             =

boards/nxp/ucans32k146/src/periphclocks.c

@@ -200,3 +200,7 @@ const struct peripheral_clock_config_s g_peripheral_clockconfig0[] = {
 		.clkgate = true,
 	},
 };
+
+unsigned int const num_of_peripheral_clocks_0 =
+	sizeof(g_peripheral_clockconfig0) /
+	sizeof(g_peripheral_clockconfig0[0]);

boards/px4/fmu-v5x/default.px4board

@@ -6,7 +6,6 @@ CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS7"
 CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6"
 CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4"
 CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1"
-CONFIG_BOARD_SERIAL_PPB="/dev/ttyS3"
 CONFIG_DRIVERS_ADC_ADS1115=y
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_BMP388=y
@@ -60,7 +59,6 @@ CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=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-v5x/nuttx-config/nsh/defconfig

@@ -154,6 +154,7 @@ CONFIG_NET_BROADCAST=y
 CONFIG_NET_ETH_PKTSIZE=1518
 CONFIG_NET_ICMP=y
 CONFIG_NET_ICMP_SOCKET=y
+CONFIG_NET_NACTIVESOCKETS=16
 CONFIG_NET_SOLINGER=y
 CONFIG_NET_TCP=y
 CONFIG_NET_TCPBACKLOG=y

boards/px4/fmu-v6x/default.px4board

@@ -6,6 +6,7 @@ CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS7"
 CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6"
 CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4"
 CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1"
+CONFIG_BOARD_SERIAL_EXT2="/dev/ttyS3"
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_BMP388=y
 CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP201XX=y

boards/px4/fmu-v6x/init/rc.board_sensors

@@ -52,21 +52,37 @@ then
 	# Internal SPI bus ICM20649
 	icm20649 -s -R 6 start
 else
-# Internal SPI BMI088
-	bmi088 -A -R 4 -s start
-	bmi088 -G -R 4 -s start
+	# Internal SPI BMI088
+	if ver hwtypecmp V6X009010 V6X010010
+	then
+		bmi088 -A -R 2 -s start
+		bmi088 -G -R 2 -s start
+	else
+		bmi088 -A -R 4 -s start
+		bmi088 -G -R 4 -s start
+	fi
 fi
 
 # Internal SPI bus ICM42688p
-icm42688p -R 6 -s start
+if ver hwtypecmp V6X009010 V6X010010
+then
+	icm42688p -R 4 -s start
+else
+	icm42688p -R 6 -s start
+fi
 
 if ver hwtypecmp V6X000003 V6X001003 V6X003003 V6X000004 V6X001004 V6X004003 V6X004004 V6X005003 V6X005004
 then
 	# Internal SPI bus ICM-42670-P (hard-mounted)
 	icm42670p -R 10 -s start
 else
-	# Internal SPI bus ICM-20649 (hard-mounted)
-	icm20649 -R 14 -s start
+	if ver hwtypecmp V6X009010 V6X010010
+	then
+		icm20602 -R 8 -s start
+	else
+		# Internal SPI bus ICM-20649 (hard-mounted)
+		icm20649 -R 14 -s start
+	fi
 fi
 
 # Internal magnetometer on I2c
@@ -74,7 +90,7 @@ if ver hwtypecmp V6X002001
 then
 	rm3100 -I -b 4 start
 else
-	bmm150 -I start
+	bmm150 -I -R 6 start
 fi
 
 # External compass on GPS1/I2C1 (the 3rd external bus): standard Holybro Pixhawk 4 or CUAV V5 GPS/compass puck (with lights, safety button, and buzzer)
@@ -88,7 +104,7 @@ else
 	bmp388 -I -a 0x77 start
 fi
 
-if ver hwtypecmp V6X000000 V6X001000
+if param compare SENS_INT_BARO_EN 1
 then
 	bmp388 -I start
 else

boards/px4/fmu-v6x/nuttx-config/bootloader/defconfig

@@ -98,17 +98,17 @@ CONFIG_STM32H7_OTGFS=y
 CONFIG_STM32H7_PROGMEM=y
 CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y
 CONFIG_STM32H7_TIM1=y
-CONFIG_STM32H7_UART7=y
+CONFIG_STM32H7_UART5=y
 CONFIG_SYSTEMTICK_HOOK=y
 CONFIG_SYSTEM_CDCACM=y
 CONFIG_TASK_NAME_SIZE=24
 CONFIG_TTY_SIGINT=y
 CONFIG_TTY_SIGINT_CHAR=0x03
 CONFIG_TTY_SIGTSTP=y
-CONFIG_UART7_RXBUFSIZE=512
-CONFIG_UART7_RXDMA=y
-CONFIG_UART7_TXBUFSIZE=512
-CONFIG_UART7_TXDMA=y
+CONFIG_UART5_RXBUFSIZE=512
+CONFIG_UART5_RXDMA=y
+CONFIG_UART5_TXBUFSIZE=512
+CONFIG_UART5_TXDMA=y
 CONFIG_USBDEV=y
 CONFIG_USBDEV_BUSPOWERED=y
 CONFIG_USBDEV_MAXPOWER=500

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

@@ -158,6 +158,7 @@ CONFIG_NET_BROADCAST=y
 CONFIG_NET_ETH_PKTSIZE=1518
 CONFIG_NET_ICMP=y
 CONFIG_NET_ICMP_SOCKET=y
+CONFIG_NET_NACTIVESOCKETS=16
 CONFIG_NET_SOLINGER=y
 CONFIG_NET_TCP=y
 CONFIG_NET_TCPBACKLOG=y

boards/px4/fmu-v6x/src/board_config.h

@@ -215,8 +215,7 @@
 #define GPIO_HW_VER_SENSE      /* PH3 */  GPIO_ADC3_INP14
 #define HW_INFO_INIT_PREFIX    "V6X"
 
-#define BOARD_NUM_SPI_CFG_HW_VERSIONS 9 // Rev 0 and Rev 3,4 Sensor sets
-
+#define BOARD_NUM_SPI_CFG_HW_VERSIONS 11 // Rev 0 and Rev 3,4 Sensor sets
 //                 Base/FMUM
 #define V6X00   HW_VER_REV(0x0,0x0) // FMUV6X,                 Rev 0
 #define V6X01   HW_VER_REV(0x0,0x1) // FMUV6X,     BMI388 I2C2 Rev 1
@@ -234,6 +233,11 @@
 #define V6X51   HW_VER_REV(0x5,0x1) // FMUV6X,     BMI388 I2C2    HB Mini Rev 1
 #define V6X53   HW_VER_REV(0x5,0x3) // FMUV6X,     Sensor Set     HB Mini Rev 3
 #define V6X54   HW_VER_REV(0x5,0x4) // FMUV6X,     Sensor Set     HB Mini Rev 4
+#define V6X90   HW_VER_REV(0x9,0x0) //                         Rev 0
+#define V6X0910   HW_VER_REV(0x9,0x10)  // FMUV6X,     rev from EEPROM     Auterion Skynode ver9
+#define V6X1010   HW_VER_REV(0x10,0x10) // FMUV6X,     rev from EEPROM     Auterion Skynode ver10
+
+
 
 #define UAVCAN_NUM_IFACES_RUNTIME  1
 

boards/px4/fmu-v6x/src/manifest.c

@@ -155,6 +155,8 @@ static px4_hw_mft_list_entry_t mft_lists[] = {
 	{V6X04, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 4
 	{V6X14, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO BMP388 moved to I2C2, Sensor Set 4
 	{V6X21, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)},
+	{V6X0910, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // FMUV6X,     rev from EEPROM     Auterion Skynode ver9
+	{V6X1010, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // FMUV6X,     rev from EEPROM     Auterion Skynode ver10
 };
 
 /************************************************************************************

boards/px4/fmu-v6x/src/spi.cpp

@@ -270,6 +270,52 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION
 			initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
 		}),
 	}),
+	initSPIHWVersion(V6X0910, {
+		initSPIBus(SPI::Bus::SPI1, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
+		}, {GPIO::PortI, GPIO::Pin11}),
+		initSPIBus(SPI::Bus::SPI2, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
+		}, {GPIO::PortF, GPIO::Pin4}),
+		initSPIBus(SPI::Bus::SPI3, {
+			initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin8}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}),
+			initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin4}),
+		}, {GPIO::PortE, GPIO::Pin7}),
+		//  initSPIBus(SPI::Bus::SPI4, {
+		//    // no devices
+		// TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h
+		//  }, {GPIO::PortG, GPIO::Pin8}),
+		initSPIBus(SPI::Bus::SPI5, {
+			initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7})
+		}),
+		initSPIBusExternal(SPI::Bus::SPI6, {
+			initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}),
+			initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
+		}),
+	}),
+	initSPIHWVersion(V6X1010, {
+		initSPIBus(SPI::Bus::SPI1, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
+		}, {GPIO::PortI, GPIO::Pin11}),
+		initSPIBus(SPI::Bus::SPI2, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
+		}, {GPIO::PortF, GPIO::Pin4}),
+		initSPIBus(SPI::Bus::SPI3, {
+			initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin8}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}),
+			initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin4}),
+		}, {GPIO::PortE, GPIO::Pin7}),
+		//  initSPIBus(SPI::Bus::SPI4, {
+		//    // no devices
+		// TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h
+		//  }, {GPIO::PortG, GPIO::Pin8}),
+		initSPIBus(SPI::Bus::SPI5, {
+			initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7})
+		}),
+		initSPIBusExternal(SPI::Bus::SPI6, {
+			initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}),
+			initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
+		}),
+	}),
 };
 
 static constexpr bool unused = validateSPIConfig(px4_spi_buses_all_hw);

boards/px4/raspberrypi/default.px4board

@@ -1,5 +1,5 @@
 CONFIG_PLATFORM_POSIX=y
-CONFIG_BOARD_LINUX=y
+CONFIG_BOARD_LINUX_TARGET=y
 CONFIG_BOARD_TOOLCHAIN="arm-linux-gnueabihf"
 CONFIG_BOARD_ARCHITECTURE="cortex-a53"
 CONFIG_BOARD_TESTING=y

boards/scumaker/pilotpi/default.px4board

@@ -1,5 +1,5 @@
 CONFIG_PLATFORM_POSIX=y
-CONFIG_BOARD_LINUX=y
+CONFIG_BOARD_LINUX_TARGET=y
 CONFIG_BOARD_TOOLCHAIN="arm-linux-gnueabihf"
 CONFIG_BOARD_ARCHITECTURE="cortex-a53"
 CONFIG_BOARD_TESTING=y

boards/sky-drones/smartap-airlink/default.px4board

@@ -6,7 +6,7 @@ CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS7"
 CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6"
 CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4"
 CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1"
-CONFIG_BOARD_SERIAL_PPB="/dev/ttyS3"
+CONFIG_BOARD_SERIAL_EXT2="/dev/ttyS3"
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_COMMON_BAROMETERS=y
 CONFIG_DRIVERS_BATT_SMBUS=y

cmake/kconfig.cmake

@@ -301,8 +301,8 @@ if(EXISTS ${BOARD_DEFCONFIG})
 	if(SERIAL_WIFI)
         list(APPEND board_serial_ports WIFI:${SERIAL_WIFI})
 	endif()
-	if(SERIAL_PPB)
-        list(APPEND board_serial_ports PPB:${SERIAL_PPB})
+	if(SERIAL_EXT2)
+        list(APPEND board_serial_ports EXT2:${SERIAL_EXT2})
 	endif()
 
 
@@ -362,7 +362,7 @@ if(EXISTS ${BOARD_DEFCONFIG})
         add_definitions( ${COMPILE_DEFINITIONS})
 	endif()
 
-	if(LINUX)
+	if(LINUX_TARGET)
         add_definitions( "-D__PX4_LINUX" )
 	endif()
 

msg/CMakeLists.txt

@@ -134,6 +134,7 @@ set(msg_files
 	OrbTest.msg
 	OrbTestLarge.msg
 	OrbTestMedium.msg
+	Parameter.msg
 	ParameterUpdate.msg
 	Ping.msg
 	PositionControllerLandingStatus.msg
@@ -145,6 +146,8 @@ set(msg_files
 	PpsCapture.msg
 	PwmInput.msg
 	Px4ioStatus.msg
+	QshellReq.msg
+	QshellRetval.msg
 	RadioStatus.msg
 	RateCtrlStatus.msg
 	RcChannels.msg

msg/Parameter.msg

@@ -0,0 +1,12 @@
+char[17] name                   # parameter name
+int16 index		# -1 if the param_id field should be used as identifier
+int16 index_used		# -1 if the param_id field should be used as identifier
+
+uint8 TYPE_INVALID = 0
+uint8 TYPE_BOOL    = 1
+uint8 TYPE_INT32   = 2
+uint8 TYPE_FLOAT32 = 3
+uint8 type                # parameter type
+
+int32 int32_value               # current value if param_type is int-like
+float32 float32_value           # current value if param_type is float-like

msg/ParameterUpdate.msg

@@ -4,11 +4,8 @@ uint64 timestamp		# time since system start (microseconds)
 
 uint32 instance		# Instance count - constantly incrementing
 
-uint32 get_count
-uint32 set_count
-uint32 find_count
-uint32 export_count
+uint16 count
 
-uint16 active
-uint16 changed
-uint16 custom_default
+Parameter changed_param
+
+uint8 ORB_QUEUE_LENGTH = 8

msg/QshellReq.msg

@@ -0,0 +1,5 @@
+uint64 timestamp		# time since system start (microseconds)
+char[100] cmd
+uint32 MAX_STRLEN = 100
+uint32 strlen
+uint32 request_sequence

msg/QshellRetval.msg

@@ -0,0 +1,3 @@
+uint64 timestamp		# time since system start (microseconds)
+int32 return_value
+uint32 return_sequence

platforms/common/include/px4_platform_common/module_params.h

@@ -42,6 +42,8 @@
 #include <containers/List.hpp>
 
 #include "param.h"
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/parameter_update.h>
 
 class ModuleParams : public ListNode<ModuleParams *>
 {
@@ -68,6 +70,8 @@ public:
 	virtual ~ModuleParams()
 	{
 		if (_parent) { _parent->_children.remove(this); }
+
+		_parameter_update_sub.unsubscribe();
 	}
 
 	// Disallow copy construction and move assignment.
@@ -83,20 +87,75 @@ protected:
 	 */
 	virtual void updateParams()
 	{
-		for (const auto &child : _children) {
-			child->updateParams();
+		bool update_all = true;
+		int parameter_updates = 0;
+
+		while (_parameter_update_sub.updated() && (parameter_updates < parameter_update_s::ORB_QUEUE_LENGTH)) {
+			parameter_updates++;
+
+			parameter_update_s parameter_update;
+
+			if (_parameter_update_sub.copy(&parameter_update)
+			    && (_parameter_update_instance > 0)
+			    && (parameter_update.instance == _parameter_update_instance + 1)
+			    && (parameter_update.changed_param.index >= 0)
+			    && (static_cast<uint16_t>(parameter_update.changed_param.index) != PARAM_INVALID)
+			   ) {
+				update_all = false;
+
+				for (const auto &child : _children) {
+					child->updateParams(parameter_update);
+				}
+
+				updateParamsImpl(parameter_update);
+
+				_parameter_update_instance = parameter_update.instance;
+
+			} else {
+				update_all = true;
+				break;
+			}
 		}
 
-		updateParamsImpl();
+		if (update_all) {
+			PX4_DEBUG("updateParams: updating all params");
+			parameter_update_s parameter_update;
+
+			if (_parameter_update_sub.copy(&parameter_update)) {
+				_parameter_update_instance = parameter_update.instance;
+			}
+
+			for (const auto &child : _children) {
+				child->updateParams();
+			}
+
+			updateParamsImpl();
+
+		} else {
+			PX4_DEBUG("updateParams: updating all params skipped");
+		}
+	}
+
+	virtual void updateParams(const parameter_update_s &parameter_update)
+	{
+		for (const auto &child : _children) {
+			child->updateParams(parameter_update);
+		}
+
+		updateParamsImpl(parameter_update);
 	}
 
 	/**
 	 * @brief The implementation for this is generated with the macro DEFINE_PARAMETERS()
 	 */
 	virtual void updateParamsImpl() {}
+	virtual void updateParamsImpl(const parameter_update_s &parameter_update) {}
 
 private:
 	/** @list _children The module parameter list of inheriting classes. */
 	List<ModuleParams *> _children;
 	ModuleParams *_parent{nullptr};
+
+	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
+	uint32_t _parameter_update_instance{0};
 };

platforms/common/include/px4_platform_common/param.h

@@ -44,11 +44,13 @@
 #include <math.h>
 
 #include <parameters/px4_parameters.hpp>
+#include <uORB/topics/parameter.h>
+#include <uORB/topics/parameter_update.h>
 
 /**
  * get the parameter handle from a parameter enum
  */
-inline static param_t param_handle(px4::params p)
+inline static constexpr param_t param_handle(px4::params p)
 {
 	return (param_t)p;
 }
@@ -62,6 +64,9 @@ inline static param_t param_handle(px4::params p)
 #define _CALL_UPDATE(x) \
 	STRIP(x).update();
 
+#define _SET_PARAMETER_UPDATE(x) \
+	case STRIP(x).handle(): STRIP(x).setValue(parameter_update.changed_param); return;
+
 // define the parameter update method, which will update all parameters.
 // It is marked as 'final', so that wrong usages lead to a compile error (see below)
 #define _DEFINE_PARAMETER_UPDATE_METHOD(...) \
@@ -69,6 +74,11 @@ inline static param_t param_handle(px4::params p)
 	void updateParamsImpl() final { \
 		APPLY_ALL(_CALL_UPDATE, __VA_ARGS__) \
 	} \
+	void updateParamsImpl(const parameter_update_s &parameter_update) { \
+		switch(parameter_update.changed_param.index) { \
+			APPLY_ALL(_SET_PARAMETER_UPDATE, __VA_ARGS__) \
+		} \
+	} \
 	private:
 
 // Define a list of parameters. This macro also creates code to update parameters.
@@ -88,6 +98,12 @@ inline static param_t param_handle(px4::params p)
 		parent_class::updateParamsImpl(); \
 		APPLY_ALL(_CALL_UPDATE, __VA_ARGS__) \
 	} \
+	void updateParamsImpl(const parameter_update_s &parameter_update) { \
+		parent_class::updateParamsImpl(parameter_update); \
+		switch(parameter_update.changed_param.index) { \
+			APPLY_ALL(_SET_PARAMETER_UPDATE, __VA_ARGS__) \
+		} \
+	} \
 	private:
 
 #define DEFINE_PARAMETERS_CUSTOM_PARENT(parent_class, ...) \
@@ -118,7 +134,6 @@ public:
 
 	Param()
 	{
-		param_set_used(handle());
 		update();
 	}
 
@@ -126,35 +141,22 @@ public:
 
 	const float &reference() const { return _val; }
 
-	/// Store the parameter value to the parameter storage (@see param_set())
-	bool commit() const { return param_set(handle(), &_val) == 0; }
-
-	/// Store the parameter value to the parameter storage, w/o notifying the system (@see param_set_no_notification())
-	bool commit_no_notification() const { return param_set_no_notification(handle(), &_val) == 0; }
-
 	/// Set and commit a new value. Returns true if the value changed.
-	bool commit_no_notification(float val)
+	bool set(float val)
 	{
 		if (fabsf(val - _val) > FLT_EPSILON) {
-			set(val);
-			commit_no_notification();
-			return true;
+			_val = val;
+			return (param_set(handle(), &_val) == 0);
 		}
 
 		return false;
 	}
 
-	void set(float val) { _val = val; }
-
-	void reset()
-	{
-		param_reset_no_notification(handle());
-		update();
-	}
-
 	bool update() { return param_get(handle(), &_val) == 0; }
 
-	param_t handle() const { return param_handle(p); }
+	void setValue(const parameter_s &parameter_update) { _val = parameter_update.float32_value; }
+
+	static constexpr param_t handle() { return param_handle(p); }
 private:
 	float _val;
 };
@@ -170,7 +172,6 @@ public:
 	Param(float &external_val)
 		: _val(external_val)
 	{
-		param_set_used(handle());
 		update();
 	}
 
@@ -178,35 +179,18 @@ public:
 
 	const float &reference() const { return _val; }
 
-	/// Store the parameter value to the parameter storage (@see param_set())
-	bool commit() const { return param_set(handle(), &_val) == 0; }
-
-	/// Store the parameter value to the parameter storage, w/o notifying the system (@see param_set_no_notification())
-	bool commit_no_notification() const { return param_set_no_notification(handle(), &_val) == 0; }
-
 	/// Set and commit a new value. Returns true if the value changed.
-	bool commit_no_notification(float val)
+	bool set(float val)
 	{
-		if (fabsf(val - _val) > FLT_EPSILON) {
-			set(val);
-			commit_no_notification();
-			return true;
-		}
-
-		return false;
-	}
-
-	void set(float val) { _val = val; }
-
-	void reset()
-	{
-		param_reset_no_notification(handle());
-		update();
+		_val = val;
+		return (param_set(handle(), &_val) == 0);
 	}
 
 	bool update() { return param_get(handle(), &_val) == 0; }
 
-	param_t handle() const { return param_handle(p); }
+	void setValue(const parameter_s &parameter_update) { _val = parameter_update.float32_value; }
+
+	static constexpr param_t handle() { return param_handle(p); }
 private:
 	float &_val;
 };
@@ -220,7 +204,6 @@ public:
 
 	Param()
 	{
-		param_set_used(handle());
 		update();
 	}
 
@@ -228,35 +211,18 @@ public:
 
 	const int32_t &reference() const { return _val; }
 
-	/// Store the parameter value to the parameter storage (@see param_set())
-	bool commit() const { return param_set(handle(), &_val) == 0; }
-
-	/// Store the parameter value to the parameter storage, w/o notifying the system (@see param_set_no_notification())
-	bool commit_no_notification() const { return param_set_no_notification(handle(), &_val) == 0; }
-
 	/// Set and commit a new value. Returns true if the value changed.
-	bool commit_no_notification(int32_t val)
+	bool set(int32_t val)
 	{
-		if (val != _val) {
-			set(val);
-			commit_no_notification();
-			return true;
-		}
-
-		return false;
-	}
-
-	void set(int32_t val) { _val = val; }
-
-	void reset()
-	{
-		param_reset_no_notification(handle());
-		update();
+		_val = val;
+		return (param_set(handle(), &_val) == 0);
 	}
 
 	bool update() { return param_get(handle(), &_val) == 0; }
 
-	param_t handle() const { return param_handle(p); }
+	void setValue(const parameter_s &parameter_update) { _val = parameter_update.int32_value; }
+
+	static constexpr param_t handle() { return param_handle(p); }
 private:
 	int32_t _val;
 };
@@ -272,7 +238,6 @@ public:
 	Param(int32_t &external_val)
 		: _val(external_val)
 	{
-		param_set_used(handle());
 		update();
 	}
 
@@ -281,34 +246,17 @@ public:
 	const int32_t &reference() const { return _val; }
 
 	/// Store the parameter value to the parameter storage (@see param_set())
-	bool commit() const { return param_set(handle(), &_val) == 0; }
-
-	/// Store the parameter value to the parameter storage, w/o notifying the system (@see param_set_no_notification())
-	bool commit_no_notification() const { return param_set_no_notification(handle(), &_val) == 0; }
-
-	/// Set and commit a new value. Returns true if the value changed.
-	bool commit_no_notification(int32_t val)
+	bool set(int32_t val)
 	{
-		if (val != _val) {
-			set(val);
-			commit_no_notification();
-			return true;
-		}
-
-		return false;
-	}
-
-	void set(int32_t val) { _val = val; }
-
-	void reset()
-	{
-		param_reset_no_notification(handle());
-		update();
+		_val = val;
+		return (param_set(handle(), &_val) == 0);
 	}
 
 	bool update() { return param_get(handle(), &_val) == 0; }
 
-	param_t handle() const { return param_handle(p); }
+	void setValue(const parameter_s &parameter_update) { _val = parameter_update.int32_value; }
+
+	static constexpr param_t handle() { return param_handle(p); }
 private:
 	int32_t &_val;
 };
@@ -322,7 +270,6 @@ public:
 
 	Param()
 	{
-		param_set_used(handle());
 		update();
 	}
 
@@ -330,38 +277,12 @@ public:
 
 	const bool &reference() const { return _val; }
 
-	/// Store the parameter value to the parameter storage (@see param_set())
-	bool commit() const
-	{
-		int32_t value_int = (int32_t)_val;
-		return param_set(handle(), &value_int) == 0;
-	}
-
-	/// Store the parameter value to the parameter storage, w/o notifying the system (@see param_set_no_notification())
-	bool commit_no_notification() const
-	{
-		int32_t value_int = (int32_t)_val;
-		return param_set_no_notification(handle(), &value_int) == 0;
-	}
-
 	/// Set and commit a new value. Returns true if the value changed.
-	bool commit_no_notification(bool val)
+	bool set(bool val)
 	{
-		if (val != _val) {
-			set(val);
-			commit_no_notification();
-			return true;
-		}
-
-		return false;
-	}
-
-	void set(bool val) { _val = val; }
-
-	void reset()
-	{
-		param_reset_no_notification(handle());
-		update();
+		_val = val;
+		int32_t value_int = (int32_t)_val;
+		return (param_set(handle(), &value_int) == 0);
 	}
 
 	bool update()
@@ -377,7 +298,9 @@ public:
 		return false;
 	}
 
-	param_t handle() const { return param_handle(p); }
+	void setValue(const parameter_s &parameter_update) { _val = parameter_update.int32_value; }
+
+	static constexpr param_t handle() { return param_handle(p); }
 private:
 	bool _val;
 };

platforms/common/include/px4_platform_common/posix.h

@@ -104,6 +104,13 @@ typedef struct {
 // Qurt has no fsync implementation so need to declare one here
 // and then define a fake one in the Qurt platform code.
 void fsync(int fd);
+// Qurt doesn't have a way to set the scheduler policy. It is always, essentially,
+// SCHED_FIFO. So have to add a fake function for the code that tries to set it.
+#include <pthread.h>
+__EXPORT int pthread_attr_setschedpolicy(pthread_attr_t *attr, int policy);
+// Qurt POSIX implementation doesn't define the SIGCONT signal so we just map it
+// to a reasonable alternative
+#define SIGCONT SIGALRM
 #endif
 
 __EXPORT int 		px4_open(const char *path, int flags, ...);

platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp

@@ -50,13 +50,13 @@ namespace wq_configurations
 {
 static constexpr wq_config_t rate_ctrl{"wq:rate_ctrl", 3150, 0}; // PX4 inner loop highest priority
 
-static constexpr wq_config_t SPI0{"wq:SPI0", 2336, -1};
-static constexpr wq_config_t SPI1{"wq:SPI1", 2336, -2};
-static constexpr wq_config_t SPI2{"wq:SPI2", 2336, -3};
-static constexpr wq_config_t SPI3{"wq:SPI3", 2336, -4};
-static constexpr wq_config_t SPI4{"wq:SPI4", 2336, -5};
-static constexpr wq_config_t SPI5{"wq:SPI5", 2336, -6};
-static constexpr wq_config_t SPI6{"wq:SPI6", 2336, -7};
+static constexpr wq_config_t SPI0{"wq:SPI0", 2352, -1};
+static constexpr wq_config_t SPI1{"wq:SPI1", 2352, -2};
+static constexpr wq_config_t SPI2{"wq:SPI2", 2352, -3};
+static constexpr wq_config_t SPI3{"wq:SPI3", 2352, -4};
+static constexpr wq_config_t SPI4{"wq:SPI4", 2352, -5};
+static constexpr wq_config_t SPI5{"wq:SPI5", 2352, -6};
+static constexpr wq_config_t SPI6{"wq:SPI6", 2352, -7};
 
 static constexpr wq_config_t I2C0{"wq:I2C0", 2336, -8};
 static constexpr wq_config_t I2C1{"wq:I2C1", 2336, -9};

platforms/common/include/px4_platform_common/tasks.h

@@ -59,7 +59,7 @@ typedef int px4_task_t;
 
 #define px4_task_exit(x) _exit(x)
 
-#elif defined(__PX4_POSIX) || defined(__PX4_QURT)
+#elif defined(__PX4_POSIX)
 
 #include <pthread.h>
 #include <sched.h>

platforms/common/include/px4_platform_common/time.h

@@ -5,7 +5,7 @@
 #include <time.h>
 #include <pthread.h>
 
-#if defined(__PX4_POSIX) || defined(__PX4_QURT)
+#if defined(__PX4_POSIX)
 __BEGIN_DECLS
 __EXPORT int px4_clock_gettime(clockid_t clk_id, struct timespec *tp);
 __END_DECLS

platforms/common/px4_work_queue/WorkQueueManager.cpp

@@ -267,9 +267,7 @@ WorkQueueManagerRun(int, char **)
 			// create new work queue
 
 			// stack size
-#if defined(__PX4_QURT)
-			const size_t stacksize = math::max(8 * 1024, PX4_STACK_ADJUSTED(wq->stacksize));
-#elif defined(__PX4_NUTTX)
+#if defined(__PX4_NUTTX) || defined(__PX4_QURT)
 			const size_t stacksize = math::max(PTHREAD_STACK_MIN, PX4_STACK_ADJUSTED(wq->stacksize));
 #elif defined(__PX4_POSIX)
 			// On posix system , the desired stacksize round to the nearest multiplier of the system pagesize
@@ -304,8 +302,6 @@ WorkQueueManagerRun(int, char **)
 				PX4_ERR("getting sched param for %s failed (%i)", wq->name, ret_getschedparam);
 			}
 
-#ifndef __PX4_QURT
-
 			// schedule policy FIFO
 			int ret_setschedpolicy = pthread_attr_setschedpolicy(&attr, SCHED_FIFO);
 
@@ -313,8 +309,6 @@ WorkQueueManagerRun(int, char **)
 				PX4_ERR("failed to set sched policy SCHED_FIFO (%i)", ret_setschedpolicy);
 			}
 
-#endif // ! QuRT
-
 			// priority
 			param.sched_priority = sched_priority;
 			int ret_setschedparam = pthread_attr_setschedparam(&attr, &param);

platforms/common/spi.cpp

@@ -71,7 +71,7 @@ const px4_spi_bus_t *px4_spi_buses{nullptr};
 
 int px4_find_spi_bus(uint32_t devid)
 {
-	for (int i = 0; px4_spi_buses != nullptr && i < SPI_BUS_MAX_BUS_ITEMS; ++i) {
+	for (int i = 0; (px4_spi_bus_t *) px4_spi_buses != nullptr && i < SPI_BUS_MAX_BUS_ITEMS; ++i) {
 		const px4_spi_bus_t &bus_data = px4_spi_buses[i];
 
 		if (bus_data.bus == -1) {

platforms/common/uORB/uORBDeviceNode.cpp

@@ -385,7 +385,7 @@ uORB::DeviceNode::print_statistics(int max_topic_length)
 	lock();
 
 	const uint8_t instance = get_instance();
-	const int8_t sub_count = subscriber_count();
+	const int16_t sub_count = subscriber_count();
 	const uint8_t queue_size = get_queue_size();
 
 	unlock();

platforms/common/uORB/uORBDeviceNode.hpp

@@ -197,7 +197,7 @@ public:
 
 	uint8_t get_queue_size() const { return _queue_size; }
 
-	int8_t subscriber_count() const { return _subscriber_count; }
+	int16_t subscriber_count() const { return _subscriber_count; }
 
 	/**
 	 * Returns the number of updated data relative to the parameter 'generation'
@@ -296,7 +296,7 @@ private:
 	const uint8_t _instance; /**< orb multi instance identifier */
 	bool _advertised{false};  /**< has ever been advertised (not necessarily published data yet) */
 	uint8_t _queue_size; /**< maximum number of elements in the queue */
-	int8_t _subscriber_count{0};
+	int16_t _subscriber_count{0};
 
 
 // Determine the data range

platforms/common/work_queue/hrt_queue.c

@@ -126,15 +126,10 @@ int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t del
 	dq_addlast(&work->dq, &wqueue->q);
 
 	if (px4_getpid() != wqueue->pid) { /* only need to wake up if called from a different thread */
-#ifdef __PX4_QURT
-		px4_task_kill(wqueue->pid, SIGALRM);      /* Wake up the worker thread */
-#else
 		//wqueue->pid == own task? -> don't signal
 		px4_task_kill(wqueue->pid, SIGCONT);      /* Wake up the worker thread */
-#endif
 	}
 
 	hrt_work_unlock();
 	return PX4_OK;
 }
-

platforms/common/work_queue/hrt_thread.c

@@ -138,6 +138,16 @@ static void hrt_work_process()
 	/* Default to sleeping for 1 sec */
 	next  = 1000000;
 
+#ifdef __PX4_QURT
+	// In Posix certain signals wake up a sleeping thread but it isn't the case
+	// with the Qurt POSIX implementation. So rather than assume we can come out
+	// of the sleep early by a signal we just wake up more often. The best way to
+	// fix this would be to move to a px4_sem_timedwait. But the current implementation
+	// of that function on Qurt uses this hrt_thread! So, it would all have to be
+	// re-written to use Qurt semaphores which do have timed waits.
+	next = 1000;
+#endif
+
 	hrt_work_lock();
 
 	work  = (struct work_s *)wqueue->q.head;
@@ -287,10 +297,5 @@ void hrt_work_queue_init(void)
 					    (char *const *)NULL);
 
 
-#ifdef __PX4_QURT
-	signal(SIGALRM, _sighandler);
-#else
 	signal(SIGCONT, _sighandler);
-#endif
 }
-

platforms/common/work_queue/work_queue.c

@@ -41,6 +41,7 @@
 #include <px4_platform_common/defines.h>
 #include <px4_platform_common/workqueue.h>
 #include <px4_platform_common/tasks.h>
+#include <px4_platform_common/posix.h>
 
 #include <signal.h>
 #include <stdint.h>
@@ -125,11 +126,7 @@ int work_queue(int qid, struct work_s *work, worker_t worker, void *arg, uint32_
 	work->qtime  = clock_systimer(); /* Time work queued */
 
 	dq_addlast((dq_entry_t *)work, &wqueue->q);
-#ifdef __PX4_QURT
-	px4_task_kill(wqueue->pid, SIGALRM);      /* Wake up the worker thread */
-#else
 	px4_task_kill(wqueue->pid, SIGCONT);      /* Wake up the worker thread */
-#endif
 
 	work_unlock(qid);
 	return PX4_OK;

platforms/nuttx/src/px4/nxp/s32k3xx/include/px4_arch/hw_description.h

@@ -131,7 +131,6 @@ enum Pin {
 struct GPIOPin {
 	Port port;
 	Pin pin;
-	uint16_t imcr;
 };
 }
 
@@ -244,7 +243,9 @@ enum class Bus {
 };
 
 using CS = GPIO::GPIOPin;
-using DRDY = GPIO::GPIOPin;
+using DRDY = uint32_t;
+
+#define DRDYInvalid 0
 
 struct bus_device_external_cfg_t {
 	CS cs_gpio;

platforms/nuttx/src/px4/nxp/s32k3xx/include/px4_arch/spi_hw_description.h

@@ -45,9 +45,8 @@ static inline constexpr px4_spi_bus_device_t initSPIDevice(uint32_t devid, SPI::
 	px4_spi_bus_device_t ret{};
 	ret.cs_gpio = getGPIOPort(cs_gpio.port) | getGPIOPin(cs_gpio.pin) | (GPIO_LOWDRIVE | GPIO_OUTPUT_ONE);
 
-	if (drdy_gpio.port != GPIO::PortInvalid) {
-		ret.drdy_gpio = getGPIOPort(drdy_gpio.port) | getGPIOPin(drdy_gpio.pin) | getIMCR(drdy_gpio.imcr) |
-				(GPIO_PULLUP | PIN_INT_BOTH);
+	if (drdy_gpio != DRDYInvalid) {
+		ret.drdy_gpio = drdy_gpio;
 	}
 
 	if (PX4_SPIDEVID_TYPE(devid) == 0) { // it's a PX4 device (internal or external)

platforms/qurt/CMakeLists.txt

@@ -33,12 +33,18 @@
 
 get_property(module_libraries GLOBAL PROPERTY PX4_MODULE_LIBRARIES)
 
+px4_qurt_generate_builtin_commands(
+        OUT ${PX4_BINARY_DIR}/apps
+        MODULE_LIST ${module_libraries}
+)
+
 include_directories(
 	${CMAKE_CURRENT_BINARY_DIR}
 	)
 
 add_library(px4 SHARED
 	${PX4_SOURCE_DIR}/platforms/qurt/unresolved_symbols.c
+	${PX4_BINARY_DIR}/apps.cpp
 	)
 
 target_link_libraries(px4

platforms/qurt/cmake/px4_impl_os.cmake

@@ -200,6 +200,7 @@ function(px4_os_prebuild_targets)
 			ARGN ${ARGN})
 
 	add_library(prebuild_targets INTERFACE)
+	target_link_libraries(prebuild_targets INTERFACE drivers_board)
 	add_dependencies(prebuild_targets DEPENDS uorb_headers)
 
 endfunction()

platforms/qurt/cmake/qurt_reqs.cmake

@@ -99,6 +99,7 @@ set(ARCHCPUFLAGS
 
 add_definitions(
 	-D __QURT
+	# -D DEBUG_BUILD # Add this to get extra debug output
 	-D _PROVIDE_POSIX_TIME_DECLS
 	-D _HAS_C9X
 	-D restrict=__restrict__

platforms/qurt/include/hrt_work.h

@@ -0,0 +1,62 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2015 Mark Charlebois. 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/log.h>
+#include <px4_platform_common/posix.h>
+#include <semaphore.h>
+#include <px4_platform_common/workqueue.h>
+
+#pragma once
+
+__BEGIN_DECLS
+
+extern px4_sem_t _hrt_work_lock;
+extern struct wqueue_s g_hrt_work;
+
+void hrt_work_queue_init(void);
+int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t usdelay);
+void hrt_work_cancel(struct work_s *work);
+
+static inline void hrt_work_lock(void);
+static inline void hrt_work_lock()
+{
+	px4_sem_wait(&_hrt_work_lock);
+}
+
+static inline void hrt_work_unlock(void);
+static inline void hrt_work_unlock()
+{
+	px4_sem_post(&_hrt_work_lock);
+}
+
+__END_DECLS

platforms/qurt/include/px4_arch/spi_hw_description.h

@@ -0,0 +1,60 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *	notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *	notice, this list of conditions and the following disclaimer in
+ *	the documentation and/or other materials provided with the
+ *	distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *	used to endorse or promote products derived from this software
+ *	without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <px4_platform_common/spi.h>
+
+static inline constexpr px4_spi_bus_device_t initSPIDevice(uint8_t devid_driver)
+{
+	px4_spi_bus_device_t ret{};
+	ret.cs_gpio = 1; // set to some non-zero value to indicate this is used
+	ret.drdy_gpio = 1;
+	ret.devid = PX4_SPIDEV_ID(PX4_SPI_DEVICE_ID, 0);
+	ret.devtype_driver = devid_driver;
+	return ret;
+}
+
+static inline constexpr px4_spi_bus_t initSPIBus(int bus, const px4_spi_bus_devices_t &devices)
+{
+	px4_spi_bus_t ret{};
+
+	for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) {
+		ret.devices[i] = devices.devices[i];
+	}
+
+	ret.bus = bus;
+	ret.is_external = false; // all buses are marked internal on QuRT
+	ret.requires_locking = false;
+	return ret;
+}

platforms/qurt/src/px4/CMakeLists.txt

@@ -36,6 +36,7 @@ set(QURT_LAYER_SRCS
 	drv_hrt.cpp
 	tasks.cpp
 	px4_qurt_impl.cpp
+	main.cpp
 	)
 
 add_library(px4_layer
@@ -43,3 +44,4 @@ add_library(px4_layer
 )
 
 target_link_libraries(px4_layer PRIVATE work_queue)
+target_link_libraries(px4_layer PRIVATE drivers_board)

platforms/qurt/src/px4/main.cpp

@@ -0,0 +1,206 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2022 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 <px4_platform_common/tasks.h>
+#include <px4_platform_common/time.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/log.h>
+#include <px4_platform_common/defines.h>
+#include <vector>
+#include <string>
+#include <map>
+#include <stdio.h>
+#include <stdlib.h>
+
+#include "apps.h"
+
+#define MAX_ARGS 8 // max number of whitespace separated args after app name
+
+__BEGIN_DECLS
+int slpi_main(int argc, char *argv[]);
+__END_DECLS
+
+static void run_cmd(apps_map_type &apps, const std::vector<std::string> &appargs)
+{
+	// command is appargs[0]
+	std::string command = appargs[0];
+
+	//replaces app.find with iterator code to avoid null pointer exception
+	for (apps_map_type::iterator it = apps.begin(); it != apps.end(); ++it)
+		if (it->first == command) {
+			// one for command name, one for null terminator
+			const char *arg[MAX_ARGS + 2];
+
+			unsigned int i = 0;
+
+			if (appargs.size() > MAX_ARGS + 1) {
+				PX4_ERR("%d too many arguments in run_cmd", appargs.size() - (MAX_ARGS + 1));
+				return;
+			}
+
+			while (i < appargs.size() && appargs[i].c_str()[0] != '\0') {
+				arg[i] = (char *)appargs[i].c_str();
+				PX4_DEBUG("  arg%d = '%s'\n", i, arg[i]);
+				++i;
+			}
+
+			arg[i] = (char *)0;
+
+			//PX4_DEBUG_PRINTF(i);
+			if (apps[command] == NULL) {
+				PX4_ERR("Null function !!\n");
+
+			} else {
+				apps[command](i, (char **)arg);
+				break;
+			}
+
+		}
+}
+
+void eat_whitespace(const char *&b, int &i)
+{
+	// Eat whitespace
+	while (b[i] == ' ' || b[i] == '\t') { ++i; }
+
+	b = &b[i];
+	i = 0;
+}
+
+static void process_commands(apps_map_type &apps, const char *cmds)
+{
+	std::vector<std::string> appargs;
+	int i = 0;
+	const char *b = cmds;
+	char arg[256];
+
+	// Eat leading whitespace
+	eat_whitespace(b, i);
+
+	for (;;) {
+		// End of command line
+		if (b[i] == '\n' || b[i] == '\0') {
+			strncpy(arg, b, i);
+			arg[i] = '\0';
+			appargs.push_back(arg);
+
+			// If we have a command to run
+			if (appargs.size() > 0) {
+				PX4_DEBUG("Processing command: %s", appargs[0].c_str());
+
+				for (int ai = 1; ai < (int)appargs.size(); ai++) {
+					PX4_DEBUG("   > arg: %s", appargs[ai].c_str());
+				}
+
+				run_cmd(apps, appargs);
+			}
+
+			appargs.clear();
+
+			if (b[i] == '\n') {
+				eat_whitespace(b, ++i);
+				continue;
+
+			} else {
+				break;
+			}
+		}
+
+		// End of arg
+		else if (b[i] == ' ') {
+			strncpy(arg, b, i);
+			arg[i] = '\0';
+			appargs.push_back(arg);
+			eat_whitespace(b, ++i);
+			continue;
+		}
+
+		++i;
+	}
+}
+
+
+const char *get_commands()
+{
+	// All that needs to be started automatically on the DSP side
+	// are uorb and qshell. After that, everything else can get
+	// started from the main startup script on the Linux side.
+	static const char *commands = "uorb start\nqshell start\n";
+
+	return commands;
+}
+
+
+int slpi_entry(int argc, char *argv[])
+{
+	PX4_INFO("Inside slpi_entry");
+	apps_map_type apps;
+	init_app_map(apps);
+	process_commands(apps, get_commands());
+	sleep(1); // give time for all commands to execute before starting external function
+
+	for (;;) {
+		sleep(1);
+	}
+
+	return 0;
+}
+
+static void usage()
+{
+	PX4_INFO("Usage: slpi {start |stop}");
+}
+
+
+extern "C" {
+
+	int slpi_main(int argc, char *argv[])
+	{
+		int ret = 0;
+
+		if (argc == 2 && strcmp(argv[1], "start") == 0) {
+			(void) px4_task_spawn_cmd("slpi",
+						  SCHED_DEFAULT,
+						  SCHED_PRIORITY_MAX - 5,
+						  1500,
+						  slpi_entry,
+						  argv);
+
+		} else {
+			usage();
+			ret = -1;
+		}
+
+		return ret;
+	}
+}

platforms/qurt/src/px4/tasks.cpp

@@ -66,6 +66,14 @@ static task_entry_t taskmap[PX4_MAX_TASKS];
 static bool task_mutex_initialized = false;
 static pthread_mutex_t task_mutex;
 
+// These are some Qurt pthread stubs needed for compilation and linking
+extern "C" {
+
+	int pthread_setname_np(pthread_t __target_thread, const char *__name) {	return 0; }
+	int pthread_attr_setschedpolicy(pthread_attr_t *attr, int policy) { return 0; }
+
+}
+
 static void *entry_adapter(void *ptr)
 {
 	task_entry_t *data;
@@ -147,11 +155,12 @@ static px4_task_t px4_task_spawn_internal(const char *name, int priority, px4_ma
 		return -1;
 	}
 
-	priority = 255 - priority;
-
-	if (priority < 5) { priority = 5; }
-
-	if (priority > 250) { priority = 250; }
+	// Qurt threads have different priority numbers. 1 is the highest
+	// priority and 254 is the lowest. But we are using the pthread
+	// implementation on Qurt which returns 255 when you call sched_get_priority_max.
+	// So, the big assumption is that the Qurt pthread implementation deals with
+	// this properly when creating the underlying Qurt task.
+	// TODO: Needs to be verified!
 
 	if (strlen(name) >= PX4_TASK_MAX_NAME_LENGTH) {
 		pthread_mutex_unlock(&task_mutex);

platforms/ros2/include/px4_platform_common/param.h

@@ -107,7 +107,6 @@ public:
 
 	Param()
 	{
-		//param_set_used(handle());
 		update();
 	}
 
@@ -122,19 +121,12 @@ public:
 		//return param_set(handle(), &_val) == 0;
 	}
 
-	/// Store the parameter value to the parameter storage, w/o notifying the system (@see param_set_no_notification())
-	bool commit_no_notification() const
-	{
-		return true; // TODO
-		//return param_set_no_notification(handle(), &_val) == 0;
-	}
-
 	/// Set and commit a new value. Returns true if the value changed.
-	bool commit_no_notification(float val)
+	bool commit(float val)
 	{
 		if (fabsf(val - _val) > FLT_EPSILON) {
 			set(val);
-			commit_no_notification();
+			commit();
 			return true;
 		}
 
@@ -143,12 +135,6 @@ public:
 
 	void set(float val) { _val = val; }
 
-	void reset()
-	{
-		//param_reset_no_notification(handle()); // TODO
-		update();
-	}
-
 	bool update()
 	{
 		return true; // TODO
@@ -171,7 +157,6 @@ public:
 	Param(float &external_val)
 		: _val(external_val)
 	{
-		//param_set_used(handle()); // TODO
 		update();
 	}
 
@@ -186,19 +171,12 @@ public:
 		return true;
 	}
 
-	/// Store the parameter value to the parameter storage, w/o notifying the system (@see param_set_no_notification())
-	bool commit_no_notification() const
-	{
-		return true;
-		//return param_set_no_notification(handle(), &_val) == 0;
-	}
-
 	/// Set and commit a new value. Returns true if the value changed.
-	bool commit_no_notification(float val)
+	bool commit(float val)
 	{
 		if (fabsf(val - _val) > FLT_EPSILON) {
 			set(val);
-			commit_no_notification();
+			commit();
 			return true;
 		}
 
@@ -207,12 +185,6 @@ public:
 
 	void set(float val) { _val = val; }
 
-	void reset()
-	{
-		//param_reset_no_notification(handle());
-		update();
-	}
-
 	bool update()
 	{
 		return true;
@@ -233,7 +205,6 @@ public:
 
 	Param()
 	{
-		//param_set_used(handle());
 		update();
 	}
 
@@ -248,19 +219,12 @@ public:
 		//return param_set(handle(), &_val) == 0;
 	}
 
-	/// Store the parameter value to the parameter storage, w/o notifying the system (@see param_set_no_notification())
-	bool commit_no_notification() const
-	{
-		//return param_set_no_notification(handle(), &_val) == 0;
-		return true;
-	}
-
 	/// Set and commit a new value. Returns true if the value changed.
-	bool commit_no_notification(int32_t val)
+	bool commit(int32_t val)
 	{
 		if (val != _val) {
 			set(val);
-			commit_no_notification();
+			commit();
 			return true;
 		}
 
@@ -269,12 +233,6 @@ public:
 
 	void set(int32_t val) { _val = val; }
 
-	void reset()
-	{
-		//param_reset_no_notification(handle());
-		update();
-	}
-
 	bool update()
 	{
 		//return param_get(handle(), &_val) == 0;
@@ -297,7 +255,6 @@ public:
 	Param(int32_t &external_val)
 		: _val(external_val)
 	{
-		//param_set_used(handle());
 		update();
 	}
 
@@ -312,19 +269,12 @@ public:
 		//return param_set(handle(), &_val) == 0;
 	}
 
-	/// Store the parameter value to the parameter storage, w/o notifying the system (@see param_set_no_notification())
-	bool commit_no_notification() const
-	{
-		return true;
-		//return param_set_no_notification(handle(), &_val) == 0;
-	}
-
 	/// Set and commit a new value. Returns true if the value changed.
-	bool commit_no_notification(int32_t val)
+	bool commit(int32_t val)
 	{
 		if (val != _val) {
 			set(val);
-			commit_no_notification();
+			commit();
 			return true;
 		}
 
@@ -333,12 +283,6 @@ public:
 
 	void set(int32_t val) { _val = val; }
 
-	void reset()
-	{
-		//param_reset_no_notification(handle());
-		update();
-	}
-
 	bool update()
 	{
 		//return param_get(handle(), &_val) == 0;
@@ -359,7 +303,6 @@ public:
 
 	Param()
 	{
-		//param_set_used(handle());
 		update();
 	}
 
@@ -375,20 +318,12 @@ public:
 		return true;
 	}
 
-	/// Store the parameter value to the parameter storage, w/o notifying the system (@see param_set_no_notification())
-	bool commit_no_notification() const
-	{
-		int32_t value_int = (int32_t)_val;
-		//return param_set_no_notification(handle(), &value_int) == 0;
-		return true;
-	}
-
 	/// Set and commit a new value. Returns true if the value changed.
-	bool commit_no_notification(bool val)
+	bool commit(bool val)
 	{
 		if (val != _val) {
 			set(val);
-			commit_no_notification();
+			commit();
 			return true;
 		}
 
@@ -397,12 +332,6 @@ public:
 
 	void set(bool val) { _val = val; }
 
-	void reset()
-	{
-		//param_reset_no_notification(handle());
-		update();
-	}
-
 	bool update()
 	{
 		int32_t value_int;

src/drivers/camera_trigger/camera_trigger.cpp

@@ -308,7 +308,7 @@ CameraTrigger::CameraTrigger() :
 
 		_activation_time = 40.0f;
 		PX4_WARN("Trigger interval too low for PWM interface, setting to 40 ms");
-		param_set_no_notification(_p_activation_time, &(_activation_time));
+		param_set(_p_activation_time, &(_activation_time));
 	}
 
 	// Advertise critical publishers here, because we cannot advertise in interrupt context
@@ -581,7 +581,7 @@ CameraTrigger::Run()
 			 */
 			if (cmd.param1 > 0.0f) {
 				_distance = cmd.param1;
-				param_set_no_notification(_p_distance, &_distance);
+				param_set(_p_distance, &_distance);
 
 				_trigger_enabled = true;
 				_trigger_paused = false;
@@ -598,7 +598,7 @@ CameraTrigger::Run()
 			if (cmd.param2 > 0.0f) {
 				if (_camera_interface_mode == CAMERA_INTERFACE_MODE_GPIO) {
 					_activation_time = cmd.param2;
-					param_set_no_notification(_p_activation_time, &(_activation_time));
+					param_set(_p_activation_time, &(_activation_time));
 				}
 			}
 
@@ -616,14 +616,14 @@ CameraTrigger::Run()
 
 			if (cmd.param1 > 0.0f) {
 				_interval = cmd.param1;
-				param_set_no_notification(_p_interval, &(_interval));
+				param_set(_p_interval, &_interval);
 			}
 
 			// We can only control the shutter integration time of the camera in GPIO mode
 			if (cmd.param2 > 0.0f) {
 				if (_camera_interface_mode == CAMERA_INTERFACE_MODE_GPIO) {
 					_activation_time = cmd.param2;
-					param_set_no_notification(_p_activation_time, &_activation_time);
+					param_set(_p_activation_time, &_activation_time);
 				}
 			}
 
@@ -637,7 +637,7 @@ CameraTrigger::Run()
 
 			if (cmd.param1 > 0.0f) {
 				_distance = cmd.param1;
-				param_set_no_notification(_p_distance, &_distance);
+				param_set(_p_distance, &_distance);
 
 				_trigger_enabled = true;
 				_trigger_paused = false;
@@ -654,14 +654,14 @@ CameraTrigger::Run()
 			if (cmd.param2 > 0.0f) {
 				if (_camera_interface_mode == CAMERA_INTERFACE_MODE_GPIO) {
 					_activation_time = cmd.param2;
-					param_set_no_notification(_p_activation_time, &(_activation_time));
+					param_set(_p_activation_time, &_activation_time);
 				}
 			}
 
 			// Set Param for minimum camera trigger interval
 			if (cmd.param3 > 0.0f) {
 				_min_interval = cmd.param3;
-				param_set_no_notification(_p_min_interval, &(_min_interval));
+				param_set(_p_min_interval, &_min_interval);
 			}
 
 			if (cmd.param4 >= 2.0f) {

src/drivers/dshot/DShot.cpp

@@ -135,7 +135,7 @@ void DShot::enable_dshot_outputs(const bool enabled)
 			if (dshot_frequency_request != 0) {
 				if (dshot_frequency != 0 && dshot_frequency != dshot_frequency_request) {
 					PX4_WARN("Only supporting a single frequency, adjusting param %s", param_name);
-					param_set_no_notification(handle, &dshot_frequency_param);
+					param_set(handle, &dshot_frequency_param);
 
 				} else {
 					dshot_frequency = dshot_frequency_request;

src/drivers/gps/gps.cpp

@@ -904,6 +904,15 @@ GPS::run()
 			gpsConfig.output_mode = GPSHelper::OutputMode::GPS;
 		}
 
+		int32_t gps_ubx_cfg_intf = static_cast<int32_t>(GPSHelper::InterfaceProtocolsMask::ALL_DISABLED);
+		handle = param_find("GPS_UBX_CFG_INTF");
+
+		if (handle != PARAM_INVALID) {
+			param_get(handle, &gps_ubx_cfg_intf);
+		}
+
+		gpsConfig.interface_protocols = static_cast<GPSHelper::InterfaceProtocolsMask>(gps_ubx_cfg_intf);
+
 		if (_helper && _helper->configure(_baudrate, gpsConfig) == 0) {
 
 			/* reset report */

src/drivers/gps/params.c

@@ -125,6 +125,22 @@ PARAM_DEFINE_INT32(GPS_UBX_MODE, 0);
  */
 PARAM_DEFINE_INT32(GPS_UBX_BAUD2, 230400);
 
+/**
+ * u-blox protocol configuration for interfaces
+ *
+ * @min 0
+ * @max 32
+ * @bit 0 Enable I2C input protocol UBX
+ * @bit 1 Enable I2C input protocol NMEA
+ * @bit 2 Enable I2C input protocol RTCM3X
+ * @bit 3 Enable I2C output protocol UBX
+ * @bit 4 Enable I2C output protocol NMEA
+ * @bit 5 Enable I2C output protocol RTCM3X
+ *
+ * @reboot_required true
+ * @group GPS
+ */
+PARAM_DEFINE_INT32(GPS_UBX_CFG_INTF, 0);
 
 /**
  * Heading/Yaw offset for dual antenna GPS

src/drivers/qshell/Kconfig

@@ -0,0 +1,3 @@
+menu "qshell"
+rsource "*/Kconfig"
+endmenu #qshell

src/drivers/qshell/posix/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2022 ModalAI, Inc. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -31,11 +31,11 @@
 #
 ############################################################################
 
-px4_add_library(AngularVelocityControl
-	AngularVelocityControl.cpp
-)
-target_compile_options(AngularVelocityControl PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
-target_include_directories(AngularVelocityControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
-target_link_libraries(AngularVelocityControl PRIVATE mathlib)
-
-px4_add_unit_gtest(SRC AngularVelocityControlTest.cpp LINKLIBS AngularVelocityControl)
+px4_add_module(
+	MODULE drivers__qshell__posix
+	MAIN qshell
+	SRCS
+		qshell_start_posix.cpp
+		qshell.cpp
+	DEPENDS
+	)

src/drivers/qshell/posix/qshell.cpp

@@ -0,0 +1,132 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2022 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 <px4_platform_common/log.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <string.h>
+#include <string>
+#include <stdlib.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/topics/qshell_retval.h>
+
+#include "qshell.h"
+
+#define QSHELL_RESPONSE_WAIT_TIME_US (20 * 1000000) // 20 sec, for temporary ESC calibration
+
+// Static variables
+px4::AppState QShell::appState;
+uint32_t QShell::_current_sequence{0};
+
+int QShell::main(std::vector<std::string> argList)
+{
+	int ret = _send_cmd(argList);
+
+	if (ret != 0) {
+		PX4_ERR("Could not send command");
+		return -1;
+	}
+
+	ret = _wait_for_retval();
+
+	if (ret != 0) {
+		PX4_ERR("Command failed");
+		return -1;
+	}
+
+	return 0;
+}
+
+int QShell::_send_cmd(std::vector<std::string> &argList)
+{
+	// Let's use a sequence number to check if a return value belongs to the
+	// command that we just sent and not a previous one that we assumed that
+	// it had timed out.
+	++_current_sequence;
+
+	qshell_req_s qshell_req{};
+	std::string cmd;
+
+	for (size_t i = 0; i < argList.size(); i++) {
+		cmd += argList[i];
+
+		if (i < argList.size() - 1) {
+			cmd += " ";
+		}
+	}
+
+	if (cmd.size() >= qshell_req.MAX_STRLEN) {
+		PX4_ERR("Command too long: %d >= %d", (int) cmd.size(), (int) qshell_req.MAX_STRLEN);
+		return -1;
+	}
+
+	PX4_INFO("Send cmd: '%s'", cmd.c_str());
+
+	qshell_req.strlen = cmd.size();
+	strcpy((char *)qshell_req.cmd, cmd.c_str());
+	qshell_req.request_sequence = _current_sequence;
+	qshell_req.timestamp = hrt_absolute_time();
+
+	_qshell_req_pub.publish(qshell_req);
+
+	return 0;
+}
+
+int QShell::_wait_for_retval()
+{
+	const hrt_abstime time_started_us = hrt_absolute_time();
+	qshell_retval_s retval;
+	memset(&retval, 0, sizeof(qshell_retval_s));
+
+	while (hrt_elapsed_time(&time_started_us) < QSHELL_RESPONSE_WAIT_TIME_US) {
+		if (_qshell_retval_sub.update(&retval)) {
+			if (retval.return_sequence != _current_sequence) {
+				PX4_WARN("Ignoring return value with wrong sequence");
+
+			} else {
+				if (retval.return_value) {
+					PX4_INFO("cmd returned with: %d", retval.return_value);
+				}
+
+				PX4_INFO("qshell return value timestamp: %lu, local time: %lu", retval.timestamp, hrt_absolute_time());
+
+				return retval.return_value;
+			}
+		}
+
+		px4_usleep(1000);
+	}
+
+	PX4_ERR("command timed out");
+	return -1;
+}

src/drivers/qshell/posix/qshell.h

@@ -0,0 +1,64 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2022 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <px4_platform_common/app.h>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/qshell_req.h>
+#include <uORB/topics/qshell_retval.h>
+
+#include <vector>
+#include <string>
+
+class QShell
+{
+public:
+	QShell() = default;
+	~QShell() = default;
+
+	int main(std::vector<std::string> argList);
+
+	static px4::AppState appState; /* track requests to terminate app */
+
+private:
+	int _send_cmd(std::vector<std::string> &argList);
+	int _wait_for_retval();
+
+	uORB::Publication<qshell_req_s>	_qshell_req_pub{ORB_ID(qshell_req)};
+
+	uORB::Subscription		_qshell_retval_sub{ORB_ID(qshell_retval)};
+
+	static uint32_t			_current_sequence;
+};

src/drivers/qshell/posix/qshell_start_posix.cpp

@@ -0,0 +1,70 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2022 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 "qshell.h"
+#include <px4_platform_common/log.h>
+#include <px4_platform_common/app.h>
+#include <px4_platform_common/tasks.h>
+#include <stdio.h>
+#include <string.h>
+#include <sched.h>
+#include <cstdlib>
+#include <string>
+#include <vector>
+
+//using namespace px4;
+
+extern "C" __EXPORT int qshell_main(int argc, char *argv[]);
+
+static void usage()
+{
+	PX4_DEBUG("usage: qshell cmd [args]");
+}
+
+int qshell_main(int argc, char *argv[])
+{
+	if (argc < 2) {
+		usage();
+		return 1;
+	}
+
+	std::vector<std::string> argList;
+
+	for (int i = 1; i < argc; i++) {
+		argList.push_back(argv[i]);
+	}
+
+
+	QShell qshell;
+	return qshell.main(argList);
+}

src/drivers/qshell/qurt/CMakeLists.txt

@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 2022 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.
+#
+############################################################################
+
+px4_add_module(
+	MODULE drivers__qshell__qurt
+	MAIN qshell
+	SRCS
+		qshell_start_qurt.cpp
+		qshell.cpp
+	DEPENDS
+	)

src/drivers/qshell/qurt/qshell.cpp

@@ -0,0 +1,183 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2022 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 "qshell.h"
+
+#include <px4_platform_common/log.h>
+#include <px4_platform_common/time.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/defines.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <string.h>
+#include <iostream>
+#include <fstream>
+#include <sstream>
+#include <signal.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <uORB/topics/qshell_retval.h>
+#include <drivers/drv_hrt.h>
+
+#define MAX_ARGS 8 // max number of whitespace separated args after app name
+
+px4::AppState QShell::appState;
+
+QShell::QShell()
+{
+	PX4_INFO("Init app map initialized");
+	init_app_map(m_apps);
+}
+
+int QShell::main()
+{
+	appState.setRunning(true);
+
+	usleep(2000);
+
+	int sub_qshell_req = orb_subscribe(ORB_ID(qshell_req));
+
+	if (sub_qshell_req == PX4_ERROR) {
+		PX4_ERR("Error subscribing to qshell_req topic");
+		return -1;
+	}
+
+	px4_pollfd_struct_t fds[1] = {};
+	fds[0].fd = sub_qshell_req;
+	fds[0].events = POLLIN;
+
+	while (!appState.exitRequested()) {
+
+		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000);
+
+		if (pret > 0 && fds[0].revents & POLLIN) {
+
+			orb_copy(ORB_ID(qshell_req), sub_qshell_req, &m_qshell_req);
+
+			PX4_INFO("qshell gotten: %s", m_qshell_req.cmd);
+
+			char current_char;
+			std::string arg;
+			std::vector<std::string> appargs;
+
+			for (unsigned str_idx = 0; str_idx < m_qshell_req.strlen; str_idx++) {
+				current_char = m_qshell_req.cmd[str_idx];
+
+				if (isspace(current_char)) { // split at spaces
+					if (arg.length()) {
+						appargs.push_back(arg);
+						arg = "";
+
+					}
+
+				} else {
+					arg += current_char;
+				}
+			}
+
+			appargs.push_back(arg);  // push last argument
+
+			qshell_retval_s retval{};
+			retval.return_value = run_cmd(appargs);
+			retval.return_sequence = m_qshell_req.request_sequence;
+
+			if (retval.return_value) {
+				PX4_ERR("Failed to execute command: %s", m_qshell_req.cmd);
+
+			} else {
+				PX4_INFO("Ok executing command: %s", m_qshell_req.cmd);
+			}
+
+			retval.timestamp = hrt_absolute_time();
+			_qshell_retval_pub.publish(retval);
+
+		} else if (pret == 0) {
+
+			// Timing out is fine.
+		} else {
+			// Something is wrong.
+			usleep(10000);
+		}
+	}
+
+	appState.setRunning(false);
+	return 0;
+}
+
+int QShell::run_cmd(const std::vector<std::string> &appargs)
+{
+	// command is appargs[0]
+	std::string command = appargs[0];
+
+	if (command.compare("help") == 0) {
+		list_builtins(m_apps);
+		return 0;
+	}
+
+	//replaces app.find with iterator code to avoid null pointer exception
+	for (apps_map_type::iterator it = m_apps.begin(); it != m_apps.end(); ++it) {
+		if (it->first == command) {
+			// one for command name, one for null terminator
+			const char *arg[MAX_ARGS + 2];
+
+			unsigned int i = 0;
+
+			if (appargs.size() > MAX_ARGS + 1) {
+				PX4_ERR("%d too many arguments in run_cmd", appargs.size() - (MAX_ARGS + 1));
+				return 1;
+			}
+
+			while (i < appargs.size() && appargs[i].c_str()[0] != '\0') {
+				arg[i] = (char *)appargs[i].c_str();
+				PX4_INFO("  arg%d = '%s'\n", i, arg[i]);
+				++i;
+			}
+
+			arg[i] = (char *)0;
+
+			//PX4_DEBUG_PRINTF(i);
+			if (m_apps[command] == NULL) {
+				PX4_ERR("Null function !!\n");
+
+			} else {
+				int x = m_apps[command](i, (char **)arg);
+				return x;
+			}
+
+		}
+	}
+
+	PX4_ERR("Command %s not found", command.c_str());
+	return 1;
+}

src/drivers/qshell/qurt/qshell.h

@@ -0,0 +1,63 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2022 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <px4_platform_common/app.h>
+#include <string>
+#include <vector>
+#include <uORB/Publication.hpp>
+#include <uORB/topics/qshell_retval.h>
+#include <uORB/topics/qshell_req.h>
+
+#include "apps.h"
+
+class QShell
+{
+public:
+	QShell();
+	~QShell() {}
+
+	int main();
+	int run_cmd(const std::vector<std::string> &appargs);
+
+	static px4::AppState appState; /* track requests to terminate app */
+
+private:
+
+	uORB::Publication<qshell_retval_s>	_qshell_retval_pub{ORB_ID(qshell_retval)};
+
+	qshell_req_s m_qshell_req{};
+
+	apps_map_type m_apps;
+};

src/drivers/qshell/qurt/qshell_start_qurt.cpp

@@ -0,0 +1,107 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2022 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 "qshell.h"
+#include <px4_platform_common/log.h>
+#include <px4_platform_common/app.h>
+#include <px4_platform_common/tasks.h>
+#include <stdio.h>
+#include <string.h>
+#include <sched.h>
+
+static int daemon_task;             /* Handle of deamon task / thread */
+
+extern "C" __EXPORT int qshell_main(int argc, char *argv[]);
+
+int qshell_entry(int argc, char **argv)
+{
+	PX4_INFO("qshell entry.....");
+	QShell qshell;
+	qshell.main();
+
+	PX4_INFO("goodbye");
+	return 0;
+}
+
+static void usage()
+{
+	PX4_INFO("usage: qshell {start|stop|status}");
+}
+
+int qshell_main(int argc, char *argv[])
+{
+	if (argc < 2) {
+		usage();
+		return 1;
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (QShell::appState.isRunning()) {
+			PX4_INFO("already running");
+			/* this is not an error */
+			return 0;
+		}
+
+		PX4_INFO("before starting the qshell_entry task");
+
+		daemon_task = px4_task_spawn_cmd("qshell",
+						 SCHED_DEFAULT,
+						 SCHED_PRIORITY_MAX - 5,
+						 8192,
+						 qshell_entry,
+						 (char *const *)argv);
+		PX4_INFO("after starting the qshell_entry task");
+
+		return 0;
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		QShell::appState.requestExit();
+		return 0;
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (QShell::appState.isRunning()) {
+			PX4_INFO("is running");
+
+		} else {
+			PX4_INFO("not started");
+		}
+
+		return 0;
+	}
+
+	usage();
+	return 1;
+}

src/drivers/rc_input/RCInput.cpp

@@ -773,7 +773,6 @@ void RCInput::Run()
 		   ) {
 			// RC_INPUT_PROTO auto => locked selection
 			_param_rc_input_proto.set(_rc_scan_state);
-			_param_rc_input_proto.commit();
 		}
 	}
 }

src/drivers/transponder/sagetech_mxs/SagetechMXS.cpp

@@ -106,8 +106,6 @@ int SagetechMXS::task_spawn(int argc, char *argv[])
 	return PX4_ERROR;
 }
 
-
-
 bool SagetechMXS::init()
 {
 	ScheduleOnInterval(UPDATE_INTERVAL_US);	// 50Hz
@@ -117,7 +115,6 @@ bool SagetechMXS::init()
 	if (vehicle_list == nullptr) {
 		if (_adsb_list_max.get() > MAX_VEHICLES_LIMIT) {	// Safety Check
 			_adsb_list_max.set(MAX_VEHICLES_LIMIT);
-			_adsb_list_max.commit();
 			list_size_allocated = MAX_VEHICLES_LIMIT;
 		}
 
@@ -158,8 +155,7 @@ int SagetechMXS::custom_command(int argc, char *argv[])
 	}
 
 	if (!strcmp(verb, "ident")) {
-		get_instance()->_adsb_ident.set(1);
-		return get_instance()->_adsb_ident.commit();
+		return get_instance()->_adsb_ident.set(1);
 	}
 
 	if (!strcmp(verb, "opmode")) {
@@ -170,20 +166,16 @@ int SagetechMXS::custom_command(int argc, char *argv[])
 			return PX4_ERROR;
 
 		} else if (!strcmp(opmode, "off") || !strcmp(opmode, "0")) {
-			get_instance()->_mxs_op_mode.set(0);
-			return get_instance()->_mxs_op_mode.commit();
+			return get_instance()->_mxs_op_mode.set(0);
 
 		} else if (!strcmp(opmode, "on") || !strcmp(opmode, "1")) {
-			get_instance()->_mxs_op_mode.set(1);
-			return get_instance()->_mxs_op_mode.commit();
+			return get_instance()->_mxs_op_mode.set(1);
 
 		} else if (!strcmp(opmode, "stby") || !strcmp(opmode, "2")) {
-			get_instance()->_mxs_op_mode.set(2);
-			return get_instance()->_mxs_op_mode.commit();
+			return get_instance()->_mxs_op_mode.set(2);
 
 		} else if (!strcmp(opmode, "alt") || !strcmp(opmode, "3")) {
-			get_instance()->_mxs_op_mode.set(3);
-			return get_instance()->_mxs_op_mode.commit();
+			return get_instance()->_mxs_op_mode.set(3);
 
 		} else {
 			print_usage("Invalid Op Mode");
@@ -207,8 +199,7 @@ int SagetechMXS::custom_command(int argc, char *argv[])
 			return PX4_ERROR;
 
 		} else {
-			get_instance()->_adsb_squawk.set(sqk);
-			return get_instance()->_adsb_squawk.commit();
+			return get_instance()->_adsb_squawk.set(sqk);
 		}
 	}
 
@@ -305,7 +296,6 @@ void SagetechMXS::Run()
 			auto_config_installation();
 			auto_config_flightid();
 			_mxs_op_mode.set(sg_op_mode_t::modeStby);
-			_mxs_op_mode.commit();
 			send_targetreq_msg();
 			mxs_state.initialized = true;
 		}
@@ -751,7 +741,6 @@ void SagetechMXS::send_operating_msg()
 
 	if (mxs_state.op.identOn) {
 		_adsb_ident.set(0);
-		_adsb_ident.commit();
 	}
 
 	if (_gps.vel_ned_valid) {
@@ -1248,22 +1237,16 @@ int SagetechMXS::handle_fid(const char *fid)
 int SagetechMXS::store_inst_resp()
 {
 	_mxs_op_mode.set(mxs_state.ack.opMode);
-	_mxs_op_mode.commit();
 	_adsb_icao.set(mxs_state.inst.icao);
-	_adsb_icao.commit();
 	_adsb_len_width.set(mxs_state.inst.size);
-	_adsb_len_width.commit();
 	_adsb_emit_type.set(convert_sg_to_emitter_type(mxs_state.inst.emitter));
-	_adsb_emit_type.commit();
 	return PX4_OK;
 }
 
-
 void SagetechMXS::auto_config_operating()
 {
 	mxs_state.op.opMode = sg_op_mode_t::modeOff;
 	_mxs_op_mode.set(sg_op_mode_t::modeOff);
-	_mxs_op_mode.commit();
 
 	if (check_valid_squawk(_adsb_squawk.get())) {
 		mxs_state.op.squawk = convert_base_to_decimal(BASE_OCTAL, _adsb_squawk.get());

src/drivers/uavcan/CMakeLists.txt

@@ -40,7 +40,10 @@ set(UAVCAN_USE_CPP03 ON CACHE BOOL "uavcan cpp03")
 set(UAVCAN_PLATFORM "generic")
 
 if(CONFIG_ARCH_CHIP)
-	if(${CONFIG_ARCH_CHIP} MATCHES "kinetis")
+	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")

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

@@ -360,18 +360,13 @@ public:
 	 * This function can either initialize the driver at a fixed bit rate, or it can perform
 	 * automatic bit rate detection. For theory please refer to the CiA application note #801.
 	 *
-	 * @param delay_callable    A callable entity that suspends execution for strictly more than one second.
-	 *                          The callable entity will be invoked without arguments.
-	 *                          @ref getRecommendedListeningDelay().
-	 *
-	 * @param inout_bitrate     Fixed bit rate or zero. Zero invokes the bit rate detection process.
+     * @param inout_bitrate     Fixed bit rate or zero. Zero invokes the bit rate detection process.
 	 *                          If auto detection was used, the function will update the argument
 	 *                          with established bit rate. In case of an error the value will be undefined.
 	 *
 	 * @return                  Negative value on error; non-negative on success. Refer to constants Err*.
 	 */
-	template <typename DelayCallable>
-	int init(DelayCallable delay_callable, uavcan::uint32_t &inout_bitrate = BitRateAutoDetect)
+	int init(uavcan::uint32_t &inout_bitrate = BitRateAutoDetect)
 	{
 		if (inout_bitrate > 0) {
 			return driver.init(inout_bitrate, CanIface::NormalMode);
@@ -389,7 +384,7 @@ public:
 
 				const int res = driver.init(inout_bitrate, CanIface::SilentMode);
 
-				delay_callable();
+				usleep(1000000);
 
 				if (res >= 0) {
 					for (uavcan::uint8_t iface = 0; iface < driver.getNumIfaces(); iface++) {

src/drivers/uavcan/uavcan_drivers/socketcan/driver/CMakeLists.txt

@@ -6,6 +6,7 @@ add_compile_options(-Wno-unused-variable)
 add_library(uavcan_socketcan_driver STATIC
     src/socketcan.cpp
     src/thread.cpp
+    src/clock.cpp
     )
 
 add_dependencies(uavcan_socketcan_driver uavcan)

src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/clock.hpp

@@ -46,6 +46,16 @@
 
 namespace uavcan_socketcan
 {
+namespace clock
+{
+/**
+ * Performs UTC phase and frequency adjustment.
+ * The UTC time will be zero until first adjustment has been performed.
+ * This function is thread safe.
+ */
+void adjustUtc(uavcan::UtcDuration adjustment);
+//FIXME
+}
 /**
  * Different adjustment modes can be used for time synchronization
  */