mirror of https://github.com/ArduPilot/ardupilot
AP_UAVCAN: setup uavcan server with internal storage
This commit is contained in:
Siddharth Purohit
Andrew Tridgell
parent
9d1c6ae46e
commit
9bd379e236
|
|
@ -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
|
||||
_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);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
Loading…
Reference in New Issue