How to subscribe to a message correctly on Libcanard

Applications & Usage Libcanard
1

I use stm32G4 with CAN ID.
I have 2 devices: device A and device B.

Device A: i send real32 array on while cycle

static inline void sendReal32ArrayMessage(void) {
	static uint8_t real32_transfer_id = 0;
    uavcan_primitive_array_Real32_1_0 message = {
        .value.elements = {1.6f, 2.0f, 3.0f, 4.0f, 5.1f},
		.value.count = 5
    };

    size_t message_ser_buf_size = uavcan_primitive_array_Real32_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_;
    uint8_t message_ser_buf[uavcan_primitive_array_Real32_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_] = {0};

    if (uavcan_primitive_array_Real32_1_0_serialize_(&message, message_ser_buf, &message_ser_buf_size) < 0) {
        Error_Handler();
    }

    const CanardTransferMetadata transfer_metadata = {
        .priority = CanardPriorityNominal,
        .transfer_kind = CanardTransferKindMessage,
        .port_id = 4,  // Укажите свой ID порта
        .remote_node_id = CANARD_NODE_ID_UNSET,
        .transfer_id = real32_transfer_id,
    };

    if (canardTxPush(&queue, &canard, 0, &transfer_metadata, message_ser_buf_size, message_ser_buf) < 0) {
        Error_Handler();
    }

    real32_transfer_id++;
}

Device B: Have subscribe to this port_id and handle this message on CANFD interrupt handler

	CanardRxSubscription rx_message_subscription;
	if( canardRxSubscribe(        &canard,
								CanardTransferKindMessage,
								4,
								uavcan_primitive_array_Real32_1_0_EXTENT_BYTES_,
								CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
								&rx_message_subscription) != 1 ){ Error_Handler(); }

But in the canardRxAccept function and the serializer for this data type does not work correctly, what should I do?

2

We’ll need more details to help. Share the full source code of both nodes. Check that there is CAN traffic on the bus at all.

3

Client Code

/*
 * UavCan.c
 *
 *  Created on: October 21, 2024
 *      Author: goman_dp
 */
#include "UavCan.h"
#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>

#include "fdcan.h"
#include "usart.h"
#include "tim.h"
#include "gpio.h"

#include "o1heap/o1heap.h"
#include "libcanard/canard.h"
#include "nunavut/support/serialization.h"
// UavCan messages
#include "uavcan/node/Heartbeat_1_0.h"
#include "uavcan/node/ExecuteCommand_1_1.h"
#include "uavcan/_register/List_1_0.h"
#include "uavcan/_register/Access_1_0.h"
#include "uavcan/primitive/array/Real32_1_0.h"

#define HEAP_SIZE 1024
uint64_t tx_deadline_usec = 0;

O1HeapInstance* my_allocator;
CanardInstance canard;
CanardTxQueue 	queue;

CanardRxSubscription heartbeat_subscription;
CanardRxSubscription my_service_subscription;

// buffer for serialization of heartbeat message
size_t hbeat_ser_buf_size = uavcan_node_Heartbeat_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_;
uint8_t hbeat_ser_buf[uavcan_node_Heartbeat_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];

uint8_t my_message_transfer_id = 0;

// ----------------- Static functions definition -----------------------------
static void canFdConfigFilter(void);
static void uavCanRxSubscribe(void);
static void processCanardTxQueue(void);
static bool transmitFrame(uint32_t can_id, uint8_t* payload, size_t payload_size);
static bool pleaseTransmit(const CanardTxQueueItem* ti);
// -------------------- TX messages functions ------------------------------
static inline void sendExecuteCommandRequest(void);
static inline void sendReal32ArrayMessage(void);
//static inline void rxMessageCallback(CanardRxTransfer *transfer);
static inline void rxMessageCallback(CanardRxTransfer *transfer, uint8_t *array_ptr, size_t array_size);
// ----------------- Service functions definition -----------------------------
static inline uint8_t allocateHeapMemory(void);
static inline void*   memAllocate(CanardInstance* const canard, const size_t amount);
static inline void    memFree(CanardInstance* const canard, void* const pointer);
static uint8_t  	  LengthDecoder(uint32_t length);
static uint32_t 	  LengthCoder(uint8_t length);
static void 		  startMicrosecondsTimer(void);
static void 		  clearMicrosecondTimer(void);
static uint32_t 	  getCurrentMicroseconds(void);
static void 		  processReceivedTransfer(uint8_t redundant_interface_index, CanardRxTransfer* transfer);

// ----------------- Global functions -------------------------------------

uint8_t uavcanAppInit(void){
	startMicrosecondsTimer(); // start counting timer

	canard = canardInit(&memAllocate, &memFree);
	canard.node_id = CURRENT_PORT_ID;
	queue = canardTxInit(100, CANARD_MTU_CAN_FD);

	uavCanRxSubscribe();

	if (!allocateHeapMemory())
		return 0;

	HAL_FDCAN_Start(&hfdcan1);
	// Activate the notification for new data in FIFO0 for FDCAN1
	if (HAL_FDCAN_ActivateNotification(&hfdcan1, FDCAN_IT_RX_FIFO0_NEW_MESSAGE, 0) != HAL_OK) {
		Error_Handler();
	}
	canFdConfigFilter();

	return 1; // program normal
}

void uavcanAppProcess(void){
    uavcan_node_Heartbeat_1_0 test_heartbeat = {.uptime = getCurrentMicroseconds()/1000000u,
                                                .health = {uavcan_node_Health_1_0_NOMINAL},
                                                .mode = {uavcan_node_Mode_1_0_OPERATIONAL}};

    // Serialize the heartbeat message
    if (uavcan_node_Heartbeat_1_0_serialize_(&test_heartbeat, hbeat_ser_buf, &hbeat_ser_buf_size) < 0){
    	Error_Handler();
    }

    // Create a transfer for the heartbeat message
    const CanardTransferMetadata transfer_metadata = {.priority = CanardPriorityNominal,
                                                      .transfer_kind = CanardTransferKindMessage,
                                                      .port_id = uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_,
                                                      .remote_node_id = CANARD_NODE_ID_UNSET,
                                                      .transfer_id = my_message_transfer_id,};

    if(canardTxPush(&queue, &canard, 0, &transfer_metadata, hbeat_ser_buf_size, hbeat_ser_buf) < 0){
		Error_Handler();
	}

    uint32_t timestamp = HAL_GetTick();
    while(HAL_GetTick() < timestamp + 1000u)
    	processCanardTxQueue();

    my_message_transfer_id++;
}

void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo0ITs) {
	if((RxFifo0ITs & FDCAN_IT_RX_FIFO0_NEW_MESSAGE) != RESET) {
		  FDCAN_RxHeaderTypeDef RxHeader = {0};
		  uint8_t RxData[64] = {0};

		  if (HAL_FDCAN_GetRxMessage(hfdcan, FDCAN_RX_FIFO0, &RxHeader, RxData) != HAL_OK)
			  Error_Handler();

		  CanardFrame rxf;
		  rxf.extended_can_id = (uint32_t) RxHeader.Identifier;
		  rxf.payload_size 	  = (size_t) LengthDecoder(RxHeader.DataLength);
		  rxf.payload         = (void*)  RxData;

		  CanardRxTransfer transfer;
		  uint64_t rx_timestamp_usec = getCurrentMicroseconds();
		  int8_t result = canardRxAccept(&canard, rx_timestamp_usec, &rxf, 0, &transfer, NULL);
		  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.
		  }
		  else if (result == 1){
				processReceivedTransfer(0, &transfer);              // A transfer has been received, process it.
				canard.memory_free(&canard, transfer.payload);      // Deallocate the dynamic memory afterwards.
		  }
		  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.
			  rxMessageCallback(&transfer, RxData, (size_t)RxHeader.DataLength);
		  }

		  return ;
	}
}

