From 9bd379e236d3e09f18859edc12ff2c1025acc121 Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Sat, 21 Sep 2019 11:16:06 +0800 Subject: [PATCH] AP_UAVCAN: setup uavcan server with internal storage --- libraries/AP_UAVCAN/AP_UAVCAN.cpp | 16 +- libraries/AP_UAVCAN/AP_UAVCAN.h | 6 +- libraries/AP_UAVCAN/AP_UAVCAN_Server.cpp | 673 ++++++++++++++++++++++ libraries/AP_UAVCAN/AP_UAVCAN_Server.h | 124 ++++ libraries/AP_UAVCAN/AP_UAVCAN_Servers.cpp | 315 ---------- libraries/AP_UAVCAN/AP_UAVCAN_Servers.h | 32 - 6 files changed, 811 insertions(+), 355 deletions(-) create mode 100644 libraries/AP_UAVCAN/AP_UAVCAN_Server.cpp create mode 100644 libraries/AP_UAVCAN/AP_UAVCAN_Server.h delete mode 100644 libraries/AP_UAVCAN/AP_UAVCAN_Servers.cpp delete mode 100644 libraries/AP_UAVCAN/AP_UAVCAN_Servers.h diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp index 92d6114618..83cf2961dc 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp @@ -49,6 +49,7 @@ #include #include #include +#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 - _servers.init(*_node); -#endif + if (!AP::uavcan_server().init(this)) { + debug_uavcan(1, "UAVCAN: Failed to start DNA Server\n\r"); + 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); } } diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.h b/libraries/AP_UAVCAN/AP_UAVCAN.h index 7c7f4bcc49..8892c71ab2 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.h +++ b/libraries/AP_UAVCAN/AP_UAVCAN.h @@ -18,13 +18,13 @@ #define AP_UAVCAN_H_ #include +#include "AP_UAVCAN_Server.h" #include #include #include #include -#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; diff --git a/libraries/AP_UAVCAN/AP_UAVCAN_Server.cpp b/libraries/AP_UAVCAN/AP_UAVCAN_Server.cpp new file mode 100644 index 0000000000..3aed5a5c7e --- /dev/null +++ b/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 . + * + * Author: Siddharth Bharat Purohit + */ + +#include + +#if HAL_WITH_UAVCAN + +#include "AP_UAVCAN_Server.h" +#include "AP_UAVCAN.h" +#include +#include +#include +#include +#include + +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& 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* getNodeInfo_client[MAX_NUMBER_OF_CAN_DRIVERS]; + +static uavcan::Publisher* 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 *AllocationListener; + AllocationListener = new uavcan::Subscriber(*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 *NodeStatusListener; + NodeStatusListener = new uavcan::Subscriber(*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(*_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(*_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& 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 diff --git a/libraries/AP_UAVCAN/AP_UAVCAN_Server.h b/libraries/AP_UAVCAN/AP_UAVCAN_Server.h new file mode 100644 index 0000000000..29dddd2a69 --- /dev/null +++ b/libraries/AP_UAVCAN/AP_UAVCAN_Server.h @@ -0,0 +1,124 @@ +#pragma once +#include + +#if HAL_WITH_UAVCAN +#include +#include +#include + +//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 \ No newline at end of file diff --git a/libraries/AP_UAVCAN/AP_UAVCAN_Servers.cpp b/libraries/AP_UAVCAN/AP_UAVCAN_Servers.cpp deleted file mode 100644 index 7411f44cf3..0000000000 --- a/libraries/AP_UAVCAN/AP_UAVCAN_Servers.cpp +++ /dev/null @@ -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 . - * - * Author: Siddharth Bharat Purohit - */ - -#include - -#if HAL_WITH_UAVCAN - -#include "AP_UAVCAN_Servers.h" - -#ifdef HAS_UAVCAN_SERVERS - -#include -#include -#include -#include - -#include -#include - -#include -#include -#include - -#ifndef UAVCAN_NODE_DB_PATH -#define UAVCAN_NODE_DB_PATH HAL_BOARD_STORAGE_DIRECTORY "/UAVCAN" -#endif - -#include - -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::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 diff --git a/libraries/AP_UAVCAN/AP_UAVCAN_Servers.h b/libraries/AP_UAVCAN/AP_UAVCAN_Servers.h deleted file mode 100644 index d2d3e0cbff..0000000000 --- a/libraries/AP_UAVCAN/AP_UAVCAN_Servers.h +++ /dev/null @@ -1,32 +0,0 @@ -#pragma once -#include -#include - -#if HAVE_FILESYSTEM_SUPPORT - -#define HAS_UAVCAN_SERVERS - -#include - -//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