diff --git a/src/modules/uavcan/actuators/hardpoint.cpp b/src/modules/uavcan/actuators/hardpoint.cpp index 86d6d8b425..3ea985a909 100644 --- a/src/modules/uavcan/actuators/hardpoint.cpp +++ b/src/modules/uavcan/actuators/hardpoint.cpp @@ -40,6 +40,14 @@ #include "hardpoint.hpp" #include +UavcanHardpointController::UavcanHardpointController(uavcan::INode &node) : + _node(node), + _uavcan_pub_raw_cmd(node), + _timer(node) +{ + +} + UavcanHardpointController::~UavcanHardpointController() { diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index fbb0708723..469e83d74c 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -81,10 +81,12 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys _node(can_driver, system_clock, _pool_allocator), _node_mutex(), _esc_controller(_node), + _hardpoint_controller(_node), _time_sync_master(_node), _time_sync_slave(_node), _master_timer(_node), _setget_response(0) + { _task_should_exit = false; _fw_server_action = None; @@ -1103,14 +1105,14 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg) _mixers->groups_required(_groups_required); } } - - break; } + break; + - case UAVCANIOC_HARDPOINT_SET: + case UAVCANIOC_HARDPOINT_SET: { const auto& cmd = *reinterpret_cast(arg); _hardpoint_controller.set_command(cmd.hardpoint_id, cmd.command); - + } break; default: @@ -1370,6 +1372,15 @@ int uavcan_main(int argc, char *argv[]) return inst->set_param(nodeid, argv[4], argv[5]); } } + if (!std::strcmp(argv[1],"uavcan_hardpoint_set")) { + if (argc < 3) { + errx(1, "Hardpoint Id and value is requred"); + } + //UavcanNode::ioctl(file *filp, int cmd, unsigned long arg) + //UavcanNode.ioctl(???) + // _hardpoint_controller.set_command() + + } if (!std::strcmp(argv[1], "stop")) { if (fw) { diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index b2c570fb25..61678b9e74 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -53,6 +53,7 @@ #include #include "actuators/esc.hpp" +#include "actuators/hardpoint.hpp" #include "sensors/sensor_bridge.hpp" #include "uavcan_servers.hpp" @@ -71,9 +72,9 @@ // we add two to allow for actuator_direct and busevent #define UAVCAN_NUM_POLL_FDS (NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN+2) -// no idea something to do with hardpoint controller +// IOCTL control codes static constexpr unsigned UAVCANIOCBASE = 0x7800; -static constexpr unsigned UAVCANIOC_HARDPOINT_SET = PX4_IOC(UAVCANIOCBASE, 0x10); +static constexpr unsigned UAVCANIOC_HARDPOINT_SET = _PX4_IOC(UAVCANIOCBASE, 0x10); /** * A UAVCAN node. @@ -87,6 +88,7 @@ class UavcanNode : public device::CDev static constexpr unsigned PollTimeoutMs = 3; + /* * This memory is reserved for uavcan to use for queuing CAN frames. * At 1Mbit there is approximately one CAN frame every 145 uS. @@ -180,6 +182,7 @@ private: pthread_mutex_t _node_mutex; px4_sem_t _server_command_sem; UavcanEscController _esc_controller; + UavcanHardpointController _hardpoint_controller; uavcan::GlobalTimeSyncMaster _time_sync_master; uavcan::GlobalTimeSyncSlave _time_sync_slave;