// ------------------- Static functions ----------------------------------

static void canFdConfigFilter(void){
	FDCAN_FilterTypeDef sFilterConfig;
	sFilterConfig.IdType = FDCAN_STANDARD_ID;
	sFilterConfig.FilterIndex = 0;
	sFilterConfig.FilterType = FDCAN_FILTER_MASK;
	sFilterConfig.FilterConfig = FDCAN_FILTER_TO_RXFIFO0;
	sFilterConfig.FilterID1 = 0x11;
	sFilterConfig.FilterID2 = 0x11;
	//sFilterConfig.RxBufferIndex = 0;
	if (HAL_FDCAN_ConfigFilter(&hfdcan1, &sFilterConfig) != HAL_OK) {
		/* Filter configuration Error */
		Error_Handler();
	}
}

static void uavCanRxSubscribe(void){
	CanardRxSubscription rx_message_subscription;
	if( canardRxSubscribe(        &canard,
								CanardTransferKindMessage,
								4,
								uavcan_primitive_array_Real32_1_0_EXTENT_BYTES_,
								CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
								&rx_message_subscription) != 1 ){ Error_Handler(); }
	#if 0
	CanardRxSubscription command_subscription;
	if( canardRxSubscribe(        &canard,
								CanardTransferKindRequest,
								uavcan_node_ExecuteCommand_1_1_FIXED_PORT_ID_,
								uavcan_node_ExecuteCommand_Request_1_1_EXTENT_BYTES_,
								CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
								&command_subscription) != 1 ){ Error_Handler(); }

	CanardRxSubscription reg_list_subscription;
	if( canardRxSubscribe(        &canard,
								CanardTransferKindRequest,
								uavcan_register_List_1_0_FIXED_PORT_ID_,
								uavcan_register_List_Request_1_0_EXTENT_BYTES_,
								CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
								&reg_list_subscription) != 1 ){ Error_Handler(); }

	CanardRxSubscription reg_access_subscription;
	if( canardRxSubscribe(        &canard,
								CanardTransferKindRequest,
								uavcan_register_Access_1_0_FIXED_PORT_ID_,
								uavcan_register_Access_Request_1_0_EXTENT_BYTES_,
								CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
								&reg_access_subscription) != 1 ){ Error_Handler(); }
	#endif
}

static void processCanardTxQueue(void){
	for (const CanardTxQueueItem* ti = NULL; (ti = canardTxPeek(&queue)) != NULL;){  // Peek at the top of the queue.
		if ((0U == ti->tx_deadline_usec) || (ti->tx_deadline_usec > getCurrentMicroseconds())){  // Check the deadline.
			if (!pleaseTransmit(ti))               // Send the frame over this redundant CAN iface.
		    {
			    break;                             // If the driver is busy, break and retry later.
		    }
	    }
	    // After the frame is transmitted or if it has timed out while waiting, pop it from the queue and deallocate:
	    canard.memory_free(&canard, canardTxPop(&queue, ti));
	}
}

static bool transmitFrame(uint32_t can_id, uint8_t* payload, size_t payload_size){
	FDCAN_TxHeaderTypeDef fdcan1TxHeader;

    fdcan1TxHeader.Identifier = 0x1;
    fdcan1TxHeader.IdType = FDCAN_STANDARD_ID;
    fdcan1TxHeader.TxFrameType = FDCAN_DATA_FRAME;
    fdcan1TxHeader.DataLength = LengthCoder( payload_size );
    fdcan1TxHeader.ErrorStateIndicator = FDCAN_ESI_ACTIVE;
    fdcan1TxHeader.BitRateSwitch = FDCAN_BRS_ON;
    fdcan1TxHeader.FDFormat = FDCAN_FD_CAN;
    fdcan1TxHeader.TxEventFifoControl = FDCAN_NO_TX_EVENTS;
    fdcan1TxHeader.MessageMarker = 0;

    if (HAL_FDCAN_AddMessageToTxFifoQ(&hfdcan1, &fdcan1TxHeader, payload) != HAL_OK)
    {
        return false;  // Отправка не удалась
    }

    return true;  // Отправка успешна
}

static bool pleaseTransmit(const CanardTxQueueItem* ti){
    bool success = transmitFrame(ti->frame.extended_can_id, ti->frame.payload, ti->frame.payload_size);
    return success;
}


// -------------------- TX messages functions ------------------------------

static inline void sendExecuteCommandRequest(void){
	static uint8_t execute_command_transfer_id = 0;
	uavcan_node_ExecuteCommand_Request_1_1 request = {
        .command = uavcan_node_ExecuteCommand_Request_1_1_COMMAND_RESTART,
        .parameter = {0}
    };

    size_t request_ser_buf_size = uavcan_node_ExecuteCommand_Request_1_1_SERIALIZATION_BUFFER_SIZE_BYTES_;
    uint8_t request_ser_buf[uavcan_node_ExecuteCommand_Request_1_1_SERIALIZATION_BUFFER_SIZE_BYTES_];

    if (uavcan_node_ExecuteCommand_Request_1_1_serialize_(&request, request_ser_buf, &request_ser_buf_size) < 0) {
        Error_Handler();
    }

    const CanardTransferMetadata transfer_metadata = {
        .priority = CanardPriorityNominal,
        .transfer_kind = CanardTransferKindRequest,
        .port_id = uavcan_node_ExecuteCommand_1_1_FIXED_PORT_ID_,
        .remote_node_id = 42,  // ID узла, которому отправляется запрос
        .transfer_id = execute_command_transfer_id,
    };

    if (canardTxPush(&queue, &canard, 0, &transfer_metadata, request_ser_buf_size, request_ser_buf) < 0) {
        Error_Handler();
    }
}

static inline void sendReal32ArrayMessage(void) {
	static uint8_t real32_transfer_id = 0;
    uavcan_primitive_array_Real32_1_0 message = {
        .value.elements = {1.0f, 2.0f, 3.0f, 4.0f},
		.value.count = 4
    };

    size_t message_ser_buf_size = uavcan_primitive_array_Real32_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_;
    uint8_t message_ser_buf[uavcan_primitive_array_Real32_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];

    if (uavcan_primitive_array_Real32_1_0_serialize_(&message, message_ser_buf, &message_ser_buf_size) < 0) {
        Error_Handler();
    }

    const CanardTransferMetadata transfer_metadata = {
        .priority = CanardPriorityNominal,
        .transfer_kind = CanardTransferKindMessage,
        .port_id = CURRENT_PORT_ID,  // Укажите свой ID порта
        .remote_node_id = CANARD_NODE_ID_UNSET,
        .transfer_id = real32_transfer_id,
    };

    if (canardTxPush(&queue, &canard, 0, &transfer_metadata, message_ser_buf_size, message_ser_buf) < 0) {
        Error_Handler();
    }

    real32_transfer_id++;
}


static inline void rxMessageCallback(CanardRxTransfer *transfer, uint8_t *array_ptr, size_t array_size){
	  uavcan_primitive_array_Real32_1_0 array = {0};
	  size_t array_ser_buf_size = uavcan_primitive_array_Real32_1_0_EXTENT_BYTES_;

	  //if ( uavcan_primitive_array_Real32_1_0_deserialize_( &array, transfer->payload, &array_ser_buf_size) < 0 )
	  if ( uavcan_primitive_array_Real32_1_0_deserialize_( &array, array_ptr, &array_size) < 0 )
	  {
	    Error_Handler();
	  }
}
// ------------------- Service functions ------------------------------------

