Add static_assert to verify LOG_FILEPATH_SCANF_WIDTH is synchronized

- Add compile-time check that LOG_FILEPATH_SIZE == 256
- Ensures LOG_FILEPATH_SCANF_WIDTH stays synchronized if buffer size changes
- Provides clear error message if someone modifies LOG_FILEPATH_SIZE

Co-authored-by: dakejahl <37091262+dakejahl@users.noreply.github.com>

boards/ark/fmu-v6x/default.px4board

@@ -40,6 +40,7 @@ CONFIG_DRIVERS_MAGNETOMETER_ST_IIS2MDC=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_POWER_MONITOR_INA228=y
 CONFIG_DRIVERS_POWER_MONITOR_INA238=y
+CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_PX4IO=y
 CONFIG_COMMON_RC=y

boards/ark/fmu-v6x/init/rc.board_sensors

@@ -97,5 +97,11 @@ bmm150 -I start
 # Internal Baro on I2C
 bmp388 -I start
 
+# Start an external PWM generator
+if param greater PCA9685_EN_BUS 0
+then
+	pca9685_pwm_out start
+fi
+
 unset HAVE_PM2
 unset HAVE_PM3

boards/ark/fpv/default.px4board

@@ -21,6 +21,7 @@ CONFIG_DRIVERS_IMU_MURATA_SCH16T=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_COMMON_MAGNETOMETER=y
 CONFIG_DRIVERS_OSD_MSP_OSD=y
+CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_COMMON_RC=y
 CONFIG_DRIVERS_UAVCAN=y

boards/ark/fpv/init/rc.board_sensors

@@ -16,3 +16,9 @@ iis2mdc -R 0 -I -b 4 start
 
 # Internal Baro on I2C
 bmp388 -I -b 2 start
+
+# Start an external PWM generator
+if param greater PCA9685_EN_BUS 0
+then
+	pca9685_pwm_out start
+fi

boards/ark/pi6x/default.px4board

@@ -19,6 +19,7 @@ CONFIG_COMMON_LIGHT=y
 CONFIG_DRIVERS_MAGNETOMETER_MEMSIC_MMC5983MA=y
 CONFIG_DRIVERS_MAGNETOMETER_ST_IIS2MDC=y
 CONFIG_DRIVERS_OPTICAL_FLOW_PAW3902=y
+CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_COMMON_RC=y

boards/ark/pi6x/init/rc.board_sensors

@@ -34,3 +34,9 @@ paw3902 -s -b 3 start -Y 90
 
 # Internal distance sensor
 afbrs50 start
+
+# Start an external PWM generator
+if param greater PCA9685_EN_BUS 0
+then
+	pca9685_pwm_out start
+fi

boards/auterion/fmu-v6s/init/rc.board_sensors

@@ -74,5 +74,5 @@ ist8310 -X -b 1 -R 10 start
 # Start an external PWM generator
 if param greater PCA9685_EN_BUS 0
 then
-	pca9685_pwm_out start -b 1
+	pca9685_pwm_out start
 fi

boards/bluerobotics/navigator/init/rc.board_defaults

@@ -11,3 +11,7 @@ param set BAT1_V_DIV 5.7
 
 # Always keep current config
 param set SYS_AUTOCONFIG 0
+
+# PCA9685 PWM Out defaults
+param set-default PCA9685_EN_BUS 4
+param set-default PCA9685_I2C_ADDR 64

boards/bluerobotics/navigator/init/rc.board_sensors

@@ -28,7 +28,7 @@ then
 	echo "ads1115 not found."
 fi
 
-if ! pca9685_pwm_out start -a 0x40 -b 4
+if ! pca9685_pwm_out start
 then
 	echo "pca9685_pwm_out not found."
 fi

boards/px4/fmu-v6xrt/default.px4board

@@ -47,6 +47,7 @@ CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_DRIVERS_UAVCAN=y
+CONFIG_BOARD_UAVCAN_INTERFACES=3
 CONFIG_COMMON_UWB=y
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
 CONFIG_MODULES_BATTERY_STATUS=y

