From ce79d462d75e95ea00c6b3a3cdf5bdb6976b1fe4 Mon Sep 17 00:00:00 2001
From: ryanjAA <rjohnston@appliedaeronautics.com>
Date: Wed, 3 Jul 2024 12:48:56 -0500
Subject: [PATCH] ARK v6x tracking #22527

---
 ROMFS/px4fmu_common/init.d/rcS | 40 ++++++++++++++++++----------------
 src/drivers/px4io/px4io.cpp    |  6 ++++-
 2 files changed, 26 insertions(+), 20 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 8a216d8c8a5b..937eeb91e4ae 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -266,37 +266,39 @@ else
 		. $FCONFIG
 	fi
 
-	# Check if PX4IO present and update firmware if needed.
-	if [ -f $IOFW ]
+	if px4io supported
 	then
-		if ! px4io checkcrc ${IOFW}
+		# Check if PX4IO present and update firmware if needed.
+ 		if [ -f $IOFW ]
 		then
-			# tune Program PX4IO
-			tune_control play -t 16 # tune 16 = PROG_PX4IO
-
-			if px4io update ${IOFW}
+			if ! px4io checkcrc ${IOFW}
 			then
-				usleep 10000
-				tune_control stop
-				if px4io checkcrc ${IOFW}
+				# tune Program PX4IO
+ 				tune_control play -t 16 # tune 16 = PROG_PX4IO
+
+ 				if px4io update ${IOFW}
 				then
-					tune_control play -t 17 # tune 17 = PROG_PX4IO_OK
+					usleep 10000
+ 					tune_control stop
+ 					if px4io checkcrc ${IOFW}
+ 					then
+ 						tune_control play -t 17 # tune 17 = PROG_PX4IO_OK
+ 					else
+ 						tune_control play -t 18 # tune 18 = PROG_PX4IO_ERR
+ 					fi
 				else
-					tune_control play -t 18 # tune 18 = PROG_PX4IO_ERR
+					tune_control stop
 				fi
-			else
-				tune_control stop
-			fi
 		fi
 
 		if ! px4io start
-		then
-			echo "PX4IO start failed"
-			set STARTUP_TUNE 2 # tune 2 = ERROR_TUNE
+ 			then
+ 				echo "PX4IO start failed"
+ 				set STARTUP_TUNE 2 # tune 2 = ERROR_TUNE
+ 			fi
 		fi
 	fi
 
-
 	#
 	# RC update (map raw RC input to calibrate manual control)
 	#  start before commander
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 3f8d0f89a125..9e2074e6ebf0 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -1556,6 +1556,10 @@ int PX4IO::custom_command(int argc, char *argv[])
 {
 	const char *verb = argv[0];
 
+	if (!strcmp(verb, "supported")) {
+ 		return 0;
+ 	}
+ 	
 	if (!strcmp(verb, "checkcrc")) {
 		if (is_running()) {
 			PX4_ERR("io must be stopped");
@@ -1761,7 +1765,7 @@ Output driver communicating with the IO co-processor.
 extern "C" __EXPORT int px4io_main(int argc, char *argv[])
 {
 	if (!PX4_MFT_HW_SUPPORTED(PX4_MFT_PX4IO)) {
-		PX4_ERR("PX4IO Not Supported");
+		PX4_INFO("PX4IO Not Supported");
 		return -1;
 	}
 	return PX4IO::main(argc, argv);