From 2a7a0a9381efe744ffa475211e6a93f16152c0d7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 1 Sep 2023 09:20:06 +1000 Subject: [PATCH] support both multicast UDP and socketcan in ESCNode --- drivers/linux/linux.c | 92 +++++++++++++++++++++++++++++++++++++ drivers/linux/linux.h | 59 ++++++++++++++++++++++++ examples/ESCNode/Makefile | 6 ++- examples/ESCNode/esc_node.c | 22 +++++---- 4 files changed, 167 insertions(+), 12 deletions(-) create mode 100644 drivers/linux/linux.c create mode 100644 drivers/linux/linux.h diff --git a/drivers/linux/linux.c b/drivers/linux/linux.c new file mode 100644 index 0000000..102bde1 --- /dev/null +++ b/drivers/linux/linux.c @@ -0,0 +1,92 @@ +/* + implement LinuxCAN, wrapper around socketcan and multicast UDP + */ + +#include "linux.h" +#include +#include +#include + +/* + Initializes the instance. + Returns 0 on success, negative on error. +*/ +#if CANARD_ENABLE_CANFD +int16_t LinuxCANInit(LinuxCANInstance* out_ins, const char* can_iface_name, bool canfd) +#else +int16_t LinuxCANInit(LinuxCANInstance* out_ins, const char* can_iface_name) +#endif +{ + out_ins->socketcan = NULL; + out_ins->mcast = NULL; + if (strncmp(can_iface_name, "vcan", 4) == 0) { + out_ins->socketcan = (SocketCANInstance *)calloc(1, sizeof(SocketCANInstance)); + if (out_ins->socketcan == NULL) { + return -ENOMEM; + } +#if CANARD_ENABLE_CANFD + return socketcanInit(out_ins->socketcan, can_iface_name, can_fd); +#else + return socketcanInit(out_ins->socketcan, can_iface_name); +#endif + } + if (strncmp(can_iface_name, "mcast", 5) == 0) { + out_ins->mcast = (MCASTCANInstance *)calloc(1, sizeof(MCASTCANInstance)); + if (out_ins->mcast == NULL) { + return -ENOMEM; + } +#if CANARD_ENABLE_CANFD + return mcastInit(out_ins->mcast, can_iface_name, can_fd); +#else + return mcastInit(out_ins->mcast, can_iface_name); +#endif + } + return -EINVAL; +} + +/* + Deinitializes the mcast instance. + Returns 0 on success, negative on error. + */ +int16_t LinuxCANClose(LinuxCANInstance* ins) +{ + if (ins->socketcan != NULL) { + return socketcanClose(ins->socketcan); + } + if (ins->mcast != NULL) { + return mcastClose(ins->mcast); + } + return -EINVAL; +} + +/* + Transmits a CanardCANFrame to the CAN socket. + Use negative timeout to block infinitely. + Returns 1 on successful transmission, 0 on timeout, negative on error. + */ +int16_t LinuxCANTransmit(LinuxCANInstance* ins, const CanardCANFrame* frame, int32_t timeout_msec) +{ + if (ins->socketcan != NULL) { + return socketcanTransmit(ins->socketcan, frame, timeout_msec); + } + if (ins->mcast != NULL) { + return mcastTransmit(ins->mcast, frame, timeout_msec); + } + return -EINVAL; +} + +/* + Receives a CanardCANFrame from the CAN socket. + Use negative timeout to block infinitely. + Returns 1 on successful reception, 0 on timeout, negative on error. + */ +int16_t LinuxCANReceive(LinuxCANInstance* ins, CanardCANFrame* out_frame, int32_t timeout_msec) +{ + if (ins->socketcan != NULL) { + return socketcanReceive(ins->socketcan, out_frame, timeout_msec); + } + if (ins->mcast != NULL) { + return mcastReceive(ins->mcast, out_frame, timeout_msec); + } + return -EINVAL; +} diff --git a/drivers/linux/linux.h b/drivers/linux/linux.h new file mode 100644 index 0000000..7d75195 --- /dev/null +++ b/drivers/linux/linux.h @@ -0,0 +1,59 @@ +/* + * Copyright (c) 2023 DroneCAN Team + * + * Distributed under the MIT License, available in the file LICENSE. + */ +/* + this wraps the socketcan and multicast drivers, allowing either to be selected + */ + +#pragma once + +#include +#include "../mcast/mcast.h" +#include "../socketcan/socketcan.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +typedef struct +{ + MCASTCANInstance *mcast; + SocketCANInstance *socketcan; +} LinuxCANInstance; + +/* + Initializes the instance. + Returns 0 on success, negative on error. +*/ +#if CANARD_ENABLE_CANFD +int16_t LinuxCANInit(LinuxCANInstance* out_ins, const char* can_iface_name, bool canfd); +#else +int16_t LinuxCANInit(LinuxCANInstance* out_ins, const char* can_iface_name); +#endif + +/* + Deinitializes the mcast instance. + Returns 0 on success, negative on error. + */ +int16_t LinuxCANClose(LinuxCANInstance* ins); + +/* + Transmits a CanardCANFrame to the CAN socket. + Use negative timeout to block infinitely. + Returns 1 on successful transmission, 0 on timeout, negative on error. + */ +int16_t LinuxCANTransmit(LinuxCANInstance* ins, const CanardCANFrame* frame, int32_t timeout_msec); + +/* + Receives a CanardCANFrame from the CAN socket. + Use negative timeout to block infinitely. + Returns 1 on successful reception, 0 on timeout, negative on error. + */ +int16_t LinuxCANReceive(LinuxCANInstance* ins, CanardCANFrame* out_frame, int32_t timeout_msec); + +#ifdef __cplusplus +} +#endif diff --git a/examples/ESCNode/Makefile b/examples/ESCNode/Makefile index 4786ca5..8bd1859 100644 --- a/examples/ESCNode/Makefile +++ b/examples/ESCNode/Makefile @@ -2,12 +2,14 @@ CANARD_BASE=../.. CC=gcc -CFLAGS = -g -Wall -Idsdl_generated/include -I$(CANARD_BASE) -I$(CANARD_BASE)/drivers/socketcan +CFLAGS = -g -Wall -Idsdl_generated/include -I$(CANARD_BASE) -I$(CANARD_BASE)/drivers/linux LIBS=$(CANARD_BASE)/canard.c -# add socketcan driver for linux +# add socketcan and multicast drivers for linux LIBS+=$(CANARD_BASE)/drivers/socketcan/socketcan.c +LIBS+=$(CANARD_BASE)/drivers/mcast/mcast.c +LIBS+=$(CANARD_BASE)/drivers/linux/linux.c # add in generated code LIBS+=dsdl_generated/src/uavcan.protocol.NodeStatus.c diff --git a/examples/ESCNode/esc_node.c b/examples/ESCNode/esc_node.c index 3d9664b..7f9e386 100644 --- a/examples/ESCNode/esc_node.c +++ b/examples/ESCNode/esc_node.c @@ -10,9 +10,11 @@ - sends ESC Status messages (with synthetic data based on throttles) - a parameter server for reading and writing node parameters - This example uses socketcan on Linux for CAN transport + This example uses socketcan or multicast UDP on Linux for CAN transport - Example usage: ./esc_node vcan0 + Example usage: + ./esc_node vcan0 + ./esc_node mcast:0 */ /* This example application is distributed under the terms of CC0 (public domain dedication). @@ -24,7 +26,7 @@ #endif #include -#include +#include #include #include @@ -631,11 +633,11 @@ static void send_ESCStatus(void) /* Transmits all frames from the TX queue, receives up to one frame. */ -static void processTxRxOnce(SocketCANInstance *socketcan, int32_t timeout_msec) +static void processTxRxOnce(LinuxCANInstance *can, int32_t timeout_msec) { // Transmitting for (const CanardCANFrame* txf = NULL; (txf = canardPeekTxQueue(&canard)) != NULL;) { - const int16_t tx_res = socketcanTransmit(socketcan, txf, 0); + const int16_t tx_res = LinuxCANTransmit(can, txf, 0); if (tx_res < 0) { // Failure - drop the frame canardPopTxQueue(&canard); } @@ -653,7 +655,7 @@ static void processTxRxOnce(SocketCANInstance *socketcan, int32_t timeout_msec) CanardCANFrame rx_frame; const uint64_t timestamp = micros64(); - const int16_t rx_res = socketcanReceive(socketcan, &rx_frame, timeout_msec); + const int16_t rx_res = LinuxCANReceive(can, &rx_frame, timeout_msec); if (rx_res < 0) { (void)fprintf(stderr, "Receive error %d, errno '%s'\n", rx_res, strerror(errno)); } @@ -679,11 +681,11 @@ int main(int argc, char** argv) load_settings(); /* - * Initializing the CAN backend driver; in this example we're using SocketCAN + * Initializing the CAN backend driver */ - SocketCANInstance socketcan; + LinuxCANInstance can; const char* const can_iface_name = argv[1]; - int16_t res = socketcanInit(&socketcan, can_iface_name); + int16_t res = LinuxCANInit(&can, can_iface_name); if (res < 0) { (void)fprintf(stderr, "Failed to open CAN iface '%s'\n", can_iface_name); return 1; @@ -712,7 +714,7 @@ int main(int argc, char** argv) uint64_t next_50hz_service_at = micros64(); while (true) { - processTxRxOnce(&socketcan, 10); + processTxRxOnce(&can, 10); const uint64_t ts = micros64();