static inline uint8_t allocateHeapMemory(void){
	void* heap_memory = malloc(HEAP_SIZE);
	if (heap_memory == NULL)
		return 0;

	uintptr_t aligned_address = (uintptr_t)heap_memory;
	if (aligned_address % O1HEAP_ALIGNMENT != 0)
	    aligned_address += O1HEAP_ALIGNMENT - (aligned_address % O1HEAP_ALIGNMENT);

	heap_memory = (void*)aligned_address;

	my_allocator = o1heapInit(heap_memory, HEAP_SIZE);
	return 1;
}

static inline void* memAllocate(CanardInstance* const canard, const size_t amount){
    (void) canard;
    return o1heapAllocate(my_allocator, amount);
}

static inline void memFree(CanardInstance* const canard, void* const pointer){
    (void) canard;
    o1heapFree(my_allocator, pointer);
}

/*
  * @brief Decodes FDCAN_data_length_code into the decimal length of FDCAN message
  * @param[in]          length           FDCAN_data_length_code
  * @retval             uint8_t         Decimal message length (bytes)
*/
static uint8_t LengthDecoder( uint32_t length )
{
  switch( length )
  {
    case FDCAN_DLC_BYTES_0:     return 0;
    case FDCAN_DLC_BYTES_1:     return 1;
    case FDCAN_DLC_BYTES_2:     return 2;
    case FDCAN_DLC_BYTES_3:     return 3;
    case FDCAN_DLC_BYTES_4:     return 4;
    case FDCAN_DLC_BYTES_5:     return 5;
    case FDCAN_DLC_BYTES_6:     return 6;
    case FDCAN_DLC_BYTES_7:     return 7;
    case FDCAN_DLC_BYTES_8:     return 8;
    case FDCAN_DLC_BYTES_12:    return 12;
    case FDCAN_DLC_BYTES_16:    return 16;
    case FDCAN_DLC_BYTES_20:    return 20;
    case FDCAN_DLC_BYTES_24:    return 24;
    case FDCAN_DLC_BYTES_32:    return 32;
    case FDCAN_DLC_BYTES_48:    return 48;
    case FDCAN_DLC_BYTES_64:    return 64;

    default:
      while(1); //error
  }
}

/*
  * @brief Codes decimal length of FDCAN message into the FDCAN_data_length_code
  * @param[in]          length              Decimal message length (bytes)
  * @retval             FDCAN_data_length_code        Code of required message length
*/
static uint32_t LengthCoder( uint8_t length )
{
  switch( length )
  {
    case 0:     return FDCAN_DLC_BYTES_0;
    case 1:     return FDCAN_DLC_BYTES_1;
    case 2:     return FDCAN_DLC_BYTES_2;
    case 3:     return FDCAN_DLC_BYTES_3;
    case 4:     return FDCAN_DLC_BYTES_4;
    case 5:     return FDCAN_DLC_BYTES_5;
    case 6:     return FDCAN_DLC_BYTES_6;
    case 7:     return FDCAN_DLC_BYTES_7;
    case 8:     return FDCAN_DLC_BYTES_8;
    case 12:    return FDCAN_DLC_BYTES_12;
    case 16:    return FDCAN_DLC_BYTES_16;
    case 20:    return FDCAN_DLC_BYTES_20;
    case 24:    return FDCAN_DLC_BYTES_24;
    case 32:    return FDCAN_DLC_BYTES_32;
    case 48:    return FDCAN_DLC_BYTES_48;
    case 64:    return FDCAN_DLC_BYTES_64;

    default:
      while(1); //error
  }
}

static void startMicrosecondsTimer(void){
	HAL_TIM_Base_Start(&htim16);
}

static void clearMicrosecondTimer(void){
	__HAL_TIM_SET_COUNTER(&htim16, 0);
}

static uint32_t getCurrentMicroseconds(void){
	return __HAL_TIM_GET_COUNTER(&htim16);
}

static void processReceivedTransfer(uint8_t redundant_interface_index, CanardRxTransfer* transfer)
{
    /*printf("Received transfer on interface %d:\n", redundant_interface_index);
    printf("  Priority: %d\n", transfer->metadata.priority);
    printf("  Transfer kind: %d\n", transfer->metadata.transfer_kind);
    printf("  Port ID: %d\n", transfer->metadata.port_id);
    printf("  Remote node ID: %d\n", transfer->metadata.remote_node_id);
    printf("  Transfer ID: %d\n", transfer->metadata.transfer_id);
    printf("  Payload size: %zu\n", transfer->payload_size);
    printf("  Payload: ");
    for (size_t i = 0; i < transfer->payload_size; i++)
    {
        printf("%02X ", ((uint8_t*)transfer->payload)[i]);
    }
    printf("\n");*/

    ///if (transfer->metadata.port_id == 4){
    	//rxMessageCallback(transfer);
    //}
}

// Functions for use printf()
int __io_putchar(uint8_t ch)
{
  HAL_UART_Transmit(&hlpuart1, &ch, 1, HAL_MAX_DELAY);
  return ch;
}

int _write(int file, char *ptr, int len)
{
  int DataIdx;
  for (DataIdx = 0; DataIdx < len; DataIdx++)
  {
    __io_putchar(*ptr++);
  }
  return len;
}

Server code

/*
 * UavCan.c
 *
 *  Created on: October 21, 2024
 *      Author: goman_dp
 */
#include "UavCan.h"
#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>

#include "fdcan.h"
#include "usart.h"
#include "tim.h"
#include "gpio.h"

#include "o1heap/o1heap.h"
#include "libcanard/canard.h"
#include "nunavut/support/serialization.h"
#include "uavcan/node/Heartbeat_1_0.h"
#include "uavcan/node/ExecuteCommand_1_1.h"
#include "uavcan/_register/List_1_0.h"
#include "uavcan/_register/Access_1_0.h"
#include "uavcan/primitive/array/Real32_1_0.h"
#include "uavcan/primitive/array/Real64_1_0.h"

#define HEAP_SIZE 1024
uint64_t tx_deadline_usec = 0;

O1HeapInstance* my_allocator;
CanardInstance canard;
CanardTxQueue 	queue;

CanardRxSubscription heartbeat_subscription;
CanardRxSubscription my_service_subscription;

// buffer for serialization of heartbeat message
size_t hbeat_ser_buf_size = uavcan_node_Heartbeat_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_;
uint8_t hbeat_ser_buf[uavcan_node_Heartbeat_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_] = {0};

uint8_t my_message_transfer_id = 0;

// ----------------- Static functions definition -----------------------------
static void canFdConfigFilter(void);
static void uavCanRxSubscribe(void);
static void processCanardTxQueue(void);
static bool transmitFrame(uint32_t can_id, uint8_t* payload, size_t payload_size);
static bool pleaseTransmit(const CanardTxQueueItem* ti);
// -------------------- TX messages functions ------------------------------
static inline void sendExecuteCommandRequest(void);
static inline void sendReal32ArrayMessage(void);
// ----------------- Service functions definition -----------------------------
static inline uint8_t   allocateHeapMemory(void);
static inline void* 	memAllocate(CanardInstance* const canard, const size_t amount);
static inline void  	memFree(CanardInstance* const canard, void* const pointer);
static uint8_t  		LengthDecoder(uint32_t length);
static uint32_t 		LengthCoder(uint8_t length);
static void 			startMicrosecondsTimer(void);
static void 			clearMicrosecondTimer(void);
static uint32_t 		getCurrentMicroseconds(void);
static void 			processReceivedTransfer(uint8_t redundant_interface_index, CanardRxTransfer* transfer);
// ----------------- Global functions -------------------------------------

