From 76e15b4a2150ca00a9d126a4cefc24344d9fdfac Mon Sep 17 00:00:00 2001 From: benjinne Date: Thu, 15 Apr 2021 14:41:40 -0400 Subject: [PATCH] RTPS client get baudrate parameter if requested, and mark the device baud rate parameter as used so it shows up in QGC --- .../micrortps_client/microRTPS_client_main.cpp | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp b/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp index ce6617516a..0a8d6b3cb8 100644 --- a/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp +++ b/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp @@ -96,7 +96,21 @@ static int parse_options(int argc, char *argv[]) case 'w': _options.sleep_ms = strtoul(myoptarg, nullptr, 10); break; - case 'b': _options.baudrate = strtoul(myoptarg, nullptr, 10); break; + case 'b': + { + int baudrate = 0; + if (px4_get_parameter_value(myoptarg, baudrate) != 0) { + PX4_ERR("baudrate parsing failed"); + } + + if (baudrate < 9600 || baudrate > 3000000) { + PX4_ERR("invalid baud rate '%s'", myoptarg); + } + + _options.baudrate = baudrate; + + break; + } case 'r': _options.recv_port = strtoul(myoptarg, nullptr, 10); break;