feat(UML-1462): Доработана очередь

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<custom_promise::IPromise> *next;
-
     mutex_write_.lock();
 
-    next = queue_write_.get_first();
+    queue::Queue<custom_promise::IPromise> 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->set_value(process_data, len);
-        next = next->get_next();
+        next = queue.dequeue();
     }
 }
 
 void EthEcatPdoFMMU::process_read_queue(uint8_t* process_data, uint32_t len) {
-    queue::QueueEntity<custom_promise::IPromise> *next;
-
     mutex_read_.lock();
 
-    next = queue_read_.get_first();
+    queue::Queue<custom_promise::IPromise> 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->set_value(process_data, len);
-        next = next->get_next();
+        next = queue.dequeue();
     }
 }
 

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<command::AP>(), ECT_REG_STADR}}, slave.get_slave_address<command::FP>() });
     }
 
+    queue::Queue<TDatagram> queue;
+
     for(uint16_t i = 1; i < number_of_slaves; i++) {
         datagrams[i - 1] + datagrams[i];
     }

components/free_rtos/ethernet_industry/eth_ecat_buffer.hpp

@@ -199,7 +199,7 @@ public:
         SyncManager sync_manager_read = sync_managers_[static_cast<size_t>(sm_read)];
         auto datagram_read = make_sync_manager_datagram<TypeT>(sync_manager_read, buffer_properties_read_);
 
-        datagram_write + datagram_read;
+        auto queue = datagram_write + datagram_read;
 
         do {
             telegram.transfer(datagram_write);

components/free_rtos/ethernet_industry/eth_ecat_custom_promise.hpp

@@ -65,17 +65,15 @@ private:
 
 class IPromise {
 public:
+    friend class queue::Queue<IPromise>;
+
     IPromise(address::Offset offset = 0)
     : offset_{offset} { }
-
+/*
-    queue::QueueEntity<IPromise>& get_queue_entity() {
-        return queue_entity_;
-    }
-
     queue::Queue<IPromise> operator+(IPromise &next) {
-        return queue::Queue<IPromise>{queue_entity_, next.get_queue_entity()};
+        return queue::Queue<IPromise>{queue_entity_, next.queue_entity_};
     }
-
+*/
     virtual void set_value(uint8_t* process_data, uint32_t len) = 0;
 
 protected:

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>;
+
     IEcatDatagram(ec_moredatagrams more, TEcatWkc wkc)
     : more_{more}
     , wkc_{wkc}
@@ -44,14 +46,10 @@ public:
         return next->get_data();
     }
 
-    queue::QueueEntity<IEcatDatagram>& get_queue_entity() {
-        return queue_entity_;
-    }
-
     queue::Queue<IEcatDatagram> operator+(IEcatDatagram &next) {
         more_ = ec_moredatagrams::EC_MOREDATAGRAMS_MORE;
 
-        return queue::Queue<IEcatDatagram>{queue_entity_, next.get_queue_entity()};
+        return queue::Queue<IEcatDatagram>{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<IEcatDatagram> queue_entity_{this};
+
 protected:
     ec_moredatagrams more_;
     TEcatDgHeader header_;
     TEcatWkc wkc_;
 
 private:
-    queue::QueueEntity<IEcatDatagram> queue_entity_{this};
+
 };
 
 template<typename CommandT, typename... DataTypes>

components/free_rtos/ethernet_industry/eth_ecat_queue.hpp

@@ -20,6 +20,8 @@ class Queue;
 template<typename DataType>
 class QueueEntity {
 public:
+    friend class Queue<DataType>;
+
     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<DataType>& first, QueueEntity<DataType>& next)
     : first_{&first}
     , last_{&next} {
-        first.set_next(next);
+        first.next_ = &next;
     }
 
     QueueEntity<DataType>* get_first() {
@@ -67,18 +65,32 @@ public:
         return size_;
     }
 
-    Queue& operator+(QueueEntity<DataType>& 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<DataType>* 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) {

components/free_rtos/ethernet_industry/eth_ecat_telegram.cpp

@@ -46,14 +46,16 @@ uint8_t* EcatTelegram::pack(uint8_t *raw) {
                                                                                             .type = static_cast<uint16_t>(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<datagram::IEcatDatagram> 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<TEcatFrameHeader*>(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<datagram::IEcatDatagram> 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) {
+void EcatTelegram::transfer(datagram::IEcatDatagram& next) {
-    datagram_queue_ = &first;
+    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<datagram::IEcatDatagram>& 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();
+}
+*/
 }
 
 }

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<datagram::IEcatDatagram>& next);
 
 private:
     Eth& eth_;
@@ -39,7 +40,7 @@ private:
 
     free_rtos::Semaphore rx_sem_;
 
-    datagram::IEcatDatagram *datagram_queue_{nullptr};
+    queue::Queue<datagram::IEcatDatagram> datagram_queue_;
 
     uint8_t* pack(uint8_t *raw);
     uint8_t* unpack(uint8_t *raw);