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", "", "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) {