From 58c8c50174516759e83a0123bfc620b44ac225ac Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Sat, 7 Jul 2018 09:23:29 +0200
Subject: [PATCH] frsky_telemetry: add scanning timeout parameter, and set it
 to 15s

This frees unused RAM in case frsky telemetry is not connected.
---
 ROMFS/px4fmu_common/init.d/rcS                    |  2 +-
 .../telemetry/frsky_telemetry/frsky_telemetry.cpp | 15 ++++++++++++++-
 2 files changed, 15 insertions(+), 2 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 0d345029bf..c5f5dc1c45 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -617,7 +617,7 @@ fi
 		if ver hwcmp PX4FMU_V4 PX4FMU_V4PRO MINDPX_V2
 		then
 			# This is TELEM4 on Pixhawk 3 Pro
-			frsky_telemetry start -d /dev/ttyS6
+			frsky_telemetry start -d /dev/ttyS6 -t 15
 		fi
 	fi
 
diff --git a/src/drivers/telemetry/frsky_telemetry/frsky_telemetry.cpp b/src/drivers/telemetry/frsky_telemetry/frsky_telemetry.cpp
index 32603017e4..637d46d97f 100644
--- a/src/drivers/telemetry/frsky_telemetry/frsky_telemetry.cpp
+++ b/src/drivers/telemetry/frsky_telemetry/frsky_telemetry.cpp
@@ -217,6 +217,7 @@ static void usage()
 	PRINT_MODULE_USAGE_NAME("frsky_telemetry", "communication");
 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/ttyS6", "<file:dev>", "Select Serial Device", true);
+	PRINT_MODULE_USAGE_PARAM_INT('t', 0, 0, 60, "Scanning timeout [s] (default: no timeout)", true);
 	PRINT_MODULE_USAGE_COMMAND("stop");
 	PRINT_MODULE_USAGE_COMMAND("status");
 }
@@ -227,17 +228,22 @@ static void usage()
 static int frsky_telemetry_thread_main(int argc, char *argv[])
 {
 	device_name = "/dev/ttyS6"; /* default USART8 */
+	unsigned scanning_timeout_ms = 0;
 
 	int myoptind = 1;
 	int ch;
 	const char *myoptarg = nullptr;
 
-	while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) {
+	while ((ch = px4_getopt(argc, argv, "d:t:", &myoptind, &myoptarg)) != EOF) {
 		switch (ch) {
 		case 'd':
 			device_name = myoptarg;
 			break;
 
+		case 't':
+			scanning_timeout_ms = strtoul(myoptarg, nullptr, 10) * 1000;
+			break;
+
 		default:
 			usage();
 			return -1;
@@ -267,6 +273,8 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
 	frsky_state = SCANNING;
 	frsky_state_t baudRate = DTYPE;
 
+	const hrt_abstime start_time = hrt_absolute_time();
+
 	while (!thread_should_exit && frsky_state == SCANNING) {
 		/* 2 byte polling frames indicate SmartPort telemetry
 		 * 11 byte packets indicate D type telemetry
@@ -344,6 +352,11 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
 		// flush buffer
 		read(uart, &sbuf[0], sizeof(sbuf));
 
+		// check for a timeout
+		if (scanning_timeout_ms > 0 && (hrt_absolute_time() - start_time) / 1000 > scanning_timeout_ms) {
+			PX4_INFO("Scanning timeout: exiting");
+			break;
+		}
 	}
 
 	if (frsky_state == SPORT || frsky_state == SPORT_SINGLE_WIRE) {