safety: allow for level logic in safety switch

boards/nxp/tropic-community/nuttx-config/scripts/itcm_functions_includes.ld

@@ -561,7 +561,7 @@
 *(.text.uart_xmitchars_done)
 *(.text._ZN4EKF225PublishYawEstimatorStatusERKy)
 *(.text.sin)
-*(.text._ZN6Safety19safetyButtonHandlerEv)
+*(.text._ZN6Safety19safetySwitchHandlerEv)
 *(.text._ZN3Ekf19controlAuxVelFusionERKN9estimator9imuSampleE)
 *(.text._ZNK6matrix6MatrixIfLj2ELj1EEplERKS1_)
 *(.text._ZThn24_N7Sensors3RunEv)

boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld

@@ -573,7 +573,7 @@
 *(.text.uart_xmitchars_done)
 *(.text._ZN4EKF225PublishYawEstimatorStatusERKy)
 *(.text.sin)
-*(.text._ZN6Safety19safetyButtonHandlerEv)
+*(.text._ZN6Safety19safetySwitchHandlerEv)
 *(.text._ZN3Ekf19controlAuxVelFusionERKN9estimator9imuSampleE)
 *(.text._ZNK6matrix6MatrixIfLj2ELj1EEplERKS1_)
 *(.text._ZThn24_N7Sensors3RunEv)

msg/CMakeLists.txt

@@ -186,6 +186,7 @@ set(msg_files
 	Rpm.msg
 	RtlStatus.msg
 	RtlTimeEstimate.msg
+	SafetySwitch.msg
 	SatelliteInfo.msg
 	SensorAccel.msg
 	SensorAccelFifo.msg

msg/SafetySwitch.msg

@@ -0,0 +1,2 @@
+uint64 timestamp		# time since system start (microseconds)
+bool state			# Represents the state of the safety switch (on/off)

src/drivers/px4io/px4io.cpp

@@ -70,6 +70,7 @@
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/input_rc.h>
+#include <uORB/topics/safety_switch.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_command_ack.h>
 #include <uORB/topics/vehicle_status.h>
@@ -211,9 +212,11 @@ private:
 	/* advertised topics */
 	uORB::PublicationMulti<input_rc_s>	_input_rc_pub{ORB_ID(input_rc)};
 	uORB::Publication<px4io_status_s>	_px4io_status_pub{ORB_ID(px4io_status)};
+	uORB::Publication<safety_switch_s>	_safety_switch_pub{ORB_ID(safety_switch)};
 
 	ButtonPublisher	_button_publisher;
 	bool _previous_safety_off{false};
+	bool _previous_safety_switch_state{false};
 
 	bool			_lockdown_override{false};	///< override the safety lockdown
 
@@ -863,6 +866,20 @@ int PX4IO::io_handle_status(uint16_t status)
 		_button_publisher.safetyButtonTriggerEvent();
 	}
 
+	/**
+	 * Get and handle the safety switch status
+	 */
+	const bool safety_switch_state = status & PX4IO_P_STATUS_FLAGS_SAFETY_SWITCH_STATE;
+
+	if (safety_switch_state != _previous_safety_switch_state) {
+		safety_switch_s safety_switch{};
+		safety_switch.timestamp = hrt_absolute_time();
+		safety_switch.state = safety_switch_state;
+		_safety_switch_pub.publish(safety_switch);
+
+		_previous_safety_switch_state = safety_switch_state;
+	}
+
 	/**
 	 * Inform PX4IO board about safety_off state for LED control
 	 */

src/drivers/safety_button/SafetyButton.cpp

@@ -63,10 +63,10 @@ SafetyButton::~SafetyButton()
 }
 
 void
