uavcan: update safety button

updated uavcannode/Publishers/SafetyButton.hpp
tested successfully
make format
revert cannode publishing

src/drivers/uavcan/CMakeLists.txt

@@ -166,6 +166,7 @@ px4_add_module(
 		sensors/gyro.cpp
 		sensors/ice_status.cpp
 		sensors/hygrometer.cpp
+		sensors/safety_button.cpp
 
 	MODULE_CONFIG
 		module.yaml

src/drivers/uavcan/sensors/safety_button.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019-2021 PX4 Development Team. All rights reserved.
+ *   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
@@ -31,67 +31,47 @@
  *
  ****************************************************************************/
 
-/**
- * @author CUAVcaijie <caijie@cuav.net>
- */
-
-#include "safetybutton.hpp"
-#include <cstdint>
 #include <drivers/drv_hrt.h>
-#include <systemlib/err.h>
-#include <mathlib/mathlib.h>
+#include <parameters/param.h>
+#include "safety_button.hpp"
+#include <math.h>
 
-using namespace time_literals;
-const char *const UavcanSafetyBridge::NAME = "safety";
+const char *const UavcanSafetyButtonBridge::NAME = "safety_button";
 
-UavcanSafetyBridge::UavcanSafetyBridge(uavcan::INode &node) :
-	_node(node),
-	_sub_safety(node),
-	_pub_safety(node)
-{
-}
+UavcanSafetyButtonBridge::UavcanSafetyButtonBridge(uavcan::INode &node) :
+	UavcanSensorBridgeBase("uavcan_safety_button",
+			       ORB_ID(safety)), // TODO: either multiple publishers or `button_event` uORB topic
+	_sub_button(node)
+{ }
 
-int UavcanSafetyBridge::init()
+int UavcanSafetyButtonBridge::init()
 {
-	int res = _pub_safety.init(uavcan::TransferPriority::MiddleLower);
+	int res = _sub_button.start(ButtonCbBinder(this, &UavcanSafetyButtonBridge::button_sub_cb));
 
 	if (res < 0) {
-		printf("safety pub failed %i", res);
-		return res;
-	}
-
-	res = _sub_safety.start(SafetyCommandCbBinder(this, &UavcanSafetyBridge::safety_sub_cb));
-
-	if (res < 0) {
-		printf("safety pub failed %i", res);
+		DEVICE_LOG("failed to start uavcan sub: %d", res);
 		return res;
 	}
 
 	return 0;
 }
 
-unsigned UavcanSafetyBridge::get_num_redundant_channels() const
-{
-	return 0;
-}
-
-void UavcanSafetyBridge::print_status() const
+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
+
+	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);
+	}
 }
 
-void UavcanSafetyBridge::safety_sub_cb(const uavcan::ReceivedDataStructure<ardupilot::indication::Button> &msg)
+int UavcanSafetyButtonBridge::init_driver(uavcan_bridge::Channel *channel)
 {
-	if (msg.press_time > 10 && msg.button == 1) {
-		if (_safety_disabled) { return; }
-
-		_safety_disabled = true;
-
-	} else {
-
-		_safety_disabled = false;
-	}
-
-
-
-
+	return PX4_OK;
 }

src/drivers/uavcan/sensors/safety_button.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   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
@@ -31,49 +31,36 @@
  *
  ****************************************************************************/
 
-/**
- * @author CUAVcaijie <caijie@cuav.net>
- */
-
 #pragma once
 
-#include <uORB/Subscription.hpp>
-#include <uORB/PublicationMulti.hpp>
+#include "sensor_bridge.hpp"
 #include <uORB/topics/safety.h>
 
-#include <uavcan/uavcan.hpp>
 #include <ardupilot/indication/Button.hpp>
-#include "sensor_bridge.hpp"
 
