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/config/compass.md

@@ -10,7 +10,7 @@ If you have [mounted the compass](../assembly/mount_gps_compass.md#compass-orien
 
 ## Overview
 
-You will need to calibrate your compass(es) when you first setup your vehicle, and you may need to recalibrate it if the vehicles is ever exposed to a very strong magnetic field, or if it is used in an area with abnormal magnetic characteristics.
+You will need to calibrate your compass(es) when you first setup your vehicle, and you may need to [recalibrate](#recalibration) it if the vehicles is ever exposed to a very strong magnetic field, or if it is used in an area with abnormal magnetic characteristics.
 
 :::tip
 Indications of a poor compass calibration include multicopter circling during hover, toilet bowling (circling at increasing radius/spiraling-out, usually constant altitude, leading to fly-way), or veering off-path when attempting to fly straight.
@@ -20,13 +20,16 @@ _QGroundControl_ should also notify the error `mag sensors inconsistent`.
 The process calibrates all compasses and autodetects the orientation of any external compasses.
 If any external magnetometers are available, it then disables the internal magnetometers (these are primarily needed for automatic rotation detection of external magnetometers).
 
+### Types of Calibration
+
 Several types of compass calibration are available:
 
 1. [Complete](#complete-calibration): This calibration is required after installing the autopilot on an airframe for the first time or when the configuration of the vehicle has changed significantly.
    It compensates for hard and soft iron effects by estimating an offset and a scale factor for each axis.
 1. [Partial](#partial-quick-calibration): This calibration can be performed as a routine when preparing the vehicle for a flight, after changing the payload, or simply when the compass rose seems inaccurate.
    This type of calibration only estimates the offsets to compensate for a hard iron effect.
-1. [Large vehicle](#large-vehicle-calibration): This calibration can be performed when the vehicle is too large or heavy to perform a complete calibration. This type of calibration only estimates the offsets to compensate for a hard iron effect.
+1. [Large vehicle](#large-vehicle-calibration): This calibration can be performed when the vehicle is too large or heavy to perform a complete calibration.
+   This type of calibration only estimates the offsets to compensate for a hard iron effect.
 
 ## Performing the Calibration
 
@@ -48,23 +51,27 @@ Before starting the calibration:
 The calibration steps are:
 
 1. Start _QGroundControl_ and connect the vehicle.
-1. Select **"Q" icon > Vehicle Setup > Sensors** (sidebar) to open _Sensor Setup_.
-1. Click the **Compass** sensor button.
+2. Select **"Q" icon > Vehicle Setup > Sensors** (sidebar) to open _Sensor Setup_.
+3. Click the **Compass** sensor button.
 
    ![Select Compass calibration PX4](../../assets/qgc/setup/sensor/sensor_compass_select_px4.png)
 
    ::: info
-   You should already have set the [Autopilot Orientation](../config/flight_controller_orientation.md). If not, you can also set it here.
+   You should already have set the [Autopilot Orientation](../config/flight_controller_orientation.md).
+   If not, you can also set it here.
    :::
 
-1. Click **OK** to start the calibration.
-1. Place the vehicle in any of the orientations shown in red (incomplete) and hold it still. Once prompted (the orientation-image turns yellow) rotate the vehicle around the specified axis in either/both directions. Once the calibration is complete for the current orientation the associated image on the screen will turn green.
+4. Click **OK** to start the calibration.
+5. Place the vehicle in any of the orientations shown in red (incomplete) and hold it still.
+   Once prompted (the orientation-image turns yellow) rotate the vehicle around the specified axis in either/both directions.
+   Once the calibration is complete for the current orientation the associated image on the screen will turn green.
 
    ![Compass calibration steps on PX4](../../assets/qgc/setup/sensor/sensor_compass_calibrate_px4.png)
 
-1. Repeat the calibration process for all vehicle orientations.
+6. Repeat the calibration process for all vehicle orientations.
 
-Once you've calibrated the vehicle in all the positions _QGroundControl_ will display _Calibration complete_ (all orientation images will be displayed in green and the progress bar will fill completely). You can then proceed to the next sensor.
+Once you've calibrated the vehicle in all the positions _QGroundControl_ will display _Calibration complete_ (all orientation images will be displayed in green and the progress bar will fill completely).
+You can then proceed to the next sensor.
 
 ### Partial "Quick" Calibration
 
@@ -87,7 +94,8 @@ Notes:
 
 This calibration process leverages external knowledge of vehicle's orientation and location, and a World Magnetic Model (WMM) to calibrate the hard iron biases.
 
-1. Ensure GNSS Fix. This is required to find the expected Earth magnetic field in WMM tables.
+1. Ensure GNSS Fix.
+   This is required to find the expected Earth magnetic field in WMM tables.
 2. Align the vehicle to face True North.
    Be as accurate as possible for best results.
 3. Open the [QGroundControl MAVLink Console](https://docs.qgroundcontrol.com/master/en/qgc-user-guide/analyze_view/mavlink_console.html) and send the following command:
@@ -107,6 +115,30 @@ Notes:
 
 After the calibration is complete, check that the heading indicator and the heading of the arrow on the map are stable and match the orientation of the vehicle when turning it e.g. to the cardinal directions.
 
+## Recalibration
+
+Recalibration is recommended whenever the magnetic environment of the vehicle has changed or when heading behavior appears unreliable.
+
+You can use either complete calibration or mag quick calibration depending on the size of the vehicle and your ability to rotate it through the required orientations.
+Complete calibration provides the most accurate soft-iron compensation.
+
+Recalibrate the compass when:
+
+- _The compass module or its mounting orientation has changed._
+  This includes replacing the GPS or mag unit, rotating the mast, or altering how the module is fixed to the airframe.
+- _The vehicle has been exposed to a strong magnetic disturbance._
+  Examples include transport or storage near large steel structures, welding operations near the airframe, or operation close to high-current equipment.
+- _Structural, wiring, or payload changes may have altered the magnetic field around the sensors._
+  New payloads, rerouted wires, additional batteries, or metal fasteners can introduce soft-iron effects that affect heading accuracy.
+- _The vehicle is operated in a region with significantly different magnetic characteristics._
+  Large changes in latitude, longitude, or magnetic inclination can require re-estimation of offsets.
+- _QGroundControl reports magnetometer inconsistencies_.
+  For example, if you see the error `mag sensors inconsistent`.
+- _Heading behavior does not match the vehicle’s observed orientation._
+  Symptoms include drifting yaw, sudden heading jumps when attempting to fly straight, and toilet bowling
+- _QGroundControl_ sends the error `mag sensors inconsistent`.
+  This indicates that multiple magnetometers are reporting different headings.
+
 ## Additional Calibration/Configuration
 
 The process above will autodetect, [set default rotations](../advanced_config/parameter_reference.md#SENS_MAG_AUTOROT), calibrate, and prioritise, all available magnetometers.

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:

docs/en/releases/main.md

@@ -55,6 +55,7 @@ Please continue reading for [upgrade instructions](#upgrade-guide).
 ### Sensors
 
 - Add [sbgECom INS driver](../sensor/sbgecom.md) ([PX4-Autopilot#24137](https://github.com/PX4/PX4-Autopilot/pull/24137))
+- Quick magnetometer calibration now supports specifying an arbitrary initial heading ([PX4-Autopilot#24637](https://github.com/PX4/PX4-Autopilot/pull/24637))
 
 ### Simulation
 

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;