docs/en/advanced_config/parameter_reference.md

@@ -486,6 +486,16 @@ The integer refers to the I2C bus number where PCA9685 is connected.
 | ------ | -------- | -------- | --------- | ------- | ---- |
 |   | 0        | 10       |           | 0       |
 
+### PCA9685_I2C_ADDR (`INT32`) {#PCA9685_I2C_ADDR}
+
+I2C address of PCA9685.
+
+The default address is 0x40 (64).
+
+| Reboot | minValue | maxValue | increment | default | unit |
+| ------ | -------- | -------- | --------- | ------- | ---- |
+|   | 0        | 127      |           | 64      |
+
 ### PCA9685_FAIL1 (`INT32`) {#PCA9685_FAIL1}
 
 PCA9685 Output Channel 1 Failsafe Value.

docs/en/modules/modules_driver.md

@@ -899,6 +899,8 @@ fetching the latest mixing result and write them to PCA9685 at its scheduling ti
 It can do full 12bits output as duty-cycle mode, while also able to output precious pulse width
 that can be accepted by most ESCs and servos.
 
+The I2C bus and address can be configured via parameters `PCA9685_EN_BUS` and `PCA9685_I2C_ADDR`, or via command line arguments.
+
 ### Examples
 
 It is typically started with:

msg/versioned/VehicleCommand.msg

@@ -100,6 +100,7 @@ uint16 VEHICLE_CMD_LOGGING_START = 2510 # Start streaming ULog data.
 uint16 VEHICLE_CMD_LOGGING_STOP = 2511 # Stop streaming ULog data.
 uint16 VEHICLE_CMD_CONTROL_HIGH_LATENCY = 2600 # Control starting/stopping transmitting data over the high latency link.
 uint16 VEHICLE_CMD_DO_VTOL_TRANSITION = 3000 # Command VTOL transition.
+uint16 VEHICLE_CMD_DO_SET_SAFETY_SWITCH_STATE = 5300 # Command safety on/off. |1 to activate safety, 0 to deactivate safety and allow control surface movements|Unused|Unused|Unused|Unused|Unused|Unused|
 uint16 VEHICLE_CMD_ARM_AUTHORIZATION_REQUEST = 3001 # Request arm authorization.
 uint16 VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001 # Prepare a payload deployment in the flight plan.
 uint16 VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002 # Control a pre-programmed payload deployment.
@@ -179,6 +180,10 @@ int8 ARMING_ACTION_ARM = 1
 uint8 GRIPPER_ACTION_RELEASE = 0
 uint8 GRIPPER_ACTION_GRAB = 1
 
+# Used as param1 in DO_SET_SAFETY_SWITCH_STATE command.
+uint8 SAFETY_OFF = 0
+uint8 SAFETY_ON = 1
+
 uint8 ORB_QUEUE_LENGTH = 8
 
 float32 param1 # Parameter 1, as defined by MAVLink uint16 VEHICLE_CMD enum.

platforms/common/Serial.cpp

@@ -74,6 +74,11 @@ ssize_t Serial::bytesAvailable()
 	return _impl.bytesAvailable();
 }
 
+ssize_t Serial::txSpaceAvailable()
+{
+	return _impl.txSpaceAvailable();
+}
+
 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

@@ -62,6 +62,7 @@ public:
 	bool close();
 
 	ssize_t bytesAvailable();
+	ssize_t txSpaceAvailable();
 	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

@@ -293,12 +293,36 @@ ssize_t SerialImpl::bytesAvailable()
 {
 	if (!_open) {
 		PX4_ERR("Device not open!");
+		errno = EBADF;
 		return -1;
 	}
 
 	ssize_t bytes_available = 0;
 	int ret = ioctl(_serial_fd, FIONREAD, &bytes_available);
-	return ret >= 0 ? bytes_available : 0;
+
+	if (ret < 0) {
+		return -1;
+	}
+
+	return bytes_available;
+}
+
+ssize_t SerialImpl::txSpaceAvailable()
+{
+	if (!_open) {
+		PX4_ERR("Device not open!");
+		errno = EBADF;
+		return -1;
+	}
+
+	ssize_t space_available = 0;
+	int ret = ioctl(_serial_fd, FIONSPACE, &space_available);
+
+	if (ret < 0) {
+		return -1;
+	}
+
+	return space_available;
 }
 
 ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size)

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

