diff --git a/components/free_rtos/ethernet_industry/CoE/eth_ecat_pdo_fmmu.cpp b/components/free_rtos/ethernet_industry/CoE/eth_ecat_pdo_fmmu.cpp index 38ed614..b54cd66 100644 --- a/components/free_rtos/ethernet_industry/CoE/eth_ecat_pdo_fmmu.cpp +++ b/components/free_rtos/ethernet_industry/CoE/eth_ecat_pdo_fmmu.cpp @@ -69,15 +69,24 @@ void EthEcatPdoFMMU::wait_op() { } void EthEcatPdoFMMU::process_write_queue(uint8_t* process_data, uint32_t len) { - //DebugP_log("write lock\r\n"); mutex_write_.lock(); - auto queue = queue_write_; // Копия очереди + auto static_queue = static_queue_write_; + auto queue = queue_write_; + queue_write_.clear(); - //DebugP_log("write unlock\r\n"); mutex_write_.unlock(); - //DebugP_log("queue write dequeue first\r\n"); + + auto static_next = static_queue.dequeue(); + + while(static_next != nullptr) { + //DebugP_log("set value\r\n"); + static_next->set_value(process_data, len); + //DebugP_log("queue read dequeue\r\n"); + static_next = static_queue.dequeue(); + } + auto next = queue.dequeue(); while(next != nullptr) { @@ -92,12 +101,22 @@ void EthEcatPdoFMMU::process_read_queue(uint8_t* process_data, uint32_t len) { //DebugP_log("read lock\r\n"); mutex_read_.lock(); - auto queue = queue_read_; // Копия очереди + auto static_queue = static_queue_read_; + auto queue = queue_read_; + queue_read_.clear(); - //DebugP_log("read unlock\r\n"); mutex_read_.unlock(); - //DebugP_log("queue read dequeue first\r\n"); + + auto static_next = static_queue.dequeue(); + + while(static_next != nullptr) { + //DebugP_log("set value\r\n"); + static_next->set_value(process_data, len); + //DebugP_log("queue read dequeue\r\n"); + static_next = static_queue.dequeue(); + } + auto next = queue.dequeue(); while(next != nullptr) { diff --git a/components/free_rtos/ethernet_industry/CoE/eth_ecat_pdo_fmmu.hpp b/components/free_rtos/ethernet_industry/CoE/eth_ecat_pdo_fmmu.hpp index 6ba6ba8..5f8c6cd 100644 --- a/components/free_rtos/ethernet_industry/CoE/eth_ecat_pdo_fmmu.hpp +++ b/components/free_rtos/ethernet_industry/CoE/eth_ecat_pdo_fmmu.hpp @@ -95,19 +95,27 @@ public: } + void pdo_write_async_static(custom_promise::IPromise& promise) { + mutex_write_.lock(); + static_queue_write_ + promise; + mutex_write_.unlock(); + } + + void pdo_read_async_static(custom_promise::IPromise& promise) { + mutex_read_.lock(); + static_queue_read_ + promise; + mutex_read_.unlock(); + } + void pdo_write_async(custom_promise::IPromise& promise) { mutex_write_.lock(); - queue_write_ + promise; - mutex_write_.unlock(); } void pdo_read_async(custom_promise::IPromise& promise) { mutex_read_.lock(); - queue_read_ + promise; - mutex_read_.unlock(); } @@ -119,6 +127,9 @@ private: Mutex mutex_write_; Mutex mutex_read_; + queue::Queue static_queue_write_; + queue::Queue static_queue_read_; + queue::Queue queue_write_; queue::Queue queue_read_; diff --git a/components/free_rtos/ethernet_industry/eth_ecat_api.hpp b/components/free_rtos/ethernet_industry/eth_ecat_api.hpp index 4291797..37477b0 100644 --- a/components/free_rtos/ethernet_industry/eth_ecat_api.hpp +++ b/components/free_rtos/ethernet_industry/eth_ecat_api.hpp @@ -57,6 +57,14 @@ public: return ecat_pdo_fmmu_.pdo_read(timeout_ticks, offset, data...); } + void pdo_write_async_static(custom_promise::IPromise& promise) { + ecat_pdo_fmmu_.pdo_write_async_static(promise); + } + + void pdo_read_async_static(custom_promise::IPromise& promise) { + ecat_pdo_fmmu_.pdo_read_async_static(promise); + } + void pdo_write_async(custom_promise::IPromise& promise) { ecat_pdo_fmmu_.pdo_write_async(promise); }