uint8_t uavcanAppInit(void){
	startMicrosecondsTimer(); // start counting timer

	if (!allocateHeapMemory())
		return 0;

	canard = canardInit(&memAllocate, &memFree);
	canard.node_id = CURRENT_PORT_ID;
	queue = canardTxInit(100, CANARD_MTU_CAN_FD);

	uavCanRxSubscribe();

	HAL_FDCAN_Start(&hfdcan1);
	// Activate the notification for new data in FIFO0 for FDCAN1
	if (HAL_FDCAN_ActivateNotification(&hfdcan1, FDCAN_IT_RX_FIFO0_NEW_MESSAGE, 0) != HAL_OK) {
		Error_Handler();
	}
	canFdConfigFilter();

	return 1; // program normal
}

void uavcanAppProcess(void){
    uavcan_node_Heartbeat_1_0 test_heartbeat = {.uptime = getCurrentMicroseconds()/1000000u,
                                                .health = {uavcan_node_Health_1_0_NOMINAL},
                                                .mode = {uavcan_node_Mode_1_0_OPERATIONAL}};

    // Serialize the heartbeat message
    if (uavcan_node_Heartbeat_1_0_serialize_(&test_heartbeat, hbeat_ser_buf, &hbeat_ser_buf_size) < 0){
    	Error_Handler();
    }

    // Create a transfer for the heartbeat message
    const CanardTransferMetadata transfer_metadata = {.priority = CanardPriorityNominal,
                                                      .transfer_kind = CanardTransferKindMessage,
                                                      .port_id = uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_,
                                                      .remote_node_id = CANARD_NODE_ID_UNSET,
                                                      .transfer_id = my_message_transfer_id,};

    if(canardTxPush(&queue, &canard, 0, &transfer_metadata, hbeat_ser_buf_size, hbeat_ser_buf) < 0){
		Error_Handler();
	}

    //sendExecuteCommandRequest();
    sendReal32ArrayMessage();

    uint32_t timestamp = HAL_GetTick();
    while(HAL_GetTick() < timestamp + 1000u)
    	processCanardTxQueue();

    my_message_transfer_id++;
}

void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo0ITs) {
	if((RxFifo0ITs & FDCAN_IT_RX_FIFO0_NEW_MESSAGE) != RESET) {
		  FDCAN_RxHeaderTypeDef RxHeader = {0};
		  uint8_t RxData[64] = {0};

		  if (HAL_FDCAN_GetRxMessage(hfdcan, FDCAN_RX_FIFO0, &RxHeader, RxData) != HAL_OK)
			  Error_Handler();

		  CanardFrame rxf;
		  rxf.extended_can_id = (uint32_t) RxHeader.Identifier;
		  rxf.payload_size 	  = (size_t) LengthDecoder(RxHeader.DataLength);
		  rxf.payload         = (void*)  RxData;

		  CanardRxTransfer transfer;
		  uint64_t rx_timestamp_usec = getCurrentMicroseconds();
		  int8_t result = canardRxAccept(&canard, rx_timestamp_usec, &rxf, 0, &transfer, NULL);
		  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.
		  }
		  else if (result == 1){
		    processReceivedTransfer(0, &transfer);              // A transfer has been received, process it.
		    canard.memory_free(&canard, transfer.payload);      // Deallocate the dynamic memory afterwards.
		  }
		  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.
		  }

		  return ;
	}
}

// ------------------- Static functions ----------------------------------

static void canFdConfigFilter(void){
	FDCAN_FilterTypeDef sFilterConfig;
	sFilterConfig.IdType = FDCAN_STANDARD_ID;
	sFilterConfig.FilterIndex = 0;
	sFilterConfig.FilterType = FDCAN_FILTER_MASK;
	sFilterConfig.FilterConfig = FDCAN_FILTER_TO_RXFIFO0;
	sFilterConfig.FilterID1 = 0x1;
	sFilterConfig.FilterID2 = 0x1;
	//sFilterConfig.RxBufferIndex = 0;
	if (HAL_FDCAN_ConfigFilter(&hfdcan1, &sFilterConfig) != HAL_OK) {
		/* Filter configuration Error */
		Error_Handler();
	}
}

static void uavCanRxSubscribe(void){
	#if 0
	CanardRxSubscription command_subscription;
	if( canardRxSubscribe(        &canard,
								CanardTransferKindRequest,
								uavcan_node_ExecuteCommand_1_1_FIXED_PORT_ID_,
								uavcan_node_ExecuteCommand_Request_1_1_EXTENT_BYTES_,
								CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
								&command_subscription) != 1 ){ Error_Handler(); }

	CanardRxSubscription reg_list_subscription;
	if( canardRxSubscribe(        &canard,
								CanardTransferKindRequest,
								uavcan_register_List_1_0_FIXED_PORT_ID_,
								uavcan_register_List_Request_1_0_EXTENT_BYTES_,
								CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
								&reg_list_subscription) != 1 ){ Error_Handler(); }

	CanardRxSubscription reg_access_subscription;
	if( canardRxSubscribe(        &canard,
								CanardTransferKindRequest,
								uavcan_register_Access_1_0_FIXED_PORT_ID_,
								uavcan_register_Access_Request_1_0_EXTENT_BYTES_,
								CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
								&reg_access_subscription) != 1 ){ Error_Handler(); }
	#endif
	CanardRxSubscription rx_message_subscription;
	if( canardRxSubscribe(        &canard,
								CanardTransferKindMessage,
								4,
								uavcan_primitive_array_Real32_1_0_EXTENT_BYTES_,
								CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
								&rx_message_subscription) != 1 ){ Error_Handler(); }

}

static void processCanardTxQueue(void){
	for (const CanardTxQueueItem* ti = NULL; (ti = canardTxPeek(&queue)) != NULL;){  // Peek at the top of the queue.
		if ((0U == ti->tx_deadline_usec) || (ti->tx_deadline_usec > getCurrentMicroseconds())){  // Check the deadline.
			if (!pleaseTransmit(ti))               // Send the frame over this redundant CAN iface.
		    {
			    break;                             // If the driver is busy, break and retry later.
		    }
	    }
	    // After the frame is transmitted or if it has timed out while waiting, pop it from the queue and deallocate:
	    canard.memory_free(&canard, canardTxPop(&queue, ti));
	}
}

static bool transmitFrame(uint32_t can_id, uint8_t* payload, size_t payload_size){
	FDCAN_TxHeaderTypeDef fdcan1TxHeader;

    fdcan1TxHeader.Identifier = 0x11;
    fdcan1TxHeader.IdType = FDCAN_STANDARD_ID;
    fdcan1TxHeader.TxFrameType = FDCAN_DATA_FRAME;
    fdcan1TxHeader.DataLength = LengthCoder( payload_size );
    fdcan1TxHeader.ErrorStateIndicator = FDCAN_ESI_ACTIVE;
    fdcan1TxHeader.BitRateSwitch = FDCAN_BRS_ON;
    fdcan1TxHeader.FDFormat = FDCAN_FD_CAN;
    fdcan1TxHeader.TxEventFifoControl = FDCAN_NO_TX_EVENTS;
    fdcan1TxHeader.MessageMarker = 0;

    if (HAL_FDCAN_AddMessageToTxFifoQ(&hfdcan1, &fdcan1TxHeader, payload) != HAL_OK)
    {
        return false;  // Отправка не удалась
    }

    return true;  // Отправка успешна
}

static bool pleaseTransmit(const CanardTxQueueItem* ti){
    bool success = transmitFrame(ti->frame.extended_can_id, ti->frame.payload, ti->frame.payload_size);
    return success;
}


// -------------------- TX messages functions ------------------------------

static inline void sendExecuteCommandRequest(void){
	static uint8_t execute_command_transfer_id = 0;
	uavcan_node_ExecuteCommand_Request_1_1 request = {
        .command = uavcan_node_ExecuteCommand_Request_1_1_COMMAND_RESTART,
        .parameter = {0}
    };

    size_t request_ser_buf_size = uavcan_node_ExecuteCommand_Request_1_1_SERIALIZATION_BUFFER_SIZE_BYTES_;
    uint8_t request_ser_buf[uavcan_node_ExecuteCommand_Request_1_1_SERIALIZATION_BUFFER_SIZE_BYTES_] = {0};

    if (uavcan_node_ExecuteCommand_Request_1_1_serialize_(&request, request_ser_buf, &request_ser_buf_size) < 0) {
        Error_Handler();
    }

    const CanardTransferMetadata transfer_metadata = {
        .priority = CanardPriorityNominal,
        .transfer_kind = CanardTransferKindRequest,
        .port_id = uavcan_node_ExecuteCommand_1_1_FIXED_PORT_ID_,
        .remote_node_id = DESTANATION_NODE_ID,  // ID узла, которому отправляется запрос
        .transfer_id = execute_command_transfer_id,
    };

    if (canardTxPush(&queue, &canard, 0, &transfer_metadata, request_ser_buf_size, request_ser_buf) < 0) {
        Error_Handler();
    }
}

static inline void sendReal32ArrayMessage(void) {
	static uint8_t real32_transfer_id = 0;
    uavcan_primitive_array_Real32_1_0 message = {
        .value.elements = {1.6f, 2.0f, 3.0f, 4.0f, 5.1f},
		.value.count = 5
    };

    size_t message_ser_buf_size = uavcan_primitive_array_Real32_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_;
    uint8_t message_ser_buf[uavcan_primitive_array_Real32_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_] = {0};

    if (uavcan_primitive_array_Real32_1_0_serialize_(&message, message_ser_buf, &message_ser_buf_size) < 0) {
        Error_Handler();
    }

    const CanardTransferMetadata transfer_metadata = {
        .priority = CanardPriorityNominal,
        .transfer_kind = CanardTransferKindMessage,
        .port_id = 4,  // Укажите свой ID порта
        .remote_node_id = CANARD_NODE_ID_UNSET,
        .transfer_id = real32_transfer_id,
    };

    if (canardTxPush(&queue, &canard, 0, &transfer_metadata, message_ser_buf_size, message_ser_buf) < 0) {
        Error_Handler();
    }

    real32_transfer_id++;
}

// ------------------- Service functions ------------------------------------
static inline uint8_t allocateHeapMemory(void){
	void* heap_memory = malloc(HEAP_SIZE);
	if (heap_memory == NULL)
		return 0;

	uintptr_t aligned_address = (uintptr_t)heap_memory;
	if (aligned_address % O1HEAP_ALIGNMENT != 0)
	    aligned_address += O1HEAP_ALIGNMENT - (aligned_address % O1HEAP_ALIGNMENT);

	heap_memory = (void*)aligned_address;

	my_allocator = o1heapInit(heap_memory, HEAP_SIZE);
	return 1;
}

static inline void* memAllocate(CanardInstance* const canard, const size_t amount){
    (void) canard;
    return o1heapAllocate(my_allocator, amount);
}

static inline void memFree(CanardInstance* const canard, void* const pointer){
    (void) canard;
    o1heapFree(my_allocator, pointer);
}

/*
  * @brief Decodes FDCAN_data_length_code into the decimal length of FDCAN message
  * @param[in]          length           FDCAN_data_length_code
  * @retval             uint8_t         Decimal message length (bytes)
*/
static uint8_t LengthDecoder( uint32_t length )
{
  switch( length )
  {
    case FDCAN_DLC_BYTES_0:     return 0;
    case FDCAN_DLC_BYTES_1:     return 1;
    case FDCAN_DLC_BYTES_2:     return 2;
    case FDCAN_DLC_BYTES_3:     return 3;
    case FDCAN_DLC_BYTES_4:     return 4;
    case FDCAN_DLC_BYTES_5:     return 5;
    case FDCAN_DLC_BYTES_6:     return 6;
    case FDCAN_DLC_BYTES_7:     return 7;
    case FDCAN_DLC_BYTES_8:     return 8;
    case FDCAN_DLC_BYTES_12:    return 12;
    case FDCAN_DLC_BYTES_16:    return 16;
    case FDCAN_DLC_BYTES_20:    return 20;
    case FDCAN_DLC_BYTES_24:    return 24;
    case FDCAN_DLC_BYTES_32:    return 32;
    case FDCAN_DLC_BYTES_48:    return 48;
    case FDCAN_DLC_BYTES_64:    return 64;

    default:
      while(1); //error
  }
}

/*
  * @brief Codes decimal length of FDCAN message into the FDCAN_data_length_code
  * @param[in]          length              Decimal message length (bytes)
  * @retval             FDCAN_data_length_code        Code of required message length
*/
static uint32_t LengthCoder( uint8_t length )
{
  switch( length )
  {
    case 0:     return FDCAN_DLC_BYTES_0;
    case 1:     return FDCAN_DLC_BYTES_1;
    case 2:     return FDCAN_DLC_BYTES_2;
    case 3:     return FDCAN_DLC_BYTES_3;
    case 4:     return FDCAN_DLC_BYTES_4;
    case 5:     return FDCAN_DLC_BYTES_5;
    case 6:     return FDCAN_DLC_BYTES_6;
    case 7:     return FDCAN_DLC_BYTES_7;
    case 8:     return FDCAN_DLC_BYTES_8;
    case 12:    return FDCAN_DLC_BYTES_12;
    case 16:    return FDCAN_DLC_BYTES_16;
    case 20:    return FDCAN_DLC_BYTES_20;
    case 24:    return FDCAN_DLC_BYTES_24;
    case 32:    return FDCAN_DLC_BYTES_32;
    case 48:    return FDCAN_DLC_BYTES_48;
    case 64:    return FDCAN_DLC_BYTES_64;

    default:
      while(1); //error
  }
}

static void startMicrosecondsTimer(void){
	HAL_TIM_Base_Start(&htim16);
}

static void clearMicrosecondTimer(void){
	__HAL_TIM_SET_COUNTER(&htim16, 0);
}

static uint32_t getCurrentMicroseconds(void){
	return __HAL_TIM_GET_COUNTER(&htim16);
}

static void processReceivedTransfer(uint8_t redundant_interface_index, CanardRxTransfer* transfer)
{
    printf("Received transfer on interface %d:\n", redundant_interface_index);
    printf("  Priority: %d\n", transfer->metadata.priority);
    printf("  Transfer kind: %d\n", transfer->metadata.transfer_kind);
    printf("  Port ID: %d\n", transfer->metadata.port_id);
    printf("  Remote node ID: %d\n", transfer->metadata.remote_node_id);
    printf("  Transfer ID: %d\n", transfer->metadata.transfer_id);
    printf("  Payload size: %zu\n", transfer->payload_size);
    printf("  Payload: ");
    for (size_t i = 0; i < transfer->payload_size; i++)
    {
        printf("%02X ", ((uint8_t*)transfer->payload)[i]);
    }
    printf("\n");
}

I receive packets from the server and they are correct, I was even able to decrypt from HEX to float, but canardRxAccept returns 0 in response to the message and the deserializer cannot decrypt this packet normally

