safety and safety button: refactoring #19413

src/drivers/px4io/CMakeLists.txt

@@ -43,6 +43,7 @@ px4_add_module(
 		module.yaml
 	DEPENDS
 		arch_px4io_serial
+		button_publisher
 		circuit_breaker
 		mixer_module
 	)

src/drivers/px4io/px4io.cpp

@@ -61,6 +61,7 @@
 #include <lib/perf/perf_counter.h>
 #include <lib/rc/dsm.h>
 #include <lib/systemlib/mavlink_log.h>
+#include <lib/button/ButtonPublisher.hpp>
 
 #include <uORB/Publication.hpp>
 #include <uORB/PublicationMulti.hpp>
@@ -71,7 +72,6 @@
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/input_rc.h>
-#include <uORB/topics/safety.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_command_ack.h>
 #include <uORB/topics/px4io_status.h>
@@ -207,10 +207,10 @@ private:
 
 	/* advertised topics */
 	uORB::PublicationMulti<input_rc_s>	_to_input_rc{ORB_ID(input_rc)};
-	uORB::PublicationMulti<safety_s>	_to_safety{ORB_ID(safety)};
 	uORB::Publication<px4io_status_s>	_px4io_status_pub{ORB_ID(px4io_status)};
 
-	safety_s _safety{};
+	ButtonPublisher	_button_publisher;
+	bool _previous_safety_off{false};
 
 	bool			_lockdown_override{false};	///< override the safety lockdown
 
@@ -477,6 +477,9 @@ int PX4IO::init()
 	/* set safety to off if circuit breaker enabled */
 	if (circuit_breaker_enabled("CBRK_IO_SAFETY", CBRK_IO_SAFETY_KEY)) {
 		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC);
+
+	} else {
+		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_ON, PX4IO_FORCE_SAFETY_MAGIC);
 	}
 
 	/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
@@ -998,17 +1001,15 @@ int PX4IO::io_handle_status(uint16_t status)
 	 */
 	const bool safety_off = status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
 
-	// publish immediately on change, otherwise at 1 Hz
-	if ((hrt_elapsed_time(&_safety.timestamp) >= 1_s)
-	    || (_safety.safety_off != safety_off)) {
-
-		_safety.safety_switch_available = true;
-		_safety.safety_off = safety_off;
-		_safety.timestamp = hrt_absolute_time();
-
-		_to_safety.publish(_safety);
+	/* px4io board will change safety_off from false to true and stay like that until the vehicle is rebooted
+	 * where safety will change back to false. Here we are triggering the safety button event once.
+	 * TODO: change px4io firmware to act on the event. This will enable the "force safety on disarming" feature. */
+	if (_previous_safety_off != safety_off) {
+		_button_publisher.safetyButtonTriggerEvent();
 	}
 
+	_previous_safety_off = safety_off;
+
 	return ret;
 }
 

src/drivers/safety_button/CMakeLists.txt

@@ -37,6 +37,6 @@ px4_add_module(
 	SRCS
 		SafetyButton.cpp
 	DEPENDS
-		circuit_breaker
+		button_publisher
 		px4_work_queue
 	)

src/drivers/safety_button/SafetyButton.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2019 PX4 Development Team. All rights reserved.
+ *   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
@@ -31,7 +31,6 @@
  *
  ****************************************************************************/
 
-#include <drivers/drv_tone_alarm.h>
 #include "SafetyButton.hpp"
 
 #ifndef GPIO_BTN_SAFETY
@@ -54,11 +53,7 @@ enum class LED_PATTERN : uint16_t {
 SafetyButton::SafetyButton() :
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
 {
-	_safety_disabled = circuit_breaker_enabled("CBRK_IO_SAFETY", CBRK_IO_SAFETY_KEY);
-
-	if (_safety_disabled) {
-		_safety_btn_off = true;
-	}
+	_has_px4io = PX4_MFT_HW_SUPPORTED(PX4_MFT_PX4IO);
 }
 
 SafetyButton::~SafetyButton()
@@ -67,33 +62,22 @@ SafetyButton::~SafetyButton()
 }
 
 void
-SafetyButton::CheckButton()
+SafetyButton::CheckSafetyRequest(bool button_pressed)
 {
-	const bool safety_button_pressed = px4_arch_gpioread(GPIO_BTN_SAFETY);
-
-	/* Keep safety button pressed for one second to turn off safety
-	 *
-	 * Note that safety cannot be turned on again by button because a button
-	 * hardware problem could accidentally disable it in flight.
-	 */
-	if (safety_button_pressed && !_safety_btn_off) {
+	/* Keep button pressed for one second to turn off safety */
+	if (button_pressed) {
 
 		if (_button_counter <= CYCLE_COUNT) {
 			_button_counter++;
 		}
 
 		if (_button_counter == CYCLE_COUNT) {
-			// switch safety off -> ready to arm state
-			_safety_btn_off = true;
+			_button_publisher.safetyButtonTriggerEvent();
 		}
 
 	} else {
 		_button_counter = 0;
 	}
-
-	CheckPairingRequest(safety_button_pressed);
-
-	_safety_btn_prev_sate = safety_button_pressed;
 }
 
 void
@@ -108,7 +92,7 @@ SafetyButton::CheckPairingRequest(bool button_pressed)
 		_pairing_button_counter = 0;
 	}
 
