AP_UAVCAN: add support for dynamic node allocation

libraries/AP_UAVCAN/AP_UAVCAN.cpp

@@ -19,7 +19,6 @@
 #include <AP_HAL/AP_HAL.h>
 
 #if HAL_WITH_UAVCAN
-
 #include "AP_UAVCAN.h"
 #include <GCS_MAVLink/GCS.h>
 
@@ -180,6 +179,17 @@ void AP_UAVCAN::init(uint8_t driver_index)
 
     hw_version.major = AP_UAVCAN_HW_VERS_MAJOR;
     hw_version.minor = AP_UAVCAN_HW_VERS_MINOR;
+
+    uint8_t len = hw_version.unique_id.capacity();
+    uint8_t *unique_id = new uint8_t[len];
+
+    if (unique_id) {
+        hal.util->get_system_id_unformatted(unique_id, len);
+        uavcan::copy(unique_id, unique_id + len, hw_version.unique_id.begin());
+    } else {
+        AP_HAL::panic("UAVCAN: Failed to allocate Unique ID");
+    }
+
     _node->setHardwareVersion(hw_version);
 
     int start_res = _node->start();
@@ -188,6 +198,10 @@ void AP_UAVCAN::init(uint8_t driver_index)
         return;
     }
 
+    //Start Servers
+#ifdef HAS_UAVCAN_SERVERS
+    _servers.init(*_node);
+#endif
     // Roundup all subscribers from supported drivers
     AP_GPS_UAVCAN::subscribe_msgs(this);
     AP_Compass_UAVCAN::subscribe_msgs(this);

libraries/AP_UAVCAN/AP_UAVCAN.h

@@ -24,6 +24,7 @@
 #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
@@ -152,13 +153,14 @@ private:
     AP_Int32 _esc_bm;
     AP_Int16 _servo_rate_hz;
 
-
     uavcan::Node<0> *_node;
     uavcan::HeapBasedPoolAllocator<UAVCAN_NODE_POOL_BLOCK_SIZE, AP_UAVCAN::RaiiSynchronizer> _node_allocator;
     uint8_t _driver_index;
     char _thread_name[9];
     bool _initialized;
-
+#ifdef HAS_UAVCAN_SERVERS
+    AP_UAVCAN_Servers _servers;
+#endif
 
     ///// SRV output /////
     struct {

libraries/AP_UAVCAN/AP_UAVCAN_Servers.cpp

@@ -0,0 +1,304 @@
+/*
+ * 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 <DataFlash/DataFlash.h>
+
+#if HAL_OS_POSIX_IO
+#include <unistd.h>
+#include <errno.h>
+#include <stdlib.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <assert.h>
+#include <stdio.h>
+#include <time.h>
+#include <dirent.h>
+#if defined(__APPLE__) && defined(__MACH__)
+#include <sys/param.h>
+#include <sys/mount.h>
+#else
+#include <sys/statfs.h>
+#endif
+#endif
+
+#if HAL_OS_FATFS_IO
+#include <stdio.h>
+#endif
+
+#ifndef UAVCAN_NODE_DB_PATH
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+#define UAVCAN_NODE_DB_PATH "/fs/microsd/APM/UAVCAN"
+#else
+#define UAVCAN_NODE_DB_PATH "/APM/UAVCAN"
+#endif
+#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)
+    {
+        struct log_UAVCAN_EVT pkt = {
+            LOG_PACKET_HEADER_INIT(LOG_UAVCAN_EVT_MSG),
+            time_us    : AP_HAL::micros64(),
+            code       : code,
+            argument   : argument
+        };
+        DataFlash_Class::instance()->WriteBlock(&pkt, sizeof(pkt));
+    }
+};
+
+class AP_UAVCAN_FileStorageBackend : public uavcan::dynamic_node_id_server::IStorageBackend
+{
+    /**
+     * Maximum length of full path including / and key max
+     */
+    enum { MaxPathLength = 128 };
+
+    enum { FilePermissions = 438 };     ///< 0o666
+
+    /**
+     * This type is used for the path
+     */
+    typedef uavcan::MakeString<MaxPathLength>::Type PathString;
+
+    PathString base_path;
+
+protected:
+    virtual String get(const String& key) const
+    {
+        using namespace std;
+        PathString path = base_path.c_str();
+        path += key;
+        String value;
+        int fd = 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 = ::read(fd, &buffer[total_read], remaining);
+                if (nread > 0)
+                {
+                    remaining -= nread,
+                    total_read += nread;
+                }
+            }
+            while (nread > 0 && remaining > 0);
+            (void)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)
+    {
+        using namespace std;
+        PathString path = base_path.c_str();
+        path += key;
+#if HAL_OS_POSIX_IO
+        int fd = open(path.c_str(), O_WRONLY | O_CREAT | O_TRUNC, FilePermissions);
+#else
+        int fd = open(path.c_str(), O_WRONLY | O_CREAT | O_TRUNC);
+#endif
+        if (fd >= 0)
+        {
+            ssize_t remaining = value.size();
+            ssize_t total_written = 0;
+            ssize_t written = 0;
+            do
+            {
+                written = write(fd, &value.c_str()[total_written], remaining);
+                if (written > 0)
+                {
+                    total_written += written;
+                    remaining -=  written;
+                }
+            }
+            while (written > 0 && remaining > 0);
+
+            (void)fsync(fd);
+            (void)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 (stat(base_path.c_str(), &sb) != 0 || !S_ISDIR(sb.st_mode))
+            {
+                // coverity[toctou]
+                rv = mkdir(base_path.c_str(), S_IRWXU | S_IRWXG | S_IRWXO);
+            }
+            if (rv >= 0)
+            {
+                base_path.push_back('/');
+                if ((base_path.size() + MaxStringLength) > MaxPathLength)
+                {
+                    rv = -uavcan::ErrInvalidConfiguration;
+                }
+            }
+        }
+        return rv;
+    }
+
+};
+
+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();
+        }
+    }
+
+    //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 _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 = opendir(UAVCAN_NODE_DB_PATH);
+    char abs_filename[100];
+    if (dp != NULL)
+    {
+        while((ep = readdir(dp))) {
+            snprintf(abs_filename, 100, "%s/%s", UAVCAN_NODE_DB_PATH, ep->d_name);
+            unlink(abs_filename);
+        }
+    }
+    closedir(dp);
+}
+
+#endif
+
+#endif //HAL_WITH_UAVCAN

libraries/AP_UAVCAN/AP_UAVCAN_Servers.h

@@ -0,0 +1,28 @@
+#pragma once
+#include <AP_HAL/AP_HAL.h>
+
+#if (HAL_OS_POSIX_IO || HAL_OS_FATFS_IO)
+
+#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_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;
+};
+
+#endif