@@ -60,6 +60,7 @@ public:
 	bool close();
 
 	ssize_t bytesAvailable();
+	ssize_t txSpaceAvailable();
 	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

@@ -60,6 +60,7 @@ public:
 	bool close();
 
 	ssize_t bytesAvailable();
+	ssize_t txSpaceAvailable();
 	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

@@ -281,12 +281,31 @@ ssize_t SerialImpl::bytesAvailable()
 {
 	if (!_open) {
 		PX4_ERR("Device not open!");
+		errno = EBADF;
 		return -1;
 	}
 
 	ssize_t bytes_available = 0;
 	int ret = ioctl(_serial_fd, FIONREAD, &bytes_available);
-	return ret >= 0 ? bytes_available : 0;
+
+	if (ret < 0) {
+		return -1;
+	}
+
+	return bytes_available;
+}
+
+ssize_t SerialImpl::txSpaceAvailable()
+{
+	if (!_open) {
+		PX4_ERR("Device not open!");
+		errno = EBADF;
+		return -1;
+	}
+
+	// POSIX/Linux doesn't have a direct equivalent to NuttX's FIONSPACE
+	errno = ENOSYS;
+	return -1;
 }
 
 ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size)

platforms/qurt/include/SerialImpl.hpp

@@ -59,6 +59,7 @@ public:
 	bool close();
 
 	ssize_t bytesAvailable();
+	ssize_t txSpaceAvailable();
 	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

@@ -156,14 +156,33 @@ ssize_t SerialImpl::bytesAvailable()
 {
 	if (!_open) {
 		PX4_ERR("Device not open!");
+		errno = EBADF;
 		return -1;
 	}
 
 	uint32_t rx_bytes = 0;
-	(void) fc_uart_rx_available(_serial_fd, &rx_bytes);
+	int ret = fc_uart_rx_available(_serial_fd, &rx_bytes);
+
+	if (ret < 0) {
+		return -1;
+	}
+
 	return (ssize_t) rx_bytes;
 }
 