-class UavcanSafetyBridge : public IUavcanSensorBridge
+class UavcanSafetyButtonBridge : public UavcanSensorBridgeBase
 {
 public:
 	static const char *const NAME;
 
-	UavcanSafetyBridge(uavcan::INode &node);
-	~UavcanSafetyBridge() = default;
+	UavcanSafetyButtonBridge(uavcan::INode &node);
 
 	const char *get_name() const override { return NAME; }
 
 	int init() override;
 
-	unsigned get_num_redundant_channels() const override;
-
-	void print_status() const override;
 private:
-	safety_s _safety{};  //
-	bool _safety_disabled{false};
 
-	bool _safety_btn_off{false};		///< State of the safety button read from the HW button
-	void safety_sub_cb(const uavcan::ReceivedDataStructure<ardupilot::indication::Button> &msg);
+	int init_driver(uavcan_bridge::Channel *channel) override;
+
+	void button_sub_cb(const uavcan::ReceivedDataStructure<ardupilot::indication::Button> &msg);
+
+	typedef uavcan::MethodBinder < UavcanSafetyButtonBridge *,
+		void (UavcanSafetyButtonBridge::*)
+		(const uavcan::ReceivedDataStructure<ardupilot::indication::Button> &) >
+		ButtonCbBinder;
 
-	typedef uavcan::MethodBinder < UavcanSafetyBridge *,
-		void (UavcanSafetyBridge::*)(const uavcan::ReceivedDataStructure<ardupilot::indication::Button> &) >
-		SafetyCommandCbBinder;
+	uavcan::Subscriber<ardupilot::indication::Button, ButtonCbBinder> _sub_button;
 
-	uavcan::INode &_node;
-	uavcan::Subscriber<ardupilot::indication::Button, SafetyCommandCbBinder> _sub_safety;
-	uavcan::Publisher<ardupilot::indication::Button> _pub_safety;
 
-	uORB::PublicationMulti<safety_s> _safety_pub{ORB_ID(safety)};
 };

src/drivers/uavcan/sensors/sensor_bridge.cpp

@@ -50,6 +50,7 @@
 #include "ice_status.hpp"
 #include "mag.hpp"
 #include "rangefinder.hpp"
+#include "safety_button.hpp"
 
 /*
  * IUavcanSensorBridge
@@ -144,6 +145,14 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge
 	if (uavcan_sub_rng != 0) {
 		list.add(new UavcanRangefinderBridge(node));
 	}
+
+	// safety button
+	int32_t uavcan_sub_button = 1;
+	param_get(param_find("UAVCAN_SUB_BTN"), &uavcan_sub_button);
+
+	if (uavcan_sub_button != 0) {
+		list.add(new UavcanSafetyButtonBridge(node));
+	}
 }
 
 /*

src/drivers/uavcan/uavcan_params.c

@@ -310,7 +310,7 @@ PARAM_DEFINE_INT32(UAVCAN_SUB_IMU, 0);
 /**
  * subscription magnetometer
  *
- * Enable UAVCAN GPS subscription.
+ * Enable UAVCAN mag subscription.
  *  uavcan::equipment::ahrs::MagneticFieldStrength
  *  uavcan::equipment::ahrs::MagneticFieldStrength2
  *
@@ -323,7 +323,7 @@ PARAM_DEFINE_INT32(UAVCAN_SUB_MAG, 1);
 /**
  * subscription range finder
  *
- * Enable UAVCAN GPS subscription.
+ * Enable UAVCAN range finder subscription.
  *  uavcan::equipment::range_sensor::Measurement
  *
  * @boolean
@@ -331,3 +331,15 @@ PARAM_DEFINE_INT32(UAVCAN_SUB_MAG, 1);
  * @group UAVCAN
  */
 PARAM_DEFINE_INT32(UAVCAN_SUB_RNG, 0);
+
+/**
+ * subscription button
+ *
+ * Enable UAVCAN button subscription.
+ *  ardupilot::indication::Button
+ *
+ * @boolean
+ * @reboot_required true
+ * @group UAVCAN
+ */
+PARAM_DEFINE_INT32(UAVCAN_SUB_BTN, 0);