From d2c09001a2a8de4e4fc27f2cfe8ce03f7a7643ba Mon Sep 17 00:00:00 2001 From: algin Date: Mon, 2 Oct 2023 18:01:35 +0300 Subject: [PATCH] =?UTF-8?q?fix:=20=D0=98=D0=B7=D0=BC=D0=B5=D0=BD?= =?UTF-8?q?=D0=B5=D0=BD=20=D0=B0=D0=BB=D0=BB=D0=BE=D0=BA=D0=B0=D1=82=D0=BE?= =?UTF-8?q?=D1=80=20ethercat=20PDO.=20=D0=9E=D1=82=D0=BD=D0=BE=D1=81=D0=B8?= =?UTF-8?q?=D1=82=D0=B5=D0=BB=D1=8C=D0=BD=D0=BE=20=D1=81=D1=82=D0=B0=D0=B1?= =?UTF-8?q?=D0=B8=D0=BB=D1=8C=D0=BD=D1=8B=D0=B9.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../CoE/eth_ecat_pdo_fmmu.cpp | 184 ---------------- .../CoE/eth_ecat_pdo_fmmu.hpp | 206 +++++++++++++++++- .../ethernet_industry/eth_ecat_api.cpp | 8 - .../ethernet_industry/eth_ecat_api.hpp | 13 +- .../ethernet_industry/eth_ecat_packer.hpp | 8 +- 5 files changed, 216 insertions(+), 203 deletions(-) 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 a3a855f..8eb9de8 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 @@ -25,49 +25,6 @@ void EthEcatPdoFMMU::init(ProcessCallback callback) { callback_ = callback; } -bool EthEcatPdoFMMU::wait_op() { - free_rtos::Semaphore& init_sem = ecat_buffer_.get_ecat().get_init_sem(); - free_rtos::Semaphore& process_sem = ecat_buffer_.get_ecat().get_process_sem(); - - process_sem.pend(); - - uint32_t logical_full_length_write = ecat_buffer_.get_fmmu_global_properties().logical_full_length_write; - uint32_t logical_full_length_read = ecat_buffer_.get_fmmu_global_properties().logical_full_length_read; - std::vector pdo_read(logical_full_length_read, 0x00); - std::vector pdo_write(logical_full_length_write, 0x00); - custom_tuple&> data_tuple_read{pdo_read}; - custom_tuple&> data_tuple_write{pdo_write}; - bool status; - - status = read(0, pdo_write); // read to pdo_write ! - - if(status != true) { - return status; - } - - status = write(0, pdo_write); - - if(status != true) { - return status; - } - - init_sem.post(); - process_sem.pend(); - - for(uint32_t i = 0; i < 250; i++) { - status = read_write(0, data_tuple_read, 0, data_tuple_write); - - if(status != true) { - return status; - } - } - - init_sem.post(); - process_sem.pend(); - - return status; -} - void EthEcatPdoFMMU::process_write_queue(uint8_t* process_data, uint32_t len) { mutex_write_.lock(); @@ -127,147 +84,6 @@ void EthEcatPdoFMMU::process_read_queue(uint8_t* process_data, uint32_t len) { } } -void EthEcatPdoFMMU::process() { - bool status; - - status = wait_op(); - - if(status != true) { - return; - } - - uint32_t logical_full_length_write = ecat_buffer_.get_fmmu_global_properties().logical_full_length_write; - uint32_t logical_full_length_read = ecat_buffer_.get_fmmu_global_properties().logical_full_length_read; - std::vector pdo_read(logical_full_length_read, 0x00); - std::vector pdo_write(logical_full_length_write, 0x00); - custom_tuple&> data_tuple_read{pdo_read}; - custom_tuple&> data_tuple_write{pdo_write}; - - status = read(0, pdo_write); // read to pdo_write ! - - if(status != true) { - return; - } - - status = write(0, pdo_write); - - if(status != true) { - return; - } - - while(1) { - status = read_write(0, data_tuple_read, 0, data_tuple_write); - - if(status != true) { - return; - } - - #if PROFILE_LEVEL > 0 - { - #warning ( "Profiling for RPDO subscribers is enabled !" ) - - PROFILE(process_read_queue, PROFILE_SAMPLES, 0); - - process_read_queue(pdo_read.data(), pdo_read.size()); - } - #else - process_read_queue(pdo_read.data(), pdo_read.size()); - #endif - - #if PROFILE_LEVEL > 0 - { - #warning ( "Profiling for process callback is enabled !" ) - - PROFILE(callback_, PROFILE_SAMPLES, 0); - - if(callback_ != nullptr) { - callback_(); - } - } - #else - if(callback_ != nullptr) { - callback_(); - } - #endif - - #if PROFILE_LEVEL > 0 - { - #warning ( "Profiling for TPDO subscribers is enabled !" ) - - PROFILE(process_write_queue, PROFILE_SAMPLES, 0); - - process_write_queue(pdo_write.data(), pdo_write.size()); - } - #else - process_write_queue(pdo_write.data(), pdo_write.size()); - #endif - - pdo_counter_++; - } -} - -void EthEcatPdoFMMU::process_fake(uint32_t period_microsec, ecat_pdo_fmmu::ProcessCallback callback) { - std::vector pdo_read(110, 0x00); - std::vector pdo_write(110, 0x00); - free_rtos::Timer& ecat_timer = ecat_buffer_.get_ecat().get_ecat_timer(); - - Timer::Settings ecat_tmr_sett = { - .input_clk_Hz = 25000000, // 25MHz - .base_address = 0x2400000u, // memory mapping, - .clock_src_mux_addr = 0x430081B0u, // sysconfig - .int_num = 152u, // sysconfig - .int_priority = 4, // sysconfig - .period_us = period_microsec /// microsec - }; - - ecat_timer.Init(ecat_tmr_sett); - ecat_timer.Start(); - - while(1) { - #if PROFILE_LEVEL > 0 - { - #warning ( "Profiling for RPDO subscribers is enabled !" ) - - PROFILE(process_read_queue, PROFILE_SAMPLES, 0); - - process_read_queue(pdo_read.data(), pdo_read.size()); - } - #else - process_read_queue(pdo_read.data(), pdo_read.size()); - #endif - - #if PROFILE_LEVEL > 0 - { - #warning ( "Profiling for process callback is enabled !" ) - - PROFILE(callback_, PROFILE_SAMPLES, 0); - - if(callback_ != nullptr) { - callback_(); - } - } - #else - if(callback_ != nullptr) { - callback_(); - } - #endif - - #if PROFILE_LEVEL > 0 - { - #warning ( "Profiling for TPDO subscribers is enabled !" ) - - PROFILE(process_write_queue, PROFILE_SAMPLES, 0); - - process_write_queue(pdo_write.data(), pdo_write.size()); - } - #else - process_write_queue(pdo_write.data(), pdo_write.size()); - #endif - - pdo_counter_++; - } -} - } } 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 bfc00e8..e98110f 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 @@ -8,6 +8,8 @@ #ifndef FREE_RTOS_ETHERNET_INDUSTRY_COE_ETH_ECAT_PDO_FMMU_HPP_ #define FREE_RTOS_ETHERNET_INDUSTRY_COE_ETH_ECAT_PDO_FMMU_HPP_ +#include + #include "free_rtos/semaphore/semaphore.hpp" #include "free_rtos/mutex/mutex.hpp" @@ -53,14 +55,18 @@ private: ecat_buffer::EcatBufferSlave& buffer_slave_; }; - class EthEcatPdoFMMU { public: - EthEcatPdoFMMU(ecat_buffer::EthEcatBuffer& ecat_buffer): ecat_buffer_{ecat_buffer} { } + EthEcatPdoFMMU(ecat_buffer::EthEcatBuffer& ecat_buffer): ecat_buffer_{ecat_buffer} + { } void init(ProcessCallback callback = nullptr); - void process(); - void process_fake(uint32_t period_microsec = 250, ProcessCallback callback = nullptr); + + template + void process(Allocator& allocator); + + template + void process_fake(Allocator& allocator, uint32_t period_microsec = 250, ProcessCallback callback = nullptr); uint32_t get_pdo_counter() { return pdo_counter_; @@ -141,7 +147,8 @@ private: uint32_t pdo_counter_{0}; - bool wait_op(); + template + bool wait_op(std::vector& pdo_read, std::vector& pdo_write); void process_write_queue(uint8_t* process_data, uint32_t len); void process_read_queue(uint8_t* process_data, uint32_t len); @@ -194,6 +201,195 @@ private: }; +template +bool EthEcatPdoFMMU::wait_op(std::vector& pdo_read, std::vector& pdo_write) { + free_rtos::Semaphore& init_sem = ecat_buffer_.get_ecat().get_init_sem(); + free_rtos::Semaphore& process_sem = ecat_buffer_.get_ecat().get_process_sem(); + + process_sem.pend(); + + uint32_t logical_full_length_write = ecat_buffer_.get_fmmu_global_properties().logical_full_length_write + sizeof(DataT) - 1; + uint32_t logical_full_length_read = ecat_buffer_.get_fmmu_global_properties().logical_full_length_read + sizeof(DataT) - 1; + custom_tuple< std::vector& > data_tuple_read{pdo_read}; + custom_tuple< std::vector& > data_tuple_write{pdo_write}; + bool status; + + pdo_read.reserve(logical_full_length_read/sizeof(DataT)); + pdo_read.resize(logical_full_length_read/sizeof(DataT), 0); + + pdo_write.reserve(logical_full_length_write/sizeof(DataT)); + pdo_write.resize(logical_full_length_write/sizeof(DataT), 0); + + status = read(0, pdo_write); // read to pdo_write ! + + if(status != true) { + return status; + } + + status = write(0, pdo_write); + + if(status != true) { + return status; + } + + init_sem.post(); + process_sem.pend(); + + for(uint32_t i = 0; i < 250; i++) { + status = read_write(0, data_tuple_read, 0, data_tuple_write); + + if(status != true) { + return status; + } + } + + init_sem.post(); + process_sem.pend(); + + return status; +} + +template +void EthEcatPdoFMMU::process(Allocator& allocator) { + std::vector pdo_read{allocator}; + std::vector pdo_write{allocator}; + bool status; + + status = wait_op(pdo_read, pdo_write); + + if(status != true) { + return; + } + + custom_tuple< std::vector& > data_tuple_read{pdo_read}; + custom_tuple< std::vector& > data_tuple_write{pdo_write}; + + status = read(0, pdo_write); // read to pdo_write ! + + if(status != true) { + return; + } + + status = write(0, pdo_write); + + if(status != true) { + return; + } + + while(1) { + status = read_write(0, data_tuple_read, 0, data_tuple_write); + + if(status != true) { + return; + } + + #if PROFILE_LEVEL > 0 + { + #warning ( "Profiling for RPDO subscribers is enabled !" ) + + PROFILE(process_read_queue, PROFILE_SAMPLES, 0); + + process_read_queue(pdo_read.data(), pdo_read.size()); + } + #else + process_read_queue(pdo_read.data(), pdo_read.size()); + #endif + + #if PROFILE_LEVEL > 0 + { + #warning ( "Profiling for process callback is enabled !" ) + + PROFILE(callback_, PROFILE_SAMPLES, 0); + + if(callback_ != nullptr) { + callback_(); + } + } + #else + if(callback_ != nullptr) { + callback_(); + } + #endif + + #if PROFILE_LEVEL > 0 + { + #warning ( "Profiling for TPDO subscribers is enabled !" ) + + PROFILE(process_write_queue, PROFILE_SAMPLES, 0); + + process_write_queue(pdo_write.data(), pdo_write.size()); + } + #else + process_write_queue(pdo_write.data(), pdo_write.size()); + #endif + + pdo_counter_++; + } +} + +template +void EthEcatPdoFMMU::process_fake(Allocator& allocator, uint32_t period_microsec, ecat_pdo_fmmu::ProcessCallback callback) { + std::vector pdo_read(110, 0, allocator); + std::vector pdo_write(110, 0, allocator); + free_rtos::Timer& ecat_timer = ecat_buffer_.get_ecat().get_ecat_timer(); + + Timer::Settings ecat_tmr_sett = { + .input_clk_Hz = 25000000, // 25MHz + .base_address = 0x2400000u, // memory mapping, + .clock_src_mux_addr = 0x430081B0u, // sysconfig + .int_num = 152u, // sysconfig + .int_priority = 4, // sysconfig + .period_us = period_microsec /// microsec + }; + + ecat_timer.Init(ecat_tmr_sett); + ecat_timer.Start(); + + while(1) { + #if PROFILE_LEVEL > 0 + { + #warning ( "Profiling for RPDO subscribers is enabled !" ) + + PROFILE(process_read_queue, PROFILE_SAMPLES, 0); + + process_read_queue(pdo_read.data(), pdo_read.size()); + } + #else + process_read_queue(pdo_read.data(), pdo_read.size()); + #endif + + #if PROFILE_LEVEL > 0 + { + #warning ( "Profiling for process callback is enabled !" ) + + PROFILE(callback_, PROFILE_SAMPLES, 0); + + if(callback_ != nullptr) { + callback_(); + } + } + #else + if(callback_ != nullptr) { + callback_(); + } + #endif + + #if PROFILE_LEVEL > 0 + { + #warning ( "Profiling for TPDO subscribers is enabled !" ) + + PROFILE(process_write_queue, PROFILE_SAMPLES, 0); + + process_write_queue(pdo_write.data(), pdo_write.size()); + } + #else + process_write_queue(pdo_write.data(), pdo_write.size()); + #endif + + pdo_counter_++; + } +} + } // namespace ecat_pdo_fmmu } // namespace free_rtos diff --git a/components/free_rtos/ethernet_industry/eth_ecat_api.cpp b/components/free_rtos/ethernet_industry/eth_ecat_api.cpp index 46daaa2..18e74e4 100644 --- a/components/free_rtos/ethernet_industry/eth_ecat_api.cpp +++ b/components/free_rtos/ethernet_industry/eth_ecat_api.cpp @@ -114,14 +114,6 @@ bool EthEcatApi::config_init(TEthMacPorts port_id, uint16_t address_base, uint32 return status; } -void EthEcatApi::process() { - ecat_pdo_fmmu_.process(); -} - -void EthEcatApi::process_fake(uint32_t period_microsec, ecat_pdo_fmmu::ProcessCallback callback) { - ecat_pdo_fmmu_.process_fake(period_microsec, callback); -} - std::vector& EthEcatApi::get_ecat_pdo_map() { return ecat_sdo_mailbox_.get_pdo_map(); } diff --git a/components/free_rtos/ethernet_industry/eth_ecat_api.hpp b/components/free_rtos/ethernet_industry/eth_ecat_api.hpp index e6e3567..a532ba3 100644 --- a/components/free_rtos/ethernet_industry/eth_ecat_api.hpp +++ b/components/free_rtos/ethernet_industry/eth_ecat_api.hpp @@ -29,8 +29,17 @@ public: , ecat_pdo_fmmu_{ecat_buffer_pdo_} { } bool config_init(TEthMacPorts port_id, uint16_t address_base, uint32_t period_microsec = 250, ecat_pdo_fmmu::ProcessCallback callback = nullptr); - void process(); - void process_fake(uint32_t period_microsec = 250, ecat_pdo_fmmu::ProcessCallback callback = nullptr); + + template + void process(Allocator& allocator) { + ecat_pdo_fmmu_.process(allocator); + } + + template + void process_fake(Allocator& allocator, uint32_t period_microsec, ecat_pdo_fmmu::ProcessCallback callback) { + ecat_pdo_fmmu_.process_fake(allocator, period_microsec, callback); + } + std::vector& get_ecat_pdo_map(); uint32_t get_pdo_counter() { diff --git a/components/free_rtos/ethernet_industry/eth_ecat_packer.hpp b/components/free_rtos/ethernet_industry/eth_ecat_packer.hpp index d9ad2fe..8857488 100644 --- a/components/free_rtos/ethernet_industry/eth_ecat_packer.hpp +++ b/components/free_rtos/ethernet_industry/eth_ecat_packer.hpp @@ -45,8 +45,8 @@ struct PackFunctor : public PackFunctorBase { using PackFunctorBase::operator (); - template - void operator()(std::vector& data) { + template + void operator()(std::vector& data) { size_t size = data.size() * sizeof(DataT); if(skip == false) { @@ -99,8 +99,8 @@ struct UnpackFunctor : public UnpackFunctorBase { using UnpackFunctorBase::operator (); - template - void operator()(std::vector& data) { + template + void operator()(std::vector& data) { size_t size = data.size() * sizeof(DataT); if(skip == false) {