2022-08-01 15:44:33 +02:00
|
|
|
/****************************************************************************
|
2023-01-31 16:35:23 +01:00
|
|
|
* apps/examples/opencyphal/canard_main.c
|
2022-08-01 15:44:33 +02:00
|
|
|
*
|
|
|
|
* Licensed to the Apache Software Foundation (ASF) under one or more
|
|
|
|
* contributor license agreements. See the NOTICE file distributed with
|
|
|
|
* this work for additional information regarding copyright ownership. The
|
|
|
|
* ASF licenses this file to you under the Apache License, Version 2.0 (the
|
|
|
|
* "License"); you may not use this file except in compliance with the
|
|
|
|
* License. You may obtain a copy of the License at
|
|
|
|
*
|
|
|
|
* http://www.apache.org/licenses/LICENSE-2.0
|
|
|
|
*
|
|
|
|
* Unless required by applicable law or agreed to in writing, software
|
|
|
|
* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
|
|
|
|
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
|
|
|
|
* License for the specific language governing permissions and limitations
|
|
|
|
* under the License.
|
|
|
|
*
|
|
|
|
****************************************************************************/
|
|
|
|
|
|
|
|
/****************************************************************************
|
|
|
|
* Included Files
|
|
|
|
****************************************************************************/
|
|
|
|
|
|
|
|
#include <canard.h>
|
|
|
|
#include <canard_dsdl.h>
|
|
|
|
#include <o1heap.h>
|
|
|
|
|
|
|
|
#include <sched.h>
|
|
|
|
|
|
|
|
#include <stdio.h>
|
|
|
|
#include <stdlib.h>
|
|
|
|
#include <time.h>
|
|
|
|
#include <string.h>
|
|
|
|
#include <assert.h>
|
|
|
|
#include <errno.h>
|
|
|
|
|
|
|
|
#include <net/if.h>
|
|
|
|
#include <sys/time.h>
|
|
|
|
#include <sys/ioctl.h>
|
|
|
|
#include <sys/socket.h>
|
|
|
|
|
|
|
|
#include <poll.h>
|
|
|
|
|
|
|
|
#include <nuttx/can.h>
|
|
|
|
#include <netpacket/can.h>
|
|
|
|
|
|
|
|
#include "socketcan.h"
|
|
|
|
|
|
|
|
/****************************************************************************
|
|
|
|
* Pre-processor Definitions
|
|
|
|
****************************************************************************/
|
|
|
|
|
|
|
|
/* Application constants */
|
|
|
|
|
|
|
|
#define APP_VERSION_MAJOR 1
|
|
|
|
#define APP_VERSION_MINOR 0
|
2023-01-31 16:35:23 +01:00
|
|
|
#define APP_NODE_NAME CONFIG_EXAMPLES_LIBOPENCYPHAL_APP_NODE_NAME
|
2022-08-01 15:44:33 +02:00
|
|
|
|
|
|
|
/****************************************************************************
|
|
|
|
* Private Data
|
|
|
|
****************************************************************************/
|
|
|
|
|
|
|
|
/* Arena for memory allocation, used by the library */
|
|
|
|
|
2023-01-31 16:35:23 +01:00
|
|
|
#define O1_HEAP_SIZE CONFIG_EXAMPLES_LIBOPENCYPHAL_NODE_MEM_POOL_SIZE
|
2022-08-01 15:44:33 +02:00
|
|
|
|
|
|
|
/* Temporary development UAVCAN topic service ID to publish/subscribe from */
|
|
|
|
#define PORT_ID 4421
|
|
|
|
#define TOPIC_SIZE 512
|
|
|
|
|
|
|
|
O1HeapInstance *my_allocator;
|
|
|
|
static uint8_t uavcan_heap[O1_HEAP_SIZE]
|
|
|
|
__attribute__((aligned(O1HEAP_ALIGNMENT)));
|
|
|
|
|
|
|
|
/* Node status variables */
|
|
|
|
|
|
|
|
static bool g_canard_daemon_started;
|
|
|
|
|
|
|
|
static uint8_t my_message_transfer_id;
|
|
|
|
|
|
|
|
struct pollfd fd;
|
|
|
|
|
|
|
|
/****************************************************************************
|
|
|
|
* Public Functions
|
|
|
|
****************************************************************************/
|
|
|
|
|
|
|
|
/****************************************************************************
|
|
|
|
* Name: memallocate
|
|
|
|
*
|
|
|
|
* Description:
|
|
|
|
*
|
|
|
|
****************************************************************************/
|
|
|
|
|
|
|
|
static void *memallocate(CanardInstance *const ins, const size_t amount)
|
|
|
|
{
|
|
|
|
(void) ins;
|
|
|
|
return o1heapAllocate(my_allocator, amount);
|
|
|
|
}
|
|
|
|
|
|
|
|
/****************************************************************************
|
|
|
|
* Name: memfree
|
|
|
|
*
|
|
|
|
* Description:
|
|
|
|
*
|
|
|
|
****************************************************************************/
|
|
|
|
|
|
|
|
static void memfree(CanardInstance *const ins, void *const pointer)
|
|
|
|
{
|
|
|
|
(void) ins;
|
|
|
|
o1heapFree(my_allocator, pointer);
|
|
|
|
}
|
|
|
|
|
|
|
|
/****************************************************************************
|
|
|
|
* Name: getmonotonictimestampusec
|
|
|
|
*
|
|
|
|
* Description:
|
|
|
|
*
|
|
|
|
****************************************************************************/
|
|
|
|
|
|
|
|
uint64_t getmonotonictimestampusec(void)
|
|
|
|
{
|
|
|
|
struct timespec ts;
|
|
|
|
|
|
|
|
memset(&ts, 0, sizeof(ts));
|
|
|
|
|
|
|
|
if (clock_gettime(CLOCK_MONOTONIC, &ts) != 0)
|
|
|
|
{
|
|
|
|
abort();
|
|
|
|
}
|
|
|
|
|
|
|
|
return ts.tv_sec * 1000000ULL + ts.tv_nsec / 1000ULL;
|
|
|
|
}
|
|
|
|
|
|
|
|
/****************************************************************************
|
|
|
|
* Name: process1hztasks
|
|
|
|
*
|
|
|
|
* Description:
|
|
|
|
* This function is called at 1 Hz rate from the main loop.
|
|
|
|
*
|
|
|
|
****************************************************************************/
|
|
|
|
|
|
|
|
void process1hztasks(CanardInstance *ins, uint64_t timestamp_usec)
|
|
|
|
{
|
|
|
|
CanardMicrosecond transmission_deadline =
|
|
|
|
getmonotonictimestampusec() + 1000 * 10;
|
|
|
|
|
|
|
|
const CanardTransfer transfer =
|
|
|
|
{
|
|
|
|
.timestamp_usec = transmission_deadline, /* Zero if transmission deadline is not limited. */
|
|
|
|
.priority = CanardPriorityNominal,
|
|
|
|
.transfer_kind = CanardTransferKindMessage,
|
|
|
|
.port_id = 1234, /* This is the subject-ID. */
|
|
|
|
.remote_node_id = CANARD_NODE_ID_UNSET, /* Messages cannot be unicast, so use UNSET. */
|
|
|
|
.transfer_id = my_message_transfer_id,
|
|
|
|
.payload_size = 47,
|
|
|
|
.payload = "\x2D\x00"
|
|
|
|
"Sancho, it strikes me thou art in great fear.",
|
|
|
|
};
|
|
|
|
|
|
|
|
++my_message_transfer_id; /* The transfer-ID shall be incremented after every transmission on this subject. */
|
|
|
|
int32_t result = canardTxPush(ins, &transfer);
|
|
|
|
|
|
|
|
if (result < 0)
|
|
|
|
{
|
|
|
|
/* An error has occurred: either an argument is invalid or we've
|
|
|
|
* ran out of memory. It is possible to statically prove that an
|
|
|
|
* out-of-memory will never occur for a given application if the
|
|
|
|
* heap is sized correctly; for background, refer to the Robson's
|
|
|
|
* Proof and the documentation for O1Heap.
|
|
|
|
*/
|
|
|
|
|
|
|
|
fprintf(stderr, "Transmit error %ld\n", result);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/****************************************************************************
|
|
|
|
* Name: processTxRxOnce
|
|
|
|
*
|
|
|
|
* Description:
|
|
|
|
* Transmits all frames from the TX queue, receives up to one frame.
|
|
|
|
*
|
|
|
|
****************************************************************************/
|
|
|
|
|
|
|
|
void processtxrxonce(CanardInstance *ins, canardsocketinstance *sock_ins,
|
|
|
|
int timeout_msec)
|
|
|
|
{
|
|
|
|
int32_t result;
|
|
|
|
|
|
|
|
/* Transmitting, Look at the top of the TX queue. */
|
|
|
|
|
|
|
|
for (const CanardFrame *txf = NULL; (txf = canardTxPeek(ins)) != NULL; )
|
|
|
|
{
|
|
|
|
if (txf->timestamp_usec > getmonotonictimestampusec()) /* Check if the frame has timed out. */
|
|
|
|
{
|
|
|
|
if (socketcantransmit(sock_ins, txf) == 0) /* Send the frame. Redundant interfaces may be used here. */
|
|
|
|
{
|
|
|
|
/* If the driver is busy, break and retry later. */
|
|
|
|
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
canardTxPop(ins); /* Remove the frame from the queue after it's transmitted. */
|
|
|
|
ins->memory_free(ins, (CanardFrame *)txf);
|
|
|
|
}
|
|
|
|
|
|
|
|
/* Poll receive */
|
|
|
|
|
|
|
|
if (poll(&fd, 1, timeout_msec) <= 0)
|
|
|
|
{
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
/* Receiving */
|
|
|
|
|
|
|
|
CanardFrame received_frame;
|
|
|
|
|
|
|
|
socketcanreceive(sock_ins, &received_frame);
|
|
|
|
|
|
|
|
CanardTransfer receive;
|
|
|
|
result = canardRxAccept(ins,
|
|
|
|
&received_frame, /* The CAN frame received from the bus. */
|
|
|
|
0, /* If the transport is not redundant, use 0. */
|
|
|
|
&receive);
|
|
|
|
|
|
|
|
if (result < 0)
|
|
|
|
{
|
|
|
|
/* An error has occurred: either an argument is invalid or we've ran
|
|
|
|
* out of memory. It is possible to statically prove that an
|
|
|
|
* out-of-memory will never occur for a given application
|
|
|
|
* if the heap is sized correctly; for background, refer to the
|
|
|
|
* Robson's Proof and the documentation for O1Heap.
|
|
|
|
* Reception of an invalid frame is NOT an error.
|
|
|
|
*/
|
|
|
|
|
|
|
|
fprintf(stderr, "Receive error %ld\n", result);
|
|
|
|
}
|
|
|
|
else if (result == 1)
|
|
|
|
{
|
|
|
|
/* A transfer has been received, process it */
|
|
|
|
|
|
|
|
printf("Receive UAVCAN port id%d TODO process me\n",
|
|
|
|
receive.port_id);
|
|
|
|
|
|
|
|
ins->memory_free(ins, (void *)receive.payload);
|
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
|
|
|
/* Nothing to do.
|
|
|
|
* The received frame is either invalid or it's a non-last frame
|
|
|
|
* of a multi-frame transfer.
|
|
|
|
* Reception of an invalid frame is NOT reported as an error
|
|
|
|
* because it is not an error.
|
|
|
|
*/
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/****************************************************************************
|
|
|
|
* Name: canard_daemon
|
|
|
|
*
|
|
|
|
* Description:
|
|
|
|
*
|
|
|
|
****************************************************************************/
|
|
|
|
|
|
|
|
static int canard_daemon(int argc, char *argv[])
|
|
|
|
{
|
|
|
|
int errval = 0;
|
|
|
|
int can_fd = 0;
|
|
|
|
int pub = 1;
|
|
|
|
|
|
|
|
if (argc > 2)
|
|
|
|
{
|
|
|
|
for (int args = 2; args < argc; args++)
|
|
|
|
{
|
|
|
|
if (!strcmp(argv[args], "canfd"))
|
|
|
|
{
|
|
|
|
can_fd = 1;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (!strcmp(argv[args], "pub"))
|
|
|
|
{
|
|
|
|
pub = 1;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (!strcmp(argv[args], "sub"))
|
|
|
|
{
|
|
|
|
pub = 0;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
my_allocator = o1heapInit(&uavcan_heap, O1_HEAP_SIZE);
|
|
|
|
|
|
|
|
if (my_allocator == NULL)
|
|
|
|
{
|
|
|
|
printf("o1heapInit failed with size %d\n", O1_HEAP_SIZE);
|
|
|
|
errval = 2;
|
|
|
|
goto errout_with_dev;
|
|
|
|
}
|
|
|
|
|
|
|
|
CanardInstance ins = canardInit(&memallocate, &memfree);
|
|
|
|
|
|
|
|
if (can_fd)
|
|
|
|
{
|
|
|
|
ins.mtu_bytes = CANARD_MTU_CAN_FD;
|
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
|
|
|
ins.mtu_bytes = CANARD_MTU_CAN_CLASSIC;
|
|
|
|
}
|
|
|
|
|
2023-01-31 16:35:23 +01:00
|
|
|
ins.node_id = (pub ? CONFIG_EXAMPLES_LIBOPENCYPHAL_NODE_ID :
|
|
|
|
CONFIG_EXAMPLES_LIBOPENCYPHAL_NODE_ID + 1);
|
2022-08-01 15:44:33 +02:00
|
|
|
|
|
|
|
/* Open the CAN device for reading */
|
|
|
|
|
|
|
|
canardsocketinstance sock_ins;
|
2023-01-31 16:35:23 +01:00
|
|
|
socketcanopen(&sock_ins, CONFIG_EXAMPLES_LIBOPENCYPHAL_DEV, can_fd);
|
2022-08-01 15:44:33 +02:00
|
|
|
|
|
|
|
/* setup poll fd */
|
|
|
|
|
|
|
|
fd.fd = sock_ins.s;
|
|
|
|
fd.events = POLLIN;
|
|
|
|
|
|
|
|
if (sock_ins.s < 0)
|
|
|
|
{
|
|
|
|
printf("canard_daemon: ERROR: open %s failed: %d\n",
|
2023-01-31 16:35:23 +01:00
|
|
|
CONFIG_EXAMPLES_LIBOPENCYPHAL_DEV, errno);
|
2022-08-01 15:44:33 +02:00
|
|
|
errval = 2;
|
|
|
|
goto errout_with_dev;
|
|
|
|
}
|
|
|
|
|
|
|
|
printf("canard_daemon: canard initialized\n");
|
|
|
|
printf("start node (ID: %d Name: %s MTU: %d PUB: %d TOPIC_SIZE: %d)\n",
|
|
|
|
ins.node_id, APP_NODE_NAME, ins.mtu_bytes, pub, TOPIC_SIZE);
|
|
|
|
|
|
|
|
CanardRxSubscription heartbeat_subscription;
|
|
|
|
(void) canardRxSubscribe(&ins, /* Subscribe to messages uavcan.node.Heartbeat. */
|
|
|
|
CanardTransferKindMessage,
|
|
|
|
32085, /* The fixed Subject-ID of the Heartbeat message type (see DSDL definition). */
|
|
|
|
7, /* The maximum payload size (max DSDL object size) from the DSDL definition. */
|
|
|
|
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
|
|
|
|
&heartbeat_subscription);
|
|
|
|
|
|
|
|
CanardRxSubscription my_subscription;
|
|
|
|
(void) canardRxSubscribe(&ins,
|
|
|
|
CanardTransferKindMessage,
|
|
|
|
PORT_ID, /* The Service-ID to subscribe to. */
|
|
|
|
TOPIC_SIZE, /* The maximum payload size (max DSDL object size). */
|
|
|
|
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
|
|
|
|
&my_subscription);
|
|
|
|
|
|
|
|
g_canard_daemon_started = true;
|
|
|
|
uint64_t next_1hz_service_at = getmonotonictimestampusec();
|
|
|
|
|
|
|
|
for (; ; )
|
|
|
|
{
|
|
|
|
processtxrxonce(&ins, &sock_ins, 10);
|
|
|
|
|
|
|
|
const uint64_t ts = getmonotonictimestampusec();
|
|
|
|
|
|
|
|
if (ts >= next_1hz_service_at)
|
|
|
|
{
|
|
|
|
next_1hz_service_at += 1000000;
|
|
|
|
process1hztasks(&ins, ts);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
errout_with_dev:
|
|
|
|
|
|
|
|
g_canard_daemon_started = false;
|
|
|
|
printf("canard_daemon: Terminating!\n");
|
|
|
|
fflush(stdout);
|
|
|
|
return errval;
|
|
|
|
}
|
|
|
|
|
|
|
|
/****************************************************************************
|
|
|
|
* Name: canard_main
|
|
|
|
*
|
|
|
|
* Description:
|
|
|
|
*
|
|
|
|
****************************************************************************/
|
|
|
|
|
2023-01-31 16:35:23 +01:00
|
|
|
int opencyphal_main(int argc, FAR char *argv[])
|
2022-08-01 15:44:33 +02:00
|
|
|
{
|
|
|
|
int ret;
|
|
|
|
|
|
|
|
printf("canard_main: Starting canard_daemon\n");
|
|
|
|
|
|
|
|
if (g_canard_daemon_started)
|
|
|
|
{
|
|
|
|
printf("canard_main: receive and send task already running\n");
|
|
|
|
return EXIT_SUCCESS;
|
|
|
|
}
|
|
|
|
|
|
|
|
ret = task_create("canard_daemon",
|
2023-01-31 16:35:23 +01:00
|
|
|
CONFIG_EXAMPLES_LIBOPENCYPHAL_DAEMON_PRIORITY,
|
|
|
|
CONFIG_EXAMPLES_LIBOPENCYPHAL_DAEMON_STACK_SIZE,
|
2022-08-01 15:44:33 +02:00
|
|
|
canard_daemon, argv);
|
|
|
|
|
|
|
|
if (ret < 0)
|
|
|
|
{
|
|
|
|
int errcode = errno;
|
|
|
|
printf("canard_main: ERROR: Failed to start canard_daemon: %d\n",
|
|
|
|
errcode);
|
|
|
|
return EXIT_FAILURE;
|
|
|
|
}
|
|
|
|
|
|
|
|
printf("canard_main: canard_daemon started\n");
|
|
|
|
return EXIT_SUCCESS;
|
|
|
|
}
|