4

  1. You have race conditions in both applications where you mutate global states both from the main context and from the interrupt contexts.

  2. Since you already use malloc, you probably don’t need o1heap.

5

  1. Does this affect the operation of subscriptions in the protocol?
    I can decode a message normally only if I send data from CAN to the desirializer directly rxMessageCallback(&transfer, RxData, (size_t)RxHeader.DataLength);

  2. Am I using 01heap incorrectly?

6

  1. It affects everything. As it stands, you are lucky the program does anything at all.

  2. I didn’t look into that very closely; my point is that you probably don’t need it if you can use malloc/free.

7 
#include "UavCan.h"
#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>

#include "fdcan.h"
#include "usart.h"
#include "tim.h"
#include "gpio.h"

#include "o1heap/o1heap.h"
#include "libcanard/canard.h"
#include "nunavut/support/serialization.h"
#include "CircularBuffer/CircularBuffer.h"
// UavCan messages
#include "uavcan/node/Heartbeat_1_0.h"
#include "uavcan/node/ExecuteCommand_1_1.h"
#include "uavcan/_register/List_1_0.h"
#include "uavcan/_register/Access_1_0.h"
#include "uavcan/primitive/array/Real32_1_0.h"

// TODO: Add mutex for interrupt callback and appProcess()

#define HEAP_SIZE 1024
uint64_t tx_deadline_usec = 0;

O1HeapInstance* my_allocator;
CanardInstance canard;
CanardTxQueue 	queue;

CanardRxSubscription heartbeat_subscription;
CanardRxSubscription my_service_subscription;

// buffer for serialization of heartbeat message
size_t hbeat_ser_buf_size = uavcan_node_Heartbeat_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_;
uint8_t hbeat_ser_buf[uavcan_node_Heartbeat_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];

uint8_t my_message_transfer_id = 0;

// ----------------- Static functions definition -----------------------------
static void processRxBuffer(void);
static void canFdConfigFilter(void);
static void uavCanRxSubscribe(void);
static void processCanardTxQueue(void);
static bool transmitFrame(uint32_t can_id, uint8_t* payload, size_t payload_size);
static bool pleaseTransmit(const CanardTxQueueItem* ti);
// -------------------- TX messages functions ------------------------------
static inline void sendExecuteCommandRequest(void);
static inline void sendReal32ArrayMessage(void);
static inline void rxMessageCallback(CanardRxTransfer *transfer);
// ----------------- Service functions definition -----------------------------
static inline uint8_t allocateHeapMemory(void);
static inline void*   memAllocate(CanardInstance* const canard, const size_t amount);
static inline void    memFree(CanardInstance* const canard, void* const pointer);
static uint8_t  	  LengthDecoder(uint32_t length);
static uint32_t 	  LengthCoder(uint8_t length);
static void 		  startMicrosecondsTimer(void);
static void 		  clearMicrosecondTimer(void);
static uint32_t 	  getCurrentMicroseconds(void);
static void 		  processReceivedTransfer(uint8_t redundant_interface_index, CanardRxTransfer* transfer);

// ----------------- Global functions -------------------------------------

uint8_t uavcanAppInit(void){
	startMicrosecondsTimer(); // start counting timer

	canard = canardInit(&memAllocate, &memFree);
	canard.node_id = CURRENT_PORT_ID;
	queue = canardTxInit(100, CANARD_MTU_CAN_FD);

	uavCanRxSubscribe();

	if (!allocateHeapMemory())
		return 0;

	HAL_FDCAN_Start(&hfdcan1);
	// Activate the notification for new data in FIFO0 for FDCAN1
	if (HAL_FDCAN_ActivateNotification(&hfdcan1, FDCAN_IT_RX_FIFO0_NEW_MESSAGE, 0) != HAL_OK) {
		Error_Handler();
	}

	canFdConfigFilter();

	return 1; // program normal
}

void uavcanAppProcess(void){
	processRxBuffer();

    uavcan_node_Heartbeat_1_0 test_heartbeat = {.uptime = getCurrentMicroseconds()/1000000u,
                                                .health = {uavcan_node_Health_1_0_NOMINAL},
                                                .mode = {uavcan_node_Mode_1_0_OPERATIONAL}};

    // Serialize the heartbeat message
    if (uavcan_node_Heartbeat_1_0_serialize_(&test_heartbeat, hbeat_ser_buf, &hbeat_ser_buf_size) < 0){
    	Error_Handler();
    }

    // Create a transfer for the heartbeat message
    const CanardTransferMetadata transfer_metadata = {.priority = CanardPriorityNominal,
                                                      .transfer_kind = CanardTransferKindMessage,
                                                      .port_id = uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_,
                                                      .remote_node_id = CANARD_NODE_ID_UNSET,
                                                      .transfer_id = my_message_transfer_id,};

    if(canardTxPush(&queue, &canard, 0, &transfer_metadata, hbeat_ser_buf_size, hbeat_ser_buf) < 0){
		Error_Handler();
	}

    uint32_t timestamp = HAL_GetTick();
    while(HAL_GetTick() < timestamp + 1000u)
    	processCanardTxQueue();

    my_message_transfer_id++;
}

void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo0ITs) {
	if((RxFifo0ITs & FDCAN_IT_RX_FIFO0_NEW_MESSAGE) != RESET) {
		FDCAN_RxHeaderTypeDef RxHeader = {0};
		uint8_t RxData[64] = {0};

		if (HAL_FDCAN_GetRxMessage(hfdcan, FDCAN_RX_FIFO0, &RxHeader, RxData) != HAL_OK)
		  Error_Handler();

        if (!addToRxBuffer(&RxHeader, RxData)) {
            Error_Handler(); // Обработка ошибки: буфер переполнен
        }
		#if 0
		CanardFrame rxf;
		rxf.extended_can_id = (uint32_t) RxHeader.Identifier;
		rxf.payload_size 	  = (size_t) LengthDecoder(RxHeader.DataLength);
		rxf.payload         = (void*)  RxData;

		CanardRxTransfer transfer;
		uint64_t rx_timestamp_usec = getCurrentMicroseconds();
		int8_t result = canardRxAccept(&canard, rx_timestamp_usec, &rxf, 0, &transfer, NULL);
		if (result < 0){ }
		else if (result == 1){
			processReceivedTransfer(0, &transfer);              // A transfer has been received, process it.
			canard.memory_free(&canard, transfer.payload);      // Deallocate the dynamic memory afterwards.
		}

		return ;
		#endif
	}
}

static void processRxBuffer(void){
    FDCAN_RxHeaderTypeDef header;
    uint8_t data[64];

    while (getFromRxBuffer(&header, data)) {
        CanardFrame rxf;
        rxf.extended_can_id = (uint32_t) header.Identifier;
        rxf.payload_size = (size_t) LengthDecoder(header.DataLength);
        rxf.payload = (void*) data;

        CanardRxTransfer transfer;
        uint64_t rx_timestamp_usec = getCurrentMicroseconds();
        int8_t result = canardRxAccept(&canard, rx_timestamp_usec, &rxf, 0, &transfer, NULL);
        if (result < 0) {
            // Error
        } else if (result == 1) {
            processReceivedTransfer(0, &transfer);              // A transfer has been received, process it.
            canard.memory_free(&canard, transfer.payload);      // Deallocate the dynamic memory afterwards.
        }
    }
}

// ------------------- Static functions ----------------------------------

