From 57af2d3cf2694db122680ac181f6300eaf060644 Mon Sep 17 00:00:00 2001 From: Niklas Hauser Date: Tue, 8 Jul 2025 16:48:31 +0200 Subject: [PATCH] Run UAVCAN on CAN1 and run SocketCAN on CAN2 --- boards/px4/fmu-v6x/default.px4board | 2 + boards/px4/fmu-v6x/init/rc.board_defaults | 28 ++ boards/px4/fmu-v6x/nuttx-config/nsh/defconfig | 18 + boards/px4/fmu-v6x/src/init.cpp | 18 + src/drivers/actuators/rudder/CMakeLists.txt | 40 +++ src/drivers/actuators/rudder/Kconfig | 5 + src/drivers/actuators/rudder/Rudder.cpp | 308 ++++++++++++++++++ src/drivers/actuators/rudder/Rudder.hpp | 102 ++++++ 8 files changed, 521 insertions(+) create mode 100644 src/drivers/actuators/rudder/CMakeLists.txt create mode 100644 src/drivers/actuators/rudder/Kconfig create mode 100644 src/drivers/actuators/rudder/Rudder.cpp create mode 100644 src/drivers/actuators/rudder/Rudder.hpp diff --git a/boards/px4/fmu-v6x/default.px4board b/boards/px4/fmu-v6x/default.px4board index fa427399a6..547aa8f48a 100644 --- a/boards/px4/fmu-v6x/default.px4board +++ b/boards/px4/fmu-v6x/default.px4board @@ -7,6 +7,7 @@ CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6" CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4" CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1" CONFIG_BOARD_SERIAL_EXT2="/dev/ttyS3" +CONFIG_DRIVERS_ACTUATORS_RUDDER=y CONFIG_DRIVERS_ADC_ADS1115=y CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_DRIVERS_BAROMETER_BMP388=y @@ -44,6 +45,7 @@ CONFIG_DRIVERS_RC_INPUT=y CONFIG_DRIVERS_SAFETY_BUTTON=y CONFIG_DRIVERS_TONE_ALARM=y CONFIG_DRIVERS_UAVCAN=y +CONFIG_BOARD_UAVCAN_INTERFACES=1 CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2 CONFIG_MODULES_AIRSPEED_SELECTOR=y CONFIG_MODULES_BATTERY_STATUS=y diff --git a/boards/px4/fmu-v6x/init/rc.board_defaults b/boards/px4/fmu-v6x/init/rc.board_defaults index 2d9a6ea49a..ab2922cc96 100644 --- a/boards/px4/fmu-v6x/init/rc.board_defaults +++ b/boards/px4/fmu-v6x/init/rc.board_defaults @@ -41,3 +41,31 @@ if ver hwbasecmp 00a 008 then mcp23009 start -b 3 -X -D 0xf1 -O 0xf0 -P 0x0f -U 10 fi + +ifup can0 +ifup can1 + +# CAN1=can0 runs UAVCAN +# started automatically + +# CAN2=can1 runs the Rudder actuator driver continuosly sending dummy messages. +# Also prints any received messages on the console. +rudder start + +# Try a loopback test with CAN1 connected to CAN1 and you should see some UAVCAN traffic. +# INFO [rudder] rudder recv can_id=0x9f043901, can_dlc=8, data=60, usec=101781000 +# INFO [rudder] rudder recv can_id=0x9f043901, can_dlc=8, data=0, usec=101781000 +# INFO [rudder] rudder recv can_id=0x904e2001, can_dlc=2, data=0, usec=101831000 +# INFO [rudder] rudder recv can_id=0x9f043901, can_dlc=4, data=0, usec=101832000 +# INFO [rudder] rudder recv can_id=0x9f043901, can_dlc=4, data=0, usec=101931000 + +# Alternatively try the candump with a loopback test: +# nsh> rudder stop +# nsh> candump can1,0:0,#FFFFFFFF & +# can1 1F043901 [4] 00 00 00 DA +# can1 1F043901 [4] 00 F8 00 D9 +# can1 1F043901 [4] 00 00 00 DC +# can1 1F043901 [4] 00 F8 00 DB +# can1 1F043901 [8] 00 F7 FF DF FE 00 00 7D +# can1 1F043901 [8] 3C 35 F6 00 00 F8 00 9D +# can1 1F043901 [4] 00 F8 00 DE diff --git a/boards/px4/fmu-v6x/nuttx-config/nsh/defconfig b/boards/px4/fmu-v6x/nuttx-config/nsh/defconfig index f0a8d6ae86..701e292991 100644 --- a/boards/px4/fmu-v6x/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v6x/nuttx-config/nsh/defconfig @@ -12,6 +12,7 @@ # CONFIG_MMCSD_HAVE_WRITEPROTECT is not set # CONFIG_MMCSD_MMCSUPPORT is not set # CONFIG_MMCSD_SPI is not set +# CONFIG_NET_CAN_CANFD is not set # CONFIG_NSH_DISABLEBG is not set # CONFIG_NSH_DISABLESCRIPT is not set # CONFIG_NSH_DISABLE_ARP is not set @@ -76,6 +77,8 @@ CONFIG_BOARD_CRASHDUMP=y CONFIG_BOARD_LOOPSPERMSEC=95751 CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y +CONFIG_CANUTILS_CANDUMP=y +CONFIG_CANUTILS_CANSEND=y CONFIG_CDCACM=y CONFIG_CDCACM_IFLOWCONTROL=y CONFIG_CDCACM_PRODUCTID=0x0035 @@ -101,6 +104,8 @@ CONFIG_FAT_DMAMEMORY=y CONFIG_FAT_LCNAMES=y CONFIG_FAT_LFN=y CONFIG_FAT_LFN_ALIAS_HASH=y +CONFIG_FDCAN1_BITRATE=250000 +CONFIG_FDCAN2_BITRATE=250000 CONFIG_FDCLONE_STDIO=y CONFIG_FSUTILS_IPCFG=y CONFIG_FS_BINFS=y @@ -146,6 +151,9 @@ CONFIG_NET=y CONFIG_NETDB_DNSCLIENT=y CONFIG_NETDB_DNSCLIENT_ENTRIES=8 CONFIG_NETDB_DNSSERVER_NOADDR=y +CONFIG_NETDEV_CAN_BITRATE_IOCTL=y +CONFIG_NETDEV_IFINDEX=y +CONFIG_NETDEV_LATEINIT=y CONFIG_NETDEV_PHY_IOCTL=y CONFIG_NETINIT_DHCPC=y CONFIG_NETINIT_DNS=y @@ -158,15 +166,23 @@ CONFIG_NETUTILS_TELNETD=y CONFIG_NET_ARP_IPIN=y CONFIG_NET_ARP_SEND=y CONFIG_NET_BROADCAST=y +CONFIG_NET_CAN=y +CONFIG_NET_CAN_EXTID=y +CONFIG_NET_CAN_NOTIFIER=y +CONFIG_NET_CAN_RAW_TX_DEADLINE=y +CONFIG_NET_CAN_SOCK_OPTS=y CONFIG_NET_ETH_PKTSIZE=1518 CONFIG_NET_ICMP=y CONFIG_NET_ICMP_SOCKET=y CONFIG_NET_NACTIVESOCKETS=16 +CONFIG_NET_CANPROTO_OPTIONS=y +CONFIG_NET_SOCKOPTS=y CONFIG_NET_SOLINGER=y CONFIG_NET_TCP=y CONFIG_NET_TCPBACKLOG=y CONFIG_NET_TCP_DELAYED_ACK=y CONFIG_NET_TCP_WRITE_BUFFERS=y +CONFIG_NET_TIMESTAMP=y CONFIG_NET_UDP=y CONFIG_NET_UDP_CHECKSUMS=y CONFIG_NET_UDP_WRITE_BUFFERS=y @@ -232,6 +248,8 @@ CONFIG_STM32H7_DMA1=y CONFIG_STM32H7_DMA2=y CONFIG_STM32H7_DMACAPABLE=y CONFIG_STM32H7_ETHMAC=y +CONFIG_STM32H7_FDCAN1=y +CONFIG_STM32H7_FDCAN2=y CONFIG_STM32H7_FLASH_OVERRIDE_I=y CONFIG_STM32H7_FLOWCONTROL_BROKEN=y CONFIG_STM32H7_I2C1=y diff --git a/boards/px4/fmu-v6x/src/init.cpp b/boards/px4/fmu-v6x/src/init.cpp index d914b75497..5e5d8e9fef 100644 --- a/boards/px4/fmu-v6x/src/init.cpp +++ b/boards/px4/fmu-v6x/src/init.cpp @@ -62,6 +62,8 @@ #include #include #include +#include +#include #include #include "arm_internal.h" @@ -293,6 +295,22 @@ __EXPORT int board_app_initialize(uintptr_t arg) return ret; } +# if STM32H7_NETHERNET == 1 + stm32_ethinitialize(0); +# endif /* STM32H7_NETHERNET */ + +# ifdef CONFIG_STM32H7_FDCAN1 + stm32_fdcansockinitialize(0); +# endif /* CONFIG_STM32H7_FDCAN1 */ + +# ifdef CONFIG_STM32H7_FDCAN2 + stm32_fdcansockinitialize(1); +# endif /* CONFIG_STM32H7_FDCAN2 */ + +# ifdef CONFIG_STM32H7_FDCAN3 + stm32_fdcansockinitialize(2); +# endif /* CONFIG_STM32H7_FDCAN3 */ + #endif /* !defined(BOOTLOADER) */ return OK; diff --git a/src/drivers/actuators/rudder/CMakeLists.txt b/src/drivers/actuators/rudder/CMakeLists.txt new file mode 100644 index 0000000000..9593d1c3fe --- /dev/null +++ b/src/drivers/actuators/rudder/CMakeLists.txt @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2025 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. +# +############################################################################ + +px4_add_module( + MODULE drivers__actuators__rudder + MAIN rudder + SRCS + Rudder.cpp + Rudder.hpp + ) diff --git a/src/drivers/actuators/rudder/Kconfig b/src/drivers/actuators/rudder/Kconfig new file mode 100644 index 0000000000..99e31dfd4a --- /dev/null +++ b/src/drivers/actuators/rudder/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_ACTUATORS_RUDDER + bool "rudder" + default n + ---help--- + Enable support for rudder diff --git a/src/drivers/actuators/rudder/Rudder.cpp b/src/drivers/actuators/rudder/Rudder.cpp new file mode 100644 index 0000000000..de6306737e --- /dev/null +++ b/src/drivers/actuators/rudder/Rudder.cpp @@ -0,0 +1,308 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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. + * + ****************************************************************************/ + +#include + +#include +#include +#include +#include +#include + +#include "Rudder.hpp" + +Rudder::Rudder() : + _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": sample")) +{ +} + +Rudder::~Rudder() +{ + close(_fd); + perf_free(_sample_perf); +} + +int Rudder::init(uint8_t index) +{ + _can_index = index <= 1 ? index : 1; + return open(); +} + +void Rudder::run() +{ + while (!should_exit()) { + perf_begin(_sample_perf); + + static uint64_t msg_payload = 0; + + // send a message + // FIXME: Adapt for actual content + struct can_frame tx_frame { + .can_id = 0x12345678 | CAN_EFF_FLAG, + .can_dlc = 8 + }; + memcpy(tx_frame.data, &msg_payload, sizeof(msg_payload)); + + const int send_res = send(tx_frame, 1_s); + + if (send_res < 0) { + PX4_ERR("rudder send_res=%i", send_res); + + } else { + msg_payload++; + } + + struct can_frame rx_frame; + + uint64_t usec; + + const int recv_res = receive(&rx_frame, &usec); + + if (recv_res < 0) { + // PX4_ERR("rudder recv_res=%i", recv_res); + + } else { + PX4_INFO("rudder recv can_id=%#08lx, can_dlc=%u, data=%u, usec=%llu", + rx_frame.can_id, rx_frame.can_dlc, rx_frame.data[0], usec); + } + + perf_end(_sample_perf); + px4_usleep(50_ms); + } +} + +int Rudder::open() +{ + if (_fd >= 0) { return 0; } + + if ((_fd = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) { + PX4_ERR("socket"); + return PX4_ERROR; + } + + const char name[5] {'c', 'a', 'n', (char)((uint8_t)'0' + _can_index), 0}; + struct sockaddr_can addr {}; + memset(&addr, 0, sizeof(addr)); + addr.can_family = AF_CAN; + addr.can_ifindex = if_nametoindex(name); + + /* disable default receive filter on this RAW socket */ + setsockopt(_fd, SOL_CAN_RAW, CAN_RAW_FILTER, NULL, 0); + + /* RX Timestamping */ + const int on = 1; + + if (setsockopt(_fd, SOL_SOCKET, SO_TIMESTAMP, &on, sizeof(on)) < 0) { + PX4_ERR("SO_TIMESTAMP is disabled"); + return PX4_ERROR; + } + + if (setsockopt(_fd, SOL_CAN_RAW, CAN_RAW_TX_DEADLINE, &on, sizeof(on)) < 0) { + PX4_ERR("CAN_RAW_TX_DEADLINE is disabled"); + return PX4_ERROR; + } + + + if (bind(_fd, (struct sockaddr *)&addr, sizeof(addr)) < 0) { + PX4_ERR("bind"); + return PX4_ERROR; + } + + // Setup TX msg + _send_iov.iov_base = &_send_frame; + _send_iov.iov_len = sizeof(struct can_frame); + + memset(&_send_control, 0x00, sizeof(_send_control)); + + _send_msg.msg_iov = &_send_iov; + _send_msg.msg_iovlen = 1; + _send_msg.msg_control = &_send_control; + _send_msg.msg_controllen = sizeof(_send_control); + + _send_cmsg = CMSG_FIRSTHDR(&_send_msg); + _send_cmsg->cmsg_level = SOL_CAN_RAW; + _send_cmsg->cmsg_type = CAN_RAW_TX_DEADLINE; + _send_cmsg->cmsg_len = sizeof(struct timeval); + + _send_tv = (struct timeval *)CMSG_DATA(_send_cmsg); + + // Setup RX msg + _recv_iov.iov_base = &_recv_frame; + _recv_iov.iov_len = sizeof(struct can_frame); + + memset(_recv_control, 0x00, sizeof(_recv_control)); + + _recv_msg.msg_iov = &_recv_iov; + _recv_msg.msg_iovlen = 1; + _recv_msg.msg_control = &_recv_control; + _recv_msg.msg_controllen = sizeof(_recv_control); + + _recv_cmsg = CMSG_FIRSTHDR(&_recv_msg); + + return PX4_OK; +} + +int Rudder::send(struct can_frame &frame, hrt_abstime usec) +{ + auto *const net_frame = (struct can_frame *)&_send_frame; + net_frame->can_id = frame.can_id; + net_frame->can_dlc = frame.can_dlc; + memcpy(&net_frame->data, frame.data, frame.can_dlc); + + /* Set CAN_RAW_TX_DEADLINE timestamp */ + const hrt_abstime deadline = hrt_absolute_time() + usec; + _send_tv->tv_usec = deadline % 1000000ULL; + _send_tv->tv_sec = (deadline - _send_tv->tv_usec) / 1000000ULL; + + const int res = sendmsg(_fd, &_send_msg, MSG_DONTWAIT); + + return res > 0 ? 1 : res; +} + +int Rudder::receive(struct can_frame *frame, hrt_abstime *usec) +{ + int32_t result = recvmsg(_fd, &_recv_msg, MSG_DONTWAIT); + + if (result < 0) { + return result; + } + + /* Copy SocketCAN frame to CanardFrame */ + auto *const recv_frame = (struct can_frame *)&_recv_frame; + + if (recv_frame->can_dlc > CAN_MAX_DLEN) { + return -EFAULT; + } + + frame->can_id = recv_frame->can_id; + frame->can_dlc = recv_frame->can_dlc; + memcpy(&frame->data, &recv_frame->data, recv_frame->can_dlc); + + /* Read SO_TIMESTAMP value */ + if (_recv_cmsg->cmsg_level == SOL_SOCKET && _recv_cmsg->cmsg_type == SO_TIMESTAMP) { + struct timeval *tv = (struct timeval *)CMSG_DATA(_recv_cmsg); + *usec = (hrt_abstime)(tv->tv_sec * 1000000ULL + tv->tv_usec); + } + + return result; +} + +int Rudder::task_spawn(int argc, char *argv[]) +{ + _task_id = px4_task_spawn_cmd("rudder", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 1200, + (px4_main_t)&run_trampoline, + (char *const *)argv); + + if (_task_id < 0) { + _task_id = -1; + return -errno; + } + + return 0; +} + +Rudder *Rudder::instantiate(int argc, char *argv[]) +{ + Rudder *instance = new Rudder(); + + if (instance) { + // initializing on CAN1=0, since CAN2=1 is used for UAVCAN + int status = instance->init(1); + + if (status != PX4_OK) { + delete instance; + return nullptr; + } + + } else { + PX4_ERR("alloc failed"); + } + + return instance; +} + +int Rudder::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int Rudder::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +SPEED 3: Rudder Control + +starring + +Chris HELMsworth +David BOWie +Howard STERN +Jeff BRIDGEs +Paul RUDD(er) + +with + +Natalie PORTman +Tom CRUISE +Laurence FISHburne +Adam SANDler +David SCHWIMMER +Daniel RadCLIFFe +Kiefer SutherLAND + +Produced by + +Chris MAST +Henry HULL + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("rudder", "driver"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int rudder_main(int argc, char *argv[]) +{ + return Rudder::main(argc, argv); +} diff --git a/src/drivers/actuators/rudder/Rudder.hpp b/src/drivers/actuators/rudder/Rudder.hpp new file mode 100644 index 0000000000..0fd793876b --- /dev/null +++ b/src/drivers/actuators/rudder/Rudder.hpp @@ -0,0 +1,102 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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. + * + ****************************************************************************/ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +using namespace time_literals; + +class Rudder : public ModuleBase +{ +public: + Rudder(); + ~Rudder() override; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static Rudder *instantiate(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + /** @see ModuleBase::run() */ + void run() override; + + int init(uint8_t index); + +private: + perf_counter_t _sample_perf; + uint8_t _can_index{0}; + + int open(); + int send(struct can_frame &frame, hrt_abstime usec); + int receive(struct can_frame *frame, hrt_abstime *usec); + + int _fd{-1}; + + //// Send msg structure + struct iovec _send_iov {}; + struct can_frame _send_frame {}; + struct msghdr _send_msg {}; + struct cmsghdr *_send_cmsg {}; + struct timeval *_send_tv {}; /* TX deadline timestamp */ + uint8_t _send_control[sizeof(struct cmsghdr) + sizeof(struct timeval)] {}; + + //// Receive msg structure + struct iovec _recv_iov {}; + struct can_frame _recv_frame {}; + struct msghdr _recv_msg {}; + struct cmsghdr *_recv_cmsg {}; + uint8_t _recv_control[sizeof(struct cmsghdr) + sizeof(struct timeval)] {}; +};