Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 06:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_DroneCAN: DNA_Server: clarify comments with consistent terminology

Browse Source
This commit is contained in:
Thomas Watson 2024-08-17 13:17:42 -05:00 committed by Andrew Tridgell
parent 9669b4e85d
commit b38766c469
2 changed files with 45 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

75
libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp
View File

@ -51,8 +51,7 @@ AP_DroneCAN_DNA_Server::AP_DroneCAN_DNA_Server(AP_DroneCAN &ap_dronecan, CanardI
node_status_sub(node_status_cb, driver_index), node_status_sub(node_status_cb, driver_index),
node_info_client(_canard_iface, node_info_cb) {} node_info_client(_canard_iface, node_info_cb) {}
/* Method to generate 6byte hash from the Unique ID. // fill the given record with the hash of the given unique ID
We return it packed inside the referenced NodeRecord structure */
void AP_DroneCAN_DNA_Server::Database::compute_uid_hash(NodeRecord &record, const uint8_t unique_id[], uint8_t size) const void AP_DroneCAN_DNA_Server::Database::compute_uid_hash(NodeRecord &record, const uint8_t unique_id[], uint8_t size) const
{ {
uint64_t hash = FNV_1_OFFSET_BASIS_64; uint64_t hash = FNV_1_OFFSET_BASIS_64;
@ -67,7 +66,7 @@ void AP_DroneCAN_DNA_Server::Database::compute_uid_hash(NodeRecord &record, cons
} }
} }
//Read Node Data from Storage Region // read the given node ID's registration's record
void AP_DroneCAN_DNA_Server::Database::read_record(NodeRecord &record, uint8_t node_id) void AP_DroneCAN_DNA_Server::Database::read_record(NodeRecord &record, uint8_t node_id)
{ {
if (node_id > MAX_NODE_ID) { if (node_id > MAX_NODE_ID) {
@ -77,7 +76,7 @@ void AP_DroneCAN_DNA_Server::Database::read_record(NodeRecord &record, uint8_t n
storage->read_block(&record, NODERECORD_LOC(node_id), sizeof(NodeRecord)); storage->read_block(&record, NODERECORD_LOC(node_id), sizeof(NodeRecord));
} }
//Write Node Data to Storage Region // write the given node ID's registration's record
void AP_DroneCAN_DNA_Server::Database::write_record(const NodeRecord &record, uint8_t node_id) void AP_DroneCAN_DNA_Server::Database::write_record(const NodeRecord &record, uint8_t node_id)
{ {
if (node_id > MAX_NODE_ID) { if (node_id > MAX_NODE_ID) {
@ -87,8 +86,7 @@ void AP_DroneCAN_DNA_Server::Database::write_record(const NodeRecord &record, ui
storage->write_block(NODERECORD_LOC(node_id), &record, sizeof(NodeRecord)); storage->write_block(NODERECORD_LOC(node_id), &record, sizeof(NodeRecord));
} }
/* Remove Node Data from Server Record in Storage, // delete the given node ID's registration
and also clear Occupation Mask */
void AP_DroneCAN_DNA_Server::Database::delete_registration(uint8_t node_id) void AP_DroneCAN_DNA_Server::Database::delete_registration(uint8_t node_id)
{ {
if (node_id > MAX_NODE_ID) { if (node_id > MAX_NODE_ID) {
@ -97,17 +95,13 @@ void AP_DroneCAN_DNA_Server::Database::delete_registration(uint8_t node_id)
NodeRecord record; NodeRecord record;
//Eliminate from Server Record // all-zero record means no registration
memset(&record, 0, sizeof(record)); memset(&record, 0, sizeof(record));
write_record(record, node_id); write_record(record, node_id);
//Clear Occupation Mask
node_registered.clear(node_id); node_registered.clear(node_id);
} }
/* Go through Server Records, and fetch node id that matches the provided // retrieve node ID that matches the given unique ID. returns 0 if not found
Unique IDs hash.
Returns 0 if no Node ID was detected */
uint8_t AP_DroneCAN_DNA_Server::Database::find_node_id(const uint8_t unique_id[], uint8_t size) uint8_t AP_DroneCAN_DNA_Server::Database::find_node_id(const uint8_t unique_id[], uint8_t size)
{ {
NodeRecord record, cmp_record; NodeRecord record, cmp_record;
@ -124,22 +118,20 @@ uint8_t AP_DroneCAN_DNA_Server::Database::find_node_id(const uint8_t unique_id[]
return 0; // not found return 0; // not found
} }
/* Hash the Unique ID and add it to the Server Record // create the registration for the given node ID and set its record's unique ID
for specified Node ID. */
void AP_DroneCAN_DNA_Server::Database::create_registration(uint8_t node_id, const uint8_t unique_id[], uint8_t size) void AP_DroneCAN_DNA_Server::Database::create_registration(uint8_t node_id, const uint8_t unique_id[], uint8_t size)
{ {
NodeRecord record; NodeRecord record;
compute_uid_hash(record, unique_id, size); compute_uid_hash(record, unique_id, size);
//Generate CRC for validating the record when read back // compute and store CRC of the record's data to validate it
record.crc = crc_crc8(record.uid_hash, sizeof(record.uid_hash)); record.crc = crc_crc8(record.uid_hash, sizeof(record.uid_hash));
//Write Data to the records
write_record(record, node_id); write_record(record, node_id);
node_registered.set(node_id); node_registered.set(node_id);
} }
//Checks if a valid Server Record is present for specified Node ID // return true if the given node ID has a registration
bool AP_DroneCAN_DNA_Server::Database::check_registration(uint8_t node_id) bool AP_DroneCAN_DNA_Server::Database::check_registration(uint8_t node_id)
{ {
NodeRecord record; NodeRecord record;
@ -148,7 +140,7 @@ bool AP_DroneCAN_DNA_Server::Database::check_registration(uint8_t node_id)
uint8_t empty_uid[sizeof(NodeRecord::uid_hash)] = {0}; uint8_t empty_uid[sizeof(NodeRecord::uid_hash)] = {0};
uint8_t crc = crc_crc8(record.uid_hash, sizeof(record.uid_hash)); uint8_t crc = crc_crc8(record.uid_hash, sizeof(record.uid_hash));
if (crc == record.crc && memcmp(&record.uid_hash[0], &empty_uid[0], sizeof(empty_uid)) != 0) { if (crc == record.crc && memcmp(&record.uid_hash[0], &empty_uid[0], sizeof(empty_uid)) != 0) {
return true; return true; // CRC matches and UID hash is not all zero
} }
return false; return false;
} }
@ -167,11 +159,10 @@ void AP_DroneCAN_DNA_Server::Database::init(StorageAccess *storage_)
// validate magic number // validate magic number
uint16_t magic = storage->read_uint16(0); uint16_t magic = storage->read_uint16(0);
if (magic != NODERECORD_MAGIC) { if (magic != NODERECORD_MAGIC) {
reset(); // re-initializing the database will put the magic back reset(); // resetting the database will put the magic back
} }
/* Go through our records and look for valid NodeRecord, to initialise // check and note each possible node ID's registration's presence
occupied status */
for (uint8_t i = 1; i <= MAX_NODE_ID; i++) { for (uint8_t i = 1; i <= MAX_NODE_ID; i++) {
if (check_registration(i)) { if (check_registration(i)) {
node_registered.set(i); node_registered.set(i);
@ -206,29 +197,27 @@ bool AP_DroneCAN_DNA_Server::init(uint8_t own_unique_id[], uint8_t own_unique_id
return true; return true;
} }
// handle initializing the server with the given expected node ID and unique ID // handle initializing the server with its own node ID and unique ID
void AP_DroneCAN_DNA_Server::Database::init_server(uint8_t node_id, const uint8_t own_unique_id[], uint8_t own_unique_id_len) void AP_DroneCAN_DNA_Server::Database::init_server(uint8_t node_id, const uint8_t own_unique_id[], uint8_t own_unique_id_len)
{ {
WITH_SEMAPHORE(sem); WITH_SEMAPHORE(sem);
// Making sure that the server is started with the same node ID // ensure that its node ID and unique ID match in the database
const uint8_t stored_own_node_id = find_node_id(own_unique_id, own_unique_id_len); const uint8_t stored_own_node_id = find_node_id(own_unique_id, own_unique_id_len);
static bool reset_done; static bool reset_done;
if (stored_own_node_id != node_id) { // cannot match if not found if (stored_own_node_id != node_id) { // cannot match if its unique ID was not found
// We have no matching record of our own Unique ID do a reset // we have no record of its unique ID, do a reset
if (!reset_done) { if (!reset_done) {
/* ensure we only reset once per power cycle // only reset once per power cycle else we could wipe other servers' registrations
else we will wipe own record on next init(s) */
reset(); reset();
reset_done = true; reset_done = true;
} }
//Add ourselves to the Server Record
create_registration(node_id, own_unique_id, own_unique_id_len); create_registration(node_id, own_unique_id, own_unique_id_len);
} }
} }
//Reset the Server Records // remove all registrations from the database
void AP_DroneCAN_DNA_Server::Database::reset() void AP_DroneCAN_DNA_Server::Database::reset()
{ {
WITH_SEMAPHORE(sem); WITH_SEMAPHORE(sem);
@ -237,19 +226,19 @@ void AP_DroneCAN_DNA_Server::Database::reset()
memset(&record, 0, sizeof(record)); memset(&record, 0, sizeof(record));
node_registered.clearall(); node_registered.clearall();
//Just write empty Node Data to the Records // all-zero record means no registration
// ensure node ID 0 is cleared even if we can't use it so we know the state // ensure node ID 0 is cleared even if we can't use it so we know the state
for (uint8_t i = 0; i <= MAX_NODE_ID; i++) { for (uint8_t i = 0; i <= MAX_NODE_ID; i++) {
write_record(record, i); write_record(record, i);
} }
//Ensure we mark magic at the end // mark the magic at the start to indicate a valid (and reset) database
storage->write_uint16(0, NODERECORD_MAGIC); storage->write_uint16(0, NODERECORD_MAGIC);
} }
/* Go through the Occupation mask for available Node ID // search for a free node ID, starting at the preferred ID (which can be 0 if
based on pseudo code provided in // none are preferred). returns 0 if none found. based on pseudocode in
uavcan/protocol/dynamic_node_id/1.Allocation.uavcan */ // uavcan/protocol/dynamic_node_id/1.Allocation.uavcan
uint8_t AP_DroneCAN_DNA_Server::Database::find_free_node_id(uint8_t preferred) uint8_t AP_DroneCAN_DNA_Server::Database::find_free_node_id(uint8_t preferred)
{ {
if (preferred == 0) { if (preferred == 0) {
@ -263,7 +252,7 @@ uint8_t AP_DroneCAN_DNA_Server::Database::find_free_node_id(uint8_t preferred)
} }
candidate++; candidate++;
} }
//Search down // Search down
candidate = preferred; candidate = preferred;
while (candidate > 0) { while (candidate > 0) {
if (!node_registered.get(candidate)) { if (!node_registered.get(candidate)) {
@ -416,29 +405,25 @@ void AP_DroneCAN_DNA_Server::handleNodeInfo(const CanardRxTransfer& transfer, co
} }
} }
// handle processing the node info message. returns true if duplicate. // handle processing the node info message. returns true if from a duplicate node
bool AP_DroneCAN_DNA_Server::Database::handle_node_info(uint8_t source_node_id, const uint8_t unique_id[]) bool AP_DroneCAN_DNA_Server::Database::handle_node_info(uint8_t source_node_id, const uint8_t unique_id[])
{ {
WITH_SEMAPHORE(sem); WITH_SEMAPHORE(sem);
if (is_registered(source_node_id)) { if (is_registered(source_node_id)) {
//if node_id already registered, just verify if Unique ID matches as well // verify that the unique ID matches our registration for the node ID
if (source_node_id == find_node_id(unique_id, 16)) { if (source_node_id == find_node_id(unique_id, 16)) {
return false; return false;
} else { } else {
/* This is a device with node_id already registered // this device's node ID is associated with a different device
for another device */
return true; return true;
} }
} else { } else {
/* Node Id was not allocated by us, or during this boot, let's register this in our records // we don't know about this node ID, let's register it
Check if we allocated this Node before */ uint8_t prev_node_id = find_node_id(unique_id, 16); // have we registered this unique ID under a different node ID?
uint8_t prev_node_id = find_node_id(unique_id, 16);
if (prev_node_id != 0) { if (prev_node_id != 0) {
//yes we did, remove this registration delete_registration(prev_node_id); // yes, delete old registration
delete_registration(prev_node_id);
} }
//add a new server record
create_registration(source_node_id, unique_id, 16); create_registration(source_node_id, unique_id, 16);
return false; return false;
} }

28
libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.h
View File

@ -45,46 +45,48 @@ class AP_DroneCAN_DNA_Server
// initialize database (storage accessor is always replaced with the one supplied) // initialize database (storage accessor is always replaced with the one supplied)
void init(StorageAccess *storage_); void init(StorageAccess *storage_);
//Reset the Server Record // remove all registrations from the database
void reset(); void reset();
// returns true if the given node ID is occupied (has valid stored data) // return true if the given node ID is registered
bool is_registered(uint8_t node_id) { bool is_registered(uint8_t node_id) {
return node_registered.get(node_id); return node_registered.get(node_id);
} }
// handle initializing the server with the given expected node ID and unique ID // handle initializing the server with its own node ID and unique ID
void init_server(uint8_t node_id, const uint8_t own_unique_id[], uint8_t own_unique_id_len); void init_server(uint8_t node_id, const uint8_t own_unique_id[], uint8_t own_unique_id_len);
// handle processing the node info message. returns true if duplicate. // handle processing the node info message. returns true if from a duplicate node
bool handle_node_info(uint8_t source_node_id, const uint8_t unique_id[]); bool handle_node_info(uint8_t source_node_id, const uint8_t unique_id[]);
// handle the allocation message. returns the new node ID. // handle the allocation message. returns the allocated node ID, or 0 if allocation failed
uint8_t handle_allocation(uint8_t node_id, const uint8_t unique_id[]); uint8_t handle_allocation(uint8_t node_id, const uint8_t unique_id[]);
private: private:
//Generates 6Byte long hash from the specified unique_id // fill the given record with the hash of the given unique ID
void compute_uid_hash(NodeRecord &record, const uint8_t unique_id[], uint8_t size) const; void compute_uid_hash(NodeRecord &record, const uint8_t unique_id[], uint8_t size) const;
//Methods to set, clear and report NodeIDs allocated/registered so far // delete the given node ID's registration
void delete_registration(uint8_t node_id); void delete_registration(uint8_t node_id);
//Go through List to find node id for specified unique id // retrieve node ID that matches the given unique ID. returns 0 if not found
uint8_t find_node_id(const uint8_t unique_id[], uint8_t size); uint8_t find_node_id(const uint8_t unique_id[], uint8_t size);
//Add Node ID info to the record and setup necessary mask fields // create the registration for the given node ID and set its record's unique ID
void create_registration(uint8_t node_id, const uint8_t unique_id[], uint8_t size); void create_registration(uint8_t node_id, const uint8_t unique_id[], uint8_t size);
//Finds next available free Node, starting from preferred NodeID // search for a free node ID, starting at the preferred ID (which can be 0 if
// none are preferred). returns 0 if none found. based on pseudocode in
// uavcan/protocol/dynamic_node_id/1.Allocation.uavcan
uint8_t find_free_node_id(uint8_t preferred); uint8_t find_free_node_id(uint8_t preferred);
//Look in the storage and check if there's a valid Server Record there // return true if the given node ID has a registration
bool check_registration(uint8_t node_id); bool check_registration(uint8_t node_id);
//Reads the Server Record from storage for specified node id // read the given node ID's registration's record
void read_record(NodeRecord &record, uint8_t node_id); void read_record(NodeRecord &record, uint8_t node_id);
//Writes the Server Record from storage for specified node id // write the given node ID's registration's record
void write_record(const NodeRecord &record, uint8_t node_id); void write_record(const NodeRecord &record, uint8_t node_id);
// bitmasks containing a status for each possible node ID (except 0 and > MAX_NODE_ID) // bitmasks containing a status for each possible node ID (except 0 and > MAX_NODE_ID)