mirror of https://github.com/ArduPilot/ardupilot
AP_DroneCAN: DNA_Server: move storage read/write to its own class
There is (currently) only one storage area that is used by all servers, so it needs to be managed by its own class shared among them. The occupied mask is also moved as it reflects the storage contents and so can't be stored by each server.
This commit is contained in:
Thomas Watson
Andrew Tridgell
parent
698f8fb40e
commit
a9774d2334
|
|
@ -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) {
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue