AP_DroneCAN: add file server and client write request abd response me…

…thods

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);

libraries/AP_DroneCAN/AP_DroneCAN.h

@@ -36,6 +36,8 @@
 #include <dronecan_msgs.h>
 #include <AP_SerialManager/AP_SerialManager_config.h>
 #include <AP_Relay/AP_Relay_config.h>
+#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<uavcan_equipment_hardpoint_Command> 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;
 

libraries/AP_DroneCAN/AP_DroneCAN_File_Client.cpp

@@ -1,25 +1,93 @@
 #include "AP_DroneCAN_File_Client.h"
+#include <AP_FileSystem/AP_Filesystem.h>
+
+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;
 }

libraries/AP_DroneCAN/AP_DroneCAN_File_Client.h

@@ -1,8 +1,6 @@
 #pragma once
 #include <AP_HAL/AP_HAL_Boards.h>
 #include <AP_HAL/Semaphores.h>
-
-
 #include <AP_Common/Bitmask.h>
 #include <StorageManager/StorageManager.h>
 #include <AP_CANManager/AP_CANManager.h>
@@ -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<AP_DroneCAN_File_Client, uavcan_protocol_file_WriteResponse> file_wr_cb{this, &AP_DroneCAN_File_Client::handle_write_response};
     Canard::Client<uavcan_protocol_file_WriteResponse> _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;
 };

libraries/AP_DroneCAN/AP_DroneCAN_File_Server.cpp

@@ -1,26 +1,97 @@
 #include <AP_Filesystem/AP_Filesystem.h>
 #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;
         } else {
             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);
 }
 

libraries/AP_DroneCAN/AP_DroneCAN_File_Server.h

@@ -13,11 +13,21 @@
 #include "AP_Canard_iface.h"
 #include <dronecan_msgs.h>
 
+#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<AP_DroneCAN_File_Server, uavcan_protocol_file_ReadRequest> file_read_cb{this, &AP_DroneCAN_File_Server::handle_read_request};
     Canard::Server<uavcan_protocol_file_ReadRequest> _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);
 };