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

AP_DroneCAN: DNA_Server: miscellaneous cleanup

Browse Source
This commit is contained in:
Thomas Watson 2024-07-13 16:12:52 -05:00 committed by Andrew Tridgell
parent 5e0bb99dbb
commit 56593574a9
1 changed files with 20 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp
View File

@ -30,8 +30,12 @@
#include <stdio.h> #include <stdio.h>
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
// FORMAT REVISION DREAMS (things to address if the nodedata needs to be changed substantially)
// * have DNA server accept only a 16 byte local UID to avoid overhead from variable sized hash
// * have a real empty flag for entries and/or use a CRC which is not zero for an input of all zeros
#define NODEDATA_MAGIC 0xAC01 #define NODEDATA_MAGIC 0xAC01
#define NODEDATA_MAGIC_LEN 2 #define NODEDATA_MAGIC_LEN 2 // uint16_t
#define MAX_NODE_ID 125 #define MAX_NODE_ID 125
#define NODEDATA_LOC(node_id) ((node_id * sizeof(struct NodeData)) + NODEDATA_MAGIC_LEN) #define NODEDATA_LOC(node_id) ((node_id * sizeof(struct NodeData)) + NODEDATA_MAGIC_LEN)
@ -151,21 +155,18 @@ Unique IDs hash.
Returns 0 if no Node ID was detected */ Returns 0 if no Node ID was detected */
uint8_t AP_DroneCAN_DNA_Server::getNodeIDForUniqueID(const uint8_t unique_id[], uint8_t size) uint8_t AP_DroneCAN_DNA_Server::getNodeIDForUniqueID(const uint8_t unique_id[], uint8_t size)
{ {
uint8_t node_id = 0;
NodeData node_data, cmp_node_data; NodeData node_data, cmp_node_data;
getHash(cmp_node_data, unique_id, size); getHash(cmp_node_data, unique_id, size);
for (int i = MAX_NODE_ID; i > 0; i--) { for (int i = MAX_NODE_ID; i > 0; i--) {
if (!isNodeIDOccupied(i)) { // No point in checking NodeID that's not taken if (isNodeIDOccupied(i)) {
continue; readNodeData(node_data, i);
} if (memcmp(node_data.hwid_hash, cmp_node_data.hwid_hash, sizeof(NodeData::hwid_hash)) == 0) {
readNodeData(node_data, i); return i; // node ID found
if (memcmp(node_data.hwid_hash, cmp_node_data.hwid_hash, sizeof(NodeData::hwid_hash)) == 0) { }
node_id = i;
break;
} }
} }
return node_id; return 0; // not found
} }
/* Hash the Unique ID and add it to the Server Record /* Hash the Unique ID and add it to the Server Record
@ -210,7 +211,7 @@ bool AP_DroneCAN_DNA_Server::init(uint8_t own_unique_id[], uint8_t own_unique_id
uint16_t magic; uint16_t magic;
{ {
WITH_SEMAPHORE(storage_sem); WITH_SEMAPHORE(storage_sem);
storage.read_block(&magic, 0, NODEDATA_MAGIC_LEN); magic = storage.read_uint16(0);
} }
if (magic != NODEDATA_MAGIC) { if (magic != NODEDATA_MAGIC) {
//Its not there a reset should write it in the Storage //Its not there a reset should write it in the Storage
@ -232,21 +233,8 @@ bool AP_DroneCAN_DNA_Server::init(uint8_t own_unique_id[], uint8_t own_unique_id
// Making sure that the server is started with the same node ID // Making sure that the server is started with the same node ID
const uint8_t stored_own_node_id = getNodeIDForUniqueID(own_unique_id, own_unique_id_len); const uint8_t stored_own_node_id = getNodeIDForUniqueID(own_unique_id, own_unique_id_len);
static bool reset_done; static bool reset_done;
if (stored_own_node_id != 0) { if (stored_own_node_id != node_id) { // cannot match if not found
if (stored_own_node_id != node_id) { // We have no matching record of our own Unique ID do a reset
/* We have a different node id recorded against our own unique id
This calls for a reset */
if (!reset_done) {
/* ensure we only reset once per power cycle
else we will wipe own record on next init(s) */
reset();
reset_done = true;
}
//Add ourselves to the Server Record
addNodeIDForUniqueID(node_id, own_unique_id, own_unique_id_len);
}
} else {
//We have no record of our own Unique ID do a reset
if (!reset_done) { if (!reset_done) {
/* ensure we only reset once per power cycle /* ensure we only reset once per power cycle
else we will wipe own record on next init(s) */ else we will wipe own record on next init(s) */
@ -280,8 +268,7 @@ void AP_DroneCAN_DNA_Server::reset()
} }
WITH_SEMAPHORE(storage_sem); WITH_SEMAPHORE(storage_sem);
//Ensure we mark magic at the end //Ensure we mark magic at the end
uint16_t magic = NODEDATA_MAGIC; storage.write_uint16(0, NODEDATA_MAGIC);
storage.write_block(0, &magic, NODEDATA_MAGIC_LEN);
} }
/* Go through the Occupation mask for available Node ID /* Go through the Occupation mask for available Node ID
@ -289,8 +276,11 @@ based on pseudo code provided in
uavcan/protocol/dynamic_node_id/1.Allocation.uavcan */ uavcan/protocol/dynamic_node_id/1.Allocation.uavcan */
uint8_t AP_DroneCAN_DNA_Server::findFreeNodeID(uint8_t preferred) uint8_t AP_DroneCAN_DNA_Server::findFreeNodeID(uint8_t preferred)
{ {
if (preferred == 0) {
preferred = 125;
}
// Search up // Search up
uint8_t candidate = (preferred > 0) ? preferred : 125; uint8_t candidate = preferred;
while (candidate <= 125) { while (candidate <= 125) {
if (!isNodeIDOccupied(candidate)) { if (!isNodeIDOccupied(candidate)) {
return candidate; return candidate;
@ -298,7 +288,7 @@ uint8_t AP_DroneCAN_DNA_Server::findFreeNodeID(uint8_t preferred)
candidate++; candidate++;
} }
//Search down //Search down
candidate = (preferred > 0) ? preferred : 125; candidate = preferred;
while (candidate > 0) { while (candidate > 0) {
if (!isNodeIDOccupied(candidate)) { if (!isNodeIDOccupied(candidate)) {
return candidate; return candidate;