hardfault: add option to stream via mavlink

ROMFS/px4fmu_common/init.d/rcS

@@ -280,6 +280,14 @@ else
 	#
 	send_event start
 
+	#
+	# Start the hardfault streamer.
+	#
+	if param compare -s SYS_HF_MAV 1
+	then
+		hardfault_stream start
+	fi
+
 	#
 	# Start the resource load monitor.
 	#

boards/ark/fmu-v6x/default.px4board

@@ -62,6 +62,7 @@ CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
+CONFIG_MODULES_HARDFAULT_STREAM=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y

boards/auterion/fmu-v6s/default.px4board

@@ -43,6 +43,7 @@ CONFIG_FIGURE_OF_EIGHT=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
+CONFIG_MODULES_HARDFAULT_STREAM=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y

boards/px4/fmu-v5x/default.px4board

@@ -64,6 +64,7 @@ CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
+CONFIG_MODULES_HARDFAULT_STREAM=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y

boards/px4/fmu-v6x/default.px4board

@@ -62,6 +62,7 @@ CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
+CONFIG_MODULES_HARDFAULT_STREAM=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y

boards/px4/fmu-v6xrt/default.px4board

@@ -63,6 +63,7 @@ CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
+CONFIG_MODULES_HARDFAULT_STREAM=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y

src/modules/hardfault_stream/CMakeLists.txt

@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2016-2025 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+px4_add_module(
+	MODULE modules__hardfault_stream
+	MAIN hardfault_stream
+	COMPILE_FLAGS
+	SRCS
+		HardfaultStream.cpp
+		HardfaultStream.hpp
+	DEPENDS
+		px4_work_queue
+)

src/modules/hardfault_stream/HardfaultStream.cpp

@@ -0,0 +1,224 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "HardfaultStream.hpp"
+
+using namespace time_literals;
+
+namespace hardfault_stream
+{
+
+HardfaultStream::HardfaultStream() :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
+{
+}
+
+HardfaultStream::~HardfaultStream()
+{
+	ScheduleClear();
+
+	if (_hardfault_file != nullptr) {
+		fclose(_hardfault_file);
+	}
+}
+
+int HardfaultStream::task_spawn(int argc, char *argv[])
+{
+	HardfaultStream *obj = new HardfaultStream();
+
+	if (!obj) {
+		PX4_ERR("alloc failed");
+		return -1;
+	}
+
+	_object.store(obj);
+	_task_id = task_id_is_work_queue;
+
+	/* Schedule a cycle to start things. */
+	obj->start();
+
+	return 0;
+}
+
+void HardfaultStream::start()
+{
+	ScheduleOnInterval(150_ms);
+}
+
+bool HardfaultStream::mavlink_gcs_up()
+{
+	for (auto &telemetry_status : _telemetry_status_subs) {
+		telemetry_status_s telemetry;
+
+		if (telemetry_status.update(&telemetry)) {
+			if (telemetry.heartbeat_type_gcs) {
+				return true;
+			}
+		}
+	}
+
+	return false;
+}
+
+void HardfaultStream::search_hardfault_file()
+{
+	DIR *dp = opendir(PX4_STORAGEDIR);
+
+	if (dp != nullptr) {
+
+		struct dirent *result;
+		struct stat st;
+		time_t latest_mtime = 0;
+
+		while ((result = readdir(dp))) {
+			// Check for pattern fault_*.log
+			if (strncmp("fault_", result->d_name, 6) == 0 && strcmp(result->d_name + strlen(result->d_name) - 4, ".log") == 0) {
+				char current_file_path[CONFIG_PATH_MAX + 1];
+				snprintf(current_file_path, sizeof(current_file_path), "%s/%s", PX4_STORAGEDIR, result->d_name);
+
+				if (stat(current_file_path, &st) == 0) {
+					if (st.st_mtime >= latest_mtime) {
+						latest_mtime = st.st_mtime;
+						strncpy(_hardfault_file_path, current_file_path, sizeof(_hardfault_file_path));
+						_hardfault_file_path[sizeof(_hardfault_file_path) - 1] = '\0';
+						_hardfault_file_present = true;
+					}
+				}
+			}
+		}
+	}
+
+	closedir(dp);
+}
+
+void HardfaultStream::stream_hardfault()
+{
+	if (_hardfault_file == nullptr) {
+		_hardfault_file = fopen(_hardfault_file_path, "rb");
+
+		if (_hardfault_file == nullptr) {
+			PX4_ERR("Can't open hardfault log: %s", _hardfault_file_path);
+			_state = State::RequestStop;
+			return;
+		}
+
+		PX4_INFO("Streaming hardfault log: %s", _hardfault_file_path);
+	}
+
+	static constexpr int chunk_size = sizeof(mavlink_log_s::text) - 1;
+	uint8_t chunk[chunk_size];
+	size_t bytes_read = fread(chunk, 1, chunk_size, _hardfault_file);
+
+	if (bytes_read > 0) {
+		mavlink_vasprintf(_MSG_PRIO_CRITICAL, &_mavlink_log_pub, "%.*s", bytes_read, chunk);
+
+	} else {
+		fclose(_hardfault_file);
+		_hardfault_file = nullptr;
+		_stream_finished = true;
+	}
+}
+
+void HardfaultStream::Run()
+{
+	if (should_exit()) {
+		ScheduleClear();
+		exit_and_cleanup();
+	}
+
+	switch (_state) {
+	case State::SearchFile:
+		search_hardfault_file();
+		_state = State::WaitMavlink;
+		ScheduleNow();
+		break;
+
+	case State::WaitMavlink:
+		if (mavlink_gcs_up()) {
+			_state = State::StreamFile;
+			ScheduleNow();
+		}
+
+		break;
+
+	case State::StreamFile:
+		if (!_hardfault_file_present || _stream_finished) {
+			_state = State::RequestStop;
+
+		} else {
+			stream_hardfault();
+		}
+
+		break;
+
+	case State::RequestStop:
+		request_stop();
+		_state = State::WaitStop;
+
+	case State::WaitStop:
+		break;
+	}
+}
+
+int HardfaultStream::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_ERR("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+Background process that streams the latest hardfault via MAVLink.
+
+The module is especially useful when it is necessary to quickly push a hard fault to the ground station.
+This is useful in cases where the drone experiences a hard fault during flight.
+It ensures that some data is retained in case the permanent storage is destroyed during a crash.
+
+To reliably stream, it is necessary to send the STATUSTEXT message via MAVLink at a
+high enough frequency. The recommended frequency is 10 Hz or higher.
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("hardfault_stream", "system");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the background task");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+	return 0;
+}
+
+extern "C" __EXPORT int hardfault_stream_main(int argc, char *argv[])
+{
+	return HardfaultStream::main(argc, argv);
+}
+
+} // namespace hardfault_stream

