Line data Source code
1 : /*
2 : * This file is free software: you can redistribute it and/or modify it
3 : * under the terms of the GNU General Public License as published by the
4 : * Free Software Foundation, either version 3 of the License, or
5 : * (at your option) any later version.
6 : *
7 : * This file is distributed in the hope that it will be useful, but
8 : * WITHOUT ANY WARRANTY; without even the implied warranty of
9 : * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
10 : * See the GNU General Public License for more details.
11 : *
12 : * You should have received a copy of the GNU General Public License along
13 : * with this program. If not, see <http://www.gnu.org/licenses/>.
14 : *
15 : * Author: Siddharth Bharat Purohit
16 : */
17 :
18 : #include <AP_HAL/AP_HAL.h>
19 :
20 : #if HAL_ENABLE_DRONECAN_DRIVERS
21 :
22 : #include "AP_DroneCAN_DNA_Server.h"
23 : #include "AP_DroneCAN.h"
24 : #include <StorageManager/StorageManager.h>
25 : #include <AP_Math/AP_Math.h>
26 : #include <GCS_MAVLink/GCS.h>
27 : #include <AP_Logger/AP_Logger.h>
28 : #include <AP_BoardConfig/AP_BoardConfig.h>
29 : #include <stdio.h>
30 : extern const AP_HAL::HAL& hal;
31 :
32 : // FORMAT REVISION DREAMS (things to address if the NodeRecord needs to be changed substantially)
33 : // * have DNA server accept only a 16 byte local UID to avoid overhead from variable sized hash
34 : // * have a real empty flag for entries and/or use a CRC which is not zero for an input of all zeros
35 : // * fix FNV-1a hash folding to be to 48 bits (6 bytes) instead of 56
36 :
37 : #define NODERECORD_MAGIC 0xAC01
38 : #define NODERECORD_MAGIC_LEN 2 // uint16_t
39 : #define MAX_NODE_ID 125
40 : #define NODERECORD_LOC(node_id) ((node_id * sizeof(NodeRecord)) + NODERECORD_MAGIC_LEN)
41 :
42 : // database is currently shared by all DNA servers
43 : AP_DroneCAN_DNA_Server::Database AP_DroneCAN_DNA_Server::db;
44 :
45 : // initialize database (storage accessor is always replaced with the one supplied)
46 5 : void AP_DroneCAN_DNA_Server::Database::init(StorageAccess *storage_)
47 : {
48 : // storage size must be synced with StorageCANDNA entry in StorageManager.cpp
49 : static_assert(NODERECORD_LOC(MAX_NODE_ID+1) <= 1024, "DNA storage too small");
50 :
51 : // might be called from multiple threads if multiple servers use the same database
52 5 : WITH_SEMAPHORE(sem);
53 :
54 5 : storage = storage_; // use supplied accessor
55 :
56 : // validate magic number
57 5 : uint16_t magic = storage->read_uint16(0);
58 5 : if (magic != NODERECORD_MAGIC) {
59 4 : reset(); // resetting the database will put the magic back
60 : }
61 :
62 : // check and note each possible node ID's registration's presence
63 630 : for (uint8_t i = 1; i <= MAX_NODE_ID; i++) {
64 625 : if (check_registration(i)) {
65 2 : node_registered.set(i);
66 : }
67 : }
68 5 : }
69 :
70 : // remove all registrations from the database
71 4 : void AP_DroneCAN_DNA_Server::Database::reset()
72 : {
73 4 : WITH_SEMAPHORE(sem);
74 :
75 : NodeRecord record;
76 4 : memset(&record, 0, sizeof(record));
77 4 : node_registered.clearall();
78 :
79 : // all-zero record means no registration
80 : // ensure node ID 0 is cleared even if we can't use it so we know the state
81 508 : for (uint8_t i = 0; i <= MAX_NODE_ID; i++) {
82 504 : write_record(record, i);
83 : }
84 :
85 : // mark the magic at the start to indicate a valid (and reset) database
86 4 : storage->write_uint16(0, NODERECORD_MAGIC);
87 4 : }
88 :
89 : // handle initializing the server with its own node ID and unique ID
90 5 : void AP_DroneCAN_DNA_Server::Database::init_server(uint8_t own_node_id, const uint8_t own_unique_id[], uint8_t own_unique_id_len)
91 : {
92 5 : WITH_SEMAPHORE(sem);
93 :
94 : // register server node ID and unique ID if not correctly registered. note
95 : // that ardupilot mixes the node ID into the unique ID so changing the node
96 : // ID will "leak" the old node ID
97 5 : if (own_node_id != find_node_id(own_unique_id, own_unique_id_len)) {
98 4 : register_uid(own_node_id, own_unique_id, own_unique_id_len);
99 : }
100 5 : }
101 :
102 : // handle processing the node info message. returns true if from a duplicate node
103 9 : bool AP_DroneCAN_DNA_Server::Database::handle_node_info(uint8_t source_node_id, const uint8_t unique_id[])
104 : {
105 9 : WITH_SEMAPHORE(sem);
106 :
107 9 : if (is_registered(source_node_id)) {
108 : // this device's node ID is associated with a different unique ID
109 9 : if (source_node_id != find_node_id(unique_id, 16)) {
110 0 : return true; // so raise as duplicate
111 : }
112 : } else {
113 0 : register_uid(source_node_id, unique_id, 16); // we don't know about this node ID, let's register it
114 : }
115 9 : return false; // not a duplicate
116 9 : }
117 :
118 : // handle the allocation message. returns the allocated node ID, or 0 if allocation failed
119 1 : uint8_t AP_DroneCAN_DNA_Server::Database::handle_allocation(const uint8_t unique_id[])
120 : {
121 1 : WITH_SEMAPHORE(sem);
122 :
123 1 : uint8_t resp_node_id = find_node_id(unique_id, 16);
124 1 : if (resp_node_id == 0) {
125 : // find free node ID, starting at the max as prescribed by the standard
126 1 : resp_node_id = MAX_NODE_ID;
127 1 : while (resp_node_id > 0) {
128 1 : if (!node_registered.get(resp_node_id)) {
129 1 : break;
130 : }
131 0 : resp_node_id--;
132 : }
133 :
134 1 : if (resp_node_id != 0) {
135 1 : create_registration(resp_node_id, unique_id, 16);
136 : }
137 : }
138 2 : return resp_node_id; // will be 0 if not found and not created
139 1 : }
140 :
141 : // retrieve node ID that matches the given unique ID. returns 0 if not found
142 19 : uint8_t AP_DroneCAN_DNA_Server::Database::find_node_id(const uint8_t unique_id[], uint8_t size)
143 : {
144 : NodeRecord record, cmp_record;
145 19 : compute_uid_hash(cmp_record, unique_id, size);
146 :
147 1259 : for (int i = MAX_NODE_ID; i > 0; i--) {
148 1250 : if (node_registered.get(i)) {
149 12 : read_record(record, i);
150 12 : if (memcmp(record.uid_hash, cmp_record.uid_hash, sizeof(NodeRecord::uid_hash)) == 0) {
151 10 : return i; // node ID found
152 : }
153 : }
154 : }
155 9 : return 0; // not found
156 : }
157 :
158 : // fill the given record with the hash of the given unique ID
159 24 : void AP_DroneCAN_DNA_Server::Database::compute_uid_hash(NodeRecord &record, const uint8_t unique_id[], uint8_t size) const
160 : {
161 24 : uint64_t hash = FNV_1_OFFSET_BASIS_64;
162 24 : hash_fnv_1a(size, unique_id, &hash);
163 :
164 : // xor-folding per http://www.isthe.com/chongo/tech/comp/fnv/
165 24 : hash = (hash>>56) ^ (hash&(((uint64_t)1<<56)-1)); // 56 should be 48 since we use 6 bytes
166 :
167 : // write it to ret
168 168 : for (uint8_t i=0; i<6; i++) {
169 144 : record.uid_hash[i] = (hash >> (8*i)) & 0xff;
170 : }
171 24 : }
172 :
173 : // register a given unique ID to a given node ID, deleting any existing registration for the unique ID
174 4 : void AP_DroneCAN_DNA_Server::Database::register_uid(uint8_t node_id, const uint8_t unique_id[], uint8_t size)
175 : {
176 4 : uint8_t prev_node_id = find_node_id(unique_id, size); // have we registered this unique ID under a different node ID?
177 4 : if (prev_node_id != 0) {
178 0 : delete_registration(prev_node_id); // yes, delete old node ID's registration
179 : }
180 : // overwrite an existing registration with this node ID, if any
181 4 : create_registration(node_id, unique_id, size);
182 4 : }
183 :
184 : // create the registration for the given node ID and set its record's unique ID
185 5 : void AP_DroneCAN_DNA_Server::Database::create_registration(uint8_t node_id, const uint8_t unique_id[], uint8_t size)
186 : {
187 : NodeRecord record;
188 5 : compute_uid_hash(record, unique_id, size);
189 : // compute and store CRC of the record's data to validate it
190 5 : record.crc = crc_crc8(record.uid_hash, sizeof(record.uid_hash));
191 :
192 5 : write_record(record, node_id);
193 :
194 5 : node_registered.set(node_id);
195 5 : }
196 :
197 : // delete the given node ID's registration
198 0 : void AP_DroneCAN_DNA_Server::Database::delete_registration(uint8_t node_id)
199 : {
200 0 : if (node_id > MAX_NODE_ID) {
201 0 : return;
202 : }
203 :
204 : NodeRecord record;
205 :
206 : // all-zero record means no registration
207 0 : memset(&record, 0, sizeof(record));
208 0 : write_record(record, node_id);
209 0 : node_registered.clear(node_id);
210 : }
211 :
212 : // return true if the given node ID has a registration
213 625 : bool AP_DroneCAN_DNA_Server::Database::check_registration(uint8_t node_id)
214 : {
215 : NodeRecord record;
216 625 : read_record(record, node_id);
217 :
218 625 : uint8_t empty_uid[sizeof(NodeRecord::uid_hash)] {};
219 625 : uint8_t crc = crc_crc8(record.uid_hash, sizeof(record.uid_hash));
220 625 : if (crc == record.crc && memcmp(&record.uid_hash[0], &empty_uid[0], sizeof(empty_uid)) != 0) {
221 2 : return true; // CRC matches and UID hash is not all zero
222 : }
223 623 : return false;
224 : }
225 :
226 : // read the given node ID's registration's record
227 637 : void AP_DroneCAN_DNA_Server::Database::read_record(NodeRecord &record, uint8_t node_id)
228 : {
229 637 : if (node_id > MAX_NODE_ID) {
230 0 : return;
231 : }
232 :
233 637 : storage->read_block(&record, NODERECORD_LOC(node_id), sizeof(NodeRecord));
234 : }
235 :
236 : // write the given node ID's registration's record
237 509 : void AP_DroneCAN_DNA_Server::Database::write_record(const NodeRecord &record, uint8_t node_id)
238 : {
239 509 : if (node_id > MAX_NODE_ID) {
240 0 : return;
241 : }
242 :
243 509 : storage->write_block(NODERECORD_LOC(node_id), &record, sizeof(NodeRecord));
244 : }
245 :
246 :
247 5 : AP_DroneCAN_DNA_Server::AP_DroneCAN_DNA_Server(AP_DroneCAN &ap_dronecan, CanardInterface &canard_iface, uint8_t driver_index) :
248 5 : storage(StorageManager::StorageCANDNA),
249 5 : _ap_dronecan(ap_dronecan),
250 5 : _canard_iface(canard_iface),
251 5 : allocation_sub(allocation_cb, driver_index),
252 5 : node_status_sub(node_status_cb, driver_index),
253 15 : node_info_client(_canard_iface, node_info_cb) {}
254 :
255 : /* Initialises Publishers for respective UAVCAN Instance
256 : Also resets the Server Record in case there is a mismatch
257 : between specified node id and unique id against the existing
258 : Server Record. */
259 5 : bool AP_DroneCAN_DNA_Server::init(uint8_t own_unique_id[], uint8_t own_unique_id_len, uint8_t node_id)
260 : {
261 : //Read the details from AP_DroneCAN
262 5 : server_state = HEALTHY;
263 :
264 5 : db.init(&storage); // initialize the database with our accessor
265 :
266 5 : if (_ap_dronecan.check_and_reset_option(AP_DroneCAN::Options::DNA_CLEAR_DATABASE)) {
267 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "UC DNA database reset");
268 0 : db.reset();
269 : }
270 :
271 5 : db.init_server(node_id, own_unique_id, own_unique_id_len);
272 :
273 : /* Also add to seen node id this is to verify
274 : if any duplicates are on the bus carrying our Node ID */
275 5 : node_seen.set(node_id);
276 5 : node_verified.set(node_id);
277 5 : node_healthy.set(node_id);
278 5 : self_node_id = node_id;
279 5 : return true;
280 : }
281 :
282 : /* Run through the list of seen node ids for verification no more
283 : than once per 5 second. We continually verify the nodes in our
284 : seen list, So that we can raise issue if there are duplicates
285 : on the bus. */
286 21794 : void AP_DroneCAN_DNA_Server::verify_nodes()
287 : {
288 21794 : uint32_t now = AP_HAL::millis();
289 21794 : if ((now - last_verification_request) < 5000) {
290 21694 : return;
291 : }
292 :
293 : #if HAL_LOGGING_ENABLED
294 100 : uint8_t log_count = AP::logger().get_log_start_count();
295 100 : if (log_count != last_logging_count) {
296 3 : last_logging_count = log_count;
297 3 : node_logged.clearall();
298 : }
299 : #endif
300 :
301 : //Check if we got acknowledgement from previous request
302 : //except for requests using our own node_id
303 100 : if (curr_verifying_node == self_node_id) {
304 0 : nodeInfo_resp_rcvd = true;
305 : }
306 :
307 100 : if (!nodeInfo_resp_rcvd) {
308 : /* Also notify GCS about this
309 : Reason for this could be either the node was disconnected
310 : Or a node with conflicting ID appeared and is sending response
311 : at the same time. */
312 93 : node_verified.clear(curr_verifying_node);
313 : }
314 :
315 100 : last_verification_request = now;
316 : //Find the next registered Node ID to be verified.
317 12691 : for (uint8_t i = 0; i <= MAX_NODE_ID; i++) {
318 12599 : curr_verifying_node = (curr_verifying_node + 1) % (MAX_NODE_ID + 1);
319 12599 : if ((curr_verifying_node == self_node_id) || (curr_verifying_node == 0)) {
320 199 : continue;
321 : }
322 12400 : if (node_seen.get(curr_verifying_node)) {
323 8 : break;
324 : }
325 : }
326 100 : if (db.is_registered(curr_verifying_node)) {
327 : uavcan_protocol_GetNodeInfoRequest request;
328 8 : node_info_client.request(curr_verifying_node, request);
329 8 : nodeInfo_resp_rcvd = false;
330 : }
331 : }
332 :
333 : #if HAL_LOGGING_ENABLED
334 : // Find the node status item for the given node id, will add new item if no existing item can be found
335 52 : AP_DroneCAN_DNA_Server::node_status_log_data * AP_DroneCAN_DNA_Server::find_node_status_item(const uint8_t source_node_id)
336 : {
337 : // Search exiting items
338 52 : for (auto *node = node_status_list; node != nullptr; node = node->next) {
339 50 : if (node->id == source_node_id) {
340 : // Return match
341 50 : return node;
342 : }
343 : }
344 :
345 : // Add new item
346 2 : auto *node = NEW_NOTHROW node_status_log_data(source_node_id);
347 2 : if (node == nullptr) {
348 0 : return nullptr;
349 : }
350 :
351 : // link into the list
352 2 : node->next = node_status_list;
353 2 : node_status_list = node;
354 :
355 : // Return item
356 2 : return node;
357 : }
358 :
359 : // Update node status linked list and log
360 52 : void AP_DroneCAN_DNA_Server::update_node_status(const uint8_t source_node_id, const uavcan_protocol_NodeStatus& msg)
361 : {
362 52 : AP_DroneCAN_DNA_Server::node_status_log_data *node = find_node_status_item(source_node_id);
363 52 : if (node == nullptr) {
364 0 : return;
365 : }
366 :
367 : // Rate limit logs to no more than 1.1Hz. AP_Periph sends at 1Hz, the head room is to
368 : // make sure that timing jitter does not result in skipping every other message
369 52 : const uint32_t now_ms = AP_HAL::millis();
370 52 : if ((now_ms - node->last_log_ms) >= 900) {
371 42 : node->last_log_ms = now_ms;
372 :
373 : // @LoggerMessage: CANH
374 : // @Description: DroneCAN heartbeat status
375 : // @Field: TimeUS: Time since system startup
376 : // @Field: Driver: Driver index
377 : // @Field: NodeId: Node ID
378 : // @Field: UpTime: Time since node startup
379 : // @Field: Health: 0:Ok, 1:Warning, 2:Error, 3:Critical
380 : // @Field: Mode: 0:Operational, 1:Initialization, 2:Maintenance, 3:Software update, 7:Offline
381 : // @Field: SubMode: Expected to be 0
382 : // @Field: VendorCode: vendor specific code. In AP_Periph this is available memory in bytes.
383 :
384 84 : AP::logger().WriteStreaming("CANH",
385 : "TimeUS,Driver,NodeId,UpTime,Health,Mode,SubMode,VendorCode",
386 : "s-#s----",
387 : "F-------",
388 : "QBBIBBBH",
389 : AP_HAL::micros64(),
390 42 : _ap_dronecan._driver_index,
391 : source_node_id,
392 42 : msg.uptime_sec,
393 42 : msg.health,
394 42 : msg.mode,
395 42 : msg.sub_mode,
396 42 : msg.vendor_specific_status_code);
397 : }
398 :
399 : // If uptime goes backwards re-trigger node info log
400 52 : if (node->last_uptime_sec > msg.uptime_sec) {
401 0 : node_logged.clear(source_node_id);
402 : }
403 52 : node->last_uptime_sec = msg.uptime_sec;
404 : }
405 : #endif // HAL_LOGGING_ENABLED
406 :
407 : /* Handles Node Status Message, adds to the Seen Node list
408 : Also starts the Service call for Node Info to complete the
409 : Verification process. */
410 43 : void AP_DroneCAN_DNA_Server::handleNodeStatus(const CanardRxTransfer& transfer, const uavcan_protocol_NodeStatus& msg)
411 : {
412 43 : if (transfer.source_node_id > MAX_NODE_ID || transfer.source_node_id == 0) {
413 0 : return;
414 : }
415 129 : if ((msg.health != UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK ||
416 43 : msg.mode != UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL) &&
417 0 : !_ap_dronecan.option_is_set(AP_DroneCAN::Options::DNA_IGNORE_UNHEALTHY_NODE)) {
418 : //if node is not healthy or operational, clear resp health mask, and set fault_node_id
419 0 : fault_node_id = transfer.source_node_id;
420 0 : server_state = NODE_STATUS_UNHEALTHY;
421 0 : node_healthy.clear(transfer.source_node_id);
422 : } else {
423 43 : node_healthy.set(transfer.source_node_id);
424 43 : if (node_healthy == node_verified) {
425 41 : server_state = HEALTHY;
426 : }
427 : }
428 43 : if (!node_verified.get(transfer.source_node_id)) {
429 : //immediately begin verification of the node_id
430 : uavcan_protocol_GetNodeInfoRequest request;
431 2 : node_info_client.request(transfer.source_node_id, request);
432 : }
433 : //Add node to seen list if not seen before
434 43 : node_seen.set(transfer.source_node_id);
435 :
436 : #if HAL_LOGGING_ENABLED
437 : // Update status data for logging
438 43 : update_node_status(transfer.source_node_id, msg);
439 : #endif // HAL_LOGGING_ENABLED
440 : }
441 :
442 : /* Node Info message handler
443 : Handle responses from GetNodeInfo Request. We verify the node info
444 : against our records. Marks Verification mask if already recorded,
445 : Or register if the node id is available and not recorded for the
446 : received Unique ID */
447 9 : void AP_DroneCAN_DNA_Server::handleNodeInfo(const CanardRxTransfer& transfer, const uavcan_protocol_GetNodeInfoResponse& rsp)
448 : {
449 9 : if (transfer.source_node_id > MAX_NODE_ID || transfer.source_node_id == 0) {
450 0 : return;
451 : }
452 : /*
453 : if we haven't logged this node then log it now
454 : */
455 : #if HAL_LOGGING_ENABLED
456 9 : if (!node_logged.get(transfer.source_node_id) && AP::logger().logging_started()) {
457 0 : node_logged.set(transfer.source_node_id);
458 : uint64_t uid[2];
459 0 : memcpy(uid, rsp.hardware_version.unique_id, sizeof(rsp.hardware_version.unique_id));
460 : // @LoggerMessage: CAND
461 : // @Description: Info from GetNodeInfo request
462 : // @Field: TimeUS: Time since system startup
463 : // @Field: Driver: Driver index
464 : // @Field: NodeId: Node ID
465 : // @Field: UID1: Hardware ID, part 1
466 : // @Field: UID2: Hardware ID, part 2
467 : // @Field: Name: Name string
468 : // @Field: Major: major revision id
469 : // @Field: Minor: minor revision id
470 : // @Field: Version: AP_Periph git hash
471 0 : AP::logger().Write("CAND", "TimeUS,Driver,NodeId,UID1,UID2,Name,Major,Minor,Version",
472 : "s-#------", "F--------", "QBBQQZBBI",
473 : AP_HAL::micros64(),
474 0 : _ap_dronecan.get_driver_index(),
475 0 : transfer.source_node_id,
476 : uid[0], uid[1],
477 0 : rsp.name.data,
478 0 : rsp.software_version.major,
479 0 : rsp.software_version.minor,
480 0 : rsp.software_version.vcs_commit);
481 : }
482 :
483 : // Update status data for logging
484 9 : update_node_status(transfer.source_node_id, rsp.status);
485 : #endif // HAL_LOGGING_ENABLED
486 :
487 9 : bool duplicate = db.handle_node_info(transfer.source_node_id, rsp.hardware_version.unique_id);
488 9 : if (duplicate) {
489 0 : if (!_ap_dronecan.option_is_set(AP_DroneCAN::Options::DNA_IGNORE_DUPLICATE_NODE)) {
490 : /* This is a device with node_id already registered
491 : for another device */
492 0 : server_state = DUPLICATE_NODES;
493 0 : fault_node_id = transfer.source_node_id;
494 0 : memcpy(fault_node_name, rsp.name.data, sizeof(fault_node_name));
495 : }
496 : } else {
497 : //Verify as well
498 9 : node_verified.set(transfer.source_node_id);
499 9 : if (transfer.source_node_id == curr_verifying_node) {
500 7 : nodeInfo_resp_rcvd = true;
501 : }
502 : }
503 : }
504 :
505 : // process node ID allocation messages for DNA
506 10 : void AP_DroneCAN_DNA_Server::handle_allocation(const CanardRxTransfer& transfer, const uavcan_protocol_dynamic_node_id_Allocation& msg)
507 : {
508 10 : if (transfer.source_node_id != 0) {
509 3 : return; // ignore allocation messages that are not DNA requests
510 : }
511 10 : uint32_t now = AP_HAL::millis();
512 :
513 10 : if ((now - last_alloc_msg_ms) > UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_FOLLOWUP_TIMEOUT_MS) {
514 7 : rcvd_unique_id_offset = 0; // reset state, timed out
515 : }
516 :
517 10 : if (msg.first_part_of_unique_id) {
518 : // nodes waiting to send a follow up will instead send a first part
519 : // again if they see another allocation message. therefore we should
520 : // always reset and process a received first part, since any node we
521 : // were allocating will never send its follow up and we'd just time out
522 5 : rcvd_unique_id_offset = 0;
523 5 : } else if (rcvd_unique_id_offset == 0) {
524 3 : return; // not first part but we are expecting one, ignore
525 : }
526 :
527 7 : last_alloc_msg_ms = now;
528 7 : if ((rcvd_unique_id_offset + msg.unique_id.len) > sizeof(rcvd_unique_id)) {
529 0 : rcvd_unique_id_offset = 0; // reset state, request contains an over-long ID
530 0 : return;
531 : }
532 :
533 : // save the new portion of the unique ID
534 7 : memcpy(&rcvd_unique_id[rcvd_unique_id_offset], msg.unique_id.data, msg.unique_id.len);
535 7 : rcvd_unique_id_offset += msg.unique_id.len;
536 :
537 : // respond with the message containing the received unique ID so far, or
538 : // with the node ID if we successfully allocated one
539 7 : uavcan_protocol_dynamic_node_id_Allocation rsp {};
540 7 : memcpy(rsp.unique_id.data, rcvd_unique_id, rcvd_unique_id_offset);
541 7 : rsp.unique_id.len = rcvd_unique_id_offset;
542 :
543 7 : if (rcvd_unique_id_offset == sizeof(rcvd_unique_id)) { // full unique ID received, allocate it!
544 : // we ignore the preferred node ID as it seems nobody uses the feature
545 : // and we couldn't guarantee it anyway. we will always remember and
546 : // re-assign node IDs consistently, so the node could send a status
547 : // with a particular ID once then switch back to no preference for DNA
548 1 : rsp.node_id = db.handle_allocation(rcvd_unique_id);
549 1 : rcvd_unique_id_offset = 0; // reset state for next allocation
550 1 : if (rsp.node_id == 0) { // allocation failed
551 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "DroneCAN DNA allocation failed; database full");
552 : // don't send reply with a failed ID in case the allocatee does
553 : // silly things, though it is technically legal. the allocatee will
554 : // then time out and try again (though we still won't have an ID!)
555 0 : return;
556 : }
557 : }
558 :
559 7 : allocation_pub.broadcast(rsp, false); // never publish allocation message with CAN FD
560 : }
561 :
562 : //report the server state, along with failure message if any
563 200 : bool AP_DroneCAN_DNA_Server::prearm_check(char* fail_msg, uint8_t fail_msg_len) const
564 : {
565 200 : switch (server_state) {
566 200 : case HEALTHY:
567 200 : return true;
568 0 : case DUPLICATE_NODES: {
569 0 : if (_ap_dronecan.option_is_set(AP_DroneCAN::Options::DNA_IGNORE_DUPLICATE_NODE)) {
570 : // ignore error
571 0 : return true;
572 : }
573 0 : snprintf(fail_msg, fail_msg_len, "Duplicate Node %s../%d!", fault_node_name, fault_node_id);
574 0 : return false;
575 : }
576 0 : case NODE_STATUS_UNHEALTHY: {
577 0 : if (_ap_dronecan.option_is_set(AP_DroneCAN::Options::DNA_IGNORE_UNHEALTHY_NODE)) {
578 : // ignore error
579 0 : return true;
580 : }
581 0 : snprintf(fail_msg, fail_msg_len, "Node %d unhealthy!", fault_node_id);
582 0 : return false;
583 : }
584 : }
585 : // should never get; compiler should enforce all server_states are covered
586 0 : return false;
587 : }
588 :
589 : #endif // HAL_ENABLE_DRONECAN_DRIVERS
|
