diff --git a/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp b/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp index d61aae7995..5a8c654384 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp +++ b/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp @@ -1,4 +1,3 @@ - /* * This file is free software: you can redistribute it and/or modify it * under the terms of the GNU General Public License as published by the @@ -41,17 +40,16 @@ extern const AP_HAL::HAL& hal; #define debug_dronecan(level_debug, fmt, args...) do { AP::can().log_text(level_debug, "DroneCAN", fmt, ##args); } while (0) +// database is currently shared by all DNA servers +AP_DroneCAN_DNA_Server::Database AP_DroneCAN_DNA_Server::db; + AP_DroneCAN_DNA_Server::AP_DroneCAN_DNA_Server(AP_DroneCAN &ap_dronecan, CanardInterface &canard_iface, uint8_t driver_index) : _ap_dronecan(ap_dronecan), _canard_iface(canard_iface), storage(StorageManager::StorageCANDNA), allocation_sub(allocation_cb, driver_index), node_status_sub(node_status_cb, driver_index), - node_info_client(_canard_iface, node_info_cb) -{ - // storage size must be synced with StorageCANDNA entry in StorageManager.cpp - static_assert(NODEDATA_LOC(MAX_NODE_ID+1) <= 1024, "DNA storage too small"); -} + node_info_client(_canard_iface, node_info_cb) {} /* Method to generate 6byte hash from the Unique ID. We return it packed inside the referenced NodeData structure */ @@ -70,25 +68,25 @@ void AP_DroneCAN_DNA_Server::getHash(NodeData &node_data, const uint8_t unique_i } //Read Node Data from Storage Region -void AP_DroneCAN_DNA_Server::readNodeData(NodeData &data, uint8_t node_id) +void AP_DroneCAN_DNA_Server::Database::readNodeData(NodeData &data, uint8_t node_id) { if (node_id > MAX_NODE_ID) { return; } - WITH_SEMAPHORE(storage_sem); - storage.read_block(&data, NODEDATA_LOC(node_id), sizeof(struct NodeData)); + WITH_SEMAPHORE(sem); + storage->read_block(&data, NODEDATA_LOC(node_id), sizeof(struct NodeData)); } //Write Node Data to Storage Region -void AP_DroneCAN_DNA_Server::writeNodeData(const NodeData &data, uint8_t node_id) +void AP_DroneCAN_DNA_Server::Database::writeNodeData(const NodeData &data, uint8_t node_id) { if (node_id > MAX_NODE_ID) { return; } - WITH_SEMAPHORE(storage_sem); - storage.write_block(NODEDATA_LOC(node_id), &data, sizeof(struct NodeData)); + WITH_SEMAPHORE(sem); + storage->write_block(NODEDATA_LOC(node_id), &data, sizeof(struct NodeData)); } /* Remove Node Data from Server Record in Storage, @@ -103,10 +101,10 @@ void AP_DroneCAN_DNA_Server::freeNodeID(uint8_t node_id) //Eliminate from Server Record memset(&node_data, 0, sizeof(node_data)); - writeNodeData(node_data, node_id); + db.writeNodeData(node_data, node_id); //Clear Occupation Mask - node_storage_occupied.clear(node_id); + db.node_storage_occupied.clear(node_id); } /* Go through Server Records, and fetch node id that matches the provided @@ -118,8 +116,8 @@ uint8_t AP_DroneCAN_DNA_Server::getNodeIDForUniqueID(const uint8_t unique_id[], getHash(cmp_node_data, unique_id, size); for (int i = MAX_NODE_ID; i > 0; i--) { - if (node_storage_occupied.get(i)) { - readNodeData(node_data, i); + if (db.node_storage_occupied.get(i)) { + db.readNodeData(node_data, i); if (memcmp(node_data.hwid_hash, cmp_node_data.hwid_hash, sizeof(NodeData::hwid_hash)) == 0) { return i; // node ID found } @@ -138,13 +136,13 @@ void AP_DroneCAN_DNA_Server::addNodeIDForUniqueID(uint8_t node_id, const uint8_t node_data.crc = crc_crc8(node_data.hwid_hash, sizeof(node_data.hwid_hash)); //Write Data to the records - writeNodeData(node_data, node_id); + db.writeNodeData(node_data, node_id); - node_storage_occupied.set(node_id); + db.node_storage_occupied.set(node_id); } //Checks if a valid Server Record is present for specified Node ID -bool AP_DroneCAN_DNA_Server::isValidNodeDataAvailable(uint8_t node_id) +bool AP_DroneCAN_DNA_Server::Database::isValidNodeDataAvailable(uint8_t node_id) { NodeData node_data; readNodeData(node_data, node_id); @@ -157,6 +155,32 @@ bool AP_DroneCAN_DNA_Server::isValidNodeDataAvailable(uint8_t node_id) return false; } +// initialize database (storage accessor is always replaced with the one supplied) +void AP_DroneCAN_DNA_Server::Database::init(StorageAccess *storage_) +{ + // storage size must be synced with StorageCANDNA entry in StorageManager.cpp + static_assert(NODEDATA_LOC(MAX_NODE_ID+1) <= 1024, "DNA storage too small"); + + // might be called from multiple threads if multiple servers use the same database + WITH_SEMAPHORE(sem); + + storage = storage_; // use supplied accessor + + // validate magic number + uint16_t magic = storage->read_uint16(0); + if (magic != NODEDATA_MAGIC) { + reset(); // re-initializing the database will put the magic back + } + + /* Go through our records and look for valid NodeData, to initialise + occupied status */ + for (uint8_t i = 1; i <= MAX_NODE_ID; i++) { + if (isValidNodeDataAvailable(i)) { + node_storage_occupied.set(i); + } + } +} + /* Initialises Publishers for respective UAVCAN Instance Also resets the Server Record in case there is a mismatch between specified node id and unique id against the existing @@ -166,27 +190,11 @@ bool AP_DroneCAN_DNA_Server::init(uint8_t own_unique_id[], uint8_t own_unique_id //Read the details from AP_DroneCAN server_state = HEALTHY; - // Check if the magic is present - uint16_t magic; - { - WITH_SEMAPHORE(storage_sem); - magic = storage.read_uint16(0); - } - if (magic != NODEDATA_MAGIC) { - //Its not there a reset should write it in the Storage - reset(); - } + db.init(&storage); // initialize the database with our accessor + if (_ap_dronecan.check_and_reset_option(AP_DroneCAN::Options::DNA_CLEAR_DATABASE)) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "UC DNA database reset"); - reset(); - } - - /* Go through our records and look for valid NodeData, to initialise - occupied status */ - for (uint8_t i = 1; i <= MAX_NODE_ID; i++) { - if (isValidNodeDataAvailable(i)) { - node_storage_occupied.set(i); - } + db.reset(); } // Making sure that the server is started with the same node ID @@ -197,7 +205,7 @@ bool AP_DroneCAN_DNA_Server::init(uint8_t own_unique_id[], uint8_t own_unique_id if (!reset_done) { /* ensure we only reset once per power cycle else we will wipe own record on next init(s) */ - reset(); + db.reset(); reset_done = true; } //Add ourselves to the Server Record @@ -214,8 +222,10 @@ bool AP_DroneCAN_DNA_Server::init(uint8_t own_unique_id[], uint8_t own_unique_id //Reset the Server Records -void AP_DroneCAN_DNA_Server::reset() +void AP_DroneCAN_DNA_Server::Database::reset() { + WITH_SEMAPHORE(sem); + NodeData node_data; memset(&node_data, 0, sizeof(node_data)); node_storage_occupied.clearall(); @@ -225,9 +235,9 @@ void AP_DroneCAN_DNA_Server::reset() for (uint8_t i = 0; i <= MAX_NODE_ID; i++) { writeNodeData(node_data, i); } - WITH_SEMAPHORE(storage_sem); + //Ensure we mark magic at the end - storage.write_uint16(0, NODEDATA_MAGIC); + storage->write_uint16(0, NODEDATA_MAGIC); } /* Go through the Occupation mask for available Node ID @@ -241,7 +251,7 @@ uint8_t AP_DroneCAN_DNA_Server::findFreeNodeID(uint8_t preferred) // Search up uint8_t candidate = preferred; while (candidate <= 125) { - if (!node_storage_occupied.get(candidate)) { + if (!db.node_storage_occupied.get(candidate)) { return candidate; } candidate++; @@ -249,7 +259,7 @@ uint8_t AP_DroneCAN_DNA_Server::findFreeNodeID(uint8_t preferred) //Search down candidate = preferred; while (candidate > 0) { - if (!node_storage_occupied.get(candidate)) { + if (!db.node_storage_occupied.get(candidate)) { return candidate; } candidate--; @@ -302,7 +312,7 @@ void AP_DroneCAN_DNA_Server::verify_nodes() break; } } - if (node_storage_occupied.get(curr_verifying_node)) { + if (db.node_storage_occupied.get(curr_verifying_node)) { uavcan_protocol_GetNodeInfoRequest request; node_info_client.request(curr_verifying_node, request); nodeInfo_resp_rcvd = false; @@ -381,7 +391,7 @@ void AP_DroneCAN_DNA_Server::handleNodeInfo(const CanardRxTransfer& transfer, co } #endif - if (node_storage_occupied.get(transfer.source_node_id)) { + if (db.node_storage_occupied.get(transfer.source_node_id)) { //if node_id already registered, just verify if Unique ID matches as well if (transfer.source_node_id == getNodeIDForUniqueID(rsp.hardware_version.unique_id, 16)) { if (transfer.source_node_id == curr_verifying_node) { diff --git a/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.h b/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.h index 48b2c3cc31..62ddeae49c 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.h +++ b/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.h @@ -23,6 +23,35 @@ class AP_DroneCAN_DNA_Server uint8_t crc; }; + class Database { + public: + Database() {}; + + // initialize database (storage accessor is always replaced with the one supplied) + void init(StorageAccess *storage_); + + //Reset the Server Record + void reset(); + + //Look in the storage and check if there's a valid Server Record there + bool isValidNodeDataAvailable(uint8_t node_id); + + //Reads the Server Record from storage for specified node id + void readNodeData(NodeData &data, uint8_t node_id); + + //Writes the Server Record from storage for specified node id + void writeNodeData(const NodeData &data, uint8_t node_id); + + // bitmasks containing a status for each possible node ID (except 0 and > MAX_NODE_ID) + Bitmask<128> node_storage_occupied; // storage has a valid entry + + private: + StorageAccess *storage; + HAL_Semaphore sem; + }; + + static Database db; + enum ServerState { NODE_STATUS_UNHEALTHY = -5, DUPLICATE_NODES = -2, @@ -35,7 +64,6 @@ class AP_DroneCAN_DNA_Server bool nodeInfo_resp_rcvd; // bitmasks containing a status for each possible node ID (except 0 and > MAX_NODE_ID) - Bitmask<128> node_storage_occupied; // storage has a valid entry Bitmask<128> node_verified; // node seen and unique ID matches stored Bitmask<128> node_seen; // received NodeStatus Bitmask<128> node_logged; // written to log fle @@ -57,15 +85,6 @@ class AP_DroneCAN_DNA_Server //Generates 6Byte long hash from the specified unique_id void getHash(NodeData &node_data, const uint8_t unique_id[], uint8_t size) const; - //Reset the Server Record - void reset(); - - //Reads the Server Record from storage for specified node id - void readNodeData(NodeData &data, uint8_t node_id); - - //Writes the Server Record from storage for specified node id - void writeNodeData(const NodeData &data, uint8_t node_id); - //Methods to set, clear and report NodeIDs allocated/registered so far void freeNodeID(uint8_t node_id); @@ -78,10 +97,6 @@ class AP_DroneCAN_DNA_Server //Finds next available free Node, starting from preferred NodeID uint8_t findFreeNodeID(uint8_t preferred); - //Look in the storage and check if there's a valid Server Record there - bool isValidNodeDataAvailable(uint8_t node_id); - - HAL_Semaphore storage_sem; AP_DroneCAN &_ap_dronecan; CanardInterface &_canard_iface;