+ssize_t SerialImpl::txSpaceAvailable()
+{
+	if (!_open) {
+		PX4_ERR("Device not open!");
+		errno = EBADF;
+		return -1;
+	}
+
+	// QURT doesn't have a direct equivalent to NuttX's FIONSPACE
+	errno = ENOSYS;
+	return -1;
+}
+
 ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size)
 {
 	if (!_open) {

src/drivers/pca9685_pwm_out/main.cpp

@@ -47,6 +47,7 @@
 #include <drivers/drv_hrt.h>
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/sem.hpp>
+#include <lib/parameters/param.h>
 
 #include "PCA9685.h"
 
@@ -356,8 +357,30 @@ int PCA9685Wrapper::custom_command(int argc, char **argv) {
 
 int PCA9685Wrapper::task_spawn(int argc, char **argv) {
 	int ch;
-	int address=PCA9685_DEFAULT_ADDRESS;
-	int iicbus=PCA9685_DEFAULT_IICBUS;
+	int address = PCA9685_DEFAULT_ADDRESS;
+	int iicbus = PCA9685_DEFAULT_IICBUS;
+
+	int32_t en_bus = 0;
+	param_t param_handle = param_find("PCA9685_EN_BUS");
+
+	if (param_handle != PARAM_INVALID) {
+		param_get(param_handle, &en_bus);
+
+		if (en_bus > 0) {
+			iicbus = en_bus;
+		}
+	}
+
+	int32_t i2c_addr = 0;
+	param_handle = param_find("PCA9685_I2C_ADDR");
+
+	if (param_handle != PARAM_INVALID) {
+		param_get(param_handle, &i2c_addr);
+
+		if (i2c_addr > 0) {
+			address = i2c_addr;
+		}
+	}
 
 	int myoptind = 1;
 	const char *myoptarg = nullptr;

src/drivers/pca9685_pwm_out/module.yaml

@@ -29,6 +29,16 @@ parameters:
         min: 0
         max: 10
         default: 0
+      PCA9685_I2C_ADDR:
+        description:
+          short: I2C address of PCA9685
+          long: |
+            I2C address of PCA9685.
+            The default address is 0x40 (64).
+        type: int32
+        min: 1
+        max: 127
+        default: 64
       PCA9685_SCHD_HZ:
         description:
           short: PWM update rate

src/modules/commander/Commander.cpp

@@ -302,6 +302,26 @@ int Commander::custom_command(int argc, char *argv[])
 		return 0;
 	}
 
+	if (!strcmp(argv[0], "safety")) {
+		if (argc < 2) {
+			PX4_ERR("missing argument");
+			return 1;
+		}
+
+		if (!strcmp(argv[1], "on")) {
+			send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_SAFETY_SWITCH_STATE, vehicle_command_s::SAFETY_ON);
+
+		} else if (!strcmp(argv[1], "off")) {
+			send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_SAFETY_SWITCH_STATE, vehicle_command_s::SAFETY_OFF);
+
+		} else {
+			PX4_ERR("invlaid argument, use [on|off]");
+			return 1;
+		}
+
+		return 0;
+	}
+
 	if (!strcmp(argv[0], "arm")) {
 		float param2 = 0.f;
 
@@ -495,6 +515,7 @@ int Commander::custom_command(int argc, char *argv[])
 int Commander::print_status()
 {
 	PX4_INFO("%s", isArmed() ? "Armed" : "Disarmed");
+	PX4_INFO("prearm safety: %s", _safety.isSafetyOff() ? "Off" : "On");
 	PX4_INFO("navigation mode: %s", mode_util::nav_state_names[_vehicle_status.nav_state]);
 	PX4_INFO("user intended navigation mode: %s", mode_util::nav_state_names[_vehicle_status.nav_state_user_intention]);
 	PX4_INFO("in failsafe: %s", _failsafe.inFailsafe() ? "yes" : "no");
@@ -1508,6 +1529,29 @@ Commander::handle_command(const vehicle_command_s &cmd)
 		answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 		break;
 
+	case vehicle_command_s::VEHICLE_CMD_DO_SET_SAFETY_SWITCH_STATE: {
+			// reject if armed, only allow pre or post flight for safety
+			if (isArmed()) {
+				answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
+
+			} else {
+				int commanded_state = (int)cmd.param1;
+
+				if (commanded_state == vehicle_command_s::SAFETY_OFF) {
+					_safety.deactivateSafety();
+					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+
+				} else if (commanded_state == vehicle_command_s::SAFETY_ON) {
+					_safety.activateSafety();
+					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+
+				} else {
+					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED);
+				}
+			}
+		}
+		break;
+
 	case vehicle_command_s::VEHICLE_CMD_START_RX_PAIR:
 	case vehicle_command_s::VEHICLE_CMD_CUSTOM_0:
 	case vehicle_command_s::VEHICLE_CMD_CUSTOM_1:
@@ -3026,6 +3070,8 @@ The commander module contains the state machine for mode switching and failsafe
 	PRINT_MODULE_USAGE_ARG("mag|baro|accel|gyro|level|esc|airspeed", "Calibration type", false);
 	PRINT_MODULE_USAGE_ARG("quick", "Quick calibration [mag, accel (not recommended)]", false);
 	PRINT_MODULE_USAGE_COMMAND_DESCR("check", "Run preflight checks");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("safety", "Change prearm safety state");
+	PRINT_MODULE_USAGE_ARG("on|off", "[on] to activate safety, [off] to deactivate safety and allow control surface movements", false);
 	PRINT_MODULE_USAGE_COMMAND("arm");
 	PRINT_MODULE_USAGE_PARAM_FLAG('f', "Force arming (do not run preflight checks)", true);
 	PRINT_MODULE_USAGE_COMMAND("disarm");

src/modules/commander/Safety.cpp

@@ -76,3 +76,10 @@ void Safety::activateSafety()
 		_safety_off = false;
 	}
 }
