Compare commits

...

1 Commits

Author SHA1 Message Date
Niklas Hauser 57af2d3cf2 Run UAVCAN on CAN1 and run SocketCAN on CAN2 2025-07-08 16:48:31 +02:00
8 changed files with 521 additions and 0 deletions
+2
View File
@@ -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
+28
View File
@@ -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
@@ -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
+18
View File
@@ -62,6 +62,8 @@
#include <nuttx/mm/gran.h>
#include <chip.h>
#include <stm32_uart.h>
#include <stm32_ethernet.h>
#include <stm32_fdcan_sock.h>
#include <arch/board/board.h>
#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;
@@ -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
)
+5
View File
@@ -0,0 +1,5 @@
menuconfig DRIVERS_ACTUATORS_RUDDER
bool "rudder"
default n
---help---
Enable support for rudder
+308
View File
@@ -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 <uORB/Subscription.hpp>
#include <net/if.h>
#include <sys/ioctl.h>
#include <string.h>
#include <nuttx/can.h>
#include <netpacket/can.h>
#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);
}
+102
View File
@@ -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 <inttypes.h>
#include <stdint.h>
#include <drivers/drv_hrt.h>
#include <lib/cdev/CDev.hpp>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/log.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/px4_config.h>
#include <uORB/Publication.hpp>
#include <uORB/topics/actuator_servos.h>
#include <uORB/topics/actuator_motors.h>
#include <sys/time.h>
#include <sys/socket.h>
#include <nuttx/can.h>
#include <netpacket/can.h>
using namespace time_literals;
class Rudder : public ModuleBase<Rudder>
{
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)] {};
};