diff --git a/src/drivers/uavcan/CMakeLists.txt b/src/drivers/uavcan/CMakeLists.txt index 8dd796f12e..9d49cce3bb 100644 --- a/src/drivers/uavcan/CMakeLists.txt +++ b/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 diff --git a/src/drivers/uavcan/sensors/safetybutton.cpp b/src/drivers/uavcan/sensors/safety_button.cpp similarity index 59% rename from src/drivers/uavcan/sensors/safetybutton.cpp rename to src/drivers/uavcan/sensors/safety_button.cpp index b1573ef135..c42d7fe953 100644 --- a/src/drivers/uavcan/sensors/safetybutton.cpp +++ b/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 - */ - -#include "safetybutton.hpp" -#include #include -#include -#include +#include +#include "safety_button.hpp" +#include -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 &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 &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; } diff --git a/src/drivers/uavcan/sensors/safetybutton.hpp b/src/drivers/uavcan/sensors/safety_button.hpp similarity index 64% rename from src/drivers/uavcan/sensors/safetybutton.hpp rename to src/drivers/uavcan/sensors/safety_button.hpp index 9aafee56fc..f5b3f955a3 100644 --- a/src/drivers/uavcan/sensors/safetybutton.hpp +++ b/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 - */ - #pragma once -#include -#include +#include "sensor_bridge.hpp" #include -#include #include -#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 &msg); + int init_driver(uavcan_bridge::Channel *channel) override; + + void button_sub_cb(const uavcan::ReceivedDataStructure &msg); + + typedef uavcan::MethodBinder < UavcanSafetyButtonBridge *, + void (UavcanSafetyButtonBridge::*) + (const uavcan::ReceivedDataStructure &) > + ButtonCbBinder; - typedef uavcan::MethodBinder < UavcanSafetyBridge *, - void (UavcanSafetyBridge::*)(const uavcan::ReceivedDataStructure &) > - SafetyCommandCbBinder; + uavcan::Subscriber _sub_button; - uavcan::INode &_node; - uavcan::Subscriber _sub_safety; - uavcan::Publisher _pub_safety; - uORB::PublicationMulti _safety_pub{ORB_ID(safety)}; }; diff --git a/src/drivers/uavcan/sensors/sensor_bridge.cpp b/src/drivers/uavcan/sensors/sensor_bridge.cpp index c23b921cdd..023a16cf1d 100644 --- a/src/drivers/uavcan/sensors/sensor_bridge.cpp +++ b/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