+
+void Safety::deactivateSafety()
+{
+	if (!_safety_disabled) {
+		_safety_off = true;
+	}
+}

src/modules/commander/Safety.hpp

@@ -49,6 +49,7 @@ public:
 
 	bool safetyButtonHandler();
 	void activateSafety();
+	void deactivateSafety();
 	bool isButtonAvailable() const { return _button_available; }
 	bool isSafetyOff() const { return _safety_off; }
 	bool isSafetyDisabled() const { return _safety_disabled; }

src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp

@@ -104,12 +104,17 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 
 		bool do_vel_pos_reset = false;
 
-		if (!_control_status.flags.gnss_fault && (_control_status.flags.gnss_vel || _control_status.flags.gnss_pos)) {
+		if (!_control_status.flags.gnss_fault && _control_status.flags.in_air && isYawFailure()) {
+			const bool velocity_fusion_failure =  _aid_src_gnss_vel.innovation_rejected
+							      && isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay)
+							      && (_time_last_hor_vel_fuse > _time_last_on_ground_us);
 
-			if (_control_status.flags.in_air
-			    && isYawFailure()
-			    && isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay)
-			    && (_time_last_hor_vel_fuse > _time_last_on_ground_us)) {
+			const bool position_fusion_failure =  _aid_src_gnss_pos.innovation_rejected
+							      && isTimedOut(_time_last_hor_pos_fuse, _params.EKFGSF_reset_delay)
+							      && (_time_last_hor_pos_fuse > _time_last_on_ground_us);
+
+			if ((_control_status.flags.gnss_vel && velocity_fusion_failure)
+			    || (_control_status.flags.gnss_pos && position_fusion_failure)) {
 				do_vel_pos_reset = tryYawEmergencyReset();
 			}
 		}

src/modules/esc_battery/EscBattery.cpp

@@ -105,7 +105,6 @@ EscBattery::Run()
 		}
 
 		average_voltage_v /= online_esc_count;
-		total_current_a /= online_esc_count;
 		average_temperature_c /= online_esc_count;
 
 		_battery.setConnected(true);

src/modules/logger/logged_topics.cpp

@@ -63,7 +63,6 @@ void LoggedTopics::add_default_topics()
 	add_optional_topic("external_ins_attitude");
 	add_optional_topic("external_ins_global_position");
 	add_optional_topic("external_ins_local_position");
-	// add_optional_topic("esc_status", 250);
 	add_topic("esc_status");
 	add_topic("failure_detector_status", 100);
 	add_topic("failsafe_flags");
@@ -209,7 +208,6 @@ void LoggedTopics::add_default_topics()
 	add_optional_topic_multi("vehicle_magnetometer", 500, 4);
 	add_topic("vehicle_optical_flow", 500);
 	add_topic("aux_global_position", 500);
-	//add_optional_topic("vehicle_optical_flow_vel", 100);
 	add_optional_topic("pps_capture");
 
 	// additional control allocation logging

src/modules/mavlink/mavlink_log_handler.cpp

@@ -54,6 +54,17 @@ static const char *kLogDir = PX4_STORAGEDIR "/log";
 #define PX4_MAX_FILEPATH 	PATH_MAX
 #endif
 
+// Ensure PX4_MAX_FILEPATH is large enough for our buffer sizes
+// LogEntry.filepath uses MavlinkLogHandler::LOG_FILEPATH_SIZE
+static_assert(PX4_MAX_FILEPATH >= MavlinkLogHandler::LOG_FILEPATH_SIZE,
+	      "PX4_MAX_FILEPATH must be at least LOG_FILEPATH_SIZE bytes for log file paths");
+
+// Width specifier for sscanf - must be LOG_FILEPATH_SIZE - 1
+// This is hardcoded as 255 because preprocessor can't evaluate (256-1) in format strings
+// The static_assert below ensures this stays synchronized with LOG_FILEPATH_SIZE
+#define LOG_FILEPATH_SCANF_WIDTH "255"
+static_assert(MavlinkLogHandler::LOG_FILEPATH_SIZE == 256, "Update LOG_FILEPATH_SCANF_WIDTH if LOG_FILEPATH_SIZE changes");
+
 MavlinkLogHandler::MavlinkLogHandler(Mavlink &mavlink)
 	: _mavlink(mavlink)
 {}
