From b86ee61e154ed6111935dfadebd9bec5dd32b75b Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Wed, 12 Jun 2024 16:14:09 +1000 Subject: [PATCH] AP_DroneCAN: add file server and client write request abd response methods --- libraries/AP_DroneCAN/AP_DroneCAN.cpp | 26 +++++- libraries/AP_DroneCAN/AP_DroneCAN.h | 7 ++ .../AP_DroneCAN/AP_DroneCAN_File_Client.cpp | 80 +++++++++++++++++-- .../AP_DroneCAN/AP_DroneCAN_File_Client.h | 40 +++++++--- .../AP_DroneCAN/AP_DroneCAN_File_Server.cpp | 79 +++++++++++++++++- .../AP_DroneCAN/AP_DroneCAN_File_Server.h | 29 ++++--- 6 files changed, 226 insertions(+), 35 deletions(-) diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.cpp b/libraries/AP_DroneCAN/AP_DroneCAN.cpp index 67c629b241616..302f610bf0020 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN.cpp +++ b/libraries/AP_DroneCAN/AP_DroneCAN.cpp @@ -270,6 +270,15 @@ const AP_Param::GroupInfo AP_DroneCAN::var_info[] = { #endif #endif // AP_DRONECAN_SERIAL_ENABLED + + // @Param: IO_SRV + // @DisplayName: File IO server node id + // @Description: Node id of the file IO server + // @Range: 0 250 + // @Values: 0:Disabled,1-250:Enabled + // @User: Advanced + AP_GROUPINFO("IO_SRV", 24, AP_DroneCAN, _io_server_node, 0), + // RLY_RT is index 23 but has to be above SER_EN so its not hidden AP_GROUPEND @@ -282,7 +291,9 @@ const AP_Param::GroupInfo AP_DroneCAN::var_info[] = { AP_DroneCAN::AP_DroneCAN(const int driver_index) : _driver_index(driver_index), canard_iface(driver_index), -_dna_server(*this, canard_iface, driver_index) +_dna_server(*this, canard_iface, driver_index), +_file_client(*this, canard_iface), +_file_server(*this, canard_iface) { AP_Param::setup_object_defaults(this, var_info); @@ -362,6 +373,19 @@ void AP_DroneCAN::init(uint8_t driver_index, bool enable_filters) return; } + if (_io_server_node != 0) { + if (!_file_client.init(_io_server_node)) { + debug_dronecan(AP_CANManager::LOG_ERROR, "DroneCAN: Failed to start File Client\n\r"); + return; + } + } + + + if (!_file_server.init()) { + debug_dronecan(AP_CANManager::LOG_ERROR, "DroneCAN: Failed to start File Server\n\r"); + return; + } + // Roundup all subscribers from supported drivers #if AP_GPS_DRONECAN_ENABLED AP_GPS_DroneCAN::subscribe_msgs(this); diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.h b/libraries/AP_DroneCAN/AP_DroneCAN.h index 8b37c2528b89b..b0652e6a02453 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN.h +++ b/libraries/AP_DroneCAN/AP_DroneCAN.h @@ -36,6 +36,8 @@ #include #include #include +#include "AP_DroneCAN_File_Client.h" +#include "AP_DroneCAN_File_Server.h" #ifndef DRONECAN_SRV_NUMBER #define DRONECAN_SRV_NUMBER NUM_SERVO_CHANNELS @@ -172,6 +174,8 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend { Canard::Publisher relay_hardpoint{canard_iface}; #endif + AP_DroneCAN_File_Client& get_file_client() { return _file_client; } + private: void loop(void); @@ -228,10 +232,13 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend { AP_Int16 _notify_state_hz; AP_Int16 _pool_size; AP_Int32 _esc_rv; + AP_Int8 _io_server_node; uint32_t *mem_pool; AP_DroneCAN_DNA_Server _dna_server; + AP_DroneCAN_File_Client _file_client; + AP_DroneCAN_File_Server _file_server; uint8_t _driver_index; diff --git a/libraries/AP_DroneCAN/AP_DroneCAN_File_Client.cpp b/libraries/AP_DroneCAN/AP_DroneCAN_File_Client.cpp index 8b8d323931612..5c519d4e13705 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN_File_Client.cpp +++ b/libraries/AP_DroneCAN/AP_DroneCAN_File_Client.cpp @@ -1,25 +1,93 @@ #include "AP_DroneCAN_File_Client.h" +#include + +bool AP_DroneCAN_File_Client::init(uint8_t server_node_id) +{ + _server_node_id = server_node_id; +} void AP_DroneCAN_File_Client::handle_write_response(const CanardRxTransfer& transfer, const uavcan_protocol_file_WriteResponse &msg){ // uavcan_protocol_file_WriteRequest req; return; } +int AP_DroneCAN_File_Client::open(const char *path, int mode) +{ + if (_server_node_id == 0) { + errno = ENODEV; + return -1; + } + WITH_SEMAPHORE(_sem); + + if (is_opened) { + errno = EBUSY; + return -1; + } + + // we only support readonly and writeonly modes + if (mode == O_RDONLY) { + is_reading = true; + } else if (mode == O_WRONLY) { + is_reading = false; + } else { + errno = EINVAL; + return -1; + } + _total_transaction = 0; + // make sure we don't have a hangover from last close call + file_request_cb = nullptr; + is_opened = true; + + return 0; +} + bool AP_DroneCAN_File_Client::write_async(uint8_t data[], uint16_t data_len, FileRequestCb *cb) { + if (_server_node_id == 0) { + return false; + } + WITH_SEMAPHORE(_sem); + uavcan_protocol_file_WriteRequest req; + + req.offset = _total_transaction; req.data.len = data_len; memcpy(req.data.data, data, sizeof(req.data.data)); file_request_cb = cb; + _write_client.request(_server_node_id, req); - _write_client.request(,req); - + // increment total transaction + _total_transaction += data_len; return true; } -bool AP_DroneCAN_File_Client::open_async(const char *name, uint16_t flags, FileRequestCb *cb) { - // uavcan_protocol_file_ReadRequest req; +void AP_DroneCAN_File_Client::close_async(FileRequestCb *cb) { + if (_server_node_id == 0) { + return; + } + WITH_SEMAPHORE(_sem); - file_request_cb = cb; + if (!is_reading) { + // mark the end of the file by sending an empty write request with the offset set to the total transaction + uavcan_protocol_file_WriteRequest req; + req.offset = _total_transaction; + req.data.len = 0; + file_request_cb = cb; + _write_client.request(_server_node_id, req); + _total_transaction = 0; + } +} + +void handle_write_response(const CanardRxTransfer& transfer, const uavcan_protocol_file_WriteResponse &msg) +{ + if (file_request_cb) { + file_request_cb(&_ap_dronecan, msg.error.value); + } +} + +bool AP_DroneCAN_File_Client::read_async(uint16_t offset, uint16_t len, FileRequestCb *cb) { + if (_server_node_id == 0) { + return false; + } + WITH_SEMAPHORE(_sem); - return true; } \ No newline at end of file diff --git a/libraries/AP_DroneCAN/AP_DroneCAN_File_Client.h b/libraries/AP_DroneCAN/AP_DroneCAN_File_Client.h index 021f5d59b656e..96a3d933f643b 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN_File_Client.h +++ b/libraries/AP_DroneCAN/AP_DroneCAN_File_Client.h @@ -1,8 +1,6 @@ #pragma once #include #include - - #include #include #include @@ -17,7 +15,29 @@ class AP_DroneCAN; //Forward declaring classes class AP_DroneCAN_File_Client { +public: + AP_DroneCAN_File_Client(AP_DroneCAN &ap_dronecan, CanardInterface &canard_iface); + + + // Do not allow copies + CLASS_NO_COPY(AP_DroneCAN_File_Client); + FUNCTOR_TYPEDEF(FileRequestCb, void, AP_DroneCAN*, const int16_t); + + bool init(uint8_t _server_node_id); + + bool open(const char *path, uint16_t mode); + + // sends the write request to the server + bool write_async(uint8_t data[], uint16_t data_len, FileRequestCb *cb); + + // sends the read request to the server + bool read_async(uint16_t offset, uint16_t len, FileRequestCb *cb); + + // close the file + bool close(); + +private: HAL_Semaphore storage_sem; AP_DroneCAN &_ap_dronecan; CanardInterface &_canard_iface; @@ -27,20 +47,14 @@ class AP_DroneCAN_File_Client Canard::ObjCallback file_wr_cb{this, &AP_DroneCAN_File_Client::handle_write_response}; Canard::Client _write_client{_canard_iface, file_wr_cb};; -public: - AP_DroneCAN_File_Client(AP_DroneCAN &ap_dronecan, CanardInterface &canard_iface, uint8_t file_server_node_id); - - // Do not allow copies - CLASS_NO_COPY(AP_DroneCAN_File_Client); + FileRequestCb *file_request_cb; - FUNCTOR_TYPEDEF(FileRequestCb, void, AP_DroneCAN*, const int16_t); + uint8_t _server_node_id; - // sends request to open a file on the server - bool open_async(const char *name, uint16_t flags, FileRequestCb *cb); + uint32_t _total_transaction; - // sends the write request to the server - bool write_async(uint8_t data[], uint16_t data_len, FileRequestCb *cb); + bool is_opened; - FileRequestCb *file_request_cb; + HAL_Semaphore _sem; }; \ No newline at end of file diff --git a/libraries/AP_DroneCAN/AP_DroneCAN_File_Server.cpp b/libraries/AP_DroneCAN/AP_DroneCAN_File_Server.cpp index ea788d0e87b7a..c667292197a2c 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN_File_Server.cpp +++ b/libraries/AP_DroneCAN/AP_DroneCAN_File_Server.cpp @@ -1,19 +1,86 @@ #include #include "AP_DroneCAN_File_Server.h" + +AP_DroneCAN_File_Server::FileClient* AP_DroneCAN_File_Server::get_free_client(const uavcan_protocol_file_Path &path, uint8_t client_node_id) +{ + // check if the file is already open + for (int i = 0; i < MAX_NUM_CLIENTS; i++) { + if (_clients[i].fd >= 0 && _clients[i].path.path.len == path.path.len && memcmp(_clients[i].path.path.data, path.path.data, path.path.len) == 0) { + return &_clients[i]; + } + } + + for (int i = 0; i < MAX_NUM_CLIENTS; i++) { + if (_clients[i].fd < 0) { + memcpy(&_clients[i].path, &path, sizeof(_clients[i].path)); + return &_clients[i]; + } + } + return nullptr; +} + void AP_DroneCAN_File_Server::handle_write_request(const CanardRxTransfer& transfer, const uavcan_protocol_file_WriteRequest& req){ uavcan_protocol_file_WriteResponse rsp; - int fd = AP::FS().open("@ROMFS/test.123", O_CREAT, 0); - if (fd<0) { + + // get a free client record slot + FileClient* client = get_free_client(req.path, transfer.source_node_id); + if (client == nullptr) { + // we don't have any free slots, respond with stream unavailable + rsp.error.value = ENOSR; + _write_server.respond(transfer, rsp); + return; + } + + if (client->open_mode != O_WRONLY && client->fd >= 0) { + // file is not open for writing, reopen + AP::FS().close(client->fd); + client->fd = -1; + } + + // Try opening the file + if (client->fd < 0) { + // make sure the path is null terminated + if (strnlen((const char*)req.path.path.data, req.path.path.len) == req.path.path.len) { + rsp.error.value = ENAMETOOLONG; + _write_server.respond(transfer, rsp); + return; + } + // open the file + client->fd = AP::FS().open((const char*)req.path.path.data, O_WRONLY); + } + + // Check if we successfully opened the file + if (client->fd < 0) { if (errno != 0) { rsp.error.value = errno; } else { - rsp.error.value = fd; + rsp.error.value = client->fd; + } + _write_server.respond(transfer, rsp); + return; + } + + // check if it is final call to finish the write + if (req.data.len == 0 && req.offset != 0) { + AP::FS().close(client->fd); + client->fd = -1; + _write_server.respond(transfer, rsp); + return; + } + + // seek to the requested offset + int ret = AP::FS().lseek(client->fd, req.offset, SEEK_SET); + if (ret < 0) { + if (errno != 0) { + rsp.error.value = errno; + } else { + rsp.error.value = ret; } _write_server.respond(transfer, rsp); return; } - int ret = AP::FS().write(fd, req.data.data, req.data.len); + ret = AP::FS().write(client->fd, req.data.data, req.data.len); if (ret < 0) { if (errno != 0) { rsp.error.value = errno; @@ -21,6 +88,10 @@ void AP_DroneCAN_File_Server::handle_write_request(const CanardRxTransfer& trans rsp.error.value = ret; } } + if (ret < req.data.len) { + // we didn't write all the data + rsp.error.value = EAGAIN; + } _write_server.respond(transfer, rsp); } diff --git a/libraries/AP_DroneCAN/AP_DroneCAN_File_Server.h b/libraries/AP_DroneCAN/AP_DroneCAN_File_Server.h index 8845a8b579c9a..b22196ee46670 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN_File_Server.h +++ b/libraries/AP_DroneCAN/AP_DroneCAN_File_Server.h @@ -13,11 +13,21 @@ #include "AP_Canard_iface.h" #include +#define MAX_NUM_CLIENTS 5 + class AP_DroneCAN; //Forward declaring classes class AP_DroneCAN_File_Server { +public: + AP_DroneCAN_File_Server(AP_DroneCAN &ap_dronecan, CanardInterface &canard_iface); + + bool init(); + + // Do not allow copies + CLASS_NO_COPY(AP_DroneCAN_File_Server); +private: HAL_Semaphore storage_sem; AP_DroneCAN &_ap_dronecan; CanardInterface &_canard_iface; @@ -30,17 +40,14 @@ class AP_DroneCAN_File_Server Canard::ObjCallback file_read_cb{this, &AP_DroneCAN_File_Server::handle_read_request}; Canard::Server _read_server{_canard_iface, file_read_cb}; -public: - AP_DroneCAN_File_Server(AP_DroneCAN &ap_dronecan, CanardInterface &canard_iface, uint8_t driver_index); - - - // Do not allow copies - CLASS_NO_COPY(AP_DroneCAN_File_Server); + struct FileClient { + int fd = -1; + uavcan_protocol_file_Path path; + uint8_t open_mode; + uint8_t client_node_id; + uint32_t last_request_time_ms; + } _clients[MAX_NUM_CLIENTS]; - //Reset the Server Record - void reset(); + FileClient* get_free_client(const uavcan_protocol_file_Path &path, uint8_t client_node_id); - /* Subscribe to the messages to be handled for maintaining and allocating - Node ID list */ - static void subscribe_msgs(AP_DroneCAN* ap_dronecan); }; \ No newline at end of file