Merge branch 'main' into pr-fw_ctrl_api

2026-05-20 02:09:07 +08:00 · 2025-02-24 16:39:56 +01:00 · 2025-02-24 16:39:56 +01:00 · d333661fdc
commit d333661fdc
parent bc433aa3fd 689db41c57
31 changed files with 845 additions and 133 deletions
--- a/.github/workflows/flash_analysis.yml
+++ b/.github/workflows/flash_analysis.yml
@ -1,5 +1,10 @@
 name: FLASH usage analysis

+permissions:
+  contents: read
+  pull-requests: write
+  issues: write
+
 on:
  push:
    branches:
--- a/4
+++ b/4
@ -235,9 +235,9 @@ pipeline {
              sh('rm -f px4_msgs/srv/*.srv')
              sh('rm -f px4_msgs/srv/versioned/*.srv')
              sh('cp msg/*.msg px4_msgs/msg/')
-              sh('mkdir -p px4_msgs/msg/versioned && cp msg/versioned/*.msg px4_msgs/msg/versioned/')
+              sh('cp msg/versioned/*.msg px4_msgs/msg/ || true')
              sh('cp srv/*.srv px4_msgs/srv/')
-              sh('mkdir -p px4_msgs/srv/versioned && cp srv/versioned/*.srv px4_msgs/srv/versioned/')
+              sh('cp srv/versioned/*.srv px4_msgs/srv/ || true')
              sh('cd px4_msgs; git status; git add .; git commit -a -m "Update message definitions `date`" || true')
              sh('cd px4_msgs; git push origin main || true')
              sh('rm -rf px4_msgs')
--- a/ROMFS/performance-test/CMakeLists.txt
+++ b/ROMFS/performance-test/CMakeLists.txt
@ -0,0 +1,34 @@
+############################################################################
+#
+#   Copyright (c) 2025 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.
+#
+############################################################################
+
+add_subdirectory(init.d)
--- a/ROMFS/performance-test/init.d/CMakeLists.txt
+++ b/ROMFS/performance-test/init.d/CMakeLists.txt
@ -0,0 +1,36 @@
+############################################################################
+#
+#   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_romfs_files(
+	rcS
+)
--- a/ROMFS/performance-test/init.d/rcS
+++ b/ROMFS/performance-test/init.d/rcS
@ -0,0 +1,68 @@
+#!/bin/sh
+# Un comment and use set +e to ignore and set -e to enable 'exit on error control'
+set +e
+# Un comment the line below to help debug scripts by printing a trace of the script commands
+#set -x
+# PX4FMU startup script.
+#
+# NOTE: environment variable references:
+#    If the dollar sign ('$') is followed by a left bracket ('{') then the
+#    variable name is terminated with the right bracket character ('}').
+#    Otherwise, the variable name goes to the end of the argument.
+#
+#
+# NOTE: COMMENT LINES ARE REMOVED BEFORE STORED IN ROMFS.
+#
+#------------------------------------------------------------------------------
+set R /
+
+#
+# Print full system version.
+#
+ver all
+
+#
+# Set the parameter file the board supports params on
+# MTD device.
+#
+if mft query -q -k MTD -s MTD_PARAMETERS -v /fs/mtd_params
+then
+	set PARAM_FILE /fs/mtd_params
+fi
+
+#
+# Load parameters.
+#
+# if the board has a storage for (factory) calibration data
+if mft query -q -k MTD -s MTD_CALDATA -v /fs/mtd_caldata
+then
+	param load /fs/mtd_caldata
+fi
+
+#
+# Load parameters.
+#
+param select $PARAM_FILE
+if ! param load
+then
+	param reset_all
+fi
+
+#
+# Try to mount the microSD card.
+#
+mount -t vfat /dev/mmcsd0 /fs/microsd
+if [ $? = 0 ]
+then
+	echo "SD card mounted at /fs/microsd"
+else
+	echo "No SD card found"
+fi
+
+unset R
+
+echo ""
+echo "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
+echo "!!!!!!     This is the PERFORMANCE TESTING firmware! WARNs and ERRORs are expected!     !!!!!"
+echo "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
+echo ""
--- a/Tools/ci/generate_board_targets_json.py
+++ b/Tools/ci/generate_board_targets_json.py
@ -69,7 +69,7 @@ def process_target(px4board_file, target_name):
    group = None

    if px4board_file.endswith("default.px4board") or \
-        px4board_file.endswith("recovery.px4board") or \
+        px4board_file.endswith("performance-test.px4board") or \
        px4board_file.endswith("bootloader.px4board"):
        kconf.load_config(px4board_file, replace=True)
    else: # Merge config with default.px4board
--- a/Tools/copy_to_ros_ws.sh
+++ b/Tools/copy_to_ros_ws.sh
@ -29,6 +29,5 @@ then
  rm -rf "${PX4_MSGS_DIR}"/srv/*.srv
 fi
 cp -ar "${PX4_SRC_DIR}"/msg/*.msg "${PX4_MSGS_DIR}"/msg
-mkdir -p "${PX4_MSGS_DIR}"/msg/versioned
-cp -ar "${PX4_SRC_DIR}"/msg/versioned/*.msg "${PX4_MSGS_DIR}"/msg/versioned
+cp -ar "${PX4_SRC_DIR}"/msg/versioned/*.msg "${PX4_MSGS_DIR}"/msg
 cp -ar "${PX4_SRC_DIR}"/srv/*.srv "${PX4_MSGS_DIR}"/srv
--- a/Tools/kconfig/allyesconfig.py
+++ b/Tools/kconfig/allyesconfig.py
@ -74,6 +74,7 @@ exception_list_sitl = [
    'SYSTEMCMDS_I2CDETECT', # Not supported in SITL
    'SYSTEMCMDS_DMESG', # Not supported in SITL
    'SYSTEMCMDS_USB_CONNECTED', # Not supported in SITL
+    'SYSTEMCMDS_MFT_CFG', # Not supported in SITL
    'MODULES_SPACECRAFT', # Clashes with Control Allocation (mom's spaghetti code)
 ]

--- a/Tools/simulation/gz
+++ b/Tools/simulation/gz
@ -1 +1 @@
-Subproject commit db4af69088cccef4549cf3a5c195d5cd97d6b36a
+Subproject commit 183cbee9e2250d1ca5517cd76c5d9a7e47cc0b7a
--- a/boards/px4/fmu-v5x/performance-test.px4board
+++ b/boards/px4/fmu-v5x/performance-test.px4board
@ -0,0 +1,31 @@
+CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
+CONFIG_BOARD_ARCHITECTURE="cortex-m7"
+CONFIG_BOARD_ETHERNET=y
+CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0"
+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_BOARD_ROMFSROOT="performance-test"
+CONFIG_DRIVERS_ADC_BOARD_ADC=y
+CONFIG_SYSTEMCMDS_DMESG=y
+CONFIG_SYSTEMCMDS_GPIO=y
+CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
+CONFIG_SYSTEMCMDS_I2CDETECT=y
+CONFIG_SYSTEMCMDS_LED_CONTROL=y
+CONFIG_SYSTEMCMDS_MFT=y
+CONFIG_SYSTEMCMDS_MTD=y
+CONFIG_SYSTEMCMDS_MFT_CFG=y
+CONFIG_SYSTEMCMDS_PARAM=y
+CONFIG_SYSTEMCMDS_PERF=y
+CONFIG_SYSTEMCMDS_REBOOT=y
+CONFIG_SYSTEMCMDS_SD_BENCH=y
+CONFIG_SYSTEMCMDS_SD_STRESS=y
+CONFIG_SYSTEMCMDS_SERIAL_TEST=y
+CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
+CONFIG_SYSTEMCMDS_TOP=y
+CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
+CONFIG_SYSTEMCMDS_UORB=y
+CONFIG_SYSTEMCMDS_VER=y
+CONFIG_SYSTEMCMDS_WORK_QUEUE=y
--- a/boards/px4/fmu-v6x/performance-test.px4board
+++ b/boards/px4/fmu-v6x/performance-test.px4board
@ -0,0 +1,31 @@
+CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
+CONFIG_BOARD_ARCHITECTURE="cortex-m7"
+CONFIG_BOARD_ETHERNET=y
+CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0"
+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_BOARD_ROMFSROOT="performance-test"
+CONFIG_DRIVERS_ADC_BOARD_ADC=y
+CONFIG_SYSTEMCMDS_DMESG=y
+CONFIG_SYSTEMCMDS_GPIO=y
+CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
+CONFIG_SYSTEMCMDS_I2CDETECT=y
+CONFIG_SYSTEMCMDS_LED_CONTROL=y
+CONFIG_SYSTEMCMDS_MFT=y
+CONFIG_SYSTEMCMDS_MTD=y
+CONFIG_SYSTEMCMDS_MFT_CFG=y
+CONFIG_SYSTEMCMDS_PARAM=y
+CONFIG_SYSTEMCMDS_PERF=y
+CONFIG_SYSTEMCMDS_REBOOT=y
+CONFIG_SYSTEMCMDS_SD_BENCH=y
+CONFIG_SYSTEMCMDS_SD_STRESS=y
+CONFIG_SYSTEMCMDS_SERIAL_TEST=y
+CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
+CONFIG_SYSTEMCMDS_TOP=y
+CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
+CONFIG_SYSTEMCMDS_UORB=y
+CONFIG_SYSTEMCMDS_VER=y
+CONFIG_SYSTEMCMDS_WORK_QUEUE=y
--- a/cmake/kconfig.cmake
+++ b/cmake/kconfig.cmake
@ -35,7 +35,7 @@ if(EXISTS ${BOARD_DEFCONFIG})
 	# Depend on BOARD_DEFCONFIG so that we reconfigure on config change
 	set_property(DIRECTORY APPEND PROPERTY CMAKE_CONFIGURE_DEPENDS ${BOARD_DEFCONFIG})

-	if(${LABEL} MATCHES "default" OR ${LABEL} MATCHES "recovery" OR ${LABEL} MATCHES "bootloader" OR ${LABEL} MATCHES "canbootloader")
+	if(${LABEL} MATCHES "default" OR ${LABEL} MATCHES "performance-test" OR ${LABEL} MATCHES "bootloader" OR ${LABEL} MATCHES "canbootloader")
 		# Generate boardconfig from saved defconfig
 		execute_process(
 			COMMAND ${CMAKE_COMMAND} -E env ${COMMON_KCONFIG_ENV_SETTINGS}
--- a/msg/Buffer128.msg
+++ b/msg/Buffer128.msg
@ -1,8 +0,0 @@
-uint64 timestamp		# time since system start (microseconds)
-
-uint8 len                       # length of data
-uint32 MAX_BUFLEN = 128
-
-uint8[128] data                 # data
-
-# TOPICS voxl2_io_data
--- a/msg/CMakeLists.txt
+++ b/msg/CMakeLists.txt
@ -48,7 +48,6 @@ set(msg_files
 	AirspeedValidated.msg
 	AirspeedWind.msg
 	AutotuneAttitudeControlStatus.msg
-	Buffer128.msg
 	ButtonEvent.msg
 	CameraCapture.msg
 	CameraStatus.msg
--- a/platforms/nuttx/NuttX/nuttx
+++ b/platforms/nuttx/NuttX/nuttx
@ -1 +1 @@
-Subproject commit 9a213327fbb2dab252185d468fb29348e1833d8c
+Subproject commit 205b3100f8f63944a45faa5cfb5d3f86e904ee59
--- a/src/drivers/actuators/voxl_esc/voxl_esc.cpp
+++ b/src/drivers/actuators/voxl_esc/voxl_esc.cpp
@ -90,13 +90,13 @@ VoxlEsc::~VoxlEsc()

 int VoxlEsc::init()
 {
-	PX4_INFO("VOXL_ESC: Starting VOXL ESC driver");
+	PX4_ERR("Starting VOXL ESC driver");

 	/* Getting initial parameter values */
 	int ret = update_params();

 	if (ret != OK) {
-		PX4_ERR("VOXL_ESC: Failed to update params during init");
+		PX4_ERR("Failed to update params during init");
 		return ret;
 	}

