diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp index ffeacf8030..2294b6e9c8 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp @@ -19,7 +19,6 @@ #include #if HAL_WITH_UAVCAN - #include "AP_UAVCAN.h" #include @@ -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); diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.h b/libraries/AP_UAVCAN/AP_UAVCAN.h index d5a6428284..a30323475c 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.h +++ b/libraries/AP_UAVCAN/AP_UAVCAN.h @@ -24,6 +24,7 @@ #include #include +#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 _node_allocator; uint8_t _driver_index; char _thread_name[9]; bool _initialized; - +#ifdef HAS_UAVCAN_SERVERS + AP_UAVCAN_Servers _servers; +#endif ///// SRV output ///// struct { diff --git a/libraries/AP_UAVCAN/AP_UAVCAN_Servers.cpp b/libraries/AP_UAVCAN/AP_UAVCAN_Servers.cpp new file mode 100644 index 0000000000..77d504c2ea --- /dev/null +++ b/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 . + * + * 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 + +#if HAL_OS_POSIX_IO +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#if defined(__APPLE__) && defined(__MACH__) +#include +#include +#else +#include +#endif +#endif + +#if HAL_OS_FATFS_IO +#include +#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 + +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::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 diff --git a/libraries/AP_UAVCAN/AP_UAVCAN_Servers.h b/libraries/AP_UAVCAN/AP_UAVCAN_Servers.h new file mode 100644 index 0000000000..63fe2d02fe --- /dev/null +++ b/libraries/AP_UAVCAN/AP_UAVCAN_Servers.h @@ -0,0 +1,28 @@ +#pragma once +#include + +#if (HAL_OS_POSIX_IO || HAL_OS_FATFS_IO) + +#define HAS_UAVCAN_SERVERS + +#include + +//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 \ No newline at end of file