AP_UAVCAN: setup uavcan server with internal storage

libraries/AP_UAVCAN/AP_UAVCAN.cpp

@@ -49,6 +49,7 @@
 #include <SRV_Channel/SRV_Channel.h>
 #include <AP_OpticalFlow/AP_OpticalFlow_HereFlow.h>
 #include <AP_ADSB/AP_ADSB.h>
+#include "AP_UAVCAN_Server.h"
 
 #define LED_DELAY_US 50000
 
@@ -204,7 +205,12 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
     uint8_t uid_len = uid_buf_len;
     uint8_t unique_id[uid_buf_len];
 
+
     if (hal.util->get_system_id_unformatted(unique_id, uid_len)) {
+        //This is because we are maintaining a common Server Record for all UAVCAN Instances.
+        //In case the node IDs are different, and unique id same, it will create
+        //conflict in the Server Record.
+        unique_id[uid_len - 1] += _uavcan_node;
         uavcan::copy(unique_id, unique_id + uid_len, hw_version.unique_id.begin());
     }
     _node->setHardwareVersion(hw_version);
@@ -216,10 +222,13 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
     }
 
     //Start Servers
-#ifdef HAS_UAVCAN_SERVERS
+    if (!AP::uavcan_server().init(this)) {
-    _servers.init(*_node);
+        debug_uavcan(1, "UAVCAN: Failed to start DNA Server\n\r");
-#endif
+        return;
+    }
+
     // Roundup all subscribers from supported drivers
+    AP_UAVCAN_Server::subscribe_msgs(this);
     AP_GPS_UAVCAN::subscribe_msgs(this);
     AP_Compass_UAVCAN::subscribe_msgs(this);
     AP_Baro_UAVCAN::subscribe_msgs(this);
@@ -329,6 +338,7 @@ void AP_UAVCAN::loop(void)
         led_out_send();
         buzzer_send();
         safety_state_send();
+        AP::uavcan_server().verify_nodes(this);
     }
 }
 

libraries/AP_UAVCAN/AP_UAVCAN.h

@@ -18,13 +18,13 @@
 #define AP_UAVCAN_H_
 
 #include <uavcan/uavcan.hpp>
+#include "AP_UAVCAN_Server.h"
 
 #include <AP_HAL/CAN.h>
 #include <AP_HAL/Semaphores.h>
 #include <AP_Param/AP_Param.h>
 
 #include <uavcan/helpers/heap_based_pool_allocator.hpp>
-#include "AP_UAVCAN_Servers.h"
 
 #ifndef UAVCAN_NODE_POOL_SIZE
 #define UAVCAN_NODE_POOL_SIZE 8192
@@ -172,10 +172,6 @@ private:
     uint8_t _driver_index;
     char _thread_name[9];
     bool _initialized;
