From 54db3acc1e3e49321d15fcbabfa19dbabfc77301 Mon Sep 17 00:00:00 2001 From: algin Date: Thu, 15 Jun 2023 15:16:51 +0300 Subject: [PATCH] =?UTF-8?q?feat(UML-1462):=20=D0=94=D0=BE=D1=80=D0=B0?= =?UTF-8?q?=D0=B1=D0=BE=D1=82=D0=B0=D0=BD=D0=B0=20=D0=BE=D1=87=D0=B5=D1=80?= =?UTF-8?q?=D0=B5=D0=B4=D1=8C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../CoE/eth_ecat_pdo_fmmu.cpp | 20 +++++----- .../free_rtos/ethernet_industry/eth_ecat.cpp | 2 + .../ethernet_industry/eth_ecat_buffer.hpp | 2 +- .../eth_ecat_custom_promise.hpp | 12 +++--- .../ethernet_industry/eth_ecat_datagram.hpp | 12 +++--- .../ethernet_industry/eth_ecat_queue.hpp | 34 +++++++++++------ .../ethernet_industry/eth_ecat_telegram.cpp | 38 ++++++++++++++----- .../ethernet_industry/eth_ecat_telegram.hpp | 5 ++- 8 files changed, 79 insertions(+), 46 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 013426d..a6f8ca8 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 @@ -60,34 +60,34 @@ void EthEcatPdoFMMU::wait_op() { } void EthEcatPdoFMMU::process_write_queue(uint8_t* process_data, uint32_t len) { - queue::QueueEntity *next; - mutex_write_.lock(); - next = queue_write_.get_first(); + queue::Queue queue = queue_write_; queue_write_.clear(); mutex_write_.unlock(); + auto next = queue.dequeue(); + while(next != nullptr) { - next->get_data()->set_value(process_data, len); - next = next->get_next(); + next->set_value(process_data, len); + next = queue.dequeue(); } } void EthEcatPdoFMMU::process_read_queue(uint8_t* process_data, uint32_t len) { - queue::QueueEntity *next; - mutex_read_.lock(); - next = queue_read_.get_first(); + queue::Queue queue = queue_read_; queue_read_.clear(); mutex_read_.unlock(); + auto next = queue.dequeue(); + while(next != nullptr) { - next->get_data()->set_value(process_data, len); - next = next->get_next(); + next->set_value(process_data, len); + next = queue.dequeue(); } } diff --git a/components/free_rtos/ethernet_industry/eth_ecat.cpp b/components/free_rtos/ethernet_industry/eth_ecat.cpp index 9ce7a7f..5f7296a 100644 --- a/components/free_rtos/ethernet_industry/eth_ecat.cpp +++ b/components/free_rtos/ethernet_industry/eth_ecat.cpp @@ -179,6 +179,8 @@ void EthEcat::get_addresses_of_slaves() { datagrams.emplace_back(TDatagram{ {{slave.get_slave_address(), ECT_REG_STADR}}, slave.get_slave_address() }); } + queue::Queue queue; + for(uint16_t i = 1; i < number_of_slaves; i++) { datagrams[i - 1] + datagrams[i]; } diff --git a/components/free_rtos/ethernet_industry/eth_ecat_buffer.hpp b/components/free_rtos/ethernet_industry/eth_ecat_buffer.hpp index 7f75da3..6cab5a8 100644 --- a/components/free_rtos/ethernet_industry/eth_ecat_buffer.hpp +++ b/components/free_rtos/ethernet_industry/eth_ecat_buffer.hpp @@ -199,7 +199,7 @@ public: SyncManager sync_manager_read = sync_managers_[static_cast(sm_read)]; auto datagram_read = make_sync_manager_datagram(sync_manager_read, buffer_properties_read_); - datagram_write + datagram_read; + auto queue = datagram_write + datagram_read; do { telegram.transfer(datagram_write); diff --git a/components/free_rtos/ethernet_industry/eth_ecat_custom_promise.hpp b/components/free_rtos/ethernet_industry/eth_ecat_custom_promise.hpp index bb02c4d..cb672aa 100644 --- a/components/free_rtos/ethernet_industry/eth_ecat_custom_promise.hpp +++ b/components/free_rtos/ethernet_industry/eth_ecat_custom_promise.hpp @@ -65,17 +65,15 @@ private: class IPromise { public: + friend class queue::Queue; + IPromise(address::Offset offset = 0) : offset_{offset} { } - - queue::QueueEntity& get_queue_entity() { - return queue_entity_; - } - +/* queue::Queue operator+(IPromise &next) { - return queue::Queue{queue_entity_, next.get_queue_entity()}; + return queue::Queue{queue_entity_, next.queue_entity_}; } - +*/ virtual void set_value(uint8_t* process_data, uint32_t len) = 0; protected: diff --git a/components/free_rtos/ethernet_industry/eth_ecat_datagram.hpp b/components/free_rtos/ethernet_industry/eth_ecat_datagram.hpp index 9489c2b..c594b8c 100644 --- a/components/free_rtos/ethernet_industry/eth_ecat_datagram.hpp +++ b/components/free_rtos/ethernet_industry/eth_ecat_datagram.hpp @@ -25,6 +25,8 @@ using TEcatWkc = uint16_t; class IEcatDatagram { public: + friend class queue::Queue; + IEcatDatagram(ec_moredatagrams more, TEcatWkc wkc) : more_{more} , wkc_{wkc} @@ -44,14 +46,10 @@ public: return next->get_data(); } - queue::QueueEntity& get_queue_entity() { - return queue_entity_; - } - queue::Queue operator+(IEcatDatagram &next) { more_ = ec_moredatagrams::EC_MOREDATAGRAMS_MORE; - return queue::Queue{queue_entity_, next.get_queue_entity()}; + return queue::Queue{queue_entity_, next.queue_entity_}; } virtual uint8_t* pack(uint8_t *raw) = 0; @@ -75,13 +73,15 @@ public: return wkc_ + next->get_data()->get_all_wkc(); } + queue::QueueEntity queue_entity_{this}; + protected: ec_moredatagrams more_; TEcatDgHeader header_; TEcatWkc wkc_; private: - queue::QueueEntity queue_entity_{this}; + }; template diff --git a/components/free_rtos/ethernet_industry/eth_ecat_queue.hpp b/components/free_rtos/ethernet_industry/eth_ecat_queue.hpp index a6f2f1a..c03c672 100644 --- a/components/free_rtos/ethernet_industry/eth_ecat_queue.hpp +++ b/components/free_rtos/ethernet_industry/eth_ecat_queue.hpp @@ -20,6 +20,8 @@ class Queue; template class QueueEntity { public: + friend class Queue; + QueueEntity(DataType *data) : data_{data} { } @@ -31,10 +33,6 @@ public: return next_; } - void set_next(QueueEntity &next) { - next_ = &next; - } - private: DataType *data_{nullptr}; QueueEntity *next_{nullptr}; @@ -52,7 +50,7 @@ public: Queue(QueueEntity& first, QueueEntity& next) : first_{&first} , last_{&next} { - first.set_next(next); + first.next_ = &next; } QueueEntity* get_first() { @@ -67,18 +65,32 @@ public: return size_; } - Queue& operator+(QueueEntity& next) { - return append(&next); - } - Queue& operator+(DataType& data) { - return append(&data.get_queue_entity()); + return append(&data.queue_entity_); } Queue& operator+(Queue& other) { return append(other.get_first(), other.get_last(), other.get_size()); } + DataType* dequeue() { + QueueEntity* first = first_; + + if(first == nullptr) { + return nullptr; + } + + first_ = first_->next_; + + if(first_ == nullptr) { + last_ = nullptr; + } + + size_--; + + return first->data_; + } + bool empty() { return (first_ == nullptr); } @@ -100,7 +112,7 @@ private: } if(last_ != nullptr) { - last_->set_next(*other_first); + last_->next_ = other_first; } if(other_last == nullptr) { diff --git a/components/free_rtos/ethernet_industry/eth_ecat_telegram.cpp b/components/free_rtos/ethernet_industry/eth_ecat_telegram.cpp index 5475519..95b907d 100644 --- a/components/free_rtos/ethernet_industry/eth_ecat_telegram.cpp +++ b/components/free_rtos/ethernet_industry/eth_ecat_telegram.cpp @@ -46,14 +46,16 @@ uint8_t* EcatTelegram::pack(uint8_t *raw) { .type = static_cast(ec_network::PROTOCOL_TYPE) } }; uint8_t *p_datagram_first = raw + sizeof(TEthFrameHeader) + sizeof(TEcatFrameHeader); uint8_t *p_datagram_last = p_datagram_first; - datagram::IEcatDatagram *next = datagram_queue_; (void)p_eth_hdr; (void)p_hdr; + queue::Queue queue = datagram_queue_; + auto next = queue.dequeue(); + while(next != nullptr) { p_datagram_last = next->pack(p_datagram_last); - next = next->get_next(); + next = queue.dequeue(); } p_hdr->bits.length = p_datagram_last - p_datagram_first; @@ -66,7 +68,6 @@ uint8_t* EcatTelegram::unpack(uint8_t *raw) { TEcatFrameHeader *p_hdr = reinterpret_cast(raw + sizeof(TEthFrameHeader)); uint8_t *p_datagram_first = raw + sizeof(TEthFrameHeader) + sizeof(TEcatFrameHeader); uint8_t *p_datagram_last = p_datagram_first; - datagram::IEcatDatagram *next = datagram_queue_; if(p_eth_hdr->prot_id != ETH_PROT_ECAT_LE) { DebugP_log((char*)"Error: wrong protocol ID\r\n"); @@ -80,16 +81,19 @@ uint8_t* EcatTelegram::unpack(uint8_t *raw) { return raw; } + queue::Queue queue = datagram_queue_; + auto next = queue.dequeue(); + while(next != nullptr) { p_datagram_last = next->unpack(p_datagram_last); - next = next->get_next(); + next = queue.dequeue(); } return p_datagram_last; } -void EcatTelegram::transfer(datagram::IEcatDatagram& first) { - datagram_queue_ = &first; +void EcatTelegram::transfer(datagram::IEcatDatagram& next) { + datagram_queue_ + next; bool stat = eth_stack_.send_pkt(port_id_, ETH_PROT_ECAT_LE, 1); @@ -101,9 +105,25 @@ void EcatTelegram::transfer(datagram::IEcatDatagram& first) { rx_sem_.pend(); - datagram_queue_ = nullptr; -} - + datagram_queue_.clear(); +} +/* +void EcatTelegram::transfer(queue::Queue& next) { + datagram_queue_ + next; + + bool stat = eth_stack_.send_pkt(port_id_, ETH_PROT_ECAT_LE, 1); + + if(stat == false) { + DebugP_log((char*)"telegram transfer error !\r\n"); + + return; + } + + rx_sem_.pend(); + + datagram_queue_.clear(); +} +*/ } } diff --git a/components/free_rtos/ethernet_industry/eth_ecat_telegram.hpp b/components/free_rtos/ethernet_industry/eth_ecat_telegram.hpp index b9d40fd..1a8a919 100644 --- a/components/free_rtos/ethernet_industry/eth_ecat_telegram.hpp +++ b/components/free_rtos/ethernet_industry/eth_ecat_telegram.hpp @@ -30,7 +30,8 @@ public: eth_.getEthStackPtr()->Register(ETH_PROT_ECAT_LE, this); } - void transfer(datagram::IEcatDatagram& first); + void transfer(datagram::IEcatDatagram& next); + //void transfer(queue::Queue& next); private: Eth& eth_; @@ -39,7 +40,7 @@ private: free_rtos::Semaphore rx_sem_; - datagram::IEcatDatagram *datagram_queue_{nullptr}; + queue::Queue datagram_queue_; uint8_t* pack(uint8_t *raw); uint8_t* unpack(uint8_t *raw);