From ecb222c7e7b2dc7cb11bbc5f067eeb0e7fab4f41 Mon Sep 17 00:00:00 2001 From: Andrew Brahim <35986980+dirksavage88@users.noreply.github.com> Date: Sun, 8 Feb 2026 22:36:11 -0500 Subject: [PATCH] uavcannode: implement hardpoint commands (#26334) * implement cannode hardpoint commands Signed-off-by: dirksavage88 * Update src/drivers/uavcannode/Subscribers/HardpointCommand.hpp Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> * Update src/drivers/uavcannode/Subscribers/HardpointCommand.hpp Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> * add hardpoint sub to ark cannode, simplify handling of hardpoint broadcast Signed-off-by: dirksavage88 --------- Signed-off-by: dirksavage88 Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> --- boards/ark/cannode/default.px4board | 1 + src/drivers/uavcannode/Kconfig | 4 + .../Subscribers/HardpointCommand.hpp | 123 ++++++++++++++++++ src/drivers/uavcannode/UavcanNode.cpp | 8 ++ 4 files changed, 136 insertions(+) create mode 100644 src/drivers/uavcannode/Subscribers/HardpointCommand.hpp diff --git a/boards/ark/cannode/default.px4board b/boards/ark/cannode/default.px4board index 29b964b237..f886a22b2e 100644 --- a/boards/ark/cannode/default.px4board +++ b/boards/ark/cannode/default.px4board @@ -31,6 +31,7 @@ CONFIG_UAVCANNODE_ESC_RAW_COMMAND=y CONFIG_UAVCANNODE_ESC_STATUS=y CONFIG_UAVCANNODE_FLOW_MEASUREMENT=y CONFIG_UAVCANNODE_GNSS_FIX=y +CONFIG_UAVCANNODE_HARDPOINT_COMMAND=y CONFIG_UAVCANNODE_HYGROMETER_MEASUREMENT=y CONFIG_UAVCANNODE_LIGHTS_COMMAND=y CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH=y diff --git a/src/drivers/uavcannode/Kconfig b/src/drivers/uavcannode/Kconfig index 532fe3229d..d0edc5d44b 100644 --- a/src/drivers/uavcannode/Kconfig +++ b/src/drivers/uavcannode/Kconfig @@ -34,6 +34,10 @@ if DRIVERS_UAVCANNODE bool "Include GNSS fix" default n + config UAVCANNODE_HARDPOINT_COMMAND + bool "Include hardpoint commands" + default n + config UAVCANNODE_HYGROMETER_MEASUREMENT bool "Include hygrometer measurement" default n diff --git a/src/drivers/uavcannode/Subscribers/HardpointCommand.hpp b/src/drivers/uavcannode/Subscribers/HardpointCommand.hpp new file mode 100644 index 0000000000..87aba37593 --- /dev/null +++ b/src/drivers/uavcannode/Subscribers/HardpointCommand.hpp @@ -0,0 +1,123 @@ +/**************************************************************************** + * + * Copyright (c) 2026 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. + * + ****************************************************************************/ + +#pragma once + +#include "UavcanSubscriberBase.hpp" + +#include + +#include +#include + +namespace uavcannode +{ + +class HardpointCommand; + +typedef uavcan::MethodBinder&)> + HardpointCommandBinder; + +class HardpointCommand : + public UavcanSubscriberBase, + private uavcan::Subscriber +{ +public: + HardpointCommand(uavcan::INode &node) : + UavcanSubscriberBase(uavcan::equipment::hardpoint::Command::DefaultDataTypeID), + uavcan::Subscriber(node) + {} + + bool init() + { + if (start(HardpointCommandBinder(this, &HardpointCommand::callback)) < 0) { + PX4_ERR("uavcan::equipment::hardpoint::Command subscription failed"); + return false; + } + + return true; + } + + void PrintInfo() const override + { + printf("\t%s:%d -> %s\n", + uavcan::equipment::hardpoint::Command::getDataTypeFullName(), + uavcan::equipment::hardpoint::Command::DefaultDataTypeID, + _actuator_servos_pub.get_topic()->o_name); + } + +private: + void callback(const uavcan::ReceivedDataStructure &msg) + { + + uint8_t servo_id = msg.hardpoint_id; + actuator_servos_s actuator_servos {}; + + if (servo_id >= actuator_servos_s::NUM_CONTROLS) { + return; + } + + for (uint8_t i = servo_id; i < actuator_servos_s::NUM_CONTROLS; ++i) { + + actuator_servos.timestamp = hrt_absolute_time(); + actuator_servos.timestamp_sample = actuator_servos.timestamp; + + if (msg.command == 1) { + actuator_servos.control[i] = 1; // grip + + } else if (msg.command == 0) { + actuator_servos.control[i] = -1; // release + + } else { + actuator_servos.control[i] = 0; // do nothing + } + + actuator_servos.timestamp = hrt_absolute_time(); + actuator_servos.timestamp_sample = actuator_servos.timestamp; + + // If ID is not 0 (broadcast), do not iterate + if (servo_id != 0) { + break; + } + + } + + _actuator_servos_pub.publish(actuator_servos); + + } + + uORB::Publication _actuator_servos_pub{ORB_ID(actuator_servos)}; + +}; +} // namespace uavcannode diff --git a/src/drivers/uavcannode/UavcanNode.cpp b/src/drivers/uavcannode/UavcanNode.cpp index 6d0799fc27..0940791c90 100644 --- a/src/drivers/uavcannode/UavcanNode.cpp +++ b/src/drivers/uavcannode/UavcanNode.cpp @@ -121,6 +121,10 @@ #include "Subscribers/ServoArrayCommand.hpp" #endif // CONFIG_UAVCANNODE_SERVO_ARRAY_COMMAND +#if defined(CONFIG_UAVCANNODE_HARDPOINT_COMMAND) +#include "Subscribers/HardpointCommand.hpp" +#endif // CONFIG_UAVCANNODE_HARDPOINT_COMMAND + using namespace time_literals; namespace uavcannode @@ -485,6 +489,10 @@ int UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events _subscriber_list.add(new ServoArrayCommand(_node)); #endif // CONFIG_UAVCANNODE_SERVO_ARRAY_COMMAND +#if defined(CONFIG_UAVCANNODE_HARDPOINT_COMMAND) + _subscriber_list.add(new HardpointCommand(_node)); +#endif // CONFIG_UAVCANNODE_HARDPOINT_COMMAND + for (auto &subscriber : _subscriber_list) { subscriber->init(); }