From cb60384dc4a86a190d170b659cff6a8a3c401551 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 10 Feb 2016 09:37:37 +1100 Subject: [PATCH] AP_HAL_SITL: allow auto-baudrate detection in GPS driver in SITL this is useful when testing a real GPS in SITL --- libraries/AP_HAL_SITL/UARTDriver.cpp | 2 +- libraries/AP_HAL_SITL/sitl_gps.cpp | 8 ++++++-- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/libraries/AP_HAL_SITL/UARTDriver.cpp b/libraries/AP_HAL_SITL/UARTDriver.cpp index 5ca6cb5b96..df008e6e2b 100644 --- a/libraries/AP_HAL_SITL/UARTDriver.cpp +++ b/libraries/AP_HAL_SITL/UARTDriver.cpp @@ -84,7 +84,7 @@ void UARTDriver::begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace) uint16_t port = atoi(args2); _tcp_start_client(args1, port); } else if (strcmp(devtype, "uart") == 0) { - uint32_t baudrate = args2? atoi(args2) : 57600; + uint32_t baudrate = args2? atoi(args2) : baud; ::printf("UART connection %s:%u\n", args1, baudrate); _uart_start_connection(args1, baudrate); } else { diff --git a/libraries/AP_HAL_SITL/sitl_gps.cpp b/libraries/AP_HAL_SITL/sitl_gps.cpp index 2594cf4929..7dc7b93e8d 100644 --- a/libraries/AP_HAL_SITL/sitl_gps.cpp +++ b/libraries/AP_HAL_SITL/sitl_gps.cpp @@ -113,9 +113,13 @@ void SITL_State::_gps_write(const uint8_t *p, uint16_t size) continue; } } - write(gps_state.gps_fd, p, 1); + if (gps_state.gps_fd != 0) { + write(gps_state.gps_fd, p, 1); + } if (_sitl->gps2_enable) { - write(gps2_state.gps_fd, p, 1); + if (gps2_state.gps_fd != 0) { + write(gps2_state.gps_fd, p, 1); + } } p++; }