commander: add config overrides

msg/CMakeLists.txt

@@ -60,6 +60,7 @@ set(msg_files
 	CellularStatus.msg
 	CollisionConstraints.msg
 	CollisionReport.msg
+	ConfigOverrides.msg
 	ControlAllocatorStatus.msg
 	Cpuload.msg
 	DatamanRequest.msg

msg/ConfigOverrides.msg

@@ -0,0 +1,19 @@
+# Configurable overrides by (external) modes or mode executors
+uint64 timestamp		# time since system start (microseconds)
+
+bool disable_auto_disarm         # Prevent the drone from automatically disarming after landing (if configured)
+
+bool defer_failsafes             # Defer all failsafes that can be deferred (until the flag is cleared)
+int16 defer_failsafes_timeout_s  # Maximum time a failsafe can be deferred. 0 = system default, -1 = no timeout
+
+
+int8 SOURCE_TYPE_MODE = 0
+int8 SOURCE_TYPE_MODE_EXECUTOR = 1
+int8 source_type
+
+uint8 source_id                  # ID depending on source_type
+
+uint8 ORB_QUEUE_LENGTH = 4
+
+# TOPICS config_overrides config_overrides_request
+

msg/VehicleStatus.msg

@@ -88,8 +88,13 @@ uint8 VEHICLE_TYPE_FIXED_WING = 2
 uint8 VEHICLE_TYPE_ROVER = 3
 uint8 VEHICLE_TYPE_AIRSHIP = 4
 
+uint8 FAILSAFE_DEFER_STATE_DISABLED = 0
+uint8 FAILSAFE_DEFER_STATE_ENABLED = 1
+uint8 FAILSAFE_DEFER_STATE_WOULD_FAILSAFE = 2 # Failsafes deferred, but would trigger a failsafe
+
 bool failsafe # true if system is in failsafe state (e.g.:RTL, Hover, Terminate, ...)
 bool failsafe_and_user_took_over # true if system is in failsafe state but the user took over control
+uint8 failsafe_defer_state # one of FAILSAFE_DEFER_STATE_*
 
 # Link loss
 bool gcs_connection_lost              # datalink to GCS lost

src/modules/commander/Commander.cpp

@@ -2200,8 +2200,10 @@ void Commander::handleAutoDisarm()
 
 			const bool landed_amid_mission = (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION)
 							 && !_mission_result_sub.get().finished;
+			const bool auto_disarm_land_enabled = _param_com_disarm_land.get() > 0 && !landed_amid_mission
+							      && !_config_overrides.disable_auto_disarm;
 
-			if (_param_com_disarm_land.get() > 0 && _have_taken_off_since_arming && !landed_amid_mission) {
+			if (auto_disarm_land_enabled && _have_taken_off_since_arming) {
 				_auto_disarm_landed.set_hysteresis_time_from(false, _param_com_disarm_land.get() * 1_s);
 				_auto_disarm_landed.set_state_and_update(_vehicle_land_detected.landed, hrt_absolute_time());
 
@@ -2253,6 +2255,7 @@ bool Commander::handleModeIntentionAndFailsafe()
 {
 	const uint8_t prev_nav_state = _vehicle_status.nav_state;
 	const FailsafeBase::Action prev_failsafe_action = _failsafe.selectedAction();
+	const uint8_t prev_failsafe_defer_state = _vehicle_status.failsafe_defer_state;
 
 	FailsafeBase::State state{};
 	state.armed = isArmed();
@@ -2303,7 +2306,24 @@ bool Commander::handleModeIntentionAndFailsafe()
 		_vehicle_status.nav_state_timestamp = hrt_absolute_time();
 	}
 
-	return prev_nav_state != _vehicle_status.nav_state || prev_failsafe_action != _failsafe.selectedAction();
+	_mode_management.updateActiveConfigOverrides(_vehicle_status.nav_state, _config_overrides);
+
+	// Apply failsafe deferring & get the current state
+	_failsafe.deferFailsafes(_config_overrides.defer_failsafes, _config_overrides.defer_failsafes_timeout_s);
+
+	if (_failsafe.failsafeDeferred()) {
+		_vehicle_status.failsafe_defer_state = vehicle_status_s::FAILSAFE_DEFER_STATE_WOULD_FAILSAFE;
+
+	} else if (_failsafe.getDeferFailsafes()) {
+		_vehicle_status.failsafe_defer_state = vehicle_status_s::FAILSAFE_DEFER_STATE_ENABLED;
+
+	} else {
+		_vehicle_status.failsafe_defer_state = vehicle_status_s::FAILSAFE_DEFER_STATE_DISABLED;
+	}
+
+	return prev_nav_state != _vehicle_status.nav_state ||
+	       prev_failsafe_action != _failsafe.selectedAction() ||
+	       prev_failsafe_defer_state != _vehicle_status.failsafe_defer_state;
 }
 
 void Commander::checkAndInformReadyForTakeoff()

src/modules/commander/Commander.hpp

@@ -232,6 +232,7 @@ private:
 
 	const failsafe_flags_s &_failsafe_flags{_health_and_arming_checks.failsafeFlags()};
 	HomePosition 		_home_position{_failsafe_flags};
+	config_overrides_s   _config_overrides{};
 
 
 	Hysteresis _auto_disarm_landed{false};

src/modules/commander/ModeManagement.cpp

@@ -338,6 +338,7 @@ void ModeManagement::update(bool armed, uint8_t user_intended_nav_state, bool fa
 	}
 
 	update_request.control_setpoint_update = checkConfigControlSetpointUpdates();
+	checkConfigOverrides();
 }
 
 void ModeManagement::onUserIntendedNavStateChange(ModeChangeSource source, uint8_t user_intended_nav_state)
@@ -446,6 +447,44 @@ void ModeManagement::printStatus() const
 	_mode_executors.printStatus(modeExecutorInCharge());
 }
 