static void canFdConfigFilter(void){
	FDCAN_FilterTypeDef sFilterConfig;
	sFilterConfig.IdType = FDCAN_STANDARD_ID;
	sFilterConfig.FilterIndex = 0;
	sFilterConfig.FilterType = FDCAN_FILTER_MASK;
	sFilterConfig.FilterConfig = FDCAN_FILTER_TO_RXFIFO0;
	sFilterConfig.FilterID1 = 0x11;
	sFilterConfig.FilterID2 = 0x11;
	//sFilterConfig.RxBufferIndex = 0;
	if (HAL_FDCAN_ConfigFilter(&hfdcan1, &sFilterConfig) != HAL_OK) {
		/* Filter configuration Error */
		Error_Handler();
	}
}

static void uavCanRxSubscribe(void){
	CanardRxSubscription rx_message_subscription;
	if( canardRxSubscribe(        &canard,
								CanardTransferKindMessage,
								4,
								uavcan_primitive_array_Real32_1_0_EXTENT_BYTES_,
								CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
								&rx_message_subscription) != 1 ){ Error_Handler(); }
	#if 0
	CanardRxSubscription command_subscription;
	if( canardRxSubscribe(        &canard,
								CanardTransferKindRequest,
								uavcan_node_ExecuteCommand_1_1_FIXED_PORT_ID_,
								uavcan_node_ExecuteCommand_Request_1_1_EXTENT_BYTES_,
								CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
								&command_subscription) != 1 ){ Error_Handler(); }

	CanardRxSubscription reg_list_subscription;
	if( canardRxSubscribe(        &canard,
								CanardTransferKindRequest,
								uavcan_register_List_1_0_FIXED_PORT_ID_,
								uavcan_register_List_Request_1_0_EXTENT_BYTES_,
								CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
								&reg_list_subscription) != 1 ){ Error_Handler(); }

	CanardRxSubscription reg_access_subscription;
	if( canardRxSubscribe(        &canard,
								CanardTransferKindRequest,
								uavcan_register_Access_1_0_FIXED_PORT_ID_,
								uavcan_register_Access_Request_1_0_EXTENT_BYTES_,
								CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
								&reg_access_subscription) != 1 ){ Error_Handler(); }
	#endif
}

static void processCanardTxQueue(void){
	for (const CanardTxQueueItem* ti = NULL; (ti = canardTxPeek(&queue)) != NULL;){  // Peek at the top of the queue.
		if ((0U == ti->tx_deadline_usec) || (ti->tx_deadline_usec > getCurrentMicroseconds())){  // Check the deadline.
			if (!pleaseTransmit(ti))               // Send the frame over this redundant CAN iface.
		    {
			    break;                             // If the driver is busy, break and retry later.
		    }
	    }
	    // After the frame is transmitted or if it has timed out while waiting, pop it from the queue and deallocate:
	    canard.memory_free(&canard, canardTxPop(&queue, ti));
	}
}

static bool transmitFrame(uint32_t can_id, uint8_t* payload, size_t payload_size){
	FDCAN_TxHeaderTypeDef fdcan1TxHeader;

    fdcan1TxHeader.Identifier = 0x1;
    fdcan1TxHeader.IdType = FDCAN_STANDARD_ID;
    fdcan1TxHeader.TxFrameType = FDCAN_DATA_FRAME;
    fdcan1TxHeader.DataLength = LengthCoder( payload_size );
    fdcan1TxHeader.ErrorStateIndicator = FDCAN_ESI_ACTIVE;
    fdcan1TxHeader.BitRateSwitch = FDCAN_BRS_ON;
    fdcan1TxHeader.FDFormat = FDCAN_FD_CAN;
    fdcan1TxHeader.TxEventFifoControl = FDCAN_NO_TX_EVENTS;
    fdcan1TxHeader.MessageMarker = 0;

    if (HAL_FDCAN_AddMessageToTxFifoQ(&hfdcan1, &fdcan1TxHeader, payload) != HAL_OK)
    {
        return false;  // Отправка не удалась
    }

    return true;  // Отправка успешна
}

static bool pleaseTransmit(const CanardTxQueueItem* ti){
    bool success = transmitFrame(ti->frame.extended_can_id, ti->frame.payload, ti->frame.payload_size);
    return success;
}


// -------------------- TX messages functions ------------------------------

static inline void sendExecuteCommandRequest(void){
	static uint8_t execute_command_transfer_id = 0;
	uavcan_node_ExecuteCommand_Request_1_1 request = {
        .command = uavcan_node_ExecuteCommand_Request_1_1_COMMAND_RESTART,
        .parameter = {0}
    };

    size_t request_ser_buf_size = uavcan_node_ExecuteCommand_Request_1_1_SERIALIZATION_BUFFER_SIZE_BYTES_;
    uint8_t request_ser_buf[uavcan_node_ExecuteCommand_Request_1_1_SERIALIZATION_BUFFER_SIZE_BYTES_];

    if (uavcan_node_ExecuteCommand_Request_1_1_serialize_(&request, request_ser_buf, &request_ser_buf_size) < 0) {
        Error_Handler();
    }

    const CanardTransferMetadata transfer_metadata = {
        .priority = CanardPriorityNominal,
        .transfer_kind = CanardTransferKindRequest,
        .port_id = uavcan_node_ExecuteCommand_1_1_FIXED_PORT_ID_,
        .remote_node_id = 42,  // ID узла, которому отправляется запрос
        .transfer_id = execute_command_transfer_id,
    };

    if (canardTxPush(&queue, &canard, 0, &transfer_metadata, request_ser_buf_size, request_ser_buf) < 0) {
        Error_Handler();
    }
}

static inline void sendReal32ArrayMessage(void) {
	static uint8_t real32_transfer_id = 0;
    uavcan_primitive_array_Real32_1_0 message = {
        .value.elements = {1.0f, 2.0f, 3.0f, 4.0f},
		.value.count = 4
    };

    size_t message_ser_buf_size = uavcan_primitive_array_Real32_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_;
    uint8_t message_ser_buf[uavcan_primitive_array_Real32_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];

    if (uavcan_primitive_array_Real32_1_0_serialize_(&message, message_ser_buf, &message_ser_buf_size) < 0) {
        Error_Handler();
    }

    const CanardTransferMetadata transfer_metadata = {
        .priority = CanardPriorityNominal,
        .transfer_kind = CanardTransferKindMessage,
        .port_id = CURRENT_PORT_ID,  // Укажите свой ID порта
        .remote_node_id = CANARD_NODE_ID_UNSET,
        .transfer_id = real32_transfer_id,
    };

    if (canardTxPush(&queue, &canard, 0, &transfer_metadata, message_ser_buf_size, message_ser_buf) < 0) {
        Error_Handler();
    }

    real32_transfer_id++;
}


static inline void rxMessageCallback(CanardRxTransfer *transfer){
	  uavcan_primitive_array_Real32_1_0 array = {0};
	  size_t array_ser_buf_size = uavcan_primitive_array_Real32_1_0_EXTENT_BYTES_;

	  if ( uavcan_primitive_array_Real32_1_0_deserialize_( &array, transfer->payload, &array_ser_buf_size) < 0 )
	  {
	    Error_Handler();
	  }
}
// ------------------- Service functions ------------------------------------

static inline uint8_t allocateHeapMemory(void){
	void* heap_memory = malloc(HEAP_SIZE);
	if (heap_memory == NULL)
		return 0;

	uintptr_t aligned_address = (uintptr_t)heap_memory;
	if (aligned_address % O1HEAP_ALIGNMENT != 0)
	    aligned_address += O1HEAP_ALIGNMENT - (aligned_address % O1HEAP_ALIGNMENT);

	heap_memory = (void*)aligned_address;

	my_allocator = o1heapInit(heap_memory, HEAP_SIZE);
	return 1;
}