src/modules/hardfault_stream/HardfaultStream.hpp

@@ -0,0 +1,102 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <dirent.h>
+#include <string.h>
+#include <sys/stat.h>
+
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionMultiArray.hpp>
+#include <uORB/topics/mavlink_log.h>
+#include <uORB/topics/telemetry_status.h>
+
+#include <systemlib/mavlink_log.h>
+
+namespace hardfault_stream
+{
+
+class HardfaultStream : public ModuleBase<HardfaultStream>, public ModuleParams, public px4::ScheduledWorkItem
+{
+public:
+	HardfaultStream();
+	~HardfaultStream() override;
+
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[])
+	{
+		return print_usage("unknown command");
+	}
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	void start();
+
+private:
+	enum class State {
+		SearchFile,
+		WaitMavlink,
+		StreamFile,
+		RequestStop,
+		WaitStop,
+	};
+
+	/** Do a compute and schedule the next cycle. */
+	void Run() override;
+
+	bool mavlink_gcs_up();
+	void search_hardfault_file();
+	void stream_hardfault();
+
+	State _state {State::SearchFile};
+
+	bool _stream_finished {false};
+	bool _hardfault_file_present {false};
+
+	char _hardfault_file_path[CONFIG_PATH_MAX + 1];
+	FILE *_hardfault_file {nullptr};
+
+	orb_advert_t _mavlink_log_pub {nullptr};
+	uORB::SubscriptionMultiArray<telemetry_status_s> _telemetry_status_subs{ORB_ID::telemetry_status};
+};
+
+} // namespace hardfault_stream

src/modules/hardfault_stream/Kconfig

@@ -0,0 +1,6 @@
+menuconfig MODULES_HARDFAULT_STREAM
+	bool "hardfault_stream"
+	default n
+	depends on PLATFORM_NUTTX
+	---help---
+		Enable support for hardfault_stream

src/modules/hardfault_stream/params.c

@@ -0,0 +1,44 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2025 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * Enable FMU SD card hardfault streaming
+ *
+ * When this is enabled all the hardfaults on the SD card are streamed
+ * over MAVLink. This is useful for cases where the FMU does reset in-flight due
+ * to a hardfault and the SD card may not survive a crash.
+ *
+ * @group System
+ * @boolean
+ */
+PARAM_DEFINE_INT32(SYS_HF_MAV, 1);