|
|
|
|
@ -4,8 +4,10 @@
@@ -4,8 +4,10 @@
|
|
|
|
|
#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); |
|
|
|
|
@ -15,12 +17,12 @@ static CanardInstance g_canard; //The library instance
@@ -15,12 +17,12 @@ 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;
|
|
|
|
|
|
|
|
|
|
uint8_t sver_p1 = 1; |
|
|
|
|
uint8_t sver_p2 = 0; |
|
|
|
|
uint8_t sver_p3 = 4; |
|
|
|
|
|
|
|
|
|
//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
|
|
|
|
|
@ -30,6 +32,8 @@ static uint8_t transfer_id = 0;
@@ -30,6 +32,8 @@ 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;
|
|
|
|
|
|
|
|
|
|
@ -42,6 +46,12 @@ typedef struct
@@ -42,6 +46,12 @@ typedef struct
|
|
|
|
|
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))); |
|
|
|
|
@ -57,6 +67,21 @@ typedef struct
@@ -57,6 +67,21 @@ typedef struct
|
|
|
|
|
#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, |
|
|
|
|
@ -86,6 +111,19 @@ bool shouldAcceptTransfer(const CanardInstance *ins,
@@ -86,6 +111,19 @@ bool shouldAcceptTransfer(const CanardInstance *ins,
|
|
|
|
|
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; |
|
|
|
|
} |
|
|
|
|
@ -117,12 +155,13 @@ void action_transfer_received_request(CanardInstance *ins, CanardRxTransfer *tra
@@ -117,12 +155,13 @@ void action_transfer_received_request(CanardInstance *ins, CanardRxTransfer *tra
|
|
|
|
|
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;
|
|
|
|
|
@ -138,7 +177,7 @@ void action_transfer_received_broadcast(CanardInstance *ins, CanardRxTransfer *t
@@ -138,7 +177,7 @@ void action_transfer_received_broadcast(CanardInstance *ins, CanardRxTransfer *t
|
|
|
|
|
case UAVCAN_NODE_ID_ALLOCATION_DATA_TYPE_ID: |
|
|
|
|
if (canardGetLocalNodeID(ins) == CANARD_BROADCAST_NODE_ID) |
|
|
|
|
{ |
|
|
|
|
handle_allocation_data(&g_canard, transfer); |
|
|
|
|
handle_allocation_data_broadcast(&g_canard, transfer); |
|
|
|
|
} |
|
|
|
|
break; |
|
|
|
|
default: |
|
|
|
|
@ -194,7 +233,6 @@ static void handle_canard_get_node_info(CanardRxTransfer *transfer)
@@ -194,7 +233,6 @@ static void handle_canard_get_node_info(CanardRxTransfer *transfer)
|
|
|
|
|
|
|
|
|
|
void uavcan_init(void) |
|
|
|
|
{
|
|
|
|
|
|
|
|
|
|
CanardSTM32CANTimings timings; |
|
|
|
|
int result = canardSTM32ComputeCANTimings(HAL_RCC_GetPCLK1Freq(), 1000000, &timings); |
|
|
|
|
if (result) |
|
|
|
|
@ -221,7 +259,6 @@ void uavcan_init(void)
@@ -221,7 +259,6 @@ void uavcan_init(void)
|
|
|
|
|
|
|
|
|
|
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
|
|
|
|
|
@ -234,98 +271,6 @@ void uavcan_init_with_cubemx(void)
@@ -234,98 +271,6 @@ void uavcan_init_with_cubemx(void)
|
|
|
|
|
can_init_irq(); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void init_allocation(void) |
|
|
|
|
{ |
|
|
|
|
can_filter_none(); |
|
|
|
|
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(); |
|
|
|
|
|
|
|
|
|
//执行动态节点ID分配过程
|
|
|
|
|
|
|
|
|
|
static const uint8_t preferred_node_id = CANARD_BROADCAST_NODE_ID; |
|
|
|
|
node_id_allocation_unique_id_offset = 0; |
|
|
|
|
uint8_t node_id_allocation_transfer_id = 0; |
|
|
|
|
|
|
|
|
|
while (canardGetLocalNodeID(&g_canard) == CANARD_BROADCAST_NODE_ID) |
|
|
|
|
{ |
|
|
|
|
//puts("Waiting for dynamic node ID allocation...");
|
|
|
|
|
// vTaskDelay(50);
|
|
|
|
|
send_next_node_id_allocation_request_at = |
|
|
|
|
HAL_GetTick() + UAVCAN_NODE_ID_ALLOCATION_REQUEST_DELAY_OFFSET_USEC + |
|
|
|
|
(uint64_t)(get_random_float() * UAVCAN_NODE_ID_ALLOCATION_RANDOM_TIMEOUT_RANGE_USEC); |
|
|
|
|
|
|
|
|
|
// (uint64_t)(0.1 * UAVCAN_NODE_ID_ALLOCATION_RANDOM_TIMEOUT_RANGE_USEC);
|
|
|
|
|
|
|
|
|
|
while ((HAL_GetTick() < send_next_node_id_allocation_request_at) && |
|
|
|
|
(canardGetLocalNodeID(&g_canard) == CANARD_BROADCAST_NODE_ID)) |
|
|
|
|
{ |
|
|
|
|
send_canard(); |
|
|
|
|
receive_canard(); |
|
|
|
|
osDelay(5); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (canardGetLocalNodeID(&g_canard) != CANARD_BROADCAST_NODE_ID) |
|
|
|
|
{ |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// Structure of the request is documented in the DSDL definition 请求的结构记录在DSDL定义中
|
|
|
|
|
// See http://uavcan.org/Specification/6._Application_level_functions/#dynamic-node-id-allocation
|
|
|
|
|
|
|
|
|
|
uint8_t allocation_request[CANARD_CAN_FRAME_MAX_DATA_LEN - 1]; |
|
|
|
|
allocation_request[0] = (uint8_t)(preferred_node_id << 1U); |
|
|
|
|
|
|
|
|
|
if (node_id_allocation_unique_id_offset == 0) |
|
|
|
|
{ |
|
|
|
|
allocation_request[0] |= 1; // First part of unique ID
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
uint8_t my_unique_id[UNIQUE_ID_LENGTH_BYTES] = {0}; |
|
|
|
|
read_unique_id(my_unique_id); |
|
|
|
|
|
|
|
|
|
static const uint8_t max_len_of_unique_id_in_request = 6; |
|
|
|
|
uint8_t uid_size = (uint8_t)(UNIQUE_ID_LENGTH_BYTES - node_id_allocation_unique_id_offset); |
|
|
|
|
if (uid_size > max_len_of_unique_id_in_request) |
|
|
|
|
{ |
|
|
|
|
uid_size = max_len_of_unique_id_in_request; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// Paranoia time
|
|
|
|
|
assert(node_id_allocation_unique_id_offset < UNIQUE_ID_LENGTH_BYTES); |
|
|
|
|
assert(uid_size <= max_len_of_unique_id_in_request); |
|
|
|
|
assert(uid_size > 0); |
|
|
|
|
assert((uid_size + node_id_allocation_unique_id_offset) <= UNIQUE_ID_LENGTH_BYTES); |
|
|
|
|
|
|
|
|
|
memmove(&allocation_request[1], &my_unique_id[node_id_allocation_unique_id_offset], uid_size); |
|
|
|
|
|
|
|
|
|
// Broadcasting the request
|
|
|
|
|
const int16_t bcast_res = canardBroadcast(&g_canard, |
|
|
|
|
UAVCAN_NODE_ID_ALLOCATION_DATA_TYPE_SIGNATURE, |
|
|
|
|
UAVCAN_NODE_ID_ALLOCATION_DATA_TYPE_ID, |
|
|
|
|
&node_id_allocation_transfer_id, |
|
|
|
|
CANARD_TRANSFER_PRIORITY_LOW, |
|
|
|
|
&allocation_request[0], |
|
|
|
|
(uint16_t)(uid_size + 1)); |
|
|
|
|
if (bcast_res < 0) |
|
|
|
|
{ |
|
|
|
|
//(void)fprintf(stderr, "Could not broadcast dynamic node ID allocation request; error %d\n", bcast_res); //TODO ////my_printf()
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// Preparing for timeout; if response is received, this value will be updated from the callback.
|
|
|
|
|
node_id_allocation_unique_id_offset = 0; |
|
|
|
|
} |
|
|
|
|
uavcan_init_fifo(get_node_id()); |
|
|
|
|
//过滤设置
|
|
|
|
|
can_init_irq(); |
|
|
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void send_canard(void) |
|
|
|
|
{ |
|
|
|
|
osStatus_t status; |
|
|
|
|
@ -408,61 +353,6 @@ uint16_t make_node_info_message(uint8_t buffer[UAVCAN_GET_NODE_INFO_RESPONSE_MAX
@@ -408,61 +353,6 @@ uint16_t make_node_info_message(uint8_t buffer[UAVCAN_GET_NODE_INFO_RESPONSE_MAX
|
|
|
|
|
return 41 + name_len; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
//动态节点消息接收处理 //on_allocation_message
|
|
|
|
|
void handle_allocation_data(CanardInstance *ins, CanardRxTransfer *transfer) |
|
|
|
|
{ |
|
|
|
|
// Rule C - updating the randomized time interval
|
|
|
|
|
send_next_node_id_allocation_request_at = |
|
|
|
|
HAL_GetTick() + UAVCAN_NODE_ID_ALLOCATION_REQUEST_DELAY_OFFSET_USEC + //getMonotonicTimestampUSec()
|
|
|
|
|
(uint64_t)(get_random_float() * UAVCAN_NODE_ID_ALLOCATION_RANDOM_TIMEOUT_RANGE_USEC); |
|
|
|
|
|
|
|
|
|
if (transfer->source_node_id == CANARD_BROADCAST_NODE_ID) |
|
|
|
|
{ |
|
|
|
|
// puts("Allocation request from another allocatee");
|
|
|
|
|
node_id_allocation_unique_id_offset = 0; |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
//Copying the unique ID from the message unique_id_bit_offset：位偏移量 --uint8
|
|
|
|
|
static const uint8_t unique_id_bit_offset = 8; |
|
|
|
|
uint8_t received_unique_id[UNIQUE_ID_LENGTH_BYTES]; |
|
|
|
|
uint8_t received_unique_id_len = 0; |
|
|
|
|
|
|
|
|
|
for (; received_unique_id_len < (transfer->payload_len - (unique_id_bit_offset / 8U)); received_unique_id_len++) |
|
|
|
|
{ |
|
|
|
|
assert(received_unique_id_len < UNIQUE_ID_LENGTH_BYTES); |
|
|
|
|
const uint8_t bit_offset = (uint8_t)(unique_id_bit_offset + received_unique_id_len * 8U); |
|
|
|
|
(void)canardDecodeScalar(transfer, bit_offset, 8, false, &received_unique_id[received_unique_id_len]); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// Obtaining the local unique ID 获取本地 unique ID
|
|
|
|
|
uint8_t my_unique_id[UNIQUE_ID_LENGTH_BYTES] = {0}; |
|
|
|
|
read_unique_id(my_unique_id); |
|
|
|
|
|
|
|
|
|
// Matching the received UID against the local one
|
|
|
|
|
if (memcmp(received_unique_id, my_unique_id, received_unique_id_len) != 0) |
|
|
|
|
{ |
|
|
|
|
node_id_allocation_unique_id_offset = 0; |
|
|
|
|
|
|
|
|
|
return; // No match, return
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (received_unique_id_len < UNIQUE_ID_LENGTH_BYTES) |
|
|
|
|
{ |
|
|
|
|
// The allocator has confirmed part of unique ID, switching to the next stage and updating the timeout.
|
|
|
|
|
node_id_allocation_unique_id_offset = received_unique_id_len; |
|
|
|
|
send_next_node_id_allocation_request_at -= UAVCAN_NODE_ID_ALLOCATION_REQUEST_DELAY_OFFSET_USEC; |
|
|
|
|
} |
|
|
|
|
else |
|
|
|
|
{ |
|
|
|
|
// Allocation complete - copying the allocated node ID from the message
|
|
|
|
|
uint8_t allocated_node_id = 0; |
|
|
|
|
(void)canardDecodeScalar(transfer, 0, 7, false, &allocated_node_id); |
|
|
|
|
assert(allocated_node_id <= 127); |
|
|
|
|
canardSetLocalNodeID(ins, allocated_node_id); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void canard_get_node_info_handle(CanardRxTransfer *transfer) |
|
|
|
|
{ |
|
|
|
|
uint8_t buffer[UAVCAN_GET_NODE_INFO_RESPONSE_MAX_SIZE]; |
|
|
|
|
@ -618,20 +508,21 @@ static void handle_begin_firmware_update(CanardInstance *ins, CanardRxTransfer *
@@ -618,20 +508,21 @@ static void handle_begin_firmware_update(CanardInstance *ins, CanardRxTransfer *
|
|
|
|
|
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); |
|
|
|
|
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_ID_LENGTH_BYTES); |
|
|
|
|
memcpy(out_uid, (void *)UID_BASE, UNIQUE_STM32_DEVICE_ID_LENGTH); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
uint8_t get_node_id(void) |
|
|
|
|
@ -677,13 +568,221 @@ void handle_restart_node(CanardRxTransfer *transfer)
@@ -677,13 +568,221 @@ void handle_restart_node(CanardRxTransfer *transfer)
|
|
|
|
|
*/ |
|
|
|
|
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); |
|
|
|
|
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); |
|
|
|
|
} |
|
|
|
|
else |
|
|
|
|
{ |
|
|
|
|
canardSetLocalNodeID(&g_canard, DEFAULT_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++; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
binsir
3 years ago