-SafetyButton::CheckSafetyRequest(bool button_pressed)
+SafetyButton::CheckSafetyRequest(bool safety_switch_state)
 {
 	/* Keep button pressed for one second to turn off safety */
-	if (button_pressed) {
+	if (safety_switch_state) {
 
 		if (_button_counter <= CYCLE_COUNT) {
 			_button_counter++;
@@ -79,10 +79,18 @@ SafetyButton::CheckSafetyRequest(bool button_pressed)
 	} else {
 		_button_counter = 0;
 	}
+
+	/* Collect the raw safety switch state */
+	if (safety_switch_state != _previous_safety_switch_state) {
+		safety_switch_s safety_switch{};
+		safety_switch.timestamp = hrt_absolute_time();
+		safety_switch.state = safety_switch_state;
+		_safety_switch_pub.publish(safety_switch);
+	}
 }
 
 void
-SafetyButton::CheckPairingRequest(bool button_pressed)
+SafetyButton::CheckPairingRequest(bool safety_switch_state)
 {
 	// Need to press the button 3 times within 2 seconds
 	const hrt_abstime now = hrt_absolute_time();
@@ -93,7 +101,7 @@ SafetyButton::CheckPairingRequest(bool button_pressed)
 		_pairing_button_counter = 0;
 	}
 
-	if (!_button_prev_sate && button_pressed) {
+	if (!_previous_safety_switch_state && safety_switch_state) {
 		if (_pairing_start == 0) {
 			_pairing_start = now;
 		}
@@ -120,7 +128,7 @@ SafetyButton::FlashButton()
 		LED_PATTERN pattern = LED_PATTERN::FMU_REFUSE_TO_ARM;
 
 		// cycle the blink state machine
-		if (_button_prev_sate) {
+		if (_previous_safety_switch_state) {
 			if (armed.armed) {
 				pattern = LED_PATTERN::IO_FMU_ARMED;
 
@@ -154,16 +162,16 @@ SafetyButton::Run()
 		return;
 	}
 
-	const bool button_pressed = px4_arch_gpioread(GPIO_BTN_SAFETY);
+	const bool safety_switch_state = px4_arch_gpioread(GPIO_BTN_SAFETY);
 
 	// control safety switch LED & safety button
 	if (!_has_px4io) {
 		FlashButton();
-		CheckSafetyRequest(button_pressed);
+		CheckSafetyRequest(safety_switch_state);
 	}
 
-	CheckPairingRequest(button_pressed);
-	_button_prev_sate = button_pressed;
+	CheckPairingRequest(safety_switch_state);
+	_previous_safety_switch_state = safety_switch_state;
 }
 
 int

src/drivers/safety_button/SafetyButton.hpp

@@ -40,6 +40,10 @@
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
 #include <button/ButtonPublisher.hpp>
 
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/safety_switch.h>
+
 class SafetyButton : public ModuleBase<SafetyButton>, public px4::ScheduledWorkItem
 {
 public:
@@ -60,20 +64,20 @@ public:
 private:
 	void Run() override;
 
-	void CheckSafetyRequest(bool button_pressed);
-	void CheckPairingRequest(bool button_pressed);
+	void CheckSafetyRequest(bool safety_switch_state);
+	void CheckPairingRequest(bool safety_switch_state);
 	void FlashButton();
 
 	bool			_has_px4io{false};
-	ButtonPublisher	_button_publisher;
+	ButtonPublisher		_button_publisher;
 	uint8_t			_button_counter{0};
 	uint8_t			_blink_counter{0};
-	bool			_button_prev_sate{false};	///< Previous state of the HW button
+	bool			_previous_safety_switch_state{false}; ///< Previous state of the input
 
 	// Pairing request
 	hrt_abstime		_pairing_start{0};
-	int				_pairing_button_counter{0};
-
-	uORB::Subscription	_armed_sub{ORB_ID(actuator_armed)};
+	int			_pairing_button_counter{0};
 
+	uORB::Subscription _armed_sub{ORB_ID(actuator_armed)};
+	uORB::Publication<safety_switch_s> _safety_switch_pub{ORB_ID(safety_switch)};
 };

src/modules/commander/Commander.cpp

@@ -1344,7 +1344,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 					if (check_battery_disconnected(&_mavlink_log_pub)) {
 						answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 
-						if (_safety.isButtonAvailable() && !_safety.isSafetyOff()) {
+						if (_safety.isSafetySwitchAvailable() && !_safety.isSafetyOff()) {
 							mavlink_log_critical(&_mavlink_log_pub, "ESC calibration denied! Press safety button first\t");
 							events::send(events::ID("commander_esc_calibration_denied"), events::Log::Critical,
 								     "ESCs calibration denied");
@@ -1574,7 +1574,7 @@ void Commander::handleCommandsFromModeExecutors()
 
 unsigned Commander::handleCommandActuatorTest(const vehicle_command_s &cmd)
 {
-	if (isArmed() || (_safety.isButtonAvailable() && !_safety.isSafetyOff())) {
+	if (isArmed() || (_safety.isSafetySwitchAvailable() && !_safety.isSafetyOff())) {
 		return vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
 	}
 
@@ -2045,7 +2045,7 @@ bool Commander::getPrearmState() const
 		return hrt_elapsed_time(&_boot_timestamp) > 5_s;
 
 	case PrearmedMode::SAFETY_BUTTON:
-		if (_safety.isButtonAvailable()) {
+		if (_safety.isSafetySwitchAvailable()) {
 			/* safety button is present, go into prearmed if safety is off */
 			return _safety.isSafetyOff();
 		}
@@ -2135,8 +2135,8 @@ void Commander::landDetectorUpdate()
 
 void Commander::safetyButtonUpdate()
 {
-	const bool safety_changed = _safety.safetyButtonHandler();
-	_vehicle_status.safety_button_available = _safety.isButtonAvailable();
+	const bool safety_changed = _safety.safetySwitchHandler();
+	_vehicle_status.safety_button_available = _safety.isSafetySwitchAvailable();
 	_vehicle_status.safety_off = _safety.isSafetyOff();
 
 	if (safety_changed) {

src/modules/commander/Safety.cpp

@@ -42,26 +42,28 @@ using namespace time_literals;
 
 Safety::Safety()
 {
+	param_get(param_find("COM_SAFETY_MODE"), reinterpret_cast<int32_t *>(&_safety_switch_mode));
 	// Safety can be turned off with the CBRK_IO_SAFETY parameter.
 	_safety_disabled = circuit_breaker_enabled("CBRK_IO_SAFETY", CBRK_IO_SAFETY_KEY);
 
 	if (_safety_disabled) {
-		_button_available = true;
+		_safety_switch_available = true;
 		_safety_off = true;
 	}
 }
 
-bool Safety::safetyButtonHandler()
+bool Safety::safetySwitchHandler()
 {
 	if (!_safety_disabled) {
-		if (!_button_available && _safety_button_sub.advertised()) {
-			_button_available = true;
-		}
+		switch (_safety_switch_mode) {
+		case SafetyMode::SAFETY_BUTTON:
+			handleModeButton();
+			break;
 
-		button_event_s button_event;
-
-		while (_safety_button_sub.update(&button_event)) {
-			_safety_off |= button_event.triggered; // triggered safety button activates safety off
+		case SafetyMode::LEVEL_HIGH:
+		case SafetyMode::LEVEL_LOW:
+			handleModeLevel();
+			break;
 		}
 	}
 
@@ -70,6 +72,37 @@ bool Safety::safetyButtonHandler()
 	return safety_changed;
 }
 
+void Safety::handleModeButton()
+{
+	if (!_safety_switch_available) {
+		_safety_switch_available = true;
+	}
+
+	button_event_s button_event;
+
+	while (_safety_button_sub.update(&button_event)) {
+		_safety_off |= button_event.triggered; // triggered safety button activates safety off
+	}
+}
+
+void Safety::handleModeLevel()
+{
+	if (!_safety_switch_available) {
+		_safety_switch_available = true;
+	}
+
+	safety_switch_s safety_switch;
+
+	if (_safety_switch_sub.update(&safety_switch)) {
+		if (_safety_switch_mode == SafetyMode::LEVEL_HIGH) {
+			_safety_off = safety_switch.state;
+
+		} else {
+			_safety_off = !safety_switch.state;
+		}
+	}
+}
+
 void Safety::activateSafety()
 {
 	if (!_safety_disabled) {

src/modules/commander/Safety.hpp

@@ -37,9 +37,12 @@
 
 #pragma once
 
+#include <lib/parameters/param.h>
+#include <px4_platform_common/defines.h>
 #include <uORB/SubscriptionMultiArray.hpp>
 #include <uORB/Publication.hpp>
 #include <uORB/topics/button_event.h>
+#include <uORB/topics/safety_switch.h>
 
 class Safety
 {
@@ -47,17 +50,28 @@ public:
 	Safety();
 	~Safety() = default;
 
-	bool safetyButtonHandler();
+	bool safetySwitchHandler();
 	void activateSafety();
-	bool isButtonAvailable() const { return _button_available; }
+	bool isSafetySwitchAvailable() const { return _safety_switch_available; }
 	bool isSafetyOff() const { return _safety_off; }
 	bool isSafetyDisabled() const { return _safety_disabled; }
 
 private:
-	uORB::Subscription _safety_button_sub{ORB_ID::safety_button};
+	enum class SafetyMode : int32_t {
+		SAFETY_BUTTON = 0,
+		LEVEL_HIGH = 1,
+		LEVEL_LOW = 2
+	};
 
-	bool _button_available{false};///< Set to true if a safety button is connected
+	void handleModeButton();
+	void handleModeLevel();
+
+	uORB::Subscription _safety_button_sub{ORB_ID::safety_button};
+	uORB::Subscription _safety_switch_sub{ORB_ID::safety_switch};
+
+	bool _safety_switch_available{false};///< Set to true if a safety switch is connected
 	bool _safety_off{false}; ///< Set to true if safety is off
 	bool _previous_safety_off{false}; ///< Previous safety value
 	bool _safety_disabled{false}; ///< Set to true if safety is disabled
+	SafetyMode _safety_switch_mode{SafetyMode::SAFETY_BUTTON}; //< Value of the safety switch mode parameter
 };

src/modules/commander/commander_params.c

@@ -719,6 +719,27 @@ PARAM_DEFINE_INT32(COM_PREARM_MODE, 0);
  */
 PARAM_DEFINE_INT32(COM_FORCE_SAFETY, 0);
 
+/**
+ * Safety switch handling
+ *
+ * Defines how the safety switch behaves and what signaling is required to disable safety.
+ *
+ * - **Safety button mode**: Requires a HIGH signal for 1 second to disable safety.
+ *   Once disabled, safety will not be re-engaged automatically.
+ * - **Level triggered (LEVEL HIGH)**: Disables safety while the signal is HIGH.
+ *   When the signal goes LOW, safety is re-engaged.
+ * - **Level triggered (LEVEL LOW)**: Disables safety while the signal is LOW.
+ *   When the signal goes HIGH, safety is re-engaged.
+ *
+ * @value 0 Safety button
+ * @value 1 Level triggered (Level HIGH)
+ * @value 2 Level triggered (Level LOW)
+ *
+ * @group Commander
+ * @reboot_required true
+ */
+PARAM_DEFINE_INT32(COM_SAFETY_MODE, 0);
+
 /**
  * Enable Actuator Testing
  *

src/modules/px4iofirmware/protocol.h

@@ -115,6 +115,7 @@
 #define PX4IO_P_STATUS_FLAGS_RC_ST24		(1 << 12) /* ST24 input is valid */
 #define PX4IO_P_STATUS_FLAGS_RC_SUMD		(1 << 13) /* SUMD input is valid */
 #define PX4IO_P_STATUS_FLAGS_SAFETY_BUTTON_EVENT	(1 << 14) /* px4io safety button was pressed for longer than 1 second */
+#define PX4IO_P_STATUS_FLAGS_SAFETY_SWITCH_STATE	(1 << 15) /* px4io safety switch state */
 
 #define PX4IO_P_STATUS_ALARMS			3	 /* alarm flags - alarms latch, write 1 to a bit to clear it */
 #define PX4IO_P_STATUS_ALARMS_RC_LOST           (1 << 0) /* timed out waiting for RC input */

src/modules/px4iofirmware/safety_button.cpp

@@ -55,6 +55,11 @@ static struct hrt_call failsafe_call;
  */
 static unsigned counter = 0;
 
+/*
+ * Stores the previous state of the safety switch to detect edges.
+ */
+static bool previous_safety_switch_state = false;
+
 /*
  * Define the various LED flash sequences for each system state.
  */
@@ -88,13 +93,13 @@ failsafe_led_init(void)
 static void
 safety_button_check(void *arg)
 {
-	const bool safety_button_pressed = px4_arch_gpioread(GPIO_BTN_SAFETY);
+	const bool safety_switch_state = px4_arch_gpioread(GPIO_BTN_SAFETY);
 
 	/* Keep safety button pressed for one second to trigger safety button event.
 	 * The logic to prevent turning on safety again is in the commander.
 	 */
 
-	if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_BUTTON_EVENT)) {
+	if (safety_switch_state && !(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_BUTTON_EVENT)) {
 
 		counter++;
 
@@ -106,6 +111,18 @@ safety_button_check(void *arg)
 		counter = 0;
 	}
 
+	/* Pass the raw safety switch state to the FMU */
+	if (safety_switch_state != previous_safety_switch_state) {
+		if (safety_switch_state) {
+			atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_SAFETY_SWITCH_STATE);
+
+		} else {
+			atomic_modify_clear(&r_status_flags, PX4IO_P_STATUS_FLAGS_SAFETY_SWITCH_STATE);
+		}
+
+		previous_safety_switch_state = safety_switch_state;
+	}
+
 	/* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */
 	uint16_t pattern = LED_PATTERN_FMU_REFUSE_TO_ARM;