+void ModeManagement::updateActiveConfigOverrides(uint8_t nav_state, config_overrides_s &overrides_in_out)
+{
+	config_overrides_s current_overrides;
+
+	if (_modes.valid(nav_state)) {
+		current_overrides = _modes.mode(nav_state).overrides;
+
+	} else {
+		current_overrides = {};
+	}
+
+	// Apply the overrides from executors on top (executors take precedence)
+	const int executor_in_charge = modeExecutorInCharge();
+
+	if (_mode_executors.valid(executor_in_charge)) {
+		const config_overrides_s &executor_overrides = _mode_executors.executor(executor_in_charge).overrides;
+
+		if (executor_overrides.disable_auto_disarm) {
+			current_overrides.disable_auto_disarm = true;
+		}
+
+		if (executor_overrides.defer_failsafes) {
+			current_overrides.defer_failsafes = true;
+			current_overrides.defer_failsafes_timeout_s = executor_overrides.defer_failsafes_timeout_s;
+		}
+	}
+
+	// Publish if changed or at low rate
+	current_overrides.timestamp = overrides_in_out.timestamp;
+
+	if (memcmp(&overrides_in_out, &current_overrides, sizeof(current_overrides)) != 0
+	    || hrt_elapsed_time(&current_overrides.timestamp) > 500_ms) {
+		current_overrides.timestamp = hrt_absolute_time();
+		_config_overrides_pub.publish(current_overrides);
+		overrides_in_out = current_overrides;
+	}
+}
+
 bool ModeManagement::checkConfigControlSetpointUpdates()
 {
 	bool had_update = false;
@@ -468,4 +507,31 @@ bool ModeManagement::checkConfigControlSetpointUpdates()
 	return had_update;
 }
 