@ -124,13 +124,13 @@ int VoxlEsc::device_init()

 	// Open serial port
 	if (!_uart_port.isOpen()) {
-		PX4_INFO("VOXL_ESC: Opening UART ESC device %s, baud rate %" PRIi32, _device, _parameters.baud_rate);
+		PX4_ERR("Opening UART ESC device %s, baud rate %" PRIi32, _device, _parameters.baud_rate);
 #ifndef __PX4_QURT

 		//warn user that unless DMA is enabled for UART RX, data can be lost due to high frequency of per char cpu interrupts
 		//at least at 2mbit, there are definitely losses, did not test other baud rates to find the cut off
 		if (_parameters.baud_rate > 250000) {
-			PX4_WARN("VOXL_ESC: Baud rate is too high for non-DMA based UART, this can lead to loss of RX data");
+			PX4_WARN("Baud rate is too high for non-DMA based UART, this can lead to loss of RX data");
 		}

 #endif
@ -165,7 +165,7 @@ int VoxlEsc::device_init()
 	}

 	// Detect ESCs
-	PX4_INFO("VOXL_ESC: Detecting ESCs...");
+	PX4_ERR("Detecting ESCs...");
 	qc_esc_packet_init(&_fb_packet);

 	//request extended version info from each ESC and wait for reply
@ -174,7 +174,7 @@ int VoxlEsc::device_init()
 		cmd.len = qc_esc_create_extended_version_request_packet(esc_id, cmd.buf, sizeof(cmd.buf));

 		if (_uart_port.write(cmd.buf, cmd.len) != cmd.len) {
-			PX4_ERR("VOXL_ESC: Could not write version request packet to UART port");
+			PX4_ERR("Could not write version request packet to UART port");
 			return -1;
 		}

@ -201,17 +201,17 @@ int VoxlEsc::device_init()
 						QC_ESC_EXTENDED_VERSION_INFO ver;
 						memcpy(&ver, _fb_packet.buffer, packet_size);

-						PX4_INFO("VOXL_ESC: \tESC ID     : %i", ver.id);
-						PX4_INFO("VOXL_ESC: \tBoard Type : %i: %s", ver.hw_version, board_id_to_name(ver.hw_version));
+						PX4_ERR("\tESC ID     : %i", ver.id);
+						PX4_ERR("\tBoard Type : %i: %s", ver.hw_version, board_id_to_name(ver.hw_version));

 						uint8_t *u = &ver.unique_id[0];
-						PX4_INFO("VOXL_ESC: \tUnique ID  : 0x%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X",
-							 u[11], u[10], u[9], u[8], u[7], u[6], u[5], u[4], u[3], u[2], u[1], u[0]);
+						PX4_ERR("\tUnique ID  : 0x%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X",
+							u[11], u[10], u[9], u[8], u[7], u[6], u[5], u[4], u[3], u[2], u[1], u[0]);

-						PX4_INFO("VOXL_ESC: \tFirmware   : version %4d, hash %.12s", ver.sw_version, ver.firmware_git_version);
-						PX4_INFO("VOXL_ESC: \tBootloader : version %4d, hash %.12s", ver.bootloader_version, ver.bootloader_git_version);
-						PX4_INFO("VOXL_ESC: \tReply time : %" PRIu32 "us", (uint32_t)response_time);
-						PX4_INFO("VOXL_ESC:");
+						PX4_ERR("\tFirmware   : version %4d, hash %.12s", ver.sw_version, ver.firmware_git_version);
+						PX4_ERR("\tBootloader : version %4d, hash %.12s", ver.bootloader_version, ver.bootloader_git_version);
+						PX4_ERR("\tReply time : %" PRIu32 "us", (uint32_t)response_time);
+						PX4_INFO("");

 						if (ver.id == esc_id) {
 							memcpy(&_version_info[esc_id], &ver, sizeof(ver));
@ -223,7 +223,7 @@ int VoxlEsc::device_init()
 		}

 		if (!got_response) {
-			PX4_ERR("VOXL_ESC: ESC %d version info response timeout", esc_id);
+			PX4_ERR("ESC %d version info response timeout", esc_id);
 		}
 	}

@ -232,7 +232,7 @@ int VoxlEsc::device_init()

 	for (int esc_id = 0; esc_id < VOXL_ESC_OUTPUT_CHANNELS; esc_id++) {
 		if (_version_info[esc_id].sw_version == 0) {
-			PX4_ERR("VOXL_ESC: ESC ID %d was not detected", esc_id);
+			PX4_ERR("ESC ID %d was not detected", esc_id);
 			esc_detection_fault = true;
 		}
 	}
