ROS2 sdk: add config overrides

src/ros2/cpp/px4_sdk_lib/CMakeLists.txt

@@ -22,6 +22,7 @@ set(HEADER_FILES
 		include/px4_sdk/components/health_and_arming_checks.h
 		include/px4_sdk/components/mode.h
 		include/px4_sdk/components/mode_executor.h
+		include/px4_sdk/components/overrides.h
 		include/px4_sdk/components/setpoints.h
 		include/px4_sdk/components/wait_for_fmu.h
 	)
@@ -30,6 +31,7 @@ add_library(px4_sdk_lib
 		src/components/health_and_arming_checks.cpp
 		src/components/mode.cpp
 		src/components/mode_executor.cpp
+		src/components/overrides.cpp
 		src/components/registration.cpp
 		src/components/setpoints.cpp
 		src/components/wait_for_fmu.cpp
@@ -86,6 +88,12 @@ if(BUILD_TESTING)
 		target_include_directories(test_mode_executor PRIVATE include)
 	endif()
 
+	ament_add_gtest(test_overrides test/integration/overrides.cpp)
+	if(TARGET test_overrides)
+		target_link_libraries(test_overrides ${PROJECT_NAME} integration_utils)
+		target_include_directories(test_overrides PRIVATE include)
+	endif()
+
 endif()
 
 ament_package()

src/ros2/cpp/px4_sdk_lib/include/px4_sdk/components/mode.h

@@ -40,6 +40,7 @@
 #include <px4_msgs/msg/mode_completed.hpp>
 #include "health_and_arming_checks.h"
 #include "setpoints.h"
+#include "overrides.h"
 
 class Registration;
 struct RegistrationSettings;
@@ -153,10 +154,13 @@ public:
 
 	SetpointSender &setpoints() { return _setpoint_sender; }
 
+	ConfigOverrides &configOverrides() { return _config_overrides; }
+
 private:
 	friend class ModeExecutorBase;
 	void overrideRegistration(const std::shared_ptr<Registration> &registration);
 	RegistrationSettings getRegistrationSettings() const;
+	void onRegistered();
 
 	void unsubscribeVehicleStatus();
 	void vehicleStatusUpdated(const px4_msgs::msg::VehicleStatus::UniquePtr &msg, bool do_not_activate = false);
@@ -183,6 +187,8 @@ private:
 	float _setpoint_update_rate_hz{0.f};
 	rclcpp::TimerBase::SharedPtr _setpoint_update_timer;
 	SetpointSender _setpoint_sender;
+
+	ConfigOverrides _config_overrides;
 };
 
 } /* namespace px4_sdk */

src/ros2/cpp/px4_sdk_lib/include/px4_sdk/components/mode_executor.h

@@ -34,6 +34,7 @@
 #pragma once
 
 #include "mode.h"
+#include "overrides.h"
 
 #include <rclcpp/rclcpp.hpp>
 #include <px4_msgs/msg/vehicle_status.hpp>
@@ -84,6 +85,11 @@ public:
 	 */
 	virtual void onDeactivate(DeactivateReason reason) = 0;
 
+	/**
+	 * Called when failsafes are currently being deferred (see @deferFailsafes), and the FMU wants to trigger
+	 * a failsafe.
+	 */
+	virtual void onFailsafeDeferred() {}
 
 	/**
 	* Send command and wait for ack/nack
@@ -116,6 +122,22 @@ public:
 
 	rclcpp::Node &node() { return _node; }
 
+	ConfigOverrides &configOverrides() { return _config_overrides; }
+
+	/**
+	 * Enable/disable deferring failsafes. While enabled (and the executor is in charge),
+	 * most failsafes are prevented from being triggered until the given timeout is exceeded.
+	 * Some failsafes that cannot be prevented:
+	 * - vehicle exceeds attitude limits (can be disabled via parameters)
+	 * - the mode cannot run (some mode requirements are not met, such as no position estimate)
+	 *
+	 * If the executor is in charge, this method will wait for the FMU for acknowledgement (to avoid race conditions)
+	 * @param enabled
+	 * @param timeout_s 0=system default, -1=no timeout
+	 * @return true on success
+	 */
+	bool deferFailsafesSync(bool enabled, int timeout_s = 0);
+
 private:
 
 	class ScheduledMode
@@ -151,6 +173,8 @@ private:
 		RunCheckCallback _run_check_callback;
 	};
 
+	void onRegistered();
+
 	void callOnActivate();
 	void callOnDeactivate(DeactivateReason reason);
 
