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,
®_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,
®_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,
®_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,
®_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