Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
uavcan_allocator.c
Go to the documentation of this file.
1 /*
2 * Copyright (C) 2025 Fabien-B <fabien-b@github.com>
3 * This file is part of paparazzi. See LICENCE file.
4 */
5
10
11#include "uavcan/uavcan.h"
13#include "uavcan.protocol.dynamic_node_id.Allocation.h"
14#include "uavcan.protocol.GetNodeInfo.h"
15
16#ifndef UAVCAN_MAX_NODES
17#define UAVCAN_MAX_NODES 50
18#endif
19
20#define INVALID_STAGE -1
21
22static int detectRequestStage(struct uavcan_protocol_dynamic_node_id_Allocation* msg);
23static int getExpectedStage(void);
24static int findFreeNodeID(const uint8_t preferred);
25static bool unique_id_identical(int index);
26static void handleAllocationRequest(struct uavcan_iface_t *iface, uint8_t preferred_node_id);
27static void id_alloc_uavcan_cb(struct uavcan_iface_t *iface __attribute__((unused)), CanardRxTransfer *transfer);
28
29
32
33
34// keep the correspondance between node id and unique IDs. (even or the fixed ids)
36
37
40
41
43 for(int i=0; i<UAVCAN_MAX_NODES; i++) {
44 if(uavcan_node_ids[i].allocated_id == id) {
45 return &uavcan_node_ids[i];
46 }
47 }
48 return NULL;
49}
50
52 // a free slot is stored with node id == 0;
54}
55
56static int findFreeNodeID(const uint8_t preferred)
57{
58 // Search up
59 int candidate = (preferred > 0) ? preferred : 125;
60 while (candidate <= 125) {
61 if (!uavcan_get_node_id_mapping(candidate)) {
62 return candidate;
63 }
64 candidate++;
65 }
66 // Search down
67 candidate = (preferred > 0) ? preferred : 125;
68 while (candidate > 0) {
69 if (!uavcan_get_node_id_mapping(candidate)) {
70 return candidate;
71 }
72 candidate--;
73 }
74 // Not found
75 return -1;
76}
77
78
79static int detectRequestStage(struct uavcan_protocol_dynamic_node_id_Allocation* msg) {
80 if(msg->first_part_of_unique_id) {
81 return 1;
82 }
83 if(msg->unique_id.len == UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST) {
84 return 2;
85 }
86 if(msg->unique_id.len < UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST) {
87 return 3;
88 }
89 return INVALID_STAGE; //invalid
90}
91
92static int getExpectedStage() {
93 if(current_unique_id.len == 0) {
94 return 1;
95 }
96 if(current_unique_id.len >= UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST*2) {
97 return 3;
98 }
99 if(current_unique_id.len >= UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST) {
100 return 2;
101 }
102 return INVALID_STAGE; //invalid
103}
104
105static bool unique_id_identical(int index) {
106 return current_unique_id.len == uavcan_node_ids[index].unique_id.len &&
107 memcmp(current_unique_id.data, uavcan_node_ids[index].unique_id.data, current_unique_id.len) == 0;
108}
109
110static void handleAllocationRequest(struct uavcan_iface_t *iface, uint8_t preferred_node_id) {
112
113 for(int i=0; i<UAVCAN_MAX_NODES; i++) {
114
115 if(uavcan_node_ids[i].allocated_id != 0) {
116 // allocation slot taken, check if unique id matches
117 if(unique_id_identical(i)) {
118 allocated_id = uavcan_node_ids[i].allocated_id;
119 }
120 }
121 else {
122 // free allocation slot
123
124 // copy unique id
125 for(int k=0; k<current_unique_id.len; k++) {
126 uavcan_node_ids[i].unique_id.data[k] = current_unique_id.data[k];
127 }
128 uavcan_node_ids[i].unique_id.len = current_unique_id.len;
129
130 int free_id = findFreeNodeID(preferred_node_id);
131
132 if(free_id != -1) {
133 uavcan_node_ids[i].allocated_id = free_id;
134 allocated_id = free_id;
135 break;
136 } else {
137 // error: not more free IDs
138 return;
139 }
140
141 }
142 }
143
144 struct uavcan_protocol_dynamic_node_id_Allocation response_msg;
145 // copy unique id
146 for(int k=0; k<current_unique_id.len; k++) {
147 response_msg.unique_id.data[k] = current_unique_id.data[k];
148 }
149 response_msg.unique_id.len = current_unique_id.len;
150 response_msg.node_id = allocated_id;
151
152 uint8_t msg_buffer[50];
153
154 uint32_t total_size = uavcan_protocol_dynamic_node_id_Allocation_encode(&response_msg, msg_buffer);
155
157 iface,
158 UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE, UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID,
159 CANARD_TRANSFER_PRIORITY_HIGH, msg_buffer, total_size);
160}
161
162
163static void id_alloc_uavcan_cb(struct uavcan_iface_t *iface, CanardRxTransfer *transfer)
164{
165 struct uavcan_protocol_dynamic_node_id_Allocation msg;
166 if(uavcan_protocol_dynamic_node_id_Allocation_decode(transfer, &msg)) {
167 return; // decode error
168 }
169
170 const int unique_id_capacity = sizeof(msg.unique_id.data);
171
172 // only process anonymous transfers
173 if(transfer->source_node_id != 0) {
174 return;
175 }
176
177 uint32_t timestamp = transfer->timestamp_usec / 1000;
178
179 // Reset the expected stage on timeout
180 if(timestamp > last_message_timestamp + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_FOLLOWUP_TIMEOUT_MS) {
181 current_unique_id.len = 0;
182 }
183
184 // Checking if request stage matches the expected stage
185 int request_stage = detectRequestStage(&msg);
186 if(request_stage == INVALID_STAGE) { return; }
187 if(request_stage != getExpectedStage()) { return; }
188 if (msg.unique_id.len > (unique_id_capacity - current_unique_id.len)) { return; }
189
190 // Updating the local state
191 for(int i=0; i<msg.unique_id.len; i++) {
192 current_unique_id.data[current_unique_id.len] = msg.unique_id.data[i];
193 current_unique_id.len += 1;
194 }
195
196 if(current_unique_id.len == unique_id_capacity) {
197 // Proceeding with allocation.
199 current_unique_id.len = 0;
200 } else {
201 struct uavcan_protocol_dynamic_node_id_Allocation response_msg;
202 // copy unique id
203 for(int k=0; k<current_unique_id.len; k++) {
204 response_msg.unique_id.data[k] = current_unique_id.data[k];
205 }
206 response_msg.unique_id.len = current_unique_id.len;
207
208 uint8_t msg_buffer[50];
209
210 uint32_t total_size = uavcan_protocol_dynamic_node_id_Allocation_encode(&response_msg, msg_buffer);
211
213 iface,
214 UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE, UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID,
215 CANARD_TRANSFER_PRIORITY_HIGH, msg_buffer, total_size);
216 }
217
218 // It is important to update the timestamp only if the request has been processed successfully.
219 last_message_timestamp = timestamp;
220}
221
222static void node_info_resp_cb(struct uavcan_iface_t *iface, CanardRxTransfer *transfer) {
223 (void)iface;
224 struct uavcan_protocol_GetNodeInfoResponse msg;
225 if(uavcan_protocol_GetNodeInfoResponse_decode(transfer, &msg)) {
226 return; // decode error
227 }
228
229 struct uavcan_node_mapping_t* mapping = uavcan_get_node_id_mapping(transfer->source_node_id);
230
231 if(!mapping) {
232 mapping = get_free_id_mapping();
233 }
234
235
236 mapping->allocated_id = transfer->source_node_id;
237 for(int i=0; i<16; i++) {
238 mapping->unique_id.data[i] = msg.hardware_version.unique_id[i];
239 }
240 mapping->unique_id.len = 16;
241
242}
243
244void request_node_info(struct uavcan_iface_t *iface, uint8_t destination_node_id) {
245 CanardTxTransfer transfer;
246 canardInitTxTransfer(&transfer);
247 transfer.data_type_id = UAVCAN_PROTOCOL_GETNODEINFO_REQUEST_ID;
248 transfer.data_type_signature = UAVCAN_PROTOCOL_GETNODEINFO_REQUEST_SIGNATURE;
249 transfer.payload = NULL;
250 transfer.payload_len = 0;
251 transfer.priority = CANARD_TRANSFER_PRIORITY_HIGH;
252 uavcan_request(iface, destination_node_id, &transfer);
253}
254
255
257 uavcan_bind(UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID, UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE,
259
260 uavcan_bind(UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_ID, UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_SIGNATURE,
262}
263
void uavcan_request(struct uavcan_iface_t *iface, uint8_t destination_node_id, CanardTxTransfer *transfer)
Definition uavcan.c:321
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.
Definition uavcan.c:345
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.
Definition uavcan.c:272
uint8_t node_id
Definition uavcan.h:67
struct uavcan_event_t uavcan_event
Definition uavcan.h:98
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.
#define INVALID_STAGE
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)