@@ -176,6 +200,9 @@ private:
 	bool _is_armed{false};
 	bool _was_never_activated{true};
 	ModeBase::ID_t _prev_nav_state{ModeBase::ID_INVALID};
+	uint8_t _prev_failsafe_defer_state{px4_msgs::msg::VehicleStatus::FAILSAFE_DEFER_STATE_DISABLED};
+
+	ConfigOverrides _config_overrides;
 };
 
 } /* namespace px4_sdk */

src/ros2/cpp/px4_sdk_lib/include/px4_sdk/components/overrides.h

@@ -0,0 +1,70 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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 <rclcpp/rclcpp.hpp>
+
+#include <px4_msgs/msg/config_overrides.hpp>
+
+namespace px4_sdk
+{
+
+class ModeBase;
+class ModeExecutorBase;
+
+class ConfigOverrides
+{
+public:
+	ConfigOverrides(rclcpp::Node &node, const std::string &topic_namespace_prefix = "");
+
+	void controlAutoDisarm(bool enabled);
+
+
+private:
+	void update();
+
+	friend class ModeBase;
+	friend class ModeExecutorBase;
+	void deferFailsafes(bool enabled, int timeout_s = 0);
+	void setup(uint8_t type, uint8_t id);
+
+	rclcpp::Node &_node;
+
+	px4_msgs::msg::ConfigOverrides _current_overrides{};
+	rclcpp::Publisher<px4_msgs::msg::ConfigOverrides>::SharedPtr _config_overrides_pub;
+	bool _is_setup{false};
+	bool _require_update_after_setup{false};
+};
+
+} /* namespace px4_sdk */

src/ros2/cpp/px4_sdk_lib/src/components/mode.cpp