-#ifdef HAS_UAVCAN_SERVERS
-    AP_UAVCAN_Servers _servers;
-#endif
-
     ///// SRV output /////
     struct {
         uint16_t pulse;

libraries/AP_UAVCAN/AP_UAVCAN_Server.cpp

@@ -0,0 +1,673 @@
+
+/*
+ * 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
+ * Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ * Author: Siddharth Bharat Purohit
+ */
+
+#include <AP_HAL/AP_HAL.h>
+
+#if HAL_WITH_UAVCAN
+
+#include "AP_UAVCAN_Server.h"
+#include "AP_UAVCAN.h"
+#include <StorageManager/StorageManager.h>
+#include <AP_HAL/AP_HAL.h>
+#include <AP_Math/AP_Math.h>
+#include <uavcan/protocol/dynamic_node_id/Allocation.hpp>
+#include <GCS_MAVLink/GCS.h>
+
+extern const AP_HAL::HAL& hal;
+
+#define NODEDATA_MAGIC 0xAC01
+#define NODEDATA_MAGIC_LEN 2
+#define MAX_NODE_ID    125
+
+#define debug_uavcan(fmt, args...) do { hal.console->printf(fmt, ##args); } while (0)
+
+//Callback Object Definitions
+UC_REGISTRY_BINDER(AllocationCb, uavcan::protocol::dynamic_node_id::Allocation);
+UC_REGISTRY_BINDER(NodeStatusCb, uavcan::protocol::NodeStatus);
+
+static void trampoline_handleNodeInfo(const uavcan::ServiceCallResult<uavcan::protocol::GetNodeInfo>& resp);
+static void trampoline_handleAllocation(AP_UAVCAN* ap_uavcan, uint8_t node_id, const AllocationCb &cb);
+static void trampoline_handleNodeStatus(AP_UAVCAN* ap_uavcan, uint8_t node_id, const NodeStatusCb &cb);
+
+static uavcan::ServiceClient<uavcan::protocol::GetNodeInfo>* getNodeInfo_client[MAX_NUMBER_OF_CAN_DRIVERS];
+
+static uavcan::Publisher<uavcan::protocol::dynamic_node_id::Allocation>* allocation_pub[MAX_NUMBER_OF_CAN_DRIVERS];
+
+/* Subscribe to all the messages we are going to handle for
+Server registry and Node allocation. */
+void AP_UAVCAN_Server::subscribe_msgs(AP_UAVCAN* ap_uavcan)
+{
+    if (ap_uavcan == nullptr) {
+        return;
+    }
+
+    auto* node = ap_uavcan->get_node();
+    //Register Allocation Message Handler
+    uavcan::Subscriber<uavcan::protocol::dynamic_node_id::Allocation, AllocationCb> *AllocationListener;
+    AllocationListener = new uavcan::Subscriber<uavcan::protocol::dynamic_node_id::Allocation, AllocationCb>(*node);
+    if (AllocationListener == nullptr) {
+        AP_HAL::panic("Allocation Subscriber allocation failed\n\r");
+        return;
+    }
+    const int alloc_listener_res = AllocationListener->start(AllocationCb(ap_uavcan, &trampoline_handleAllocation));
+    if (alloc_listener_res < 0) {
+        AP_HAL::panic("Allocation Subscriber start problem\n\r");
+        return;
+    }
+    //We allow anonymous transfers, as they are specifically for Node Allocation
+    AllocationListener->allowAnonymousTransfers();
+
+    //Register Node Status Listener
+    uavcan::Subscriber<uavcan::protocol::NodeStatus, NodeStatusCb> *NodeStatusListener;
+    NodeStatusListener = new uavcan::Subscriber<uavcan::protocol::NodeStatus, NodeStatusCb>(*node);
+    if (NodeStatusListener == nullptr) {
+        AP_HAL::panic("NodeStatus Subscriber allocation failed\n\r");
+        return;
+    }
+    const int nodestatus_listener_res = NodeStatusListener->start(NodeStatusCb(ap_uavcan, &trampoline_handleNodeStatus));
+    if (nodestatus_listener_res < 0) {
+        AP_HAL::panic("NodeStatus Subscriber start problem\n\r");
+        return;
+    }
+}
+
+/* Method to generate 6byte hash from the Unique ID.
+We return it packed inside the referenced NodeData structure */
+void AP_UAVCAN_Server::getHash(NodeData &node_data, const uint8_t unique_id[], uint8_t size) const
+{
+    uint64_t hash = FNV_1_OFFSET_BASIS_64;
+    hash_fnv_1a(size, unique_id, &hash);
+
+    // xor-folding per http://www.isthe.com/chongo/tech/comp/fnv/
+    hash = (hash>>56) ^ (hash&(((uint64_t)1<<56)-1));
+
+    // write it to ret
+    for (uint8_t i=0; i<6; i++) {
+        node_data.hwid_hash[i] = (hash >> (8*i)) & 0xff;
+    }
+}
+
+//Read Node Data from Storage Region
+bool AP_UAVCAN_Server::readNodeData(NodeData &data, uint8_t node_id)
+{
+    if (node_id > MAX_NODE_ID) {
+        return false;
+    }
+    if (!storage.read_block(&data, (node_id * sizeof(struct NodeData)) + NODEDATA_MAGIC_LEN, sizeof(struct NodeData))) {
+        //This will fall through to Prearm Check
+        server_state = STORAGE_FAILURE;
+        return false;
+    }
+    return true;
+}
+
+//Write Node Data to Storage Region
+bool AP_UAVCAN_Server::writeNodeData(const NodeData &data, uint8_t node_id)
+{
+    if (node_id > MAX_NODE_ID) {
+        return false;
+    }
+    if (!storage.write_block((node_id * sizeof(struct NodeData)) + NODEDATA_MAGIC_LEN,
+                             &data, sizeof(struct NodeData))) {
+        server_state = STORAGE_FAILURE;
+        return false;
+    }
+    return true;
+}
+
+/* Set Occupation Mask, handy for keeping track of all node ids that
+are allocated and all that are available. */
+bool AP_UAVCAN_Server::setOccupationMask(uint8_t node_id)
+{
+    if (node_id > MAX_NODE_ID) {
+        return false;
+    }
+    occupation_mask.set(node_id);
+    return true;
+}
+
+/* Remove Node Data from Server Record in Storage,
+and also clear Occupation Mask */
+bool AP_UAVCAN_Server::freeNodeID(uint8_t node_id)
+{
+    if (node_id > MAX_NODE_ID) {
+        return false;
+    }
+
+    struct NodeData node_data;
+
+    //Eliminate from Server Record
+    memset(&node_data, 0, sizeof(node_data));
+    writeNodeData(node_data, node_id);
+
+    //Clear Occupation Mask
+    occupation_mask.clear(node_id);
+
+    return true;
+}
+
+/* Sets the verification mask. This is to be called, once
+The Seen Node has been both registered and verified against the
+Server Records. */
+void AP_UAVCAN_Server::setVerificationMask(uint8_t node_id)
+{
+    if (node_id > MAX_NODE_ID) {
+        return;
+    }
+    verified_mask.set(node_id);
+}
+
+/* Checks if the NodeID is occupied, i.e. its recorded
+in the Server Records against a unique ID */
+bool AP_UAVCAN_Server::isNodeIDOccupied(uint8_t node_id) const
+{
+    if (node_id > MAX_NODE_ID) {
+        return false;
+    }
+    return occupation_mask.get(node_id);
+}
+
+/* Checks if NodeID is verified, i.e. the unique id in
+Storage Records matches the one provided by Device with this node id. */
+bool AP_UAVCAN_Server::isNodeIDVerified(uint8_t node_id) const
+{
+    if (node_id > MAX_NODE_ID) {
+        return false;
+    }
+    return verified_mask.get(node_id);
+}
+
+/* Go through Server Records, and fetch node id that matches the provided
+Unique IDs hash.
+Returns 255 if no Node ID was detected */
+uint8_t AP_UAVCAN_Server::getNodeIDForUniqueID(const uint8_t unique_id[], uint8_t size)
+{
+    uint8_t node_id = 255;
+    NodeData node_data, cmp_node_data;
+    getHash(cmp_node_data, unique_id, size);
+
+    for (int i = MAX_NODE_ID; i >= 0; i--) {
+        if (!isNodeIDOccupied(i)) { // No point in checking NodeID that's not taken
+            continue;
+        }
+        if (!readNodeData(node_data, i)) {
+            break;  //Storage module has failed, report that as no NodeID detected
+        }
+        if (memcmp(node_data.hwid_hash, cmp_node_data.hwid_hash, sizeof(NodeData::hwid_hash)) == 0) {
+            node_id = i;
+            break;
+        }
+    }
+    return node_id;
+}
+
+/* Hash the Unique ID and add it to the Server Record
+for specified Node ID. */
+bool AP_UAVCAN_Server::addNodeIDForUniqueID(uint8_t node_id, const uint8_t unique_id[], uint8_t size)
+{
+    NodeData node_data;
+    getHash(node_data, unique_id, size);
+    //Generate CRC for validating the data when read back
+    node_data.crc = crc_crc8(node_data.hwid_hash, sizeof(node_data.hwid_hash));
+
+    //Write Data to the records
+    if (!writeNodeData(node_data, node_id)) {
+        server_state = FAILED_TO_ADD_NODE;
+        fault_node_id = node_id;
+        return false;
+    }
+
+    setOccupationMask(node_id);
+    return true;
+}
+
+//Checks if a valid Server Record is present for specified Node ID
+bool AP_UAVCAN_Server::isValidNodeDataAvailable(uint8_t node_id)
+{
+    NodeData node_data;
+    readNodeData(node_data, node_id);
+    uint8_t crc = crc_crc8(node_data.hwid_hash, sizeof(node_data.hwid_hash));
+    if (crc == node_data.crc && node_data.crc != 0) {
+        return true;
+    }
+    return false;
+}
+
+/* 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
+Server Record. */
+bool AP_UAVCAN_Server::init(AP_UAVCAN *ap_uavcan)
+{
+    if (ap_uavcan == nullptr) {
+        return false;
+    }
+
+    //Read the details from ap_uavcan
+    uavcan::Node<0>* _node = ap_uavcan->get_node();
+    uint8_t node_id = _node->getNodeID().get();
+    uint8_t driver_index = ap_uavcan->get_driver_index();
+    uint8_t own_unique_id[16] = {0};
+    uint8_t own_unique_id_len = 16;
+
+    //copy unique id from node to uint8_t array
+    uavcan::copy(_node->getHardwareVersion().unique_id.begin(),
+                 _node->getHardwareVersion().unique_id.end(),
+                 own_unique_id);
+
+    server_state = HEALTHY;
+
+    //Setup publisher for this driver index
+    allocation_pub[driver_index] = new uavcan::Publisher<uavcan::protocol::dynamic_node_id::Allocation>(*_node);
+    if (allocation_pub[driver_index] == nullptr) {
+        return false;
+    }
+
+    int res = allocation_pub[driver_index]->init(uavcan::TransferPriority::Default);
+    if (res < 0) {
+        return false;
+    }
+    allocation_pub[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(uavcan::protocol::dynamic_node_id::Allocation::FOLLOWUP_TIMEOUT_MS));
+
+    //Setup GetNodeInfo Client
+    getNodeInfo_client[driver_index] = new uavcan::ServiceClient<uavcan::protocol::GetNodeInfo>(*_node);
+    if (getNodeInfo_client[driver_index] == nullptr) {
+        return false;
+    }
+
+    res = getNodeInfo_client[driver_index]->init();
+    if (res < 0) {
+        return false;
+    }
+
+    getNodeInfo_client[driver_index]->setCallback(trampoline_handleNodeInfo);
+
+    /* Go through our records and look for valid NodeData, to initialise
+    occupation mask */
+    for (uint8_t i = 0; i <= MAX_NODE_ID; i++) {
+        if (isValidNodeDataAvailable(i)) {
+            occupation_mask.set(i);
+        }
+    }
+
+    // Check if the magic is present
+    uint16_t magic;
+    storage.read_block(&magic, 0, NODEDATA_MAGIC_LEN);
+    if (magic != NODEDATA_MAGIC) {
+        //Its not there a reset should write it in the Storage
+        reset();
+    }
+    // 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);
+    static bool reset_done;
+    if (stored_own_node_id != 255) {
+        if (stored_own_node_id != node_id) {
+            /* 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
+            if (!addNodeIDForUniqueID(node_id, own_unique_id, own_unique_id_len)) {
+                return false;
+            }
+        }
+    } else {
+        //We have no record of our own Unique ID do 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
+        if (!addNodeIDForUniqueID(node_id, own_unique_id, own_unique_id_len)) {
+            return false;
+        }
+    }
+    /* Also add to seen node id this is to verify
+    if any duplicates are on the bus carrying our Node ID */
+    addToSeenNodeMask(node_id);
+    setVerificationMask(node_id);
+    self_node_id[driver_index] = node_id;
+    return true;
+}
+
+
+//Reset the Server Records
+void AP_UAVCAN_Server::reset()
+{
+    NodeData node_data;
+    memset(&node_data, 0, sizeof(node_data));
+    occupation_mask.clearall();
+
+    //Just write empty Node Data to the Records
+    for (uint8_t i = 0; i <= MAX_NODE_ID; i++) {
+        writeNodeData(node_data, i);
+    }
+    //Ensure we mark magic at the end
+    uint16_t magic = NODEDATA_MAGIC;
+    storage.write_block(0, &magic, NODEDATA_MAGIC_LEN);
+}
+
+/* Go through the Occupation mask for available Node ID
+based on pseudo code provided in
+uavcan/protocol/dynamic_node_id/1.Allocation.uavcan */
+uint8_t AP_UAVCAN_Server::findFreeNodeID(uint8_t preferred)
+{
+    // Search up
+    uint8_t candidate = (preferred > 0) ? preferred : 125;
+    while (candidate <= 125) {
+        if (!isNodeIDOccupied(candidate)) {
+            return candidate;
+        }
+        candidate++;
+    }
+    //Search down
+    candidate = (preferred > 0) ? preferred : 125;
+    while (candidate > 0) {
+        if (!isNodeIDOccupied(candidate)) {
+            return candidate;
+        }
+        candidate--;
+    }
+    // Not found
+    return 255;
+}
+
+//Check if we have received Node Status from this node_id
+bool AP_UAVCAN_Server::isNodeSeen(uint8_t node_id)
+{
+    if (node_id > MAX_NODE_ID) {
+        return false;
+    }
+    return node_seen_mask.get(node_id);
+}
+
+/* Set the Seen Node Mask, to be called when received
+Node Status from the node id */
+void AP_UAVCAN_Server::addToSeenNodeMask(uint8_t node_id)
+{
+    if (node_id > MAX_NODE_ID) {
+        return;
+    }
+    node_seen_mask.set(node_id);
+}
+
+/* Run through the list of seen node ids for verification no more
+than once per 5 second. We continually verify the nodes in our 
+seen list, So that we can raise issue if there are duplicates
+on the bus. */
+void AP_UAVCAN_Server::verify_nodes(AP_UAVCAN *ap_uavcan)
+{
+    uint32_t now = AP_HAL::millis();
+    if ((now - last_verification_request) < 5000) {
+        return;
+    }
+
+    //Check if we got acknowledgement from previous request
+    //except for requests using our own node_id
+    for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) {
+        if (curr_verifying_node == self_node_id[i]) {
+            nodeInfo_resp_rcvd = true;
+        }
+    }
+    if (!nodeInfo_resp_rcvd) {
+        /* Also notify GCS about this
+        Reason for this could be either the node was disconnected
+        Or a node with conflicting ID appeared and is sending response
+        at the same time. */
+        /* Only report if the node was verified, otherwise ignore
+        as this could be just Bootloader to Application transition. */
+        if (isNodeIDVerified(curr_verifying_node)) {
+            gcs().send_text(MAV_SEVERITY_ERROR, "UC Node %d Down!", curr_verifying_node);
+            // remove verification flag for this node
+            verified_mask.clear(curr_verifying_node);
+        }
+    }
+
+    last_verification_request = now;
+    //Find the next registered Node ID to be verified.
+    for (uint8_t i = 0; i <= MAX_NODE_ID; i++) {
+        curr_verifying_node = (curr_verifying_node + 1) % (MAX_NODE_ID + 1);
+        if (isNodeSeen(curr_verifying_node)) {
+            break;
+        }
+    }
+    for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) {
+        if (getNodeInfo_client[i] != nullptr && isNodeIDOccupied(curr_verifying_node)) {
+            uavcan::protocol::GetNodeInfo::Request request;
+            getNodeInfo_client[i]->call(curr_verifying_node, request);
+            nodeInfo_resp_rcvd = false;
+        }
+    }
+}
+
+/* Handles Node Status Message, adds to the Seen Node list
+Also starts the Service call for Node Info to complete the
+Verification process. */
+void AP_UAVCAN_Server::handleNodeStatus(uint8_t node_id, const NodeStatusCb &cb)
+{
+    if (node_id > MAX_NODE_ID) {
+        return;
+    }
+    if (!isNodeIDVerified(node_id)) {
+        //immediately begin verification of the node_id
+        for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) {
+            if (getNodeInfo_client[i] != nullptr) {
+                uavcan::protocol::GetNodeInfo::Request request;
+                getNodeInfo_client[i]->call(node_id, request);
+            }
+        }
+    }
+    //Add node to seen list if not seen before
+    addToSeenNodeMask(node_id);
+}
+
+//Trampoline call for handleNodeStatus member method
+void trampoline_handleNodeStatus(AP_UAVCAN* ap_uavcan, uint8_t node_id, const NodeStatusCb &cb)
+{
+    if (ap_uavcan == nullptr) {
+        return;
+    }
+
+    AP::uavcan_server().handleNodeStatus(node_id, cb);
+}
+
+
+/* Node Info message handler
+Handle responses from GetNodeInfo Request. We verify the node info
+against our records. Marks Verification mask if already recorded,
+Or register if the node id is available and not recorded for the
+received Unique ID */
+void AP_UAVCAN_Server::handleNodeInfo(uint8_t node_id, uint8_t unique_id[], char name[])
+{
+    if (node_id > MAX_NODE_ID) {
+        return;
+    }
+    if (isNodeIDOccupied(node_id)) {
+        //if node_id already registered, just verify if Unique ID matches as well
+        if (node_id == getNodeIDForUniqueID(unique_id, 16)) {
+            if (node_id == curr_verifying_node) {
+                nodeInfo_resp_rcvd = true;
+            }
+            setVerificationMask(node_id);
+        } else {
+            /* This is a device with node_id already registered
+            for another device */
+            server_state = DUPLICATE_NODES;
+            fault_node_id = node_id;
+            memcpy(fault_node_name, name, sizeof(fault_node_name));
+        }
+    } else {
+        /* Node Id was not allocated by us, or during this boot, let's register this in our records
+        Check if we allocated this Node before */
+        uint8_t prev_node_id = getNodeIDForUniqueID(unique_id, 16);
+        if (prev_node_id != 255) {
+            //yes we did, remove this registration
+            freeNodeID(prev_node_id);
+        }
+        //add a new server record
+        addNodeIDForUniqueID(node_id, unique_id, 16);
+        //Verify as well
+        setVerificationMask(node_id);
+        if (node_id == curr_verifying_node) {
+            nodeInfo_resp_rcvd = true;
+        }
+    }
+}
+
+//Trampoline call for handleNodeInfo member call
+void trampoline_handleNodeInfo(const uavcan::ServiceCallResult<uavcan::protocol::GetNodeInfo>& resp)
+{
+    uint8_t node_id, unique_id[16] = {0};
+    char name[15] = {0};
+
+    node_id = resp.getResponse().getSrcNodeID().get();
+
+    //copy the unique id from message to uint8_t array
+    uavcan::copy(resp.getResponse().hardware_version.unique_id.begin(),
+                 resp.getResponse().hardware_version.unique_id.end(),
+                 unique_id);
+    snprintf(name, ARRAY_SIZE(name), "%s", resp.getResponse().name.c_str());
+    AP::uavcan_server().handleNodeInfo(node_id, unique_id, name);
+}
+
+/* Handle the allocation message from the devices supporting
+dynamic node allocation. */
+void AP_UAVCAN_Server::handleAllocation(uint8_t driver_index, uint8_t node_id, const AllocationCb &cb)
+{
+    if (allocation_pub[driver_index] == nullptr) {
+        //init has not been called for this driver.
+        return;
+    }
+    if (!cb.msg->isAnonymousTransfer()) {
+        //Ignore Allocation messages that are not DNA requests
+        return;
+    }
+    uint32_t now = AP_HAL::millis();
+    if (driver_index == current_driver_index) {
+        last_activity_ms = now;
+    } else if ((now - last_activity_ms) > 500) {
+        /* prepare for requests on another driver if we didn't had any activity on
+        current driver for more than 500ms */
+        current_driver_index = driver_index;
+        last_activity_ms = now;
+        rcvd_unique_id_offset = 0;
+        memset(rcvd_unique_id, 0, sizeof(rcvd_unique_id));
+    } else {
+        /* we ignore the requests from other drivers,
+        while busy handling the current one */
+        return;
+    }
+
+    if (cb.msg->first_part_of_unique_id) {
+        rcvd_unique_id_offset = 0;
+        memset(rcvd_unique_id, 0, sizeof(rcvd_unique_id));
+    } else if (rcvd_unique_id_offset == 0) {
+        //we are only accepting first part
+        return;
+    }
+
+    if ((rcvd_unique_id_offset + cb.msg->unique_id.size()) > 16) {
+        //This request is malformed, Reset!
+        rcvd_unique_id_offset = 0;
+        memset(rcvd_unique_id, 0, sizeof(rcvd_unique_id));
+        return;
+    }
+
+    //copy over the unique_id
+    for (uint8_t i=rcvd_unique_id_offset; i<(rcvd_unique_id_offset + cb.msg->unique_id.size()); i++) {
+        rcvd_unique_id[i] = cb.msg->unique_id[i - rcvd_unique_id_offset];
+    }
+    rcvd_unique_id_offset += cb.msg->unique_id.size();
+
+    //send follow up message
+    uavcan::protocol::dynamic_node_id::Allocation msg;
+
+    if (rcvd_unique_id_offset == 16) {
+        //We have received the full Unique ID, time to do allocation
+        uint8_t resp_node_id = getNodeIDForUniqueID((const uint8_t*)rcvd_unique_id, 16);
+        if (resp_node_id == 255) {
+            resp_node_id = findFreeNodeID(cb.msg->node_id);
+            if (resp_node_id != 255) {
+                if (addNodeIDForUniqueID(resp_node_id, (const uint8_t*)rcvd_unique_id, 16)) {
+                    msg.node_id = resp_node_id;
+                }
+            } else {
+                gcs().send_text(MAV_SEVERITY_ERROR, "UC Node Alloc Failed!");
+            }
+        } else {
+            msg.node_id = resp_node_id;
+        }
+    }
+
+    /* Respond with the message containing the received unique ID so far
+    or with node id if we successfully allocated one. */
+    for (uint8_t i = 0; i < rcvd_unique_id_offset; i++) {
+        msg.unique_id.push_back(rcvd_unique_id[i]);
+    }
+    allocation_pub[driver_index]->broadcast(msg);
+}
+
+//Trampoline Call for handleAllocation member call
+void trampoline_handleAllocation(AP_UAVCAN* ap_uavcan, uint8_t node_id, const AllocationCb &cb)
+{
+    if (ap_uavcan == nullptr) {
+        return;
+    }
+    AP::uavcan_server().handleAllocation(ap_uavcan->get_driver_index(), node_id, cb);
+}
+
+//report the server state, along with failure message if any
+bool AP_UAVCAN_Server::prearm_check(char* fail_msg, uint8_t fail_msg_len) const
+{
+    if (server_state == HEALTHY) {
+        return true;
+    }
+    switch (server_state) {
+    case STORAGE_FAILURE: {
+        snprintf(fail_msg, fail_msg_len, "UC: Failed to access storage!");
+        return false;
+    }
+    case DUPLICATE_NODES: {
+        snprintf(fail_msg, fail_msg_len, "UC: Duplicate Node %s../%d!", fault_node_name, fault_node_id);
+        return false;
+    }
+    case FAILED_TO_ADD_NODE: {
+        snprintf(fail_msg, fail_msg_len, "UC: Failed to add Node %d!", fault_node_id);
+        return false;
+    }
+    default:
+        break;
+    }
+    return false;
+}
+
+namespace AP
+{
+AP_UAVCAN_Server& uavcan_server()
+{
+    static AP_UAVCAN_Server _server(StorageAccess(StorageManager::StorageCANDNA));
+    return _server;
+}
+}
+#endif //HAL_WITH_UAVCAN

