/****************************************************************************
* apps/examples/opencyphal/canard_main.c
*
* 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
#define APP_NODE_NAME CONFIG_EXAMPLES_LIBOPENCYPHAL_APP_NODE_NAME
/****************************************************************************
* Private Data
****************************************************************************/
/* Arena for memory allocation, used by the library */
#define O1_HEAP_SIZE CONFIG_EXAMPLES_LIBOPENCYPHAL_NODE_MEM_POOL_SIZE
/* 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;
}
ins.node_id = (pub ? CONFIG_EXAMPLES_LIBOPENCYPHAL_NODE_ID :
CONFIG_EXAMPLES_LIBOPENCYPHAL_NODE_ID + 1);
/* Open the CAN device for reading */
canardsocketinstance sock_ins;
socketcanopen(&sock_ins, CONFIG_EXAMPLES_LIBOPENCYPHAL_DEV, can_fd);
/* 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",
CONFIG_EXAMPLES_LIBOPENCYPHAL_DEV, errno);
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:
*
****************************************************************************/
int opencyphal_main(int argc, FAR char *argv[])
{
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",
CONFIG_EXAMPLES_LIBOPENCYPHAL_DAEMON_PRIORITY,
CONFIG_EXAMPLES_LIBOPENCYPHAL_DAEMON_STACK_SIZE,
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;
}