diff --git a/src/drivers/uavcan/CMakeLists.txt b/src/drivers/uavcan/CMakeLists.txt index 9db9c176d0..b55a74229e 100644 --- a/src/drivers/uavcan/CMakeLists.txt +++ b/src/drivers/uavcan/CMakeLists.txt @@ -97,7 +97,7 @@ target_include_directories(uavcan_${UAVCAN_DRIVER}_driver PUBLIC set(DSDLC_DIR "${CMAKE_CURRENT_SOURCE_DIR}/dsdl") set(DSDLC_INPUTS "${DSDLC_DIR}/com" - "${DSDLC_DIR}/standard" + "${DSDLC_DIR}/ardupilot" "${LIBUAVCAN_DIR}/dsdl/uavcan" ) set(DSDLC_OUTPUT "${CMAKE_CURRENT_BINARY_DIR}/include/dsdlc_generated") diff --git a/src/drivers/uavcan/dsdl/standard/indication/21000.SafetyState.uavcan b/src/drivers/uavcan/dsdl/ardupilot/indication/21000.SafetyState.uavcan similarity index 100% rename from src/drivers/uavcan/dsdl/standard/indication/21000.SafetyState.uavcan rename to src/drivers/uavcan/dsdl/ardupilot/indication/21000.SafetyState.uavcan diff --git a/src/drivers/uavcan/dsdl/standard/indication/21001.Button.uavcan b/src/drivers/uavcan/dsdl/ardupilot/indication/21001.Button.uavcan similarity index 100% rename from src/drivers/uavcan/dsdl/standard/indication/21001.Button.uavcan rename to src/drivers/uavcan/dsdl/ardupilot/indication/21001.Button.uavcan diff --git a/src/drivers/uavcan/safety_state.cpp b/src/drivers/uavcan/safety_state.cpp index 9ef6032071..3e293c22da 100644 --- a/src/drivers/uavcan/safety_state.cpp +++ b/src/drivers/uavcan/safety_state.cpp @@ -63,7 +63,7 @@ void UavcanSafetyState::periodic_update(const uavcan::TimerEvent &) actuator_armed_s actuator_armed; if (_actuator_armed_sub.update(&actuator_armed)) { - standard::indication::SafetyState cmd; + ardupilot::indication::SafetyState cmd; if (actuator_armed.armed || actuator_armed.prearmed) { cmd.status = cmd.STATUS_SAFETY_OFF; diff --git a/src/drivers/uavcan/safety_state.hpp b/src/drivers/uavcan/safety_state.hpp index 20a0b60c24..215cd99b8e 100644 --- a/src/drivers/uavcan/safety_state.hpp +++ b/src/drivers/uavcan/safety_state.hpp @@ -42,7 +42,7 @@ #pragma once #include -#include +#include #include #include @@ -74,7 +74,7 @@ private: /* * Publish CAN Safety state led */ - uavcan::Publisher _safety_state_pub; + uavcan::Publisher _safety_state_pub; uavcan::TimerEventForwarder _timer; diff --git a/src/drivers/uavcan/sensors/safetybutton.cpp b/src/drivers/uavcan/sensors/safetybutton.cpp new file mode 100644 index 0000000000..b1573ef135 --- /dev/null +++ b/src/drivers/uavcan/sensors/safetybutton.cpp @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * Copyright (c) 2019-2021 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @author CUAVcaijie + */ + +#include "safetybutton.hpp" +#include +#include +#include +#include + +using namespace time_literals; +const char *const UavcanSafetyBridge::NAME = "safety"; + +UavcanSafetyBridge::UavcanSafetyBridge(uavcan::INode &node) : + _node(node), + _sub_safety(node), + _pub_safety(node) +{ +} + +int UavcanSafetyBridge::init() +{ + int res = _pub_safety.init(uavcan::TransferPriority::MiddleLower); + + 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); + return res; + } + + return 0; +} + +unsigned UavcanSafetyBridge::get_num_redundant_channels() const +{ + return 0; +} + +void UavcanSafetyBridge::print_status() const +{ +} + +void UavcanSafetyBridge::safety_sub_cb(const uavcan::ReceivedDataStructure &msg) +{ + if (msg.press_time > 10 && msg.button == 1) { + if (_safety_disabled) { return; } + + _safety_disabled = true; + + } else { + + _safety_disabled = false; + } + + + + +} diff --git a/src/drivers/uavcan/sensors/safetybutton.hpp b/src/drivers/uavcan/sensors/safetybutton.hpp new file mode 100644 index 0000000000..9aafee56fc --- /dev/null +++ b/src/drivers/uavcan/sensors/safetybutton.hpp @@ -0,0 +1,79 @@ +/**************************************************************************** + * + * Copyright (c) 2020-2021 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @author CUAVcaijie + */ + +#pragma once + +#include +#include +#include + +#include +#include +#include "sensor_bridge.hpp" + +class UavcanSafetyBridge : public IUavcanSensorBridge +{ +public: + static const char *const NAME; + + UavcanSafetyBridge(uavcan::INode &node); + ~UavcanSafetyBridge() = default; + + 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); + + typedef uavcan::MethodBinder < UavcanSafetyBridge *, + void (UavcanSafetyBridge::*)(const uavcan::ReceivedDataStructure &) > + SafetyCommandCbBinder; + + uavcan::INode &_node; + uavcan::Subscriber _sub_safety; + uavcan::Publisher _pub_safety; + + uORB::PublicationMulti _safety_pub{ORB_ID(safety)}; +}; diff --git a/src/drivers/uavcannode/CMakeLists.txt b/src/drivers/uavcannode/CMakeLists.txt index f41849b861..2e02adf84f 100644 --- a/src/drivers/uavcannode/CMakeLists.txt +++ b/src/drivers/uavcannode/CMakeLists.txt @@ -97,7 +97,7 @@ target_include_directories(uavcan_${UAVCAN_DRIVER}_driver PUBLIC set(DSDLC_DIR "${PX4_SOURCE_DIR}/src/drivers/uavcan/dsdl") set(DSDLC_INPUTS "${DSDLC_DIR}/com" - "${DSDLC_DIR}/standard" + "${DSDLC_DIR}/ardupilot" "${LIBUAVCAN_DIR}/dsdl/uavcan" ) set(DSDLC_OUTPUT "${CMAKE_CURRENT_BINARY_DIR}/include/dsdlc_generated") diff --git a/src/drivers/uavcannode/Publishers/SafetyButton.hpp b/src/drivers/uavcannode/Publishers/SafetyButton.hpp index cab88c1294..59b4ee6a3b 100644 --- a/src/drivers/uavcannode/Publishers/SafetyButton.hpp +++ b/src/drivers/uavcannode/Publishers/SafetyButton.hpp @@ -35,7 +35,7 @@ #include "UavcanPublisherBase.hpp" -#include +#include #include #include @@ -46,13 +46,13 @@ namespace uavcannode class SafetyButton : public UavcanPublisherBase, public uORB::SubscriptionCallbackWorkItem, - private uavcan::Publisher + private uavcan::Publisher { public: SafetyButton(px4::WorkItem *work_item, uavcan::INode &node) : - UavcanPublisherBase(standard::indication::Button::DefaultDataTypeID), + UavcanPublisherBase(ardupilot::indication::Button::DefaultDataTypeID), uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(safety)), - uavcan::Publisher(node) + uavcan::Publisher(node) {} void PrintInfo() override @@ -60,8 +60,8 @@ public: if (uORB::SubscriptionCallbackWorkItem::advertised()) { printf("\t%s -> %s:%d\n", uORB::SubscriptionCallbackWorkItem::get_topic()->o_name, - standard::indication::Button::getDataTypeFullName(), - standard::indication::Button::DefaultDataTypeID); + ardupilot::indication::Button::getDataTypeFullName(), + ardupilot::indication::Button::DefaultDataTypeID); } } @@ -72,10 +72,10 @@ public: if (uORB::SubscriptionCallbackWorkItem::update(&safety)) { if (safety.safety_switch_available) { - standard::indication::Button Button{}; - Button.button = standard::indication::Button::BUTTON_SAFETY; + ardupilot::indication::Button Button{}; + Button.button = ardupilot::indication::Button::BUTTON_SAFETY; Button.press_time = safety.safety_off ? UINT8_MAX : 0; - uavcan::Publisher::broadcast(Button); + uavcan::Publisher::broadcast(Button); } } }