static inline void* memAllocate(CanardInstance* const canard, const size_t amount){
    (void) canard;
    return o1heapAllocate(my_allocator, amount);
}

static inline void memFree(CanardInstance* const canard, void* const pointer){
    (void) canard;
    o1heapFree(my_allocator, pointer);
}

/*
  * @brief Decodes FDCAN_data_length_code into the decimal length of FDCAN message
  * @param[in]          length           FDCAN_data_length_code
  * @retval             uint8_t         Decimal message length (bytes)
*/
static uint8_t LengthDecoder( uint32_t length )
{
  switch( length )
  {
    case FDCAN_DLC_BYTES_0:     return 0;
    case FDCAN_DLC_BYTES_1:     return 1;
    case FDCAN_DLC_BYTES_2:     return 2;
    case FDCAN_DLC_BYTES_3:     return 3;
    case FDCAN_DLC_BYTES_4:     return 4;
    case FDCAN_DLC_BYTES_5:     return 5;
    case FDCAN_DLC_BYTES_6:     return 6;
    case FDCAN_DLC_BYTES_7:     return 7;
    case FDCAN_DLC_BYTES_8:     return 8;
    case FDCAN_DLC_BYTES_12:    return 12;
    case FDCAN_DLC_BYTES_16:    return 16;
    case FDCAN_DLC_BYTES_20:    return 20;
    case FDCAN_DLC_BYTES_24:    return 24;
    case FDCAN_DLC_BYTES_32:    return 32;
    case FDCAN_DLC_BYTES_48:    return 48;
    case FDCAN_DLC_BYTES_64:    return 64;

    default:
      while(1); //error
  }
}

/*
  * @brief Codes decimal length of FDCAN message into the FDCAN_data_length_code
  * @param[in]          length              Decimal message length (bytes)
  * @retval             FDCAN_data_length_code        Code of required message length
*/
static uint32_t LengthCoder( uint8_t length )
{
  switch( length )
  {
    case 0:     return FDCAN_DLC_BYTES_0;
    case 1:     return FDCAN_DLC_BYTES_1;
    case 2:     return FDCAN_DLC_BYTES_2;
    case 3:     return FDCAN_DLC_BYTES_3;
    case 4:     return FDCAN_DLC_BYTES_4;
    case 5:     return FDCAN_DLC_BYTES_5;
    case 6:     return FDCAN_DLC_BYTES_6;
    case 7:     return FDCAN_DLC_BYTES_7;
    case 8:     return FDCAN_DLC_BYTES_8;
    case 12:    return FDCAN_DLC_BYTES_12;
    case 16:    return FDCAN_DLC_BYTES_16;
    case 20:    return FDCAN_DLC_BYTES_20;
    case 24:    return FDCAN_DLC_BYTES_24;
    case 32:    return FDCAN_DLC_BYTES_32;
    case 48:    return FDCAN_DLC_BYTES_48;
    case 64:    return FDCAN_DLC_BYTES_64;

    default:
      while(1); //error
  }
}

static void startMicrosecondsTimer(void){
	HAL_TIM_Base_Start(&htim16);
}

static void clearMicrosecondTimer(void){
	__HAL_TIM_SET_COUNTER(&htim16, 0);
}

static uint32_t getCurrentMicroseconds(void){
	return __HAL_TIM_GET_COUNTER(&htim16);
}

static void processReceivedTransfer(uint8_t redundant_interface_index, CanardRxTransfer* transfer)
{
    printf("Received transfer on interface %d:\n", redundant_interface_index);
    printf("  Priority: %d\n", transfer->metadata.priority);
    printf("  Transfer kind: %d\n", transfer->metadata.transfer_kind);
    printf("  Port ID: %d\n", transfer->metadata.port_id);
    printf("  Remote node ID: %d\n", transfer->metadata.remote_node_id);
    printf("  Transfer ID: %d\n", transfer->metadata.transfer_id);
    printf("  Payload size: %zu\n", transfer->payload_size);
    printf("  Payload: ");
    for (size_t i = 0; i < transfer->payload_size; i++)
    {
        printf("%02X ", ((uint8_t*)transfer->payload)[i]);
    }
    printf("\n");

    if (transfer->metadata.port_id == 4){
    	rxMessageCallback(transfer);
    }
}

// Functions for use printf()
int __io_putchar(uint8_t ch)
{
  HAL_UART_Transmit(&hlpuart1, &ch, 1, HAL_MAX_DELAY);
  return ch;
}

int _write(int file, char *ptr, int len)
{
  int DataIdx;
  for (DataIdx = 0; DataIdx < len; DataIdx++)
  {
    __io_putchar(*ptr++);
  }
  return len;
}

Is this what you meant? Or do you need to use rtos with mutex?

When used this way, the subscription mechanism still does not work

8

I don’t know what these new functions do ( addToRxBuffer et al) so cannot comment on that. You also have a lifetime issue – the lifetime of your subscription objects ends upon return from uavCanRxSubscribe, rendering the library state invalid.

9

Interrupt Handler and Data Processing

I made sure that in the interrupt handler I only copied data to the ring buffer, and did all the processing in a regular while loop. I took out CanardRxSubscription rx_message_subscription; to the global scope.

Received Message

I receive the message:
05 cd cc cc 3f 00 00 00 40 00 00 40 40 00 00 80 40 33 33 a3 40 00 00 f9

Copy

  • 05 - Count
  • cd cc cc 3f 00 00 00 40 00 00 40 40 00 00 80 40 33 33 a3 40 - This message is {1.6f, 2.0f, 3.0f, 4.0f, 5.1f}, I converted from hex to float.

However, the libcanard library itself does not accept a subscription message.

Metadata Encoding in Libcanard

Where is metadata encoded in Libcanard?

10

I found a bug why the wrong FDCAN_TxHeaderTypeDef was being sent

You need to use the identifier from the metadata

This is the correct send function

static void processCanardTxQueue(void){
	for (const CanardTxQueueItem* ti = NULL; (ti = canardTxPeek(&queue)) != NULL;){  // Peek at the top of the queue.
		if ((0U == ti->tx_deadline_usec) || (ti->tx_deadline_usec > getCurrentMicroseconds())){  // Check the deadline.
			FDCAN_TxHeaderTypeDef fdcan1TxHeader;

		    fdcan1TxHeader.Identifier = ti->frame.extended_can_id;
		    fdcan1TxHeader.IdType = FDCAN_STANDARD_ID;
		    fdcan1TxHeader.TxFrameType = FDCAN_DATA_FRAME;
		    fdcan1TxHeader.DataLength = LengthCoder(ti->frame.payload_size);
		    fdcan1TxHeader.ErrorStateIndicator = FDCAN_ESI_ACTIVE;
		    fdcan1TxHeader.BitRateSwitch = FDCAN_BRS_ON;
		    fdcan1TxHeader.FDFormat = FDCAN_FD_CAN;
		    fdcan1TxHeader.TxEventFifoControl = FDCAN_NO_TX_EVENTS;
		    fdcan1TxHeader.MessageMarker = 0;

		    if (HAL_FDCAN_AddMessageToTxFifoQ(&hfdcan1, &fdcan1TxHeader, (uint8_t *)ti->frame.payload) != HAL_OK)
		    {
		        Error_Handler();
		    }
	    }
	    // After the frame is transmitted or if it has timed out while waiting, pop it from the queue and deallocate:
	    canard.memory_free(&canard, canardTxPop(&queue, ti));
	}
}

I’ll do it in a couple of days and post a full-fledged example, thanks