@@ -171,10 +182,12 @@ void MavlinkLogHandler::state_listing()
 		// We can send!
 		uint32_t size_bytes = 0;
 		uint32_t time_utc = 0;
-		char filepath[PX4_MAX_FILEPATH];
+		char filepath[MavlinkLogHandler::LOG_FILEPATH_SIZE];
 
 		// If parsed lined successfully, send the entry
-		if (sscanf(line, "%" PRIu32 " %" PRIu32 " %s", &time_utc, &size_bytes, filepath) != 3) {
+		// Note: Using width specifier to prevent buffer overflow
+		// Width is LOG_FILEPATH_SIZE-1 to leave room for null terminator
+		if (sscanf(line, "%" PRIu32 " %" PRIu32 " %" LOG_FILEPATH_SCANF_WIDTH "s", &time_utc, &size_bytes, filepath) != 3) {
 			PX4_DEBUG("sscanf failed");
 			continue;
 		}
@@ -506,7 +519,9 @@ bool MavlinkLogHandler::log_entry_from_id(uint16_t log_id, LogEntry *entry)
 			continue;
 		}
 
-		if (sscanf(line, "%" PRIu32 " %" PRIu32 " %s", &(entry->time_utc), &(entry->size_bytes), entry->filepath) != 3) {
+		// Parse log entry with width specifier to prevent buffer overflow
+		// Width is LOG_FILEPATH_SIZE-1 to leave room for null terminator
+		if (sscanf(line, "%" PRIu32 " %" PRIu32 " %" LOG_FILEPATH_SCANF_WIDTH "s", &(entry->time_utc), &(entry->size_bytes), entry->filepath) != 3) {
 			PX4_DEBUG("sscanf failed");
 			continue;
 		}

src/modules/mavlink/mavlink_log_handler.h

@@ -48,12 +48,14 @@ public:
 	void handle_message(const mavlink_message_t *msg);
 
 private:
+	static constexpr size_t LOG_FILEPATH_SIZE = 256;  // Buffer size for log file paths
+
 	struct LogEntry {
 		uint16_t id{0xffff};
 		uint32_t time_utc{};
 		uint32_t size_bytes{};
 		FILE *fp{nullptr};
-		char filepath[60];
+		char filepath[LOG_FILEPATH_SIZE];
 		uint32_t offset{};
 	};
 

src/modules/mavlink/mavlink_main.cpp

@@ -163,7 +163,10 @@ Mavlink::~Mavlink()
 	}
 
 	if (_instance_id >= 0) {
-		mavlink_module_instances[_instance_id] = nullptr;
+		{
+			LockGuard lg{mavlink_module_mutex};
+			mavlink_module_instances[_instance_id] = nullptr;
+		}
 		mavlink_instance_count.fetch_sub(1);
 	}
 

src/modules/mavlink/mavlink_receiver.cpp

@@ -3160,7 +3160,7 @@ MavlinkReceiver::run()
 	ssize_t nread = 0;
 	hrt_abstime last_send_update = 0;
 
-	while (!_mavlink.should_exit()) {
+	while (!_should_exit.load()) {
 
 		// check for parameter updates
 		if (_parameter_update_sub.updated()) {

test/mavsdk_tests/test_multicopter_basics.cpp

@@ -40,7 +40,7 @@
 TEST_CASE("Takeoff and hold position", "[multicopter][vtol]")
 {
 	const float takeoff_altitude = 10.f;
-	const float altitude_tolerance = 0.1f;
+	const float altitude_tolerance = 0.2f;
 	const int delay_seconds = 60.f;
 
 	AutopilotTester tester;