libraries/AP_UAVCAN/AP_UAVCAN_Server.h

@@ -0,0 +1,124 @@
+#pragma once
+#include <AP_HAL/AP_HAL.h>
+
+#if HAL_WITH_UAVCAN
+#include <uavcan/uavcan.hpp>
+#include <AP_Common/Bitmask.h>
+#include <StorageManager/StorageManager.h>
+
+//Forward declaring classes
+class AllocationCb;
+class NodeStatusCb;
+class NodeInfoCb;
+class AP_UAVCAN;
+
+class AP_UAVCAN_Server
+{
+    StorageAccess storage;
+
+    struct NodeData {
+        uint8_t hwid_hash[6];
+        uint8_t crc;
+    };
+
+    enum ServerState {
+        STORAGE_FAILURE = -3,
+        DUPLICATE_NODES = -2,
+        FAILED_TO_ADD_NODE = -1,
+        HEALTHY = 0
+    };
+
+    uint32_t last_verification_request;
+    uint8_t curr_verifying_node;
+    uint8_t self_node_id[MAX_NUMBER_OF_CAN_DRIVERS];
+    bool nodeInfo_resp_rcvd;
+
+    Bitmask<128> occupation_mask;
+    Bitmask<128> verified_mask;
+    Bitmask<128> node_seen_mask;
+
+    //Error State
+    enum ServerState server_state;
+    uint8_t fault_node_id;
+    char fault_node_name[15];
+
+
+    //Allocation params
+    uint8_t rcvd_unique_id[16];
+    uint8_t rcvd_unique_id_offset;
+    uint8_t current_driver_index;
+    uint32_t last_activity_ms;
+
+    //Methods to handle and report Node IDs seen on the bus
+    void addToSeenNodeMask(uint8_t node_id);
+    bool isNodeSeen(uint8_t node_id);
+
+    //Generates 6Byte long hash from the specified unique_id
+    void getHash(NodeData &node_data, const uint8_t unique_id[], uint8_t size) const;
+
+    //Reads the Server Record from storage for specified node id
+    bool readNodeData(NodeData &data, uint8_t node_id);
+
+    //Writes the Server Record from storage for specified node id
+    bool writeNodeData(const NodeData &data, uint8_t node_id);
+
+    //Methods to set, clear and report NodeIDs allocated/registered so far
+    bool setOccupationMask(uint8_t node_id);
+    bool isNodeIDOccupied(uint8_t node_id) const;
+    bool freeNodeID(uint8_t node_id);
+
+    //Set the mask to report that the unique id matches the record
+    void setVerificationMask(uint8_t node_id);
+
+    //Go through List to find node id for specified unique id
+    uint8_t getNodeIDForUniqueID(const uint8_t unique_id[], uint8_t size);
+
+    //Add Node ID info to the record and setup necessary mask fields
+    bool addNodeIDForUniqueID(uint8_t node_id, const uint8_t unique_id[], uint8_t size);
+
+    //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);
+
+public:
+    AP_UAVCAN_Server(StorageAccess _storage) : storage(_storage) {}
+
+    // Do not allow copies
+    AP_UAVCAN_Server(const AP_UAVCAN_Server &other) = delete;
+    AP_UAVCAN_Server &operator=(const AP_UAVCAN_Server&) = delete;
+
+    //Initialises publisher and Server Record for specified uavcan driver
+    bool init(AP_UAVCAN *ap_uavcan);
+
+    //Reset the Server Record
+    void reset();
+
+    /* Checks if the node id has been verified against the record
+    Specific CAN drivers are expected to check use this method to 
+    verify if the node is healthy and has static node_id against 
+    hwid in the records */
+    bool isNodeIDVerified(uint8_t node_id) const;
+
+    /* Subscribe to the messages to be handled for maintaining and allocating
+    Node ID list */
+    static void subscribe_msgs(AP_UAVCAN* ap_uavcan);
+
+    //report the server state, along with failure message if any
+    bool prearm_check(char* fail_msg, uint8_t fail_msg_len) const;
+
+    //Callbacks
+    void handleAllocation(uint8_t driver_index, uint8_t node_id, const AllocationCb &cb);
+    void handleNodeStatus(uint8_t node_id, const NodeStatusCb &cb);
+    void handleNodeInfo(uint8_t node_id, uint8_t unique_id[], char name[]);
+
+    //Run through the list of seen node ids for verification
+    void verify_nodes(AP_UAVCAN *ap_uavcan);
+};
+
+namespace AP
+{
+AP_UAVCAN_Server& uavcan_server();
+}
+#endif

