13#include "uavcan.protocol.dynamic_node_id.Allocation.h"
14#include "uavcan.protocol.GetNodeInfo.h"
16#ifndef UAVCAN_MAX_NODES
17#define UAVCAN_MAX_NODES 50
20#define INVALID_STAGE -1
59 int candidate = (preferred > 0) ? preferred : 125;
60 while (candidate <= 125) {
67 candidate = (preferred > 0) ? preferred : 125;
68 while (candidate > 0) {
80 if(msg->first_part_of_unique_id) {
83 if(msg->unique_id.len == UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST) {
86 if(msg->unique_id.len < UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST) {
96 if(
current_unique_id.len >= UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST*2) {
99 if(
current_unique_id.len >= UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST) {
144 struct uavcan_protocol_dynamic_node_id_Allocation response_msg;
150 response_msg.node_id = allocated_id;
154 uint32_t total_size = uavcan_protocol_dynamic_node_id_Allocation_encode(&response_msg, msg_buffer);
158 UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE, UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID,
159 CANARD_TRANSFER_PRIORITY_HIGH, msg_buffer, total_size);
165 struct uavcan_protocol_dynamic_node_id_Allocation msg;
166 if(uavcan_protocol_dynamic_node_id_Allocation_decode(
transfer, &msg)) {
170 const int unique_id_capacity =
sizeof(msg.unique_id.data);
188 if (msg.unique_id.len > (unique_id_capacity -
current_unique_id.len)) {
return; }
191 for(
int i=0; i<msg.unique_id.len; i++) {
201 struct uavcan_protocol_dynamic_node_id_Allocation response_msg;
210 uint32_t total_size = uavcan_protocol_dynamic_node_id_Allocation_encode(&response_msg, msg_buffer);
214 UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE, UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID,
215 CANARD_TRANSFER_PRIORITY_HIGH, msg_buffer, total_size);
224 struct uavcan_protocol_GetNodeInfoResponse msg;
225 if(uavcan_protocol_GetNodeInfoResponse_decode(
transfer, &msg)) {
237 for(
int i=0; i<16; i++) {
238 mapping->
unique_id.
data[i] = msg.hardware_version.unique_id[i];
247 transfer.data_type_id = UAVCAN_PROTOCOL_GETNODEINFO_REQUEST_ID;
248 transfer.data_type_signature = UAVCAN_PROTOCOL_GETNODEINFO_REQUEST_SIGNATURE;
251 transfer.priority = CANARD_TRANSFER_PRIORITY_HIGH;
257 uavcan_bind(UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID, UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE,
260 uavcan_bind(UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_ID, UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_SIGNATURE,
void uavcan_request(struct uavcan_iface_t *iface, uint8_t destination_node_id, CanardTxTransfer *transfer)
void uavcan_broadcast(struct uavcan_iface_t *iface, uint64_t data_type_signature, uint16_t data_type_id, uint8_t priority, const void *payload, uint16_t payload_len)
Legacy function Broadcast an uavcan message to a specific interface.
void uavcan_bind(uint16_t data_type_id, uint64_t data_type_signature, uavcan_event *ev, uavcan_callback cb)
Bind to a receiving message from uavcan.
struct uavcan_event_t uavcan_event
static void handleAllocationRequest(struct uavcan_iface_t *iface, uint8_t preferred_node_id)
static void node_info_resp_cb(struct uavcan_iface_t *iface, CanardRxTransfer *transfer)
static uavcan_event node_info_ev
static struct uavcan_node_mapping_t uavcan_node_ids[UAVCAN_MAX_NODES]
static int getExpectedStage(void)
static struct uavcan_node_mapping_t * get_free_id_mapping(void)
#define UAVCAN_MAX_NODES
Dynamic node ID allocation.
static bool unique_id_identical(int index)
static uavcan_event id_alloc_ev
struct uavcan_unique_id_t current_unique_id
struct uavcan_node_mapping_t * uavcan_get_node_id_mapping(const uint8_t id)
static int findFreeNodeID(const uint8_t preferred)
uint32_t last_message_timestamp
void request_node_info(struct uavcan_iface_t *iface, uint8_t destination_node_id)
static void id_alloc_uavcan_cb(struct uavcan_iface_t *iface, CanardRxTransfer *transfer)
void uavcan_allocator_init(void)
static int detectRequestStage(struct uavcan_protocol_dynamic_node_id_Allocation *msg)
struct uavcan_unique_id_t unique_id
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
int transfer(const Mat *from, const image_t *to)