+void ModeManagement::checkConfigOverrides()
+{
+	config_overrides_s override_request;
+	int max_updates = config_overrides_s::ORB_QUEUE_LENGTH;
+
+	while (_config_overrides_request_sub.update(&override_request) && --max_updates >= 0) {
+		switch (override_request.source_type) {
+		case config_overrides_s::SOURCE_TYPE_MODE_EXECUTOR:
+			if (_mode_executors.valid(override_request.source_id)) {
+				ModeExecutors::ModeExecutor &executor = _mode_executors.executor(override_request.source_id);
+				memcpy(&executor.overrides, &override_request, sizeof(executor.overrides));
+				static_assert(sizeof(executor.overrides) == sizeof(override_request), "size mismatch");
+			}
+
+			break;
+
+		case config_overrides_s::SOURCE_TYPE_MODE:
+			if (_modes.valid(override_request.source_id)) {
+				Modes::Mode &mode = _modes.mode(override_request.source_id);
+				memcpy(&mode.overrides, &override_request, sizeof(mode.overrides));
+			}
+
+			break;
+		}
+	}
+}
+
 #endif /* CONSTRAINED_FLASH */

src/modules/commander/ModeManagement.hpp

@@ -40,6 +40,7 @@
 #include <uORB/topics/unregister_ext_component.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/config_overrides.h>
 
 #include "ModeUtil/ui.hpp"
 #include "UserModeIntention.hpp"
@@ -53,6 +54,7 @@ public:
 	static constexpr int MAX_NUM = 5;
 
 	struct ModeExecutor {
+		config_overrides_s overrides{};
 		uint8_t owned_nav_state{};
 		bool valid{false};
 	};
@@ -103,6 +105,7 @@ public:
 		bool unresponsive_reported{false};
 		int arming_check_registration_id{-1};
 		int mode_executor_registration_id{-1};
+		config_overrides_s overrides{};
 		vehicle_control_mode_s config_control_setpoint{};
 	};
 
@@ -153,10 +156,13 @@ public:
 	void printStatus() const;
 
 
+	void updateActiveConfigOverrides(uint8_t nav_state, config_overrides_s &overrides_in_out);
+
 private:
 	bool checkConfigControlSetpointUpdates();
 	void checkNewRegistrations(UpdateRequest &update_request);
 	void checkUnregistrations(uint8_t user_intended_nav_state, UpdateRequest &update_request);
+	void checkConfigOverrides();
 
 	void removeModeExecutor(int mode_executor_id);
 
@@ -164,6 +170,8 @@ private:
 	uORB::Subscription _register_ext_component_request_sub{ORB_ID(register_ext_component_request)};
 	uORB::Subscription _unregister_ext_component_sub{ORB_ID(unregister_ext_component)};
 	uORB::Publication<register_ext_component_reply_s> _register_ext_component_reply_pub{ORB_ID(register_ext_component_reply)};
+	uORB::Publication<config_overrides_s> _config_overrides_pub{ORB_ID(config_overrides)};
+	uORB::Subscription _config_overrides_request_sub{ORB_ID(config_overrides_request)};
 
 	ExternalChecks &_external_checks;
 	ModeExecutors _mode_executors;

src/modules/logger/logged_topics.cpp

@@ -55,6 +55,7 @@ void LoggedTopics::add_default_topics()
 	add_optional_topic("camera_trigger");
 	add_topic("cellular_status", 200);
 	add_topic("commander_state");
+	add_topic("config_overrides");
 	add_topic("cpuload");
 	add_optional_topic("external_ins_attitude");
 	add_optional_topic("external_ins_global_position");

src/modules/uxrce_dds_client/dds_topics.yaml

@@ -69,6 +69,9 @@ subscriptions:
   - topic: /fmu/in/unregister_ext_component
     type: px4_msgs::msg::UnregisterExtComponent
 
+  - topic: /fmu/in/config_overrides_request
+    type: px4_msgs::msg::ConfigOverrides
+
   - topic: /fmu/in/arming_check_reply
     type: px4_msgs::msg::ArmingCheckReply