zrzk
/
proximity_uavcan_converter
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
6 Commits
7 Branches
0 Tags
10 MiB
 
 
 
Branch: boat
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from 'boat'
${ noResults }
proximity_uavcan_converter/UAVCAN/uavcan.c

792 lines
24 KiB
Raw Permalink Blame History

#include "uavcan.h"
#include <string.h>
#include <stdlib.h>
#include "main.h"
#include "math.h"
#include "can.h"
#include "stm32f1xx_hal_tim.h"
#define MEMPOOL_OBJECTS 16
#define ALLOCATION_TIMEOUT_MS 60000
#define DEFAULT_NODE_ID 60
static void handle_begin_firmware_update(CanardInstance *ins, CanardRxTransfer *transfer);
static uint8_t is_node_restart_requested(void);
static uint8_t is_node_restart_requested(void);
static const uint8_t RESTART_NODE_MAGIC_NUMBER_ARRAY[UAVCAN_RESTART_NODE_REQUEST_MAX_SIZE] = {0x1E, 0x1B, 0x55, 0xCE, 0xAC};
static CanardInstance g_canard; //The library instance
static uint8_t g_canard_memory_pool[2048];
static uint32_t spin_time = 0;
uint8_t feedback_status = 0;
static uint8_t node_unique_id[UNIQUE_ID_LENGTH_BYTES];
//static const uint8_t gimbal_id = 0xC5; //Arena for memory allocation, used by the library
//static uint32_t g_uptime = 0;
//static uint8_t node_health = UAVCAN_NODE_HEALTH_OK;
//static uint8_t node_mode = UAVCAN_NODE_MODE_INITIALIZATION;
static uint64_t send_next_node_id_allocation_request_at; ///< When the next node ID allocation request should be sent
static uint8_t node_id_allocation_unique_id_offset;
static uint8_t transfer_id = 0;
uint32_t node_restart_at; //TODO check where used
uint32_t last_invocation;
uint32_t allocation_init_tick;
//static uint32_t last_state_time = 0;
//extern QueueHandle_t uavcan_rev_queue;
typedef struct
{
// FieldTypes
uint8_t id; // bit len 8
float pitch; // float16 Saturate
float roll; // float16 Saturate
float yaw; // float16 Saturate
} zrzk_equipment_uav_Euler;
static struct
{
uint64_t request_timer_begin_us;
uint64_t request_delay_us;
uint32_t unique_id_offset;
} allocation_state;
#ifndef CANARD_INTERNAL_SATURATE
#define CANARD_INTERNAL_SATURATE(x, max) (((x) > max) ? max : ((-(x) > max) ? (-max) : (x)));
#endif
#ifndef CANARD_INTERNAL_SATURATE_UNSIGNED
#define CANARD_INTERNAL_SATURATE_UNSIGNED(x, max) (((x) >= max) ? max : (x));
#endif
#if defined(__GNUC__)
#define CANARD_MAYBE_UNUSED(x) x __attribute__((unused))
#else
#define CANARD_MAYBE_UNUSED(x) x
#endif
extern TIM_HandleTypeDef htim2;
static void allocation_timer_expired(void);
static void handle_allocation_data_broadcast(CanardInstance *ins, CanardRxTransfer *transfer);
static void allocation_start_request_timer(void);
static void allocation_start_followup_timer(void);
static bool allocation_running(void);
static float uavcan_get_radom_float(void);
uint64_t micros(void);
static void allocation_init(void);
static void allocation_timer_expired(void);
bool shouldAcceptTransfer(const CanardInstance *ins,
uint64_t *out_data_type_signature,
uint16_t data_type_id,
CanardTransferType transfer_type,
uint8_t source_node_id)
{
// (void)source_node_id;
if (canardGetLocalNodeID(ins) == CANARD_BROADCAST_NODE_ID)
{
// If we're in the process of allocation of dynamic node ID, accept only relevant transfers.
if ((transfer_type == CanardTransferTypeBroadcast) &&
(data_type_id == UAVCAN_NODE_ID_ALLOCATION_DATA_TYPE_ID))
{
*out_data_type_signature = UAVCAN_NODE_ID_ALLOCATION_DATA_TYPE_SIGNATURE;
return true;
}
else
{
return false;
}
}
else
{
switch (data_type_id)
{
case UAVCAN_GET_NODE_INFO_DATA_TYPE_ID:
*out_data_type_signature = UAVCAN_GET_NODE_INFO_DATA_TYPE_SIGNATURE;
return true;
case UAVCAN_RESTART_NODE_DATA_TYPE_ID:
*out_data_type_signature = UAVCAN_RESTART_NODE_DATA_TYPE_SIGNATURE;
return true;
case UAVCAN_BEGIN_FIRMWARE_UPDATE_DATA_TYPE_ID:
*out_data_type_signature = UAVCAN_BEGIN_FIRMWARE_UPDATE_DATA_TYPE_SIGNATURE;
return true;
case UAVCAN_FILE_READ_DATA_TYPE_ID:
*out_data_type_signature = UAVCAN_FILE_READ_DATA_TYPE_SIGNATURE;
return true;
case UAVCAN_PARAM_GETSET_DATA_TYPE_ID:
*out_data_type_signature = UAVCAN_PARAM_GETSET_DATA_TYPE_SIGNATURE;
return true;
default:
return false;
}
}
}
void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer)
{
if (transfer->transfer_type == CanardTransferTypeBroadcast)
{
action_transfer_received_broadcast(ins, transfer);
}
else if (transfer->transfer_type == CanardTransferTypeResponse)
{
action_transfer_received_response(ins, transfer);
}
else
{
action_transfer_received_request(ins, transfer);
}
}
void action_transfer_received_request(CanardInstance *ins, CanardRxTransfer *transfer)
{
switch (transfer->data_type_id)
{
case UAVCAN_GET_NODE_INFO_DATA_TYPE_ID:
handle_canard_get_node_info(transfer);
break;
case UAVCAN_RESTART_NODE_DATA_TYPE_ID:
handle_restart_node(transfer);
break;
case UAVCAN_BEGIN_FIRMWARE_UPDATE_DATA_TYPE_ID:
handle_begin_firmware_update(ins, transfer);
break;
case UAVCAN_PARAM_GETSET_DATA_TYPE_ID:
// handle_param_getset(transfer);
break;
default:
break;
}
}
void action_transfer_received_broadcast(CanardInstance *ins, CanardRxTransfer *transfer)
{
switch (transfer->data_type_id)
{
case UAVCAN_NODE_ID_ALLOCATION_DATA_TYPE_ID:
if (canardGetLocalNodeID(ins) == CANARD_BROADCAST_NODE_ID)
{
handle_allocation_data_broadcast(&g_canard, transfer);
}
break;
default:
break;
}
}
void action_transfer_received_response(CanardInstance *ins, CanardRxTransfer *transfer)
{
switch (transfer->data_type_id)
{
default:
break;
}
}
void receive_canard(void)
{
CanardCANFrame rx_frame;
uint32_t time_stame = HAL_GetTick();
while (canardSTM32Receive(&rx_frame) > 0)
{
canardHandleRxFrame(&g_canard, &rx_frame, HAL_GetTick());
if ((HAL_GetTick() - time_stame) > 100)
{
return;
}
}
}
void handle_uavcan_rx_frame(const uavcan_rev_t *msg)
{
canardHandleRxFrame(&g_canard, &msg->canard_CAN_frame, msg->tick_ms * 1000);
}
static void handle_canard_get_node_info(CanardRxTransfer *transfer)
{
uint8_t buffer[UAVCAN_GET_NODE_INFO_RESPONSE_MAX_SIZE];
memset(buffer, 0, UAVCAN_GET_NODE_INFO_RESPONSE_MAX_SIZE);
uint16_t len = 0;
len = make_node_info_message(buffer);
int result = canardRequestOrRespond(&g_canard,
transfer->source_node_id,
UAVCAN_GET_NODE_INFO_DATA_TYPE_SIGNATURE,
UAVCAN_GET_NODE_INFO_DATA_TYPE_ID,
&transfer->transfer_id,
transfer->priority,
CanardResponse,
&buffer[0],
(uint16_t)len);
}
void uavcan_init(void)
{
CanardSTM32CANTimings timings;
int result = canardSTM32ComputeCANTimings(HAL_RCC_GetPCLK1Freq(), 1000000, &timings);
if (result)
{
__ASM volatile("BKPT #01");
}
result = canardSTM32Init(&timings, CanardSTM32IfaceModeNormal);
if (result)
{
__ASM volatile("BKPT #01");
}
canardInit(&g_canard, // Uninitialized library instance
g_canard_memory_pool, // Raw memory chunk used for dynamic allocation
sizeof(g_canard_memory_pool), // Size of the above, in bytes
onTransferReceived, // Callback, see CanardOnTransferReception
shouldAcceptTransfer, // Callback, see CanardShouldAcceptTransfer
NULL);
canardSetLocalNodeID(&g_canard, uavcan_local_node);
}
void uavcan_init_with_cubemx(void)
{
uavcan_init_fifo(uavcan_local_node);
canardInit(&g_canard, // Uninitialized library instance
g_canard_memory_pool, // Raw memory chunk used for dynamic allocation
sizeof(g_canard_memory_pool), // Size of the above, in bytes
onTransferReceived, // Callback, see CanardOnTransferReception
shouldAcceptTransfer, // Callback, see CanardShouldAcceptTransfer
NULL);
canardSetLocalNodeID(&g_canard, uavcan_local_node);
can_init_irq();
}
void send_canard(void)
{
osStatus_t status;
if (uavcan_send_mutexHandle != NULL)
{
status = osMutexAcquire(uavcan_send_mutexHandle, 20U);
}
for (const CanardCANFrame *txf = NULL; (txf = canardPeekTxQueue(&g_canard)) != NULL;)
{
const int16_t tx_res = canardSTM32Transmit(txf);
if (tx_res < 0) // Failure - drop the frame and report
{
break;
}
else if (tx_res > 0) // Success - just drop the frame
{
canardPopTxQueue(&g_canard);
}
else // Timeout - just exit and try again later
{
break;
}
}
if (status == osOK && uavcan_send_mutexHandle != NULL)
{
status = osMutexRelease(uavcan_send_mutexHandle);
}
}
void publish_node_status(void)
{
if (HAL_GetTick() < spin_time + CANARD_SPIN_PERIOD)
return; // rate limiting
spin_time = HAL_GetTick();
uint8_t buffer[UAVCAN_NODE_STATUS_MESSAGE_SIZE];
static uint8_t transfer_id = 0; // This variable MUST BE STATIC; refer to the libcanard documentation for the background
make_node_status_message(buffer);
canardBroadcast(&g_canard,
UAVCAN_NODE_STATUS_DATA_TYPE_SIGNATURE,
UAVCAN_NODE_STATUS_DATA_TYPE_ID,
&transfer_id,
CANARD_TRANSFER_PRIORITY_LOW,
buffer,
UAVCAN_NODE_STATUS_MESSAGE_SIZE);
}
void make_node_status_message(uint8_t buffer[UAVCAN_NODE_STATUS_MESSAGE_SIZE])
{
uint8_t node_health = UAVCAN_NODE_HEALTH_OK;
uint8_t node_mode = UAVCAN_NODE_MODE_OPERATIONAL;
memset(buffer, 0, UAVCAN_NODE_STATUS_MESSAGE_SIZE);
uint32_t uptime_sec = (HAL_GetTick() / 1000);
canardEncodeScalar(buffer, 0, 32, &uptime_sec);
canardEncodeScalar(buffer, 32, 2, &node_health);
canardEncodeScalar(buffer, 34, 3, &node_mode);
}
uint16_t make_node_info_message(uint8_t buffer[UAVCAN_GET_NODE_INFO_RESPONSE_MAX_SIZE])
{
memset(buffer, 0, UAVCAN_GET_NODE_INFO_RESPONSE_MAX_SIZE);
make_node_status_message(buffer);
buffer[7] = APP_VERSION_MAJOR;
buffer[8] = APP_VERSION_MINOR;
buffer[9] = 1; // Optional field flags, VCS commit is set
uint32_t githash = GIT_HASH;
canardEncodeScalar(buffer, 80, 32, &githash);
buffer[22] = 0; //get_hw_version_minor();
buffer[23] = 0; //get_hw_version_patch();
read_unique_id(&buffer[24]);
const size_t name_len = strlen(APP_NODE_NAME);
memcpy(&buffer[41], APP_NODE_NAME, name_len);
return 41 + name_len;
}
void canard_get_node_info_handle(CanardRxTransfer *transfer)
{
uint8_t buffer[UAVCAN_GET_NODE_INFO_RESPONSE_MAX_SIZE];
memset(buffer, 0, UAVCAN_GET_NODE_INFO_RESPONSE_MAX_SIZE);
uint16_t len = 0;
len = make_node_info_message(buffer);
int result = canardRequestOrRespond(&g_canard,
transfer->source_node_id,
UAVCAN_GET_NODE_INFO_DATA_TYPE_SIGNATURE,
UAVCAN_GET_NODE_INFO_DATA_TYPE_ID,
&transfer->transfer_id,
// transfer->priority,
CANARD_TRANSFER_PRIORITY_HIGH,
CanardResponse,
&buffer[0],
(uint16_t)len);
}
/**
* Returns a pseudo random float in the range [0, 1].
*/
static float get_random_float(void)
{
static bool initialized = false;
if (!initialized)
{
initialized = true;
// desig_get_unique_id((uint32_t*)&node_unique_id[0]);
// const uint32_t *unique_32 = (uint32_t *)&node_unique_id[0];
// srand(HAL_GetTick() ^ *unique_32);
srand(HAL_GetTick());
}
return (float)rand() / (float)RAND_MAX;
}
void publish_log_message(uavcan_log_level_t log_level, const char *source, const char *text)
{
uint8_t buffer[UAVCAN_DEBUG_LOG_MESSAGE_MESSAGE_SIZE] = {0};
uint8_t source_len = strlen(source) & 31;
uint8_t msg_len = strlen(text);
static uint8_t transfer_id;
if (msg_len > 90)
{
msg_len = 90;
}
/* Use manual mutex locking here to lock the global buffer as well */
buffer[0] = (log_level << 5) | source_len;
memcpy(&buffer[1], source, source_len);
memcpy(&buffer[1 + source_len], text, msg_len);
canardBroadcast(
&g_canard,
UAVCAN_DEBUG_LOG_MESSAGE_DATA_TYPE_SIGNATURE,
UAVCAN_DEBUG_LOG_MESSAGE_DATA_TYPE_ID,
&transfer_id,
CANARD_TRANSFER_PRIORITY_LOWEST,
buffer,
1 + source_len + msg_len);
}
void send_uavcan_data(uavcan_send_data_t *send_data)
{
switch (send_data->id)
{
case UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_ID:
/* code */
break;
case UAVCAN_NODE_STATUS_DATA_TYPE_ID:
publish_node_status();
break;
case ZRZK_EQUIPMENT_RANGE_SENSOR_PROXIMITY_ID:
publish_proximity(&send_data->content.proximity);
break;
default:
break;
}
}
void publish_proximity(uavcan_proximity_t *msg)
{
uint8_t buffer[ZRZK_EQUIPMENT_RANGE_SENSOR_PROXIMITY_MAX_SIZE] = {0};
uint32_t len = zrzk_equipment_range_sensor_Proximity_encode(msg, &buffer);
if (len > 0)
{
canardBroadcast(
&g_canard,
ZRZK_EQUIPMENT_RANGE_SENSOR_PROXIMITY_SIGNATURE,
ZRZK_EQUIPMENT_RANGE_SENSOR_PROXIMITY_ID,
&transfer_id,
CANARD_TRANSFER_PRIORITY_HIGH,
buffer,
len);
}
}
void send_uavcan_proximity(uavcan_proximity_t *data)
{
uavcan_send_data_t msg = {0};
if (uavcan_send_queueHandle != NULL)
{
msg.id = ZRZK_EQUIPMENT_RANGE_SENSOR_PROXIMITY_ID;
memcpy(&msg.content.proximity, data, sizeof(uavcan_proximity_t));
osMessageQueuePut(uavcan_send_queueHandle, &msg, NULL, 10U);
}
}
void request_node_restart(void)
{
memset(&bootloader_interface, 0, sizeof(bootloader_interface));
bootloader_interface.node_id = get_node_id(); //config_get_uint(CONFIG_NODE_ID); //TODO use param
bootloader_interface.magic = BOOTLOADER_INTERFACE_VALID_MAGIC;
if (node_restart_at == 0)
{
node_restart_at = HAL_GetTick() + 1000; //chVTGetSystemTimeX() + TIME_S2I(1);
}
}
void check_need_to_jump_boot(void)
{
if ((last_invocation + 1000) < HAL_GetTick())
{
// process1HzTasks(getMonotonicTimestamp());
last_invocation = HAL_GetTick(); /* Intentionally exclude processing time */
if (node_restart_at && (HAL_GetTick() > node_restart_at))
{
NVIC_SystemReset();
}
}
}
static uint8_t is_node_restart_requested(void)
{
return (node_restart_at != 0);
}
static void handle_begin_firmware_update(CanardInstance *ins, CanardRxTransfer *transfer)
{
uint8_t response = 0;
uint8_t filename_length = transfer->payload_len - 1; /* Tail array optimisation for this data type */
uint8_t source_id = transfer->payload_head[0];
if (is_node_restart_requested())
{
response = 1;
}
if (response == 0)
{
bootloader_interface.request_from_node_id = source_id;
bootloader_interface.request_file_name_length = filename_length;
for (uint8_t i = 0; i < filename_length; ++i)
{
canardDecodeScalar(transfer, 8 + 8 * i, 8, 0, &bootloader_interface.request_file_name[i]);
}
request_node_restart();
}
//canardLock(canardReleaseRxTransferPayload(ins, transfer));
const int resp_res =
canardRequestOrRespond(ins,
transfer->source_node_id,
UAVCAN_BEGIN_FIRMWARE_UPDATE_DATA_TYPE_SIGNATURE,
UAVCAN_BEGIN_FIRMWARE_UPDATE_DATA_TYPE_ID,
&transfer->transfer_id,
transfer->priority,
CanardResponse,
&response,
1);
}
static void read_unique_id(uint8_t *out_uid)
{
memcpy(out_uid, (void *)UID_BASE, UNIQUE_STM32_DEVICE_ID_LENGTH);
}
uint8_t get_node_id(void)
{
return canardGetLocalNodeID(&g_canard);
}
void uavcan_init_fifo(uint8_t node_id)
{
uint32_t id = 0;
uint32_t mask = 0;
uint8_t bank_index = 1;
uavcan_get_uavcan_id_mask_by_msg_id(UAVCAN_BEGIN_FIRMWARE_UPDATE_DATA_TYPE_ID, &id, &mask);
can_filter_init_with_mask(bank_index, id, mask, 1);
bank_index++;
uavcan_get_uavcan_id_mask_by_msg_id(ZRZK_EQUIPMENT_RANGE_SENSOR_PROXIMITY_ID, &id, &mask);
can_filter_init_with_mask(bank_index, id, mask, 1);
bank_index++;
uavcan_get_uavcan_id_mask_by_msg_id(UAVCAN_RESTART_NODE_DATA_TYPE_ID, &id, &mask);
can_filter_init_with_mask(bank_index, id, mask, 1);
bank_index++;
uavcan_get_uavcan_id_mask_by_node_id(node_id, &id, &mask);
can_filter_init_with_mask(bank_index, id, mask, 1);
bank_index++;
}
void uavcan_cleanup_stale_transfers(uint64_t time_us)
{
canardCleanupStaleTransfers(&g_canard, time_us);
}
void handle_restart_node(CanardRxTransfer *transfer)
{
uint8_t response = 0;
if (memcmp(transfer->payload_head, RESTART_NODE_MAGIC_NUMBER_ARRAY, UAVCAN_RESTART_NODE_REQUEST_MAX_SIZE) == 0)
{
response = 1;
/* Asynchronous restart so we can send response package first.
*/
request_node_restart();
}
const int resp_res = canardRequestOrRespond(&g_canard,
transfer->source_node_id,
UAVCAN_RESTART_NODE_DATA_TYPE_SIGNATURE,
UAVCAN_RESTART_NODE_DATA_TYPE_ID,
&transfer->transfer_id,
transfer->priority,
CanardResponse,
&response,
1);
}
static void allocation_init(void)
{
if (!allocation_running())
{
return;
}
// Start request timer
allocation_start_request_timer();
}
void allocation_update(void)
{
if (!allocation_running())
{
return;
}
// Check allocation timer
if (micros() - allocation_state.request_timer_begin_us > allocation_state.request_delay_us)
{
//printf("us:%llu ,ms:%u\r\n", micros(), HAL_GetTick());
// printf("del:%llu,requst_us:%llu,begin_us:%llu\r\n",
// micros() - allocation_state.request_timer_begin_us,
// allocation_state.request_delay_us,
// allocation_state.request_timer_begin_us);
allocation_timer_expired();
// boot_application_at = HAL_GetTick() + 2000;
}
if ((allocation_init_tick + ALLOCATION_TIMEOUT_MS) < HAL_GetTick())
{
if (bootloader_interface.magic == BOOTLOADER_INTERFACE_VALID_MAGIC &&
bootloader_interface.node_id)
{
canardSetLocalNodeID(&g_canard, bootloader_interface.node_id);
init_uavcan_fifo(get_node_id());
}
else
{
canardSetLocalNodeID(&g_canard, DEFAULT_NODE_ID);
init_uavcan_fifo(get_node_id());
}
}
}
static void allocation_timer_expired(void)
{
if (!allocation_running())
{
return;
}
// Start allocation request timer
allocation_start_request_timer();
// Send allocation message
uint8_t allocation_request[CANARD_CAN_FRAME_MAX_DATA_LEN - 1];
uint8_t uid_size = MIN(UNIQUE_ID_LENGTH_BYTES - allocation_state.unique_id_offset, UAVCAN_NODE_ID_ALLOCATION_MAX_LENGTH_OF_UID_IN_REQUEST);
allocation_request[0] = (allocation_state.unique_id_offset == 0) ? 1 : 0;
//printf("allocation_request:%d \r\n",allocation_request[0]);
memcpy(&allocation_request[1], &node_unique_id[allocation_state.unique_id_offset], uid_size);
static uint8_t transfer_id;
canardBroadcast(&g_canard, UAVCAN_NODE_ID_ALLOCATION_DATA_TYPE_SIGNATURE, UAVCAN_NODE_ID_ALLOCATION_DATA_TYPE_ID, &transfer_id, CANARD_TRANSFER_PRIORITY_LOW, allocation_request, uid_size + 1);
allocation_state.unique_id_offset = 0;
}
static void handle_allocation_data_broadcast(CanardInstance *ins, CanardRxTransfer *transfer)
{
if (!allocation_running())
{
return;
}
// Always start the allocation request timer and reset the UID offset
allocation_start_request_timer();
allocation_state.unique_id_offset = 0;
if (transfer->source_node_id == CANARD_BROADCAST_NODE_ID)
{
// If source node ID is anonymous, return
return;
}
uint8_t received_unique_id[UNIQUE_ID_LENGTH_BYTES];
uint8_t received_unique_id_len = transfer->payload_len - 1;
uint8_t i;
for (i = 0; i < received_unique_id_len; i++)
{
canardDecodeScalar(transfer, i * 8 + UAVCAN_NODE_ID_ALLOCATION_UID_BIT_OFFSET, 8, false, &received_unique_id[i]);
}
if (memcmp(node_unique_id, received_unique_id, received_unique_id_len) != 0)
{
// If unique ID does not match, return
return;
}
if (received_unique_id_len < UNIQUE_ID_LENGTH_BYTES)
{
// Unique ID partially matches - set the UID offset and start the followup timer
allocation_state.unique_id_offset = received_unique_id_len;
allocation_start_followup_timer();
}
else
{
// Complete match received
uint8_t allocated_node_id = 0;
canardDecodeScalar(transfer, 0, 7, false, &allocated_node_id);
if (allocated_node_id != 0)
{
canardSetLocalNodeID(ins, allocated_node_id);
uavcan_init_fifo(canardGetLocalNodeID(&g_canard));
}
}
}
static void allocation_start_request_timer(void)
{
if (!allocation_running())
{
return;
}
allocation_state.request_timer_begin_us = micros();
allocation_state.request_delay_us = UAVCAN_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_US + (uint32_t)(uavcan_get_radom_float() * (UAVCAN_NODE_ID_ALLOCATION_MAX_REQUEST_PERIOD_US - UAVCAN_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_US));
}
static void allocation_start_followup_timer(void)
{
if (!allocation_running())
{
return;
}
allocation_state.request_timer_begin_us = micros();
allocation_state.request_delay_us = UAVCAN_NODE_ID_ALLOCATION_MIN_FOLLOWUP_PERIOD_US + (uint32_t)(uavcan_get_radom_float() * (UAVCAN_NODE_ID_ALLOCATION_MAX_FOLLOWUP_PERIOD_US - UAVCAN_NODE_ID_ALLOCATION_MIN_FOLLOWUP_PERIOD_US));
}
static bool allocation_running(void)
{
return canardGetLocalNodeID(&g_canard) == CANARD_BROADCAST_NODE_ID;
}
static float uavcan_get_radom_float(void)
{
static bool initialized = false;
if (!initialized)
{
initialized = true;
read_unique_id(&node_unique_id[0]);
const uint32_t *unique_32 = (uint32_t *)&node_unique_id[0];
srand(micros() ^ *unique_32);
}
return (float)rand() / (float)RAND_MAX;
}
void uavcan_init_by_allocation(void)
{
can_filter_none();
can_init_irq();
canardInit(&g_canard, // Uninitialized library instance
g_canard_memory_pool, // Raw memory chunk used for dynamic allocation
sizeof(g_canard_memory_pool), // Size of the above, in bytes
onTransferReceived, // Callback, see CanardOnTransferReception
shouldAcceptTransfer, // Callback, see CanardShouldAcceptTransfer
NULL);
can_init_irq();
read_unique_id(&node_unique_id[0]);
allocation_init();
// canard_initialized = true;
allocation_init_tick = HAL_GetTick();
}
uint64_t micros(void)
{
return HAL_GetTick()*1000 + __HAL_TIM_GetCounter(&htim2);
}
void init_uavcan_fifo(uint8_t node_id)
{
uint32_t id = 0;
uint32_t mask = 0;
uint8_t bank_index = 1;
uavcan_get_uavcan_id_mask_by_msg_id(UAVCAN_BEGIN_FIRMWARE_UPDATE_DATA_TYPE_ID, &id, &mask);
can_filter_init_with_mask(bank_index, id, mask, 1);
bank_index++;
uavcan_get_uavcan_id_mask_by_msg_id(UAVCAN_FILE_READ_DATA_TYPE_ID, &id, &mask);
can_filter_init_with_mask(bank_index, id, mask, 0);
bank_index++;
uavcan_get_uavcan_id_mask_by_msg_id(UAVCAN_RESTART_NODE_DATA_TYPE_ID, &id, &mask);
can_filter_init_with_mask(bank_index, id, mask, 1);
bank_index++;
uavcan_get_uavcan_id_mask_by_node_id(node_id, &id, &mask);
can_filter_init_with_mask(bank_index, id, mask, 1);
bank_index++;
}