[BACKPORT 1.16] drivers/gps: RTCM injection fixes

platforms/common/Serial.cpp

@@ -69,6 +69,11 @@ bool Serial::close()
 	return _impl.close();
 }
 
+ssize_t Serial::bytesAvailable()
+{
+	return _impl.bytesAvailable();
+}
+
 ssize_t Serial::read(uint8_t *buffer, size_t buffer_size)
 {
 	return _impl.read(buffer, buffer_size);

platforms/common/include/px4_platform_common/Serial.hpp

@@ -61,6 +61,7 @@ class Serial
 
 	bool close();
 
+	ssize_t bytesAvailable();
 	ssize_t read(uint8_t *buffer, size_t buffer_size);
 	ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_ms = 0);
 

platforms/nuttx/src/px4/common/SerialImpl.cpp

@@ -259,6 +259,18 @@ bool SerialImpl::close()
 	return true;
 }
 
+ssize_t SerialImpl::bytesAvailable()
+{
+	if (!_open) {
+		PX4_ERR("Device not open!");
+		return -1;
+	}
+
+	ssize_t bytes_available = 0;
+	int ret = ioctl(_serial_fd, FIONREAD, &bytes_available);
+	return ret >= 0 ? bytes_available : 0;
+}
+
 ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size)
 {
 	if (!_open) {

platforms/nuttx/src/px4/common/include/SerialImpl.hpp

@@ -59,6 +59,7 @@ class SerialImpl
 
 	bool close();
 
+	ssize_t bytesAvailable();
 	ssize_t read(uint8_t *buffer, size_t buffer_size);
 	ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0);
 

platforms/posix/include/SerialImpl.hpp

@@ -59,6 +59,7 @@ class SerialImpl
 
 	bool close();
 
+	ssize_t bytesAvailable();
 	ssize_t read(uint8_t *buffer, size_t buffer_size);
 	ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0);
 

platforms/posix/src/px4/common/SerialImpl.cpp

@@ -251,6 +251,18 @@ bool SerialImpl::close()
 	return true;
 }
 
+ssize_t SerialImpl::bytesAvailable()
+{
+	if (!_open) {
+		PX4_ERR("Device not open!");
+		return -1;
+	}
+
+	ssize_t bytes_available = 0;
+	int ret = ioctl(_serial_fd, FIONREAD, &bytes_available);
+	return ret >= 0 ? bytes_available : 0;
+}
+
 ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size)
 {
 	if (!_open) {

platforms/qurt/include/SerialImpl.hpp

@@ -58,6 +58,7 @@ class SerialImpl
 
 	bool close();
 
+	ssize_t bytesAvailable();
 	ssize_t read(uint8_t *buffer, size_t buffer_size);
 	ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0);
 

platforms/qurt/src/px4/SerialImpl.cpp

@@ -158,6 +158,13 @@ bool SerialImpl::close()
 	return true;
 }
 
+ssize_t SerialImpl::bytesAvailable()
+{
+	// TODO:
+	PX4_WARN("bytesAvailable not implemented!");
+	return 0;
+}
+
 ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size)
 {
 	if (!_open) {

src/drivers/gps/gps.cpp

@@ -472,10 +472,16 @@ int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout)
 	const int max_timeout = 50;
 	int timeout_adjusted = math::min(max_timeout, timeout);
 
-	handleInjectDataTopic();
-
 	if (_interface == GPSHelper::Interface::UART) {
-		ret = _uart.readAtLeast(buf, buf_length, math::min(character_count, buf_length), timeout_adjusted);
+
+		const ssize_t read_at_least = math::min(character_count, buf_length);
+
+		// handle injection data before read if caught up
+		if (_uart.bytesAvailable() < read_at_least) {
+			handleInjectDataTopic();
+		}
+
+		ret = _uart.readAtLeast(buf, buf_length, read_at_least, timeout_adjusted);
 
 		if (ret > 0) {
 			_num_bytes_read += ret;
@@ -486,6 +492,8 @@ int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout)
 
 	} else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd >= 0)) {
 
+		handleInjectDataTopic();
+
 		//Poll only for the SPI data. In the same thread we also need to handle orb messages,
 		//so ideally we would poll on both, the SPI fd and orb subscription. Unfortunately the
 		//two pollings use different underlying mechanisms (at least under posix), which makes this
@@ -593,7 +601,17 @@ void GPS::handleInjectDataTopic()
 			}
 		}
 
-		updated = _orb_inject_data_sub[_selected_rtcm_instance].update(&msg);
+		auto &gps_inject_data_sub = _orb_inject_data_sub[_selected_rtcm_instance];
+
+		const unsigned last_generation = gps_inject_data_sub.get_last_generation();
+
+		updated = gps_inject_data_sub.update(&msg);
+
+		if (updated) {
+			if (gps_inject_data_sub.get_last_generation() != last_generation + 1) {
+				PX4_WARN("gps_inject_data lost, generation %u -> %u", last_generation, gps_inject_data_sub.get_last_generation());
+			}
+		}
 
 	} while (updated && num_injections < max_num_injections);
 }