drivers/gps: warn if gps_inject_data publications have been missed

This aligns setActiveMissionItems() in rtl_direct_mission_land.cpp and in rtl_mission_fast.cpp with what was already in mission.cpp. It probably was on oversight when the RTL restructure happened. The FW landing requires the previous waypoint to be correctly set, that's why it was only noticeable there.

* Fix position setpoint update logic in Mission RTL

Currently, when proceeding to the landing point the previous setpoint is not updated, which results in an unexpected and off course landing pattern in fixed wing. (see #25436)

* Change to work more like `mission.cpp`

* Fix rtl_direct_misssion_land formatting for style guide

* rtl_mission_fast: fix FW landing by setting previous wp in landing

Signed-off-by: Silvan <silvan@auterion.com>

---------

Signed-off-by: Silvan <silvan@auterion.com>
Co-authored-by: Silvan <silvan@auterion.com>

docs/en/flight_modes_fw/return.md

@@ -25,11 +25,6 @@ The default type is recommended.
 
 :::
 
-::: warning
-There is a known issue ([PX4-Autopilot#25633](https://github.com/PX4/PX4-Autopilot/issues/#25633)) with fixed-wing approaches and landings while in RTL mode.
-Please review the issue and verify in simulation that the behavior you get is safe in an RTL landing scenario (if not, consider using rally points).
-:::
-
 ## Technical Summary
 
 Fixed-wing vehicles use the _mission landing/rally point_ return type by default.

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 @@ public:
 
 	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) {
@@ -339,12 +351,15 @@ ssize_t SerialImpl::write(const void *buffer, size_t buffer_size)
 	}
 
 	int written = ::write(_serial_fd, buffer, buffer_size);
-	tcdrain(_serial_fd); // Wait until all output is transmitted
 
 	if (written < 0) {
-		PX4_ERR("%s write error %d", _port, written);
+		if (errno != EAGAIN) {
+			PX4_ERR("%s write error %d", _port, written);
+		}
 	}
 
+	::fsync(_serial_fd);
+
 	return written;
 }
 

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

@@ -59,6 +59,7 @@ public:
 
 	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 @@ public:
 
 	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) {
@@ -325,13 +337,15 @@ ssize_t SerialImpl::write(const void *buffer, size_t buffer_size)
 	}
 
 	int written = ::write(_serial_fd, buffer, buffer_size);
-	::fsync(_serial_fd);
 
 	if (written < 0) {
-		PX4_ERR("%s write error %d", _port, written);
-
+		if (errno != EAGAIN) {
+			PX4_ERR("%s write error %d", _port, written);
+		}
 	}
 
+	::fsync(_serial_fd);
+
 	return written;
 }
 

platforms/qurt/include/SerialImpl.hpp

@@ -58,6 +58,7 @@ public:
 
 	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);
 }

src/modules/mavlink/mavlink_parameters.cpp

@@ -80,6 +80,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 #if defined(CONFIG_MAVLINK_UAVCAN_PARAMETERS)
 
 			if (req_list.target_system == mavlink_system.sysid && req_list.target_component < 127 &&
+			    !(req_list.target_component >= MAV_COMP_ID_CAMERA && req_list.target_component <= MAV_COMP_ID_CAMERA6) &&
 			    (req_list.target_component != mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) {
 				// publish list request to UAVCAN driver via uORB.
 				uavcan_parameter_request_s req{};
@@ -139,6 +140,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 #if defined(CONFIG_MAVLINK_UAVCAN_PARAMETERS)
 
 			if (set.target_system == mavlink_system.sysid && set.target_component < 127 &&
+			    !(set.target_component >= MAV_COMP_ID_CAMERA && set.target_component <= MAV_COMP_ID_CAMERA6) &&
 			    (set.target_component != mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) {
 				// publish set request to UAVCAN driver via uORB.
 				uavcan_parameter_request_s req{};
@@ -217,6 +219,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 #if defined(CONFIG_MAVLINK_UAVCAN_PARAMETERS)
 
 			if (req_read.target_system == mavlink_system.sysid && req_read.target_component < 127 &&
+			    !(req_read.target_component >= MAV_COMP_ID_CAMERA && req_read.target_component <= MAV_COMP_ID_CAMERA6) &&
 			    (req_read.target_component != mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) {
 				// publish set request to UAVCAN driver via uORB.
 				uavcan_parameter_request_s req{};

src/modules/navigator/mission.cpp

@@ -263,6 +263,10 @@ void Mission::setActiveMissionItems()
 			pos_sp_triplet->next.valid = false;
 		}
 
+	} else if (_mission_item.nav_cmd == NAV_CMD_DELAY) {
+		// Invalidate next waypoint to ensure vehicle holds position and doesn't try to track ahead
+		pos_sp_triplet->next.valid = false;
+
 	} else {
 		handleVtolTransition(new_work_item_type, next_mission_items, num_found_items);
 	}

src/modules/navigator/rtl_direct_mission_land.cpp

@@ -122,6 +122,7 @@ void RtlDirectMissionLand::setActiveMissionItems()
 {
 	WorkItemType new_work_item_type{WorkItemType::WORK_ITEM_TYPE_DEFAULT};
 	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+	const position_setpoint_s current_setpoint_copy = pos_sp_triplet->current;
 
 	// Climb to altitude
 	if (_needs_climbing && _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) {
@@ -203,8 +204,6 @@ void RtlDirectMissionLand::setActiveMissionItems()
 
 			_mission_item.autocontinue = true;
 			_mission_item.time_inside = 0.0f;
-
-			pos_sp_triplet->previous = pos_sp_triplet->current;
 		}
 
 		if (num_found_items > 0) {
@@ -213,6 +212,12 @@ void RtlDirectMissionLand::setActiveMissionItems()
 
 		mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 
+		// Only set the previous position item if the current one really changed
+		if ((_work_item_type != WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) &&
+		    !position_setpoint_equal(&pos_sp_triplet->current, &current_setpoint_copy)) {
+			pos_sp_triplet->previous = current_setpoint_copy;
+		}
+
 		// prevent lateral guidance from loitering at a waypoint as part of a mission landing if the altitude
 		// is not achieved.
 		const bool fw_on_mission_landing = _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING

src/modules/navigator/rtl_mission_fast.cpp

@@ -91,6 +91,7 @@ void RtlMissionFast::setActiveMissionItems()
 {
 	WorkItemType new_work_item_type{WorkItemType::WORK_ITEM_TYPE_DEFAULT};
 	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+	const position_setpoint_s current_setpoint_copy = pos_sp_triplet->current;
 
 	/* Skip VTOL/FW Takeoff item if in air, fixed-wing and didn't start the takeoff already*/
 	if ((_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) &&
@@ -158,17 +159,20 @@ void RtlMissionFast::setActiveMissionItems()
 			_mission_item.autocontinue = true;
 			_mission_item.time_inside = 0.0f;
 
-			pos_sp_triplet->previous = pos_sp_triplet->current;
 		}
 
-
-
 		if (num_found_items > 0) {
 			mission_item_to_position_setpoint(next_mission_items[0u], &pos_sp_triplet->next);
 		}
 
 		mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 
+		// Only set the previous position item if the current one really changed
+		if ((_work_item_type != WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) &&
+		    !position_setpoint_equal(&pos_sp_triplet->current, &current_setpoint_copy)) {
+			pos_sp_triplet->previous = current_setpoint_copy;
+		}
+
 		if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && isLanding() &&
 		    _mission_item.nav_cmd == NAV_CMD_WAYPOINT) {
 			pos_sp_triplet->current.alt_acceptance_radius = FLT_MAX;