@@ -45,7 +45,7 @@ ModeBase::ModeBase(rclcpp::Node &node, const ModeBase::Settings &settings,
 	: _node(node), _registration(std::make_shared<Registration>(node, topic_namespace_prefix)), _settings(settings),
 	  _health_and_arming_checks(node, std::bind(&ModeBase::checkArmingAndRunConditions, this, std::placeholders::_1),
 				    topic_namespace_prefix),
-	  _setpoint_sender(node, *this, topic_namespace_prefix)
+	  _setpoint_sender(node, *this, topic_namespace_prefix), _config_overrides(node, topic_namespace_prefix)
 {
 	_vehicle_status_sub = _node.create_subscription<px4_msgs::msg::VehicleStatus>(
 				      topic_namespace_prefix + "/fmu/out/vehicle_status", rclcpp::QoS(1).best_effort(),
@@ -77,7 +77,13 @@ bool ModeBase::doRegister()
 	assert(!_registration->registered());
 	_health_and_arming_checks.overrideRegistration(_registration);
 	RegistrationSettings settings = getRegistrationSettings();
-	return _registration->doRegister(settings);
+	bool ret = _registration->doRegister(settings);
+
+	if (ret) {
+		onRegistered();
+	}
+
+	return ret;
 }
 
 RegistrationSettings ModeBase::getRegistrationSettings() const
@@ -178,3 +184,8 @@ void ModeBase::completed(Result result)
 	_mode_completed_pub->publish(mode_completed);
 	_completed = true;
 }
+
+void ModeBase::onRegistered()
+{
+	_config_overrides.setup(px4_msgs::msg::ConfigOverrides::SOURCE_TYPE_MODE, _registration->modeId());
+}

src/ros2/cpp/px4_sdk_lib/src/components/mode_executor.cpp

@@ -45,7 +45,8 @@ ModeExecutorBase::ModeExecutorBase(rclcpp::Node &node, const ModeExecutorBase::S
 				   ModeBase &owned_mode, const std::string &topic_namespace_prefix)
 	: _node(node), _settings(settings), _owned_mode(owned_mode),
 	  _registration(std::make_shared<Registration>(node, topic_namespace_prefix)),
-	  _current_scheduled_mode(node, topic_namespace_prefix)
+	  _current_scheduled_mode(node, topic_namespace_prefix),
+	  _config_overrides(node, topic_namespace_prefix)
 {
 	_vehicle_status_sub = _node.create_subscription<px4_msgs::msg::VehicleStatus>(
 				      topic_namespace_prefix + "/fmu/out/vehicle_status", rclcpp::QoS(1).best_effort(),
@@ -75,7 +76,19 @@ bool ModeExecutorBase::doRegister()
 	RegistrationSettings settings = _owned_mode.getRegistrationSettings();
 	settings.register_mode_executor = true;
 	settings.activate_mode_immediately = _settings.activate_immediately;
-	return _registration->doRegister(settings);
+	bool ret = _registration->doRegister(settings);
+
+	if (ret) {
+		_owned_mode.onRegistered();
+		onRegistered();
+	}
+
+	return ret;
+}
+
+void ModeExecutorBase::onRegistered()
+{
+	_config_overrides.setup(px4_msgs::msg::ConfigOverrides::SOURCE_TYPE_MODE_EXECUTOR, _registration->modeExecutorId());
 }
 
 void ModeExecutorBase::callOnActivate()
@@ -297,6 +310,15 @@ void ModeExecutorBase::vehicleStatusUpdated(const px4_msgs::msg::VehicleStatus::
 		callOnActivate();
 	}
 
+
+	if (_is_in_charge && _prev_failsafe_defer_state != msg->failsafe_defer_state
+	    && msg->failsafe_defer_state == px4_msgs::msg::VehicleStatus::FAILSAFE_DEFER_STATE_WOULD_FAILSAFE) {
+		// FMU wants to failsafe, but got deferred -> notify the executor
+		onFailsafeDeferred();
+	}
+
+	_prev_failsafe_defer_state = msg->failsafe_defer_state;
+
 	// Do not activate the mode if we're scheduling another mode. This is only expected to happen for a brief moment,
 	// e.g. when the executor gets activated or right after arming. It thus prevents unnecessary mode activation toggling.
 	bool do_not_activate_mode = (_current_scheduled_mode.active() && _owned_mode.id() != _current_scheduled_mode.modeId())
@@ -309,6 +331,55 @@ void ModeExecutorBase::vehicleStatusUpdated(const px4_msgs::msg::VehicleStatus::
 	_current_wait_vehicle_status.update(msg);
 }
 
+bool ModeExecutorBase::deferFailsafesSync(bool enabled, int timeout_s)
+{
+	_config_overrides.deferFailsafes(enabled, timeout_s);
+
+	// To avoid race conditions we wait until the FMU sets it if the executor is in charge
+	if (enabled && _is_in_charge && _registration->registered()
+	    && _prev_failsafe_defer_state == px4_msgs::msg::VehicleStatus::FAILSAFE_DEFER_STATE_DISABLED) {
+		rclcpp::WaitSet wait_set;
+		wait_set.add_subscription(_vehicle_status_sub);
+
+		bool got_message = false;
+		auto start_time = _node.now();
+		const rclcpp::Duration timeout = 1s;
+
+		while (!got_message) {
+			auto now = _node.now();
+
+			if (now >= start_time + timeout) {
+				break;
+			}
+
+			auto wait_ret = wait_set.wait((timeout - (now - start_time)).to_chrono<std::chrono::microseconds>());
+
+			if (wait_ret.kind() == rclcpp::WaitResultKind::Ready) {
+				px4_msgs::msg::VehicleStatus msg;
+				rclcpp::MessageInfo info;
+
+				if (_vehicle_status_sub->take(msg, info)) {
+					if (msg.failsafe_defer_state != px4_msgs::msg::VehicleStatus::FAILSAFE_DEFER_STATE_DISABLED) {
+						got_message = true;
+					}
+
+				} else {
+					RCLCPP_DEBUG(_node.get_logger(), "no message received");
+				}
+
+			} else {
+				RCLCPP_DEBUG(_node.get_logger(), "timeout");
+			}
+		}
+
+		wait_set.remove_subscription(_vehicle_status_sub);
+
+		return got_message;
+	}
+
+	return true;
+}
+
 ModeExecutorBase::ScheduledMode::ScheduledMode(rclcpp::Node &node, const std::string &topic_namespace_prefix)
 {
 	_mode_completed_sub = node.create_subscription<px4_msgs::msg::ModeCompleted>(

src/ros2/cpp/px4_sdk_lib/src/components/overrides.cpp

@@ -0,0 +1,82 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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 "px4_sdk/components/overrides.h"
+
+#include <cassert>
+
+using namespace px4_sdk;
+
+ConfigOverrides::ConfigOverrides(rclcpp::Node &node, const std::string &topic_namespace_prefix)
+	: _node(node)
+{
+	_config_overrides_pub = _node.create_publisher<px4_msgs::msg::ConfigOverrides>(
+					topic_namespace_prefix + "/fmu/in/config_overrides_request", 1);
+}
+
+void ConfigOverrides::controlAutoDisarm(bool enabled)
+{
+	_current_overrides.disable_auto_disarm = !enabled;
+	update();
+}
+
+void ConfigOverrides::deferFailsafes(bool enabled, int timeout_s)
+{
+	_current_overrides.defer_failsafes = enabled;
+	_current_overrides.defer_failsafes_timeout_s = timeout_s;
+	update();
+}
+
+void ConfigOverrides::update()
+{
+	if (_is_setup) {
+		_current_overrides.timestamp = _node.get_clock()->now().nanoseconds() / 1000;
+		_config_overrides_pub->publish(_current_overrides);
+
+	} else {
+		_require_update_after_setup = true;
+	}
+}
+
+void ConfigOverrides::setup(uint8_t type, uint8_t id)
+{
+	assert(!_is_setup);
+	_current_overrides.source_type = type;
+	_current_overrides.source_id = id;
+	_is_setup = true;
+
+	if (_require_update_after_setup) {
+		update();
+		_require_update_after_setup = false;
+	}
+}

src/ros2/cpp/px4_sdk_lib/test/integration/overrides.cpp

@@ -0,0 +1,312 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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 <gtest/gtest.h>
+
+#include "util.h"
+
+#include <px4_sdk/components/mode_executor.h>
+#include <px4_sdk/components/wait_for_fmu.h>
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <Eigen/Core>
+
+using namespace px4_sdk;
+using namespace std;
+using namespace std::chrono_literals;
+
+static const char *name = "Test Flight Mode";
+
+
+class Tester : public ::testing::Test
+{
+public:
+	static void SetUpTestSuite()
+	{
+		rclcpp::init(0, nullptr);
+	}
+	static void TearDownTestSuite()
+	{
+		rclcpp::shutdown();
+	}
+};
+
+class FlightModeTest : public ModeBase
+{
+public:
+	FlightModeTest(rclcpp::Node &node)
+		: ModeBase(node, Settings{name, false}, ModeRequirements::autonomous())
+	{
+		setSetpointUpdateRate(30.f);
+	}
+
+	virtual ~FlightModeTest() = default;
+
+	void onActivate() override
+	{
+		_activation_time = node().get_clock()->now();
+		setpoints().configureSetpointsSync(SetpointSender::SetpointConfiguration{});
+		++num_activations;
+	}
+
+	void checkArmingAndRunConditions(HealthAndArmingCheckReporter &reporter) override
+	{
+		++num_arming_check_updates;
+	}
+
+	void onDeactivate() override
+	{
+		++num_deactivations;
+	}
+
+	void updateSetpoint() override
+	{
+		++num_setpoint_updates;
+		rclcpp::Time now = node().get_clock()->now();
+
+		if (now - _activation_time > rclcpp::Duration::from_seconds(8)) {
+			completed(Result::Success);
+			return;
+		}
+
+		// Send some random setpoints
+		float elapsed_s = (now - _activation_time).seconds();
+		Eigen::Vector3f velocity{5.f, elapsed_s, 0.f };
+		setpoints().sendTrajectorySetpoint(velocity);
+	}
+
+	int num_activations{0};
+	int num_deactivations{0};
+	int num_setpoint_updates{0};
+	int num_arming_check_updates{0};
+private:
+	rclcpp::Time _activation_time{};
+};
+
+class ModeExecutorTest : public ModeExecutorBase
+{
+public:
+	ModeExecutorTest(rclcpp::Node &node, FlightModeTest &owned_mode, bool activate_immediately)
+		: ModeExecutorBase(node, ModeExecutorBase::Settings{activate_immediately}, owned_mode),
+		  _node(node)
+	{}
+
+	enum class State {
+		Reset = 0,
+		WaitForArming = 1,
+		Arming = 2,
+		TakingOff = 3,
+		Landing = 4,
+		Wait = 5,
+		TakingOff2 = 6,
+		WaitForFailsafe = 7,
+		WaitForDisarming = 8,
+	};
+
+	void onActivate() override
+	{
+		++num_activations;
+		runState(State::WaitForArming, Result::Success);
+	}
+
+	void onDeactivate(DeactivateReason reason) override
+	{
+		EXPECT_EQ(_state, State::WaitForDisarming);
+		++num_deactivations;
+	}
+
+	void onFailsafeDeferred() override
+	{
+		++num_failsafe_deferred;
+		EXPECT_EQ(_state, State::WaitForFailsafe);
+		// Re-enable failsafes -> failsafe will trigger and executor deactivated
+		deferFailsafesSync(false);
+		runState(State::WaitForDisarming, Result::Success);
+	}
+
+	void runState(State state, Result previous_result)
+	{
+		on_state_completed(state, previous_result);
+
+		if (previous_result != Result::Success) {
+			RCLCPP_ERROR(_node.get_logger(), "State %i: previous state failed: %s", (int)state, resultToString(previous_result));
+			return;
+		}
+
+		RCLCPP_DEBUG(_node.get_logger(), "Executing state %i", (int)state);
+
+		_state = state;
+
+		switch (state) {
+		case State::Reset:
+			break;
+
+		case State::WaitForArming:
+			waitReadyToArm([this](Result result) { runState(State::Arming, result); });
+			break;
+
+		case State::Arming:
+			arm([this](Result result) { runState(State::TakingOff, result); });
+			break;
+
+		case State::TakingOff:
+			takeoff([this](Result result) { runState(State::Landing, result); });
+			break;
+
+		case State::Landing:
+			land([this](Result result) { runState(State::Wait, result); });
+			break;
+
+		case State::Wait:
+			_wait_timer = _node.create_wall_timer(10s, [this] {
+				_wait_timer.reset();
+				EXPECT_TRUE(isArmed());
+				runState(State::TakingOff2, Result::Success);
+			});
+			break;
+
+		case State::TakingOff2:
+			takeoff([this](Result result) {
+				EXPECT_TRUE(deferFailsafesSync(true, 60));
+				runState(State::WaitForFailsafe, result);
+			});
+			break;
+
+		case State::WaitForFailsafe:
+			// Nothing to do
+			break;
+
+		case State::WaitForDisarming:
+			// Nothing to do
+			break;
+
+		}
+	}
+
+	State getState() const { return _state; }
+
+	int num_activations{0};
+	int num_deactivations{0};
+	int num_failsafe_deferred{0};
+
+	std::function<void()> on_completed;
+	std::function<void(State, Result)> on_state_completed;
+
+private:
+	State _state{};
+	rclcpp::Node &_node;
+	rclcpp::TimerBase::SharedPtr _wait_timer;
+};
+
+
+class TestExecutionOverrides
+{
+public:
+	TestExecutionOverrides(rclcpp::Node &node)
+		: _node(node), _vehicle_state(node) {}
+
+	void run();
+private:
+	rclcpp::Node &_node;
+	rclcpp::TimerBase::SharedPtr _test_timeout;
+
+	std::unique_ptr<FlightModeTest> _mode;
+	std::unique_ptr<ModeExecutorTest> _mode_executor;
+	VehicleState _vehicle_state;
+	bool _was_armed{false};
+};
+
+void TestExecutionOverrides::run()
+{
+	_test_timeout = _node.create_wall_timer(80s, [] {
+		EXPECT_TRUE(false); // Timed out
+		rclcpp::shutdown();
+	});
+
+	_mode = std::make_unique<FlightModeTest>(_node);
+	_mode_executor = std::make_unique<ModeExecutorTest>(_node, *_mode.get(), true);
+
+
+	// Testing steps:
+	// - disable auto-disarm
+	// - register & activate executor
+	// - takeoff
+	// - land & ensure no auto-disarm triggered
+	// - takeoff
+	// - defer failsafes & trigger failsafe
+	// - wait for callback & enable failsafes
+	// - wait for failsafe triggering, landing and disarming
+
+	_mode_executor->on_state_completed = [this](ModeExecutorTest::State next_state, Result result) {
+		if (next_state == ModeExecutorTest::State::WaitForFailsafe) {
+			EXPECT_EQ(result, Result::Success);
+			// trigger failsafe (one that can be deferred)
+			_vehicle_state.setForceLowBattery(true);
+
+		} else {
+			EXPECT_EQ(result, Result::Success);
+		}
+	};
+
+	_vehicle_state.setOnVehicleStatusUpdate([this](const px4_msgs::msg::VehicleStatus::UniquePtr & msg) {
+		bool armed = msg->arming_state == px4_msgs::msg::VehicleStatus::ARMING_STATE_ARMED;
+
+		if (_was_armed && !armed) {
+			// Finish the test
+			_vehicle_state.setForceLowBattery(false);
+
+			EXPECT_EQ(_mode_executor->getState(), ModeExecutorTest::State::WaitForDisarming);
+			EXPECT_EQ(_mode_executor->num_activations, 1);
+			EXPECT_EQ(_mode_executor->num_deactivations, 1);
+			EXPECT_EQ(_mode_executor->num_failsafe_deferred, 1);
+
+			rclcpp::shutdown();
+		}
+
+		_was_armed = armed;
+	});
+
+	_mode_executor->configOverrides().controlAutoDisarm(false);
+	ASSERT_TRUE(_mode_executor->doRegister());
+}
+
+TEST_F(Tester, run_executor_overrides)
+{
+	auto test_node = initNode();
+	ASSERT_TRUE(waitForFMU(*test_node, 10s));
+	TestExecutionOverrides test_execution{*test_node};
+	test_execution.run();
+	rclcpp::spin(test_node);
+}
+