@ -240,7 +240,7 @@ int VoxlEsc::device_init()
 	//check the firmware hashes to make sure they are the same. Firmware hash has 8 chars plus optional "*"
 	for (int esc_id = 1; esc_id < VOXL_ESC_OUTPUT_CHANNELS; esc_id++) {
 		if (strncmp(_version_info[0].firmware_git_version, _version_info[esc_id].firmware_git_version, 9) != 0) {
-			PX4_ERR("VOXL_ESC: ESC %d Firmware hash does not match ESC 0 firmware hash:  (%.12s) != (%.12s)",
+			PX4_ERR("ESC %d Firmware hash does not match ESC 0 firmware hash:  (%.12s) != (%.12s)",
 				esc_id, _version_info[esc_id].firmware_git_version, _version_info[0].firmware_git_version);
 			esc_detection_fault = true;
 		}
@ -256,13 +256,13 @@ int VoxlEsc::device_init()
 	}

 	if (esc_detection_fault) {
-		PX4_ERR("VOXL_ESC: Critical error during ESC initialization");
+		PX4_ERR("Critical error during ESC initialization");
 		return -1;
 	}

-	PX4_INFO("VOXL_ESC: Use extened rpm packet : %d", _extended_rpm);
+	PX4_ERR("Use extened rpm packet : %d", _extended_rpm);

-	PX4_INFO("VOXL_ESC: All ESCs successfully detected");
+	PX4_ERR("All ESCs successfully detected");

 	_device_initialized =  true;

@ -309,45 +309,53 @@ int VoxlEsc::load_params(voxl_esc_params_t *params, ch_assign_t *map)
 	param_get(param_find("VOXL_ESC_T_WARN"), &params->esc_warn_temp_threshold);
 	param_get(param_find("VOXL_ESC_T_OVER"), &params->esc_over_temp_threshold);

+	param_get(param_find("VOXL_ESC_GPIO_CH"), &params->gpio_ctl_channel);
+
 	if (params->rpm_min >= params->rpm_max) {
-		PX4_ERR("VOXL_ESC: Invalid parameter VOXL_ESC_RPM_MIN.  Please verify parameters.");
+		PX4_ERR("Invalid parameter VOXL_ESC_RPM_MIN.  Please verify parameters.");
 		params->rpm_min = 0;
 		ret = PX4_ERROR;
 	}

 	if (params->turtle_motor_percent < 0 || params->turtle_motor_percent > 100) {
-		PX4_ERR("VOXL_ESC: Invalid parameter VOXL_ESC_T_PERC.  Please verify parameters.");
+		PX4_ERR("Invalid parameter VOXL_ESC_T_PERC.  Please verify parameters.");
 		params->turtle_motor_percent = 0;
 		ret = PX4_ERROR;
 	}

 	if (params->turtle_motor_deadband < 0 || params->turtle_motor_deadband > 100) {
-		PX4_ERR("VOXL_ESC: Invalid parameter VOXL_ESC_T_DEAD.  Please verify parameters.");
+		PX4_ERR("Invalid parameter VOXL_ESC_T_DEAD.  Please verify parameters.");
 		params->turtle_motor_deadband = 0;
 		ret = PX4_ERROR;
 	}

 	if (params->turtle_motor_expo < 0 || params->turtle_motor_expo > 100) {
-		PX4_ERR("VOXL_ESC: Invalid parameter VOXL_ESC_T_EXPO.  Please verify parameters.");
+		PX4_ERR("Invalid parameter VOXL_ESC_T_EXPO.  Please verify parameters.");
 		params->turtle_motor_expo = 0;
 		ret = PX4_ERROR;
 	}

 	if (params->turtle_stick_minf < 0.0f || params->turtle_stick_minf > 100.0f) {
-		PX4_ERR("VOXL_ESC: Invalid parameter VOXL_ESC_T_MINF.  Please verify parameters.");
+		PX4_ERR("Invalid parameter VOXL_ESC_T_MINF.  Please verify parameters.");
 		params->turtle_stick_minf = 0.0f;
 		ret = PX4_ERROR;
 	}

 	if (params->turtle_cosphi < 0.0f || params->turtle_cosphi > 100.0f) {
-		PX4_ERR("VOXL_ESC: Invalid parameter VOXL_ESC_T_COSP.  Please verify parameters.");
+		PX4_ERR("Invalid parameter VOXL_ESC_T_COSP.  Please verify parameters.");
 		params->turtle_cosphi = 0.0f;
 		ret = PX4_ERROR;
 	}

+	if (params->gpio_ctl_channel < 0 || params->gpio_ctl_channel > 6) {
+		PX4_ERR("Invalid parameter VOXL_ESC_GPIO_CH.  Please verify parameters.");
+		params->gpio_ctl_channel = 0;
+		ret = PX4_ERROR;
+	}
+
 	for (int i = 0; i < VOXL_ESC_OUTPUT_CHANNELS; i++) {
 		if (params->function_map[i] < (int)OutputFunction::Motor1 || params->function_map[i] > (int)OutputFunction::Motor4) {
-			PX4_ERR("VOXL_ESC: Invalid parameter VOXL_ESC_FUNCX.  Only supports motors 1-4.  Please verify parameters.");
+			PX4_ERR("Invalid parameter VOXL_ESC_FUNCX.  Only supports motors 1-4.  Please verify parameters.");
 			params->function_map[i] = 0;
 			ret = PX4_ERROR;

@ -365,7 +373,7 @@ int VoxlEsc::load_params(voxl_esc_params_t *params, ch_assign_t *map)
 		if (params->motor_map[i] == VOXL_ESC_OUTPUT_DISABLED ||
 		    params->motor_map[i] < -(VOXL_ESC_OUTPUT_CHANNELS) ||
 		    params->motor_map[i] > VOXL_ESC_OUTPUT_CHANNELS) {
-			PX4_ERR("VOXL_ESC: Invalid parameter VOXL_ESC_MOTORX.  Please verify parameters.");
+			PX4_ERR("Invalid parameter VOXL_ESC_MOTORX.  Please verify parameters.");
 			params->motor_map[i] = 0;
 			ret = PX4_ERROR;
 		}
@ -471,9 +479,8 @@ int VoxlEsc::parse_response(uint8_t *buf, uint8_t len, bool print_feedback)
 						uint32_t voltage     = fb.voltage;
 						int32_t  current     = fb.current * 8;
 						int32_t  temperature = fb.temperature / 100;
-						PX4_INFO("VOXL_ESC: [%" PRId64 "] ID_RAW=%d ID=%d, RPM=%5d, PWR=%3d%%, V=%5dmV, I=%+5dmA, T=%+3dC", tnow, (int)id,
-							 motor_idx + 1,
-							 (int)rpm, (int)power, (int)voltage, (int)current, (int)temperature);
+						PX4_ERR("[%" PRId64 "] ID_RAW=%d ID=%d, RPM=%5d, PWR=%3d%%, V=%5dmV, I=%+5dmA, T=%+3dC", tnow, (int)id,
+							motor_idx + 1, (int)rpm, (int)power, (int)voltage, (int)current, (int)temperature);
 					}

 					_esc_chans[id].rate_meas     = fb.rpm;
@ -541,24 +548,24 @@ int VoxlEsc::parse_response(uint8_t *buf, uint8_t len, bool print_feedback)
 				QC_ESC_VERSION_INFO ver;
 				memcpy(&ver, _fb_packet.buffer, packet_size);

-				PX4_INFO("VOXL_ESC: ESC ID: %i", ver.id);
-				PX4_INFO("VOXL_ESC: HW Version: %i", ver.hw_version);
-				PX4_INFO("VOXL_ESC: SW Version: %i", ver.sw_version);
-				PX4_INFO("VOXL_ESC: Unique ID: %i", (int)ver.unique_id);
+				PX4_ERR("ESC ID: %i", ver.id);
+				PX4_ERR("HW Version: %i", ver.hw_version);
+				PX4_ERR("SW Version: %i", ver.sw_version);
+				PX4_ERR("Unique ID: %i", (int)ver.unique_id);

 			} else if (packet_type == ESC_PACKET_TYPE_VERSION_EXT_RESPONSE && packet_size == sizeof(QC_ESC_EXTENDED_VERSION_INFO)) {
 				QC_ESC_EXTENDED_VERSION_INFO ver;
 				memcpy(&ver, _fb_packet.buffer, packet_size);
-				PX4_INFO("VOXL_ESC: \tESC ID     : %i", ver.id);
-				PX4_INFO("VOXL_ESC: \tBoard      : %i", ver.hw_version);
-				PX4_INFO("VOXL_ESC: \tSW Version : %i", ver.sw_version);
+				PX4_ERR("\tESC ID     : %i", ver.id);
+				PX4_ERR("\tBoard      : %i", ver.hw_version);
+				PX4_ERR("\tSW Version : %i", ver.sw_version);

 				uint8_t *u = &ver.unique_id[0];
-				PX4_INFO("VOXL_ESC: \tUnique ID  : 0x%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X",
-					 u[11], u[10], u[9], u[8], u[7], u[6], u[5], u[4], u[3], u[2], u[1], u[0]);
+				PX4_ERR("\tUnique ID  : 0x%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X",
+					u[11], u[10], u[9], u[8], u[7], u[6], u[5], u[4], u[3], u[2], u[1], u[0]);

-				PX4_INFO("VOXL_ESC: \tFirmware   : version %4d, hash %.12s", ver.sw_version, ver.firmware_git_version);
-				PX4_INFO("VOXL_ESC: \tBootloader : version %4d, hash %.12s", ver.bootloader_version, ver.bootloader_git_version);
+				PX4_ERR("\tFirmware   : version %4d, hash %.12s", ver.sw_version, ver.firmware_git_version);
+				PX4_ERR("\tBootloader : version %4d, hash %.12s", ver.bootloader_version, ver.bootloader_git_version);

 			} else if (packet_type == ESC_PACKET_TYPE_FB_POWER_STATUS && packet_size == sizeof(QC_ESC_FB_POWER_STATUS)) {
 				QC_ESC_FB_POWER_STATUS packet;
@ -671,7 +678,7 @@ int VoxlEsc::custom_command(int argc, char *argv[])
 	}

 	if (!is_running()) {
-		PX4_INFO("VOXL_ESC:Not running");
+		PX4_INFO("Not running");
 		return -1;

 	}
@ -730,7 +737,7 @@ int VoxlEsc::custom_command(int argc, char *argv[])

 	if (!strcmp(verb, "reset")) {
 		if (esc_id < VOXL_ESC_OUTPUT_CHANNELS) {
-			PX4_INFO("VOXL_ESC: Reset ESC: %i", esc_id);
+			PX4_ERR("Reset ESC: %i", esc_id);
 			cmd.len = qc_esc_create_reset_packet(esc_id, cmd.buf, sizeof(cmd.buf));
 			cmd.response = false;
 			return get_instance()->send_cmd_thread_safe(&cmd);
@ -742,7 +749,7 @@ int VoxlEsc::custom_command(int argc, char *argv[])

 	} else if (!strcmp(verb, "version")) {
 		if (esc_id < VOXL_ESC_OUTPUT_CHANNELS) {
-			PX4_INFO("VOXL_ESC: Request version for ESC: %i", esc_id);
+			PX4_ERR("Request version for ESC: %i", esc_id);
 			cmd.len = qc_esc_create_version_request_packet(esc_id, cmd.buf, sizeof(cmd.buf));
 			cmd.response = true;
 			cmd.resp_delay_us = 2000;
@ -755,7 +762,7 @@ int VoxlEsc::custom_command(int argc, char *argv[])

 	} else if (!strcmp(verb, "version-ext")) {
 		if (esc_id < VOXL_ESC_OUTPUT_CHANNELS) {
-			PX4_INFO("VOXL_ESC: Request extended version for ESC: %i", esc_id);
+			PX4_ERR("Request extended version for ESC: %i", esc_id);
 			cmd.len = qc_esc_create_extended_version_request_packet(esc_id, cmd.buf, sizeof(cmd.buf));
 			cmd.response = true;
 			cmd.resp_delay_us = 5000;
@ -768,7 +775,7 @@ int VoxlEsc::custom_command(int argc, char *argv[])

 	} else if (!strcmp(verb, "tone")) {
 		if (esc_id < VOXL_ESC_OUTPUT_CHANNELS) {
-			PX4_INFO("VOXL_ESC: Request tone for ESC mask: %i", esc_id);
+			PX4_ERR("Request tone for ESC mask: %i", esc_id);
 			cmd.len = qc_esc_create_sound_packet(period, duration, power, esc_id, cmd.buf, sizeof(cmd.buf));
 			cmd.response = false;
 			return get_instance()->send_cmd_thread_safe(&cmd);
@ -782,7 +789,7 @@ int VoxlEsc::custom_command(int argc, char *argv[])
 		if (led_mask <= 0x0FFF) {
 			get_instance()->_led_rsc.test = true;
 			get_instance()->_led_rsc.breath_en = false;
-			PX4_INFO("VOXL_ESC: Request LED control for ESCs with mask: %i", led_mask);
+			PX4_ERR("Request LED control for ESCs with mask: %i", led_mask);

 			get_instance()->_esc_chans[0].led = (led_mask & 0x0007);
 			get_instance()->_esc_chans[1].led = (led_mask & 0x0038) >> 3;
@ -797,7 +804,7 @@ int VoxlEsc::custom_command(int argc, char *argv[])

 	}  else if (!strcmp(verb, "rpm")) {
 		if (esc_id < VOXL_ESC_OUTPUT_CHANNELS) {
-			PX4_INFO("VOXL_ESC: Request RPM for ESC ID: %i - RPM: %i", esc_id, rate);
+			PX4_ERR("Request RPM for ESC ID: %i - RPM: %i", esc_id, rate);
 			int16_t rate_req[VOXL_ESC_OUTPUT_CHANNELS] = {0, 0, 0, 0};
 			uint8_t id_fb = 0;

@ -831,8 +838,8 @@ int VoxlEsc::custom_command(int argc, char *argv[])
 			cmd.repeat_delay_us = repeat_delay_us;
 			cmd.print_feedback  = true;

-			PX4_INFO("VOXL_ESC: Feedback id debug: %i", id_fb);
-			PX4_INFO("VOXL_ESC: Sending UART ESC RPM command %i", rate);
+			PX4_ERR("Feedback id debug: %i", id_fb);
+			PX4_ERR("Sending UART ESC RPM command %i", rate);

 			return get_instance()->send_cmd_thread_safe(&cmd);

@ -843,7 +850,7 @@ int VoxlEsc::custom_command(int argc, char *argv[])

 	} else if (!strcmp(verb, "pwm")) {
 		if (esc_id < VOXL_ESC_OUTPUT_CHANNELS) {
-			PX4_INFO("VOXL_ESC: Request PWM for ESC ID: %i - PWM: %i", esc_id, rate);
+			PX4_ERR("Request PWM for ESC ID: %i - PWM: %i", esc_id, rate);
 			int16_t rate_req[VOXL_ESC_OUTPUT_CHANNELS] = {0, 0, 0, 0};
 			uint8_t id_fb = 0;

@ -876,8 +883,8 @@ int VoxlEsc::custom_command(int argc, char *argv[])
 			cmd.repeat_delay_us = repeat_delay_us;
 			cmd.print_feedback  = true;

-			PX4_INFO("VOXL_ESC: Feedback id debug: %i", id_fb);
-			PX4_INFO("VOXL_ESC: Sending UART ESC power command %i", rate);
+			PX4_ERR("Feedback id debug: %i", id_fb);
+			PX4_ERR("Sending UART ESC power command %i", rate);

 			return get_instance()->send_cmd_thread_safe(&cmd);

@ -1242,14 +1249,58 @@ bool VoxlEsc::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 					       _extended_rpm);

 	if (_uart_port.write(cmd.buf, cmd.len) != cmd.len) {
-		PX4_ERR("VOXL_ESC: Failed to send packet");
+		PX4_ERR("Failed to send packet");
 		return false;
 	}

+	// Track and manage gpio command writes
+	bool write_gpio_command = false;
+
+	if (_gpio_ctl_en) {
+		if (_gpio_ctl_high != _prev_gpio_ctl_high) {
+			_gpio_write_counter = 0;
+		}
+
+		if (_gpio_write_counter < 10) {
+			write_gpio_command = true;
+			_gpio_write_counter++;
+		}
+
+		_prev_gpio_ctl_high = _gpio_ctl_high;
+
+		if (write_gpio_command) {
+			Command gpio_cmd;
+			const int ESC_PACKET_TYPE_GPIO_CMD = 15;
+			uint8_t data[5];
+
+			int esc_id = 0; // In future un-hardcode
+			int val = 0;
+
+			if (_gpio_ctl_high) {
+				val = 1;
+			}
+
+			data[0] = esc_id; // esc id
+			data[1] = 80; // 01010000 : pin F0
+			data[2] = 0; // 0: output, 1: input
+			data[3] = val; //cmd LSB
+			data[4] = 0; // cmd MSB
+
+			// type, data, size
+			gpio_cmd.len = qc_esc_create_packet(ESC_PACKET_TYPE_GPIO_CMD, (uint8_t *) & (data[0]), 5, gpio_cmd.buf,
+							    sizeof(gpio_cmd.buf));
+
+			if (_uart_port.write(gpio_cmd.buf, gpio_cmd.len) != gpio_cmd.len) {
+				PX4_ERR("Failed to send gpio packet");
+				return false;
+			}
+		}
+
+	}
+
 	// increment ESC id from which to request feedback in round robin order
 	_fb_idx = (_fb_idx + 1) % VOXL_ESC_OUTPUT_CHANNELS;

-
 	/*
 	 * Here we read and parse response from ESCs. Since the latest command has just been sent out,
 	 * the response packet we may read here is probabaly from previous iteration, but it is totally ok.
@ -1283,22 +1334,6 @@ bool VoxlEsc::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],

 	_esc_status_pub.publish(_esc_status);

-	// If any extra external modal io data has been received then
-	// send it over as well
-	while (_voxl2_io_data_sub.updated()) {
-		buffer128_s io_data{};
-		_voxl2_io_data_sub.copy(&io_data);
-
-		// PX4_INFO("Got Modal IO data: %u bytes", io_data.len);
-		// PX4_INFO("   0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x",
-		// 		 io_data.data[0], io_data.data[1], io_data.data[2], io_data.data[3],
-		// 		 io_data.data[4], io_data.data[5], io_data.data[6], io_data.data[7]);
-		if (_uart_port.write(io_data.data, io_data.len) != io_data.len) {
-			PX4_ERR("VOXL_ESC: Failed to send modal io data to esc");
-			return false;
-		}
-	}
-
 	perf_count(_output_update_perf);

 	return true;
@ -1308,7 +1343,7 @@ bool VoxlEsc::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 void VoxlEsc::Run()
 {
 	if (should_exit()) {
-		PX4_ERR("VOXL_ESC: Stopping the module");
+		PX4_ERR("Stopping the module");
 		ScheduleClear();
 		_mixing_output.unregister();

@ -1328,12 +1363,12 @@ void VoxlEsc::Run()
 		int dev_init_ret = device_init();

 		if (dev_init_ret != 0) {
-			PX4_WARN("VOXL_ESC: Failed to initialize device, retries left %d", retries_left);
+			PX4_WARN("Failed to initialize device, retries left %d", retries_left);
 		}
 	}

 	if (!_device_initialized) {
-		PX4_ERR("VOXL_ESC: Failed to initialize device, exiting the module");
+		PX4_ERR("Failed to initialize device, exiting the module");
 		ScheduleClear();
 		_mixing_output.unregister();
 		exit_and_cleanup();
@ -1374,11 +1409,12 @@ void VoxlEsc::Run()
 		update_leds(_led_rsc.mode, _led_rsc.control);
 	}

-	if (_parameters.mode > 0) {
-		/* if turtle mode enabled, we go straight to the sticks, no mix */
-		if (_manual_control_setpoint_sub.updated()) {
+	/* check whether sticks have been updated */
+	if (_manual_control_setpoint_sub.updated()) {
+		_manual_control_setpoint_sub.copy(&_manual_control_setpoint);

-			_manual_control_setpoint_sub.copy(&_manual_control_setpoint);
+		// if turtle mode enabled, we go straight to the sticks, no mix
+		if (_parameters.mode > 0) {

 			if (!_outputs_on) {

@ -1400,6 +1436,45 @@ void VoxlEsc::Run()
 			}
 		}

+		// check if gpio control is enabled
+		if (_parameters.gpio_ctl_channel > 0) {
+
+			_gpio_ctl_en = true;
+			float gpio_setpoint = VOXL_ESC_GPIO_CTL_DISABLED_SETPOINT;
+
+			switch (_parameters.gpio_ctl_channel) {
+			case VOXL_ESC_GPIO_CTL_AUX1:
+				gpio_setpoint = _manual_control_setpoint.aux1;
+				break;
+
+			case VOXL_ESC_GPIO_CTL_AUX2:
+				gpio_setpoint = _manual_control_setpoint.aux2;
+				break;
+
+			case VOXL_ESC_GPIO_CTL_AUX3:
+				gpio_setpoint = _manual_control_setpoint.aux3;
+				break;
+
+			case VOXL_ESC_GPIO_CTL_AUX4:
+				gpio_setpoint = _manual_control_setpoint.aux4;
+				break;
+
+			case VOXL_ESC_GPIO_CTL_AUX5:
+				gpio_setpoint = _manual_control_setpoint.aux5;
+				break;
+
+			case VOXL_ESC_GPIO_CTL_AUX6:
+				gpio_setpoint = _manual_control_setpoint.aux6;
+				break;
+			}
+
+			if (gpio_setpoint > VOXL_ESC_GPIO_CTL_THRESHOLD) {
+				_gpio_ctl_high = false;
+
+			} else {
+				_gpio_ctl_high = true;
+			}
+		}
 	}

 	if (!_outputs_on) {
@ -1431,19 +1506,19 @@ void VoxlEsc::Run()
 				} else {
 					if (_current_cmd.retries == 0) {
 						_current_cmd.clear();
-						PX4_ERR("VOXL_ESC: Failed to send command, errno: %i", errno);
+						PX4_ERR("Failed to send command, errno: %i", errno);

 					} else {
 						_current_cmd.retries--;
-						PX4_ERR("VOXL_ESC: Failed to send command, errno: %i", errno);
+						PX4_ERR("Failed to send command, errno: %i", errno);
 					}
 				}

 				px4_usleep(_current_cmd.repeat_delay_us);
 			} while (_current_cmd.repeats-- > 0);

-			PX4_INFO("VOXL_ESC: RX packet count: %d", (int)_rx_packet_count);
-			PX4_INFO("VOXL_ESC: CRC error count: %d", (int)_rx_crc_error_count);
+			PX4_ERR("RX packet count: %d", (int)_rx_packet_count);
+			PX4_ERR("CRC error count: %d", (int)_rx_crc_error_count);

 		} else {
 			Command *new_cmd = _pending_cmd.load();
@ -1550,6 +1625,8 @@ void VoxlEsc::print_params()
 	
 	PX4_INFO("Params: VOXL_ESC_T_WARN: %" PRId32, _parameters.esc_warn_temp_threshold);
 	PX4_INFO("Params: VOXL_ESC_T_OVER: %" PRId32, _parameters.esc_over_temp_threshold);
+
+	PX4_INFO("Params: VOXL_ESC_GPIO_CH: %" PRId32, _parameters.gpio_ctl_channel);
 }

 int VoxlEsc::print_status()
--- a/src/drivers/actuators/voxl_esc/voxl_esc.hpp
+++ b/src/drivers/actuators/voxl_esc/voxl_esc.hpp
@ -49,7 +49,6 @@
 #include <uORB/topics/led_control.h>
 #include <uORB/topics/esc_status.h>
 #include <uORB/topics/actuator_test.h>
-#include <uORB/topics/buffer128.h>

 #include <px4_platform_common/Serial.hpp>

@ -129,17 +128,24 @@ private:
 	static constexpr uint32_t VOXL_ESC_MODE_TURTLE_AUX1 = 1;
 	static constexpr uint32_t VOXL_ESC_MODE_TURTLE_AUX2 = 2;

-	static constexpr uint16_t VOXL_ESC_EXT_RPM =
-		39;                // minimum firmware version for extended RPM command support
-	static constexpr uint16_t VOXL_ESC_RPM_MAX = INT16_MAX -
-			1;		// 32K, Limit max standard range RPM to prevent overflow (rpm packet packing function accepts int32_t)
-	static constexpr uint16_t VOXL_ESC_RPM_MAX_EXT = UINT16_MAX -
-			5;	// 65K, Limit max extended range RPM to prevent overflow (rpm packet packing function accepts int32_t)
+	// minimum firmware version for extended RPM command support
+	static constexpr uint16_t VOXL_ESC_EXT_RPM = 39;
+	// 32K, Limit max standard range RPM to prevent overflow (rpm packet packing function accepts int32_t)
+	static constexpr uint16_t VOXL_ESC_RPM_MAX = INT16_MAX - 1;
+	// 65K, Limit max extended range RPM to prevent overflow (rpm packet packing function accepts int32_t)
+	static constexpr uint16_t VOXL_ESC_RPM_MAX_EXT = UINT16_MAX - 5;

 	static constexpr uint16_t VOXL_ESC_NUM_INIT_RETRIES = 3;

-	//static constexpr uint16_t max_pwm(uint16_t pwm) { return math::min(pwm, VOXL_ESC_PWM_MAX); }
-	//static constexpr uint16_t max_rpm(uint16_t rpm) { return math::min(rpm, VOXL_ESC_RPM_MAX); }
+	static constexpr float    VOXL_ESC_GPIO_CTL_DISABLED_SETPOINT = -0.1f;
+	static constexpr float    VOXL_ESC_GPIO_CTL_THRESHOLD = 0.0f;
+
+	static constexpr uint32_t VOXL_ESC_GPIO_CTL_AUX1 = 1;
+	static constexpr uint32_t VOXL_ESC_GPIO_CTL_AUX2 = 2;
+	static constexpr uint32_t VOXL_ESC_GPIO_CTL_AUX3 = 3;
+	static constexpr uint32_t VOXL_ESC_GPIO_CTL_AUX4 = 4;
+	static constexpr uint32_t VOXL_ESC_GPIO_CTL_AUX5 = 5;
+	static constexpr uint32_t VOXL_ESC_GPIO_CTL_AUX6 = 6;

 	Serial			_uart_port{};

@ -161,6 +167,7 @@ private:
 		int32_t		publish_battery_status{0};
 		int32_t		esc_warn_temp_threshold{0};
 		int32_t		esc_over_temp_threshold{0};
+		int32_t		gpio_ctl_channel{0};
 	} voxl_esc_params_t;

 	struct EscChan {
@ -205,7 +212,6 @@ private:
 	uORB::Subscription	_parameter_update_sub{ORB_ID(parameter_update)};
 	uORB::Subscription	_actuator_test_sub{ORB_ID(actuator_test)};
 	uORB::Subscription	_led_update_sub{ORB_ID(led_control)};
-	uORB::Subscription	_voxl2_io_data_sub{ORB_ID(voxl2_io_data)};

 	uORB::Publication<actuator_outputs_s> _outputs_debug_pub{ORB_ID(actuator_outputs_debug)};
 	uORB::Publication<esc_status_s> _esc_status_pub{ORB_ID(esc_status)};
@ -224,6 +230,11 @@ private:
 	int32_t			_rpm_fullscale{0};
 	manual_control_setpoint_s _manual_control_setpoint{};

+	int32_t			_gpio_write_counter{0};
+	bool			_gpio_ctl_en{false};
+	bool			_gpio_ctl_high{true};
+	bool			_prev_gpio_ctl_high{true};
+
 	uint16_t		_cmd_id{0};
 	Command			_current_cmd;
 	px4::atomic<Command *>	_pending_cmd{nullptr};
--- a/src/drivers/actuators/voxl_esc/voxl_esc_params.c
+++ b/src/drivers/actuators/voxl_esc/voxl_esc_params.c
@ -262,3 +262,17 @@ PARAM_DEFINE_INT32(VOXL_ESC_T_WARN, 0);
 * @max 200
 */
 PARAM_DEFINE_INT32(VOXL_ESC_T_OVER, 0);
+
+
+/**
+ * GPIO Control Channel
+ *
+ *
+ * @reboot_required true
+ *
+ * @group VOXL ESC
+ * @value 0 - Disabled
+ * @min 0
+ * @max 6
+ */
+PARAM_DEFINE_INT32(VOXL_ESC_GPIO_CH, 0);
--- a/src/drivers/imu/invensense/mpu6000/InvenSense_MPU6000_registers.hpp
+++ b/src/drivers/imu/invensense/mpu6000/InvenSense_MPU6000_registers.hpp
@ -41,6 +41,7 @@
 #pragma once

 #include <cstdint>
+#include <cstdlib>

 // TODO: move to a central header
 static constexpr uint8_t Bit0 = (1 << 0);
--- a/src/drivers/rpm_capture/rpm_capture_params.c
+++ b/src/drivers/rpm_capture/rpm_capture_params.c
@ -32,9 +32,9 @@
 ****************************************************************************/

 /**
- * RPM Capture Enable
+ * RPM capture enable
 *
- * Enables the RPM capture module on FMU channel 5.
+ * Enables the RPM capture module to estimate RPM from pulses detected on a PWM pin configured as "RPM Input".
 *
 * @boolean
 * @group System
--- a/src/lib/drivers/device/qurt/I2C.cpp
+++ b/src/lib/drivers/device/qurt/I2C.cpp
@ -55,7 +55,16 @@ I2C::_config_i2c_bus_func_t  I2C::_config_i2c_bus  = NULL;
 I2C::_set_i2c_address_func_t I2C::_set_i2c_address = NULL;
 I2C::_i2c_transfer_func_t    I2C::_i2c_transfer    = NULL;

-pthread_mutex_t I2C::_mutex = PTHREAD_MUTEX_INITIALIZER;
+// There is a mutex per I2C bus. A mutex isn't required with normal
+// use since per bus I2C accesses are made via work item tied to a per bus thread.
+// But it is here anyways in case someone decides to use the bus in some different
+// custom code that has a separate thread.
+struct I2C::_bus_mutex_t I2C::_bus_mutex[I2C::MAX_I2C_BUS] = {
+	{1, PTHREAD_MUTEX_INITIALIZER},
+	{2, PTHREAD_MUTEX_INITIALIZER},
+	{4, PTHREAD_MUTEX_INITIALIZER},
+	{5, PTHREAD_MUTEX_INITIALIZER}
+};

 I2C::I2C(uint8_t device_type, const char *name, const int bus, const uint16_t address, const uint32_t frequency) :
 	CDev(name, nullptr),
@ -91,10 +100,27 @@ I2C::init()
 		goto out;
 	}

-	pthread_mutex_lock(&_mutex);
+	if (_mutex == nullptr) {
+		for (int i = 0; i < MAX_I2C_BUS; i++) {
+			if (get_device_bus() == _bus_mutex[i]._bus) {
+				_mutex = &_bus_mutex[i]._mutex;
+				break;
+			}
+		}
+
+		if (_mutex == nullptr) {
+			PX4_ERR("NULL i2c bus mutex");
+			goto out;
+
+		} else {
+			PX4_INFO("Set up I2C bus mutex for bus %d", get_device_bus());
+		}
+	}
+
+	pthread_mutex_lock(_mutex);
 	// Open the actual I2C device
 	_i2c_fd = _config_i2c_bus(get_device_bus(), get_device_address(), _frequency);
-	pthread_mutex_unlock(&_mutex);
+	pthread_mutex_unlock(_mutex);

 	if (_i2c_fd == PX4_ERROR) {
 		PX4_ERR("i2c init failed");
@ -131,9 +157,9 @@ I2C::set_device_address(int address)
 	if ((_i2c_fd != PX4_ERROR) && (_set_i2c_address != NULL)) {
 		PX4_INFO("Set i2c address 0x%x, fd %d", address, _i2c_fd);

-		pthread_mutex_lock(&_mutex);
+		pthread_mutex_lock(_mutex);
 		_set_i2c_address(_i2c_fd, address);
-		pthread_mutex_unlock(&_mutex);
+		pthread_mutex_unlock(_mutex);

 		Device::set_device_address(address);
 	}
@ -150,9 +176,9 @@ I2C::transfer(const uint8_t *send, const unsigned send_len, uint8_t *recv, const
 		do {
 			// PX4_INFO("transfer out %p/%u  in %p/%u", send, send_len, recv, recv_len);

-			pthread_mutex_lock(&_mutex);
+			pthread_mutex_lock(_mutex);
 			ret = _i2c_transfer(_i2c_fd, send, send_len, recv, recv_len);
-			pthread_mutex_unlock(&_mutex);
+			pthread_mutex_unlock(_mutex);

 			if (ret != PX4_ERROR) { break; }

--- a/src/lib/drivers/device/qurt/I2C.hpp
+++ b/src/lib/drivers/device/qurt/I2C.hpp
@ -124,10 +124,18 @@ protected:
 private:
 	uint32_t		               _frequency{0};
 	int                            _i2c_fd{-1};
+	pthread_mutex_t               *_mutex{nullptr};
+
+	static const int MAX_I2C_BUS{4};
+
 	static _config_i2c_bus_func_t  _config_i2c_bus;
 	static _set_i2c_address_func_t _set_i2c_address;
 	static _i2c_transfer_func_t    _i2c_transfer;
-	static pthread_mutex_t         _mutex;
+
+	static struct _bus_mutex_t {
+		int _bus;
+		pthread_mutex_t _mutex{PTHREAD_MUTEX_INITIALIZER};
+	} _bus_mutex[MAX_I2C_BUS];
 };

 } // namespace device
--- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp
+++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp
@ -65,6 +65,7 @@ ActuatorEffectivenessHelicopter::ActuatorEffectivenessHelicopter(ModuleParams *p
 	_param_handles.yaw_throttle_scale = param_find("CA_HELI_YAW_TH_S");
 	_param_handles.yaw_ccw = param_find("CA_HELI_YAW_CCW");
 	_param_handles.spoolup_time = param_find("COM_SPOOLUP_TIME");
+	_param_handles.max_servo_throw = param_find("CA_MAX_SVO_THROW");

 	updateParams();
 }
@ -101,6 +102,21 @@ void ActuatorEffectivenessHelicopter::updateParams()
 	int32_t yaw_ccw = 0;
 	param_get(_param_handles.yaw_ccw, &yaw_ccw);
 	_geometry.yaw_sign = (yaw_ccw == 1) ? -1.f : 1.f;
+	float max_servo_throw_deg = 0.f;
+	param_get(_param_handles.max_servo_throw, &max_servo_throw_deg);
+
+	if (max_servo_throw_deg > 0.f) {
+		// linearization feature enabled
+		_geometry.linearize_servos = 1;
+		const float max_servo_throw = math::radians(max_servo_throw_deg);
+		_geometry.max_servo_height = sinf(max_servo_throw);
+		_geometry.inverse_max_servo_throw = 1.f / max_servo_throw;
+
+	} else {
+		// handle any undefined behaviour if disabled
+		_geometry.linearize_servos = 0;
+		_geometry.max_servo_height = _geometry.inverse_max_servo_throw = 0.f;
+	}
 }

 bool ActuatorEffectivenessHelicopter::getEffectivenessMatrix(Configuration &configuration,
@ -168,6 +184,11 @@ void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector<float,
 				- control_sp(ControlAxis::ROLL) * roll_coeff
 				+ _geometry.swash_plate_servos[i].trim;

+		// Apply linearization to the actuator setpoint if enabled
+		if (_geometry.linearize_servos) {
+			actuator_sp(_first_swash_plate_servo_index + i) = getLinearServoOutput(actuator_sp(_first_swash_plate_servo_index + i));
+		}
+
 		// Saturation check for roll & pitch
 		if (actuator_sp(_first_swash_plate_servo_index + i) < actuator_min(_first_swash_plate_servo_index + i)) {
 			setSaturationFlag(roll_coeff, _saturation_flags.roll_pos, _saturation_flags.roll_neg);
@ -180,6 +201,17 @@ void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector<float,
 	}
 }

+float ActuatorEffectivenessHelicopter::getLinearServoOutput(float input) const
+{
+	input = math::constrain(input, -1.f, 1.f);
+
+	// make sure a the maximal input of [-1,1] maps to the maximal vertical deflection the servo can reach of sin(CA_MAX_SVO_THROW)
+	float servo_height = _geometry.max_servo_height * input;
+
+	// mulitply by 1 over max arm roation in radians to normalise
+	return _geometry.inverse_max_servo_throw * asinf(servo_height);
+}
+
 bool ActuatorEffectivenessHelicopter::mainMotorEnaged()
 {
 	manual_control_switches_s manual_control_switches;
--- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp
+++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp
@ -66,6 +66,9 @@ public:
 		float yaw_throttle_scale;
 		float yaw_sign;
 		float spoolup_time;
+		int linearize_servos;
+		float max_servo_height;
+		float inverse_max_servo_throw;
 	};

 	ActuatorEffectivenessHelicopter(ModuleParams *parent, ActuatorType tail_actuator_type);
@ -86,6 +89,7 @@ public:
 private:
 	float throttleSpoolupProgress();
 	bool mainMotorEnaged();
+	float getLinearServoOutput(float input) const;

 	void updateParams() override;

@ -116,6 +120,7 @@ private:
 		param_t yaw_throttle_scale;
 		param_t yaw_ccw;
 		param_t spoolup_time;
+		param_t max_servo_throw;
 	};
 	ParamHandles _param_handles{};

--- a/src/modules/control_allocator/module.yaml
+++ b/src/modules/control_allocator/module.yaml
@ -564,6 +564,20 @@ parameters:
          min: 0
          max: 10
          default: 0.0
+        CA_MAX_SVO_THROW:
+          description:
+            short: Throw angle of swashplate servo at maximum commands for linearization
+            long: |
+              Used to linearize mechanical output of swashplate servos to avoid axis coupling and binding with 4 servo redundancy.
+              This requires a symmetric setup where the servo horn is exactly centered with a 0 command.
+              Setting to zero disables feature.
+          type: float
+          decimal: 1
+          unit: deg
+          increment: 0.1
+          min: 0
+          max: 75
+          default: 0.0

        # Others
        CA_FAILURE_MODE:
--- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp
+++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp
@ -1108,7 +1108,15 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons

 		} else {
 			// continue straight until vehicle has sufficient altitude
-			lateral_limits.lateral_accel_max = 0.0f;
+			lateral_limits.lateral_accel_max = 0.f;
+      
+      // keep flaps in landing configuration if the airspeed is below the min airspeed (keep deployed if airspeed not valid)
+			if (!_airspeed_valid || _airspeed_eas < _performance_model.getMinimumCalibratedAirspeed()) {
+				_flaps_setpoint =  _param_fw_flaps_lnd_scl.get();
+
+			} else {
+				_flaps_setpoint = 0.f;
+			}
 		}

 		enforce_low_height = true;
--- a/src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.cpp
+++ b/src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.cpp
@ -355,31 +355,52 @@ int InternalCombustionEngineControl::print_usage(const char *reason)
 	PRINT_MODULE_DESCRIPTION(
 		R"DESCR_STR(
 ### Description
+		
 The module controls internal combustion engine (ICE) features including:
-ignition (on/off),throttle and choke level, starter engine delay, and user request.
-The module publishes [InternalCombustionEngineControl.msg](../msg_docs/InternalCombustionEngineControl.md).
-The architecture is as shown below.:
-![Architecture](../../assets/diagrams/ice_control_diagram.png)
+ignition (on/off), throttle and choke level, starter engine delay, and user request.

 ### Enabling
+
 This feature is not enabled by default needs to be configured in the
 build target for your board together with the rpm capture driver:
+
+```
 CONFIG_MODULES_INTERNAL_COMBUSTION_ENGINE_CONTROL=y
 CONFIG_DRIVERS_RPM_CAPTURE=y
+```

 Additionally, to enable the module:
- set [ICE_EN](https://docs.px4.io/main/en/advanced_config/parameter_reference.html#ICE_EN)
-to true and adjust the other module parameters ICE_ according to your needs.
- set [RPM_CAP_ENABLE](https://docs.px4.io/main/en/advanced_config/parameter_reference.html#RPM_CAP_ENABLE) to true.
+
+- Set [ICE_EN](../advanced_config/parameter_reference.md#ICE_EN)
+to true and adjust the other `ICE_` module parameters according to your needs.
+- Set [RPM_CAP_ENABLE](../advanced_config/parameter_reference.md#RPM_CAP_ENABLE) to true.
+
+The module outputs control signals for ignition, throttle, and choke,
+and takes inputs from an RPM sensor.
+These must be mapped to AUX outputs/inputs in the [Actuator configuration](../config/actuators.md),
+similar to the setup shown below.
+
+![Actuator setup for ICE](../../assets/hardware/ice/ice_actuator_setup.png)

 ### Implementation
+
 The ICE is implemented with a (4) state machine:
-![Architecture](../../assets/diagrams/ice_control_state_machine.png)
+
+![Architecture](../../assets/hardware/ice/ice_control_state_machine.png)
+
 The state machine:
- checks if [Rpm.msg](../msg_docs/Rpm.md) is updated to know if the engine is running
- allows for user inputs from
- AUX{N}
- Arming state in [VehicleStatus.msg(../msg_docs/VehicleStatus.md)
+		
+- Checks if [Rpm.msg](../msg_docs/Rpm.md) is updated to know if the engine is running
+- Allows for user inputs from:
+  - AUX{N}
+  - Arming state in [VehicleStatus.msg](../msg_docs/VehicleStatus.md)
+
+The module publishes [InternalCombustionEngineControl.msg](../msg_docs/InternalCombustionEngineControl.md).
+		
+The architecture is as shown below:
+
+![Architecture](../../assets/hardware/ice/ice_control_diagram.png)
+		
 <a id="internal_combustion_engine_control_usage"></a>
 )DESCR_STR");

--- a/src/modules/mavlink/streams/DISTANCE_SENSOR.hpp
+++ b/src/modules/mavlink/streams/DISTANCE_SENSOR.hpp
@ -94,6 +94,22 @@ private:
 				msg.min_distance     = dist_sensor.min_distance * 1e2f;     // m to cm
 				msg.orientation      = dist_sensor.orientation;
 				msg.covariance       = dist_sensor.variance * 1e4f;         // m^2 to cm^2
+				msg.horizontal_fov   = dist_sensor.h_fov;
+				msg.vertical_fov     = dist_sensor.v_fov;
+				msg.quaternion[0]    = dist_sensor.q[0];
+				msg.quaternion[1]    = dist_sensor.q[1];
+				msg.quaternion[2]    = dist_sensor.q[2];
+				msg.quaternion[3]    = dist_sensor.q[3];
+
+				if (dist_sensor.signal_quality < 0) {
+					msg.signal_quality = 0;
+
+				} else if (dist_sensor.signal_quality == 0) {
+					msg.signal_quality = 1;
+
+				} else {
+					msg.signal_quality = dist_sensor.signal_quality;
+				}

 				mavlink_msg_distance_sensor_send_struct(_mavlink->get_channel(), &msg);

--- a/src/systemcmds/mft_cfg/CMakeLists.txt
+++ b/src/systemcmds/mft_cfg/CMakeLists.txt
@ -0,0 +1,39 @@
+############################################################################
+#
+#   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+px4_add_module(
+	MODULE systemcmds__mft_cfg
+	MAIN mft_cfg
+	COMPILE_FLAGS
+	SRCS
+		mft_cfg.cpp
+	)
--- a/src/systemcmds/mft_cfg/Kconfig
+++ b/src/systemcmds/mft_cfg/Kconfig
@ -0,0 +1,5 @@
+menuconfig SYSTEMCMDS_MFT_CFG
+	bool "mft_cfg"
+	default n
+	---help---
+		Enable support for mft_cfg
--- a/src/systemcmds/mft_cfg/mft_cfg.cpp
+++ b/src/systemcmds/mft_cfg/mft_cfg.cpp
@ -0,0 +1,239 @@
+/****************************************************************************
+*
+* Copyright (c) 2025 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 mft_cfg.c
+*/
+
+#include <board_config.h>
+#include <px4_platform_common/board_common.h>
+#include <px4_platform/board_hw_eeprom_rev_ver.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/px4_manifest.h>
+#include <stdio.h>
+#include <stdbool.h>
+#include <string.h>
+#include <fcntl.h>
+
+
+static void usage(const char *reason)
+{
+	if (reason != nullptr) {
+		printf("%s\n\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION("Tool to set and get manifest configuration");
+
+	PRINT_MODULE_USAGE_NAME("mft_cfg", "command");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("get", "get manifest configuration");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("set", "set manifest configuration");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("reset", "reset manifest configuration");
+	PRINT_MODULE_USAGE_ARG("hwver|hwrev", "Select type: MTD_MTF_VER|MTD_MTF_REV", false);
+	PRINT_MODULE_USAGE_PARAM_INT('i', 0x10, 0x10, 0xFFFF,
+				     "argument to set extended hardware id (id == version for <hwver>, id == revision for <hwrev> )", false);
+
+}
+
+
+static int print_extended_id(const char *type)
+{
+	mtd_mft_v0_t mtd_mft;
+	mtd_mft.version.id = MTD_MFT_v0;
+	mtd_mft.hw_extended_id = -1;
+
+	const char *path = nullptr;
+	int ret_val = px4_mtd_query(type, NULL, &path);
+
+	if (ret_val != PX4_OK) {
+		PX4_ERR("Can't get mtd query (%s, %i)", type, ret_val);
+
+	} else {
+
+		ret_val = board_get_eeprom_hw_info(path, (mtd_mft_t *)&mtd_mft);
+
+		if (ret_val == PX4_OK) {
+			PX4_INFO("%s, hw_extended_id = %#x", type, mtd_mft.hw_extended_id);
+
+		} else  {
+
+			if (ret_val == -EPROTO) {
+				PX4_ERR("Manifest data may not exist for %s", path);
+
+			} else {
+				PX4_ERR("Can't read hw_extended_id from EEPROM (%s, %i)", type, ret_val);
+			}
+		}
+	}
+
+	return ret_val;
+}
+
+extern "C" __EXPORT int mft_cfg_main(int argc, char *argv[])
+{
+	if ((argc == 2) && (!strcmp(argv[1], "get"))) {
+
+		char type_ver[] = "MTD_MFT_VER";
+		char type_rev[] = "MTD_MFT_REV";
+
+		print_extended_id(type_ver);
+		print_extended_id(type_rev);
+
+		return 0;
+
+	} else if (argc >= 3) {
+
+		int ret_val = -1;
+		const char *path = nullptr;
+		bool arg_exist = false;
+
+		for (int i = 2; i < argc; ++i) {
+			if (strcmp("hwver", argv[i]) == 0) {
+				ret_val = px4_mtd_query("MTD_MFT_VER", NULL, &path);
+				arg_exist = true;
+				break;
+			}
+
+			if (strcmp("hwrev", argv[i]) == 0) {
+				ret_val = px4_mtd_query("MTD_MFT_REV", NULL, &path);
+				arg_exist = true;
+				break;
+			}
+		}
+
+		if (!arg_exist) {
+			PX4_ERR("Missing <hwver> or <hwrev> arguments'");
+			return 1;
+		}
+
+		if (ret_val != PX4_OK) {
+			PX4_ERR("Can't get mtd query (%i)", ret_val);
+			return 1;
+		}
+
+		mtd_mft_v0_t mtd_mft;
+		mtd_mft.version.id = MTD_MFT_v0;
+		mtd_mft.hw_extended_id = -1;
+
+		if (!strcmp(argv[1], "set")) {
+			if (argc == 5) {
+
+				const char *myoptarg = NULL;
+				int ch = 0;
+				int myoptind = 1;
+				int hw_extended_id = -1;
+
+				while ((ch = px4_getopt(argc, argv, "i:", &myoptind, &myoptarg)) != EOF) {
+					switch (ch) {
+					case 'i':
+						hw_extended_id = strtol(myoptarg, NULL, 0);
+						break;
+
+					default:
+						PX4_ERR("To set id use '-i x'");
+						break;
+					}
+				}
+
+				if (hw_extended_id != -1) {
+
+					mtd_mft.hw_extended_id = (uint16_t)hw_extended_id;
+
+					ret_val = board_set_eeprom_hw_info(path, (mtd_mft_t *)&mtd_mft);
+
+					if (ret_val != PX4_OK) {
+						PX4_ERR("Can't write to EEPROM (%i)", ret_val);
+
+					} else {
+						board_get_eeprom_hw_info(path, (mtd_mft_t *)&mtd_mft);
+						PX4_INFO("New hw_extended_id = %#x", mtd_mft.hw_extended_id);
+					}
+				}
+
+			} else {
+				PX4_ERR("Not enough arguments, try 'mft_cfg set hwver -i x'");
+				return 1;
+			}
+
+			return 0;
+		}
+
+		if (!strcmp(argv[1], "get")) {
+
+			ret_val = board_get_eeprom_hw_info(path, (mtd_mft_t *)&mtd_mft);
+
+			if (ret_val == PX4_OK) {
+				PX4_INFO("hw_extended_id = %#x", mtd_mft.hw_extended_id);
+
+			} else {
+
+				if (ret_val == -EPROTO) {
+					PX4_ERR("Manifest data may not exist for %s", path);
+
+				} else {
+					PX4_ERR("Can't read from EEPROM (%s, %i)", path, ret_val);
+				}
+
+				return 1;
+			}
+
+			return 0;
+		}
+
+		if (!strcmp(argv[1], "reset")) {
+
+			uint8_t buffer[64];
+			memset(buffer, 0xFF, sizeof(buffer));
+
+			int fd = open(path, O_WRONLY);
+
+			if (fd == -1) {
+				PX4_ERR("Failed to open partition %s", path);
+				return 1;
+			}
+
+			while (write(fd, buffer, sizeof(buffer)) == sizeof(buffer)) {}
+
+			PX4_INFO("Reset for %s completed. To remove manifest data from RAM, a reboot is required.", path);
+			close(fd);
+
+			return 0;
+		}
+
+	} else {
+		usage("Error, not enough parameters.");
+		return 1;
+	}
+
+	return 0;
+}