libraries/AP_UAVCAN/AP_UAVCAN_Servers.cpp

@@ -1,315 +0,0 @@
-/*
- * 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
- * Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This file is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- * See the GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program.  If not, see <http://www.gnu.org/licenses/>.
- *
- * Author: Siddharth Bharat Purohit
- */
-
-#include <AP_HAL/AP_HAL.h>
-
-#if HAL_WITH_UAVCAN
-
-#include "AP_UAVCAN_Servers.h"
-
-#ifdef HAS_UAVCAN_SERVERS
-
-#include <uavcan/protocol/dynamic_node_id_server/event.hpp>
-#include <uavcan/protocol/dynamic_node_id_server/storage_backend.hpp>
-#include <uavcan/protocol/dynamic_node_id_server/centralized.hpp>
-#include <uavcan/protocol/HardwareVersion.hpp>
-
-#include <AP_BoardConfig/AP_BoardConfig.h>
-#include <AP_BoardConfig/AP_BoardConfig_CAN.h>
-
-#include <AP_Common/AP_Common.h>
-#include <AP_Logger/AP_Logger.h>
-#include <AP_Filesystem/AP_Filesystem.h>
-
-#ifndef UAVCAN_NODE_DB_PATH
-#define UAVCAN_NODE_DB_PATH HAL_BOARD_STORAGE_DIRECTORY "/UAVCAN"
-#endif
-
-#include <AP_HAL/AP_HAL.h>
-
-extern const AP_HAL::HAL& hal;
-
-#define debug_uavcan(fmt, args...) do { hal.console->printf(fmt, ##args); } while (0)
-
-class AP_UAVCAN_CentralizedServer : public uavcan::dynamic_node_id_server::CentralizedServer
-{
-public:
-    AP_UAVCAN_CentralizedServer(uavcan::INode& node, uavcan::dynamic_node_id_server::IStorageBackend &storage_backend, uavcan::dynamic_node_id_server::IEventTracer &tracer) :
-        uavcan::dynamic_node_id_server::CentralizedServer(node, storage_backend, tracer) {}
-};
-
-class AP_UAVCAN_FileEventTracer : public uavcan::dynamic_node_id_server::IEventTracer
-{
-protected:
-    virtual void onEvent(uavcan::dynamic_node_id_server::TraceCode code, uavcan::int64_t argument) override
-    {
-        AP::logger().Write("UCEV", "TimeUS,code,arg", "s--", "F--", "Qhq", AP_HAL::micros64(), code, argument);
-    }
-};
-
-
-class AP_UAVCAN_RestartRequestHandler : public uavcan::IRestartRequestHandler {
-public:
-    bool handleRestartRequest(uavcan::NodeID request_source) override {
-        // swiped from reboot handling in GCS_Common.cpp
-        if (hal.util->get_soft_armed()) {
-            // refuse reboot when armed
-            return false;
-        }
-        AP_Notify *notify = AP_Notify::get_singleton();
-        if (notify) {
-            AP_Notify::flags.firmware_update = 1;
-            notify->update();
-        }
-        // force safety on
-        hal.rcout->force_safety_on();
-
-        // flush pending parameter writes
-        AP_Param::flush();
-
-        hal.scheduler->delay(200);
-        hal.scheduler->reboot(false);
-        return true;
-    }
-};
-
-class AP_UAVCAN_FileStorageBackend : public uavcan::dynamic_node_id_server::IStorageBackend
-{
-    /**
-     * Maximum length of full path including / and key max
-     */
-    enum { MaxPathLength = 128 };
-
-    enum { MaxNumOpens = 100 };
-    /**
-     * This type is used for the path
-     */
-    typedef uavcan::MakeString<MaxPathLength>::Type PathString;
-
-    PathString base_path;
-
-    static uint8_t num_opens;
-protected:
-    virtual String get(const String& key) const override
-    {
-        using namespace std;
-        PathString path = base_path.c_str();
-        path += key;
-        String value;
-        //This is to deter frequent inflight opening and closing of files during an event
-        //where the device is misbehaving
-        if (num_opens >= MaxNumOpens) {
-            return value;
-        }
-        num_opens++;
-        int fd = AP::FS().open(path.c_str(), O_RDONLY);
-        if (fd >= 0)
-        {
-            char buffer[MaxStringLength + 1];
-            (void)memset(buffer, 0, sizeof(buffer));
-            ssize_t remaining = MaxStringLength;
-            ssize_t total_read = 0;
-            ssize_t nread = 0;
-            do
-            {
-                nread = AP::FS().read(fd, &buffer[total_read], remaining);
-                if (nread > 0)
-                {
-                    remaining -= nread,
-                    total_read += nread;
-                }
-            }
-            while (nread > 0 && remaining > 0);
-            AP::FS().close(fd);
-            if (total_read > 0)
-            {
-                for (int i = 0; i < total_read; i++)
-                {
-                    if (buffer[i] == ' ' || buffer[i] == '\n' || buffer[i] == '\r')
-                    {
-                        buffer[i] = '\0';
-                        break;
-                    }
-                }
-                value = buffer;
-            }
-        }
-        return value;
-    }
-
-    virtual void set(const String& key, const String& value) override
-    {
-        using namespace std;
-        PathString path = base_path.c_str();
-        path += key;
-        //This is to deter frequent inflight opening and closing of files during an event
-        //where the device is misbehaving
-        if (num_opens >= MaxNumOpens) {
-            return;
-        }
-        num_opens++;
-        int fd = AP::FS().open(path.c_str(), O_WRONLY | O_CREAT | O_TRUNC);
-        if (fd >= 0)
-        {
-            ssize_t remaining = value.size();
-            ssize_t total_written = 0;
-            ssize_t written = 0;
-            do
-            {
-                written = AP::FS().write(fd, &value.c_str()[total_written], remaining);
-                if (written > 0)
-                {
-                    total_written += written;
-                    remaining -=  written;
-                }
-            }
-            while (written > 0 && remaining > 0);
-
-            AP::FS().fsync(fd);
-            AP::FS().close(fd);
-        }
-    }
-
-public:
-    /**
-     * Initializes the file based backend storage by passing a path to
-     * the directory where the key named files will be stored.
-     * The return value should be 0 on success.
-     * If it is -ErrInvalidConfiguration then the the path name is too long to
-     * accommodate the trailing slash and max key length.
-     */
-    int init(const PathString& path)
-    {
-        using namespace std;
-
-        int rv = -uavcan::ErrInvalidParam;
-
-        if (path.size() > 0)
-        {
-            base_path = path.c_str();
-
-            if (base_path.back() == '/')
-            {
-                base_path.pop_back();
-            }
-
-            rv = 0;
-            struct stat sb;
-            if (AP::FS().stat(base_path.c_str(), &sb) != 0 || !S_ISDIR(sb.st_mode))
-            {
-                rv = AP::FS().mkdir(base_path.c_str());
-            }
-            if (rv >= 0)
-            {
-                base_path.push_back('/');
-                if ((base_path.size() + MaxStringLength) > MaxPathLength)
-                {
-                    rv = -uavcan::ErrInvalidConfiguration;
-                }
-            }
-        }
-        return rv;
-    }
-
-};
-uint8_t AP_UAVCAN_FileStorageBackend::num_opens = 0;
-
-bool AP_UAVCAN_Servers::init(uavcan::Node<0> &node)
-{
-    if (_server_instance != nullptr) {
-        return true;
-    }
-
-    int ret = 0;
-
-    _storage_backend = new AP_UAVCAN_FileStorageBackend;
-    if (_storage_backend == nullptr) {
-        debug_uavcan("UAVCAN_Servers: Failed to Allocate FileStorageBackend\n"); 
-        goto failed;
-    }
-
-    ret = _storage_backend->init(UAVCAN_NODE_DB_PATH);
-    if (ret < 0) {
-        debug_uavcan("UAVCAN_Servers: FileStorageBackend init: %d, errno: %d\n", ret, errno);
-        goto failed;
-    }
-
-    _tracer = new AP_UAVCAN_FileEventTracer;
-    if (_tracer == nullptr) {
-        debug_uavcan("UAVCAN_Servers: Failed to Allocate FileEventTracer\n"); 
-        goto failed;
-    }
-
-    _server_instance = new AP_UAVCAN_CentralizedServer(node, *_storage_backend, *_tracer);
-    if (_server_instance == nullptr) {
-        debug_uavcan("UAVCAN_Servers: Failed to Allocate Server\n"); 
-        goto failed;
-    }
-
-    {
-        uavcan::dynamic_node_id_server::centralized::Storage storage(*_storage_backend);
-        if (storage.getNodeIDForUniqueID(node.getHardwareVersion().unique_id) != node.getNodeID()) {
-            //Node ID was changed, reseting database
-            reset();
-        }
-    }
-
-    if (_restart_request_handler == nullptr) {
-        _restart_request_handler = new AP_UAVCAN_RestartRequestHandler();
-        if (_restart_request_handler == nullptr) {
-            goto failed;
-        }
-    }
-    node.setRestartRequestHandler(_restart_request_handler);
-
-    //Start Dynamic Node Server
-    ret = _server_instance->init(node.getHardwareVersion().unique_id);
-    if (ret < 0) {
-        debug_uavcan("UAVCAN_Server init: %d, errno: %d\n", ret, errno); 
-        goto failed;
-    }
-
-    return true;
-
-failed:
-    delete _restart_request_handler;
-    delete _storage_backend;
-    delete _tracer;
-    delete _server_instance;
-    return false;
-}
-
-void AP_UAVCAN_Servers::reset()
-{
-    debug_uavcan("UAVCAN_Servers: Resetting Server Database...\n");
-    DIR* dp;
-    struct dirent* ep;
-    dp = AP::FS().opendir(UAVCAN_NODE_DB_PATH);
-    char abs_filename[100];
-    if (dp != NULL)
-    {
-        while((ep = AP::FS().readdir(dp))) {
-            snprintf(abs_filename, 100, "%s/%s", UAVCAN_NODE_DB_PATH, ep->d_name);
-            AP::FS().unlink(abs_filename);
-        }
-    }
-    AP::FS().closedir(dp);
-}
-
-#endif
-
-#endif //HAL_WITH_UAVCAN

libraries/AP_UAVCAN/AP_UAVCAN_Servers.h

@@ -1,32 +0,0 @@
-#pragma once
-#include <AP_HAL/AP_HAL.h>
-#include <AP_Filesystem/AP_Filesystem.h>
-
-#if HAVE_FILESYSTEM_SUPPORT
-
-#define HAS_UAVCAN_SERVERS
-
-#include <uavcan/uavcan.hpp>
-
-//Forward declaring classes
-class AP_UAVCAN_FileEventTracer;
-class AP_UAVCAN_FileStorageBackend;
-class AP_UAVCAN_CentralizedServer;
-class AP_UAVCAN_RestartRequestHandler;
-
-class AP_UAVCAN_Servers
-{
-public:
-    bool init(uavcan::Node<0> &node);
-
-private:
-    void reset();
-
-    AP_UAVCAN_CentralizedServer *_server_instance;
-    AP_UAVCAN_FileEventTracer *_tracer;
-    AP_UAVCAN_FileStorageBackend *_storage_backend;
-    AP_UAVCAN_RestartRequestHandler *_restart_request_handler; // one for all nodes....
-
-};
-
-#endif