-	if (!_safety_btn_prev_sate && button_pressed) {
+	if (!_button_prev_sate && button_pressed) {
 		if (_pairing_start == 0) {
 			_pairing_start = now;
 		}
@@ -116,29 +100,8 @@ SafetyButton::CheckPairingRequest(bool button_pressed)
 		++_pairing_button_counter;
 	}
 
-	if (_pairing_button_counter == 3) {
-		vehicle_command_s vcmd{};
-		vcmd.command = vehicle_command_s::VEHICLE_CMD_START_RX_PAIR;
-		vcmd.param1 = 10.f; // GCS pairing request handled by a companion.
-		vcmd.timestamp = hrt_absolute_time();
-		_to_command.publish(vcmd);
-		PX4_DEBUG("Sending GCS pairing request");
-
-		led_control_s led_control{};
-		led_control.led_mask = 0xff;
-		led_control.mode = led_control_s::MODE_BLINK_FAST;
-		led_control.color = led_control_s::COLOR_GREEN;
-		led_control.num_blinks = 1;
-		led_control.priority = 0;
-		led_control.timestamp = hrt_absolute_time();
-		_to_led_control.publish(led_control);
-
-		tune_control_s tune_control{};
-		tune_control.tune_id = tune_control_s::TUNE_ID_NOTIFY_POSITIVE;
-		tune_control.volume = tune_control_s::VOLUME_LEVEL_DEFAULT;
-		tune_control.timestamp = hrt_absolute_time();
-		_to_tune_control.publish(tune_control);
-
+	if (_pairing_button_counter == ButtonPublisher::PAIRING_BUTTON_EVENT_COUNT) {
+		_button_publisher.pairingButtonTriggerEvent();
 		// reset state
 		_pairing_start = 0;
 		_pairing_button_counter = 0;
@@ -156,7 +119,7 @@ SafetyButton::FlashButton()
 		LED_PATTERN pattern = LED_PATTERN::FMU_REFUSE_TO_ARM;
 
 		// cycle the blink state machine
-		if (_safety_btn_off) {
+		if (_button_prev_sate) {
 			if (armed.armed) {
 				pattern = LED_PATTERN::IO_FMU_ARMED;
 
@@ -190,25 +153,16 @@ SafetyButton::Run()
 		return;
 	}
 
-	CheckButton();
+	const bool button_pressed = px4_arch_gpioread(GPIO_BTN_SAFETY);
 
-	// control safety switch LED & publish safety topic
-	if (!PX4_MFT_HW_SUPPORTED(PX4_MFT_PX4IO)) {
+	// control safety switch LED & safety button
+	if (!_has_px4io) {
 		FlashButton();
-
-		const bool safety_off = _safety_btn_off || _safety_disabled;
-
-		// publish immediately on change, otherwise at 1 Hz
-		if ((hrt_elapsed_time(&_safety.timestamp) >= 1_s)
-		    || (_safety.safety_off != safety_off)) {
-
-			_safety.safety_switch_available = true;
-			_safety.safety_off = safety_off;
-			_safety.timestamp = hrt_absolute_time();
-
-			_to_safety.publish(_safety);
-		}
+		CheckSafetyRequest(button_pressed);
 	}
+
+	CheckPairingRequest(button_pressed);
+	_button_prev_sate = button_pressed;
 }
 
 int
@@ -248,15 +202,6 @@ SafetyButton::custom_command(int argc, char *argv[])
 	return print_usage("unknown command");
 }
 
-int
-SafetyButton::print_status()
-{
-	PX4_INFO("Safety Disabled: %s", _safety_disabled ? "yes" : "no");
-	PX4_INFO("Safety State (from button): %s", _safety_btn_off ? "off" : "on");
-
-	return 0;
-}
-
 int
 SafetyButton::print_usage(const char *reason)
 {

src/drivers/safety_button/SafetyButton.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2019 PX4 Development Team. All rights reserved.
+ *   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
@@ -35,20 +35,10 @@
 
 #include <float.h>
 
-#include <circuit_breaker/circuit_breaker.h>
 #include <drivers/drv_hrt.h>
-#include <px4_platform_common/px4_config.h>
-#include <px4_platform_common/getopt.h>
-#include <px4_platform_common/log.h>
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
-#include <uORB/Publication.hpp>
-#include <uORB/Subscription.hpp>
-#include <uORB/topics/actuator_armed.h>
-#include <uORB/topics/safety.h>
-#include <uORB/topics/vehicle_command.h>
-#include <uORB/topics/led_control.h>
-#include <uORB/topics/tune_control.h>
+#include <button/ButtonPublisher.hpp>
 
 class SafetyButton : public ModuleBase<SafetyButton>, public px4::ScheduledWorkItem
 {
@@ -65,34 +55,25 @@ public:
 	/** @see ModuleBase */
 	static int print_usage(const char *reason = nullptr);
 
-	/** @see ModuleBase::print_status() */
-	int print_status() override;
-
 	int Start();
 
 private:
 	void Run() override;
 
-	void CheckButton();
-	void FlashButton();
+	void CheckSafetyRequest(bool button_pressed);
 	void CheckPairingRequest(bool button_pressed);
+	void FlashButton();
 
-	uORB::Subscription		_armed_sub{ORB_ID(actuator_armed)};
-	uORB::Publication<safety_s>	_to_safety{ORB_ID(safety)};
-	uORB::Publication<vehicle_command_s>	_to_command{ORB_ID(vehicle_command)};
-	uORB::Publication<led_control_s> _to_led_control{ORB_ID(led_control)};
-	uORB::Publication<tune_control_s> _to_tune_control{ORB_ID(tune_control)};
-
-	safety_s _safety{};
-
-	uint8_t				_button_counter{0};
-	uint8_t				_blink_counter{0};
-	bool				_safety_disabled{false};	///< circuit breaker to disable the safety button
-	bool				_safety_btn_off{false};		///< State of the safety button read from the HW button
-	bool				_safety_btn_prev_sate{false};	///< Previous state of the HW button
+	bool			_has_px4io{false};
+	ButtonPublisher	_button_publisher;
+	uint8_t			_button_counter{0};
+	uint8_t			_blink_counter{0};
+	bool			_button_prev_sate{false};	///< Previous state of the HW button
 
 	// Pairing request
 	hrt_abstime		_pairing_start{0};
-	int			_pairing_button_counter{0};
+	int				_pairing_button_counter{0};
+
+	uORB::Subscription	_armed_sub{ORB_ID(actuator_armed)};
 
 };

src/drivers/uavcan/CMakeLists.txt

@@ -174,6 +174,7 @@ px4_add_module(
 	DEPENDS
 		px4_uavcan_dsdlc
 
+		button_publisher
 		drivers_rangefinder
 		led
 		mixer

src/drivers/uavcan/sensors/safety_button.cpp

@@ -38,10 +38,10 @@
 
 const char *const UavcanSafetyButtonBridge::NAME = "safety_button";
 
+using namespace time_literals;
+
 UavcanSafetyButtonBridge::UavcanSafetyButtonBridge(uavcan::INode &node) :
-	UavcanSensorBridgeBase("uavcan_safety_button",
-			       ORB_ID(safety)), // TODO: either multiple publishers or `button_event` uORB topic
-	_sub_button(node)
+	UavcanSensorBridgeBase("uavcan_safety_button", ORB_ID(button_event)), _sub_button(node)
 { }
 
 int UavcanSafetyButtonBridge::init()
@@ -60,14 +60,33 @@ void UavcanSafetyButtonBridge::button_sub_cb(const
 		uavcan::ReceivedDataStructure<ardupilot::indication::Button> &msg)
 {
 	bool is_safety = msg.button == ardupilot::indication::Button::BUTTON_SAFETY;
-	bool pressed = msg.press_time >= 10; // 0.1s increments
+	bool pressed = msg.press_time >= 10; // 0.1s increments (1s press time for safety button trigger event)
 
+	// Detect safety button trigger event
 	if (is_safety && pressed) {
-		safety_s safety = {};
-		safety.timestamp = hrt_absolute_time();
-		safety.safety_switch_available = true;
-		safety.safety_off = true;
-		publish(msg.getSrcNodeID().get(), &safety);
+		_button_publisher.safetyButtonTriggerEvent();
+	}
+
+	// Detect pairing button trigger event
+	if (is_safety) {
+
+		if (hrt_elapsed_time(&_start_timestamp) > 2_s) {
+			_start_timestamp = hrt_absolute_time();
+			_pairing_button_counter = 0u;
+		}
+
+		hrt_abstime press_time_us = (msg.press_time * 100 * 1000);
+		hrt_abstime elasped_time = hrt_elapsed_time(&_new_press_timestamp);
+
+		if (elasped_time > press_time_us) {
+			_pairing_button_counter++;
+			_new_press_timestamp = hrt_absolute_time();
+		}
+
+		if (_pairing_button_counter == ButtonPublisher::PAIRING_BUTTON_EVENT_COUNT) {
+			_button_publisher.pairingButtonTriggerEvent();
+			_start_timestamp = 0u;
+		}
 	}
 }
 

src/drivers/uavcan/sensors/safety_button.hpp

@@ -34,7 +34,7 @@
 #pragma once
 
 #include "sensor_bridge.hpp"
-#include <uORB/topics/safety.h>
+#include "button/ButtonPublisher.hpp"
 
 #include <ardupilot/indication/Button.hpp>
 
@@ -61,6 +61,8 @@ private:
 		ButtonCbBinder;
 
 	uavcan::Subscriber<ardupilot::indication::Button, ButtonCbBinder> _sub_button;
-
-
+	ButtonPublisher _button_publisher;
+	uint8_t _pairing_button_counter{0u};
+	hrt_abstime _start_timestamp{0};
+	hrt_abstime _new_press_timestamp{0};
 };