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) {
-    custom_promise::IPromise *next;
-
     mutex_write_.lock();
 
-    next = queue_write_.get_next();
-    last_write_ = &queue_write_;
+    queue::Queue<custom_promise::IPromise> queue = queue_write_;
+    queue_write_.clear();
 
     mutex_write_.unlock();
 
+    auto next = queue.dequeue();
+
     while(next != nullptr) {
         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) {
-    custom_promise::IPromise *next;
-
     mutex_read_.lock();
 
-    next = queue_read_.get_next();
-    last_read_ = &queue_read_;
+    queue::Queue<custom_promise::IPromise> queue = queue_read_;
+    queue_read_.clear();
 
     mutex_read_.unlock();
 
+    auto next = queue.dequeue();
+
     while(next != nullptr) {
         next->set_value(process_data, len);
-        next = next->get_next();
+        next = queue.dequeue();
     }
 }
 

components/free_rtos/ethernet_industry/CoE/eth_ecat_pdo_fmmu.hpp

@@ -47,32 +47,7 @@ private:
     ecat_buffer::EcatBufferSlave& buffer_slave_;
 };
 
-/*
-// Функтор для обхода и передачи датаграмм в custom_tuple
-struct DatagramFunctor {
-    telegram::EcatTelegram& telegram;
-    size_t number_of_slaves;
-    datagram::IEcatDatagram *next;
 
-    void operator()(datagram::IEcatDatagram& datagram) {
-        if(next != nullptr) {
-           datagram + *next;
-        }
-
-        next = &datagram;
-    }
-
-    void operator()() {
-        if(next == nullptr) {
-            return;
-        }
-
-        do {
-            telegram.transfer(*next);
-        } while(next->get_all_wkc() < number_of_slaves);
-    }
-};
-*/
 class EthEcatPdoFMMU {
 public:
     EthEcatPdoFMMU(ecat_buffer::EthEcatBuffer& ecat_buffer): ecat_buffer_{ecat_buffer} { }
@@ -91,8 +66,7 @@ public:
 
         mutex_write_.lock();
 
-        custom_promise::IPromise& last_write = (*last_write_) >> promise;
-        last_write_ = &last_write;
+        queue_write_ + promise;
 
         mutex_write_.unlock();
 
@@ -108,8 +82,7 @@ public:
 
         mutex_read_.lock();
 
-        custom_promise::IPromise& last_read = (*last_read_) >> promise;
-        last_read_ = &last_read;
+        queue_read_ + promise;
 
         mutex_read_.unlock();
 
@@ -122,8 +95,7 @@ public:
     void pdo_write_async(custom_promise::IPromise& promise) {
         mutex_write_.lock();
 
-        custom_promise::IPromise& last_write = (*last_write_) >> promise;
-        last_write_ = &last_write;
+        queue_write_ + promise;
 
         mutex_write_.unlock();
     }
@@ -131,8 +103,7 @@ public:
     void pdo_read_async(custom_promise::IPromise& promise) {
         mutex_read_.lock();
 
-        custom_promise::IPromise& last_read = (*last_read_) >> promise;
-        last_read_ = &last_read;
+        queue_read_ + promise;
 
         mutex_read_.unlock();
     }
@@ -145,11 +116,8 @@ private:
     Mutex mutex_write_;
     Mutex mutex_read_;
 
-    custom_promise::WritePromise<> queue_write_{0};
-    custom_promise::ReadPromise<> queue_read_{0};
-
-    custom_promise::IPromise *last_write_{&queue_write_};
-    custom_promise::IPromise *last_read_{&queue_read_};
+    queue::Queue<custom_promise::IPromise> queue_write_;
+    queue::Queue<custom_promise::IPromise> queue_read_;
 
     uint32_t pdo_counter_{0};
 
@@ -207,55 +175,6 @@ private:
         } while(datagram_read.get_all_wkc() < 0x0001);
     }
 
-/*
-    // Запись PDO замапленных каждым слейвом
-    template<typename... DataTypes>
-    void write(DataTypes&... data) {
-        if(sizeof...(data) > pdo_fmmu_slaves_.size()) {
-            return;
-        }
-
-        size_t i = 0;
-        custom_tuple<datagram::EcatDatagram<command::LWR, DataTypes>...> datagram_tuple{pdo_fmmu_slaves_[i++].make_datagram_write(data)...};
-        DatagramFunctor functor{ecat_buffer_.get_ecat().get_telegram(), pdo_fmmu_slaves_.size(), nullptr};
-
-        for_each_reverse(datagram_tuple, functor);
-    }
-
-    // Чтение PDO замапленных каждым слейвом
-    template<typename... DataTypes>
-    void read(DataTypes&... data) {
-        if(sizeof...(data) > pdo_fmmu_slaves_.size()) {
-            return;
-        }
-
-        size_t i = 0;
-        custom_tuple<datagram::EcatDatagram<command::LRD, DataTypes>...> datagram_tuple{pdo_fmmu_slaves_[i++].make_datagram_read(data)...};
-        DatagramFunctor functor{ecat_buffer_.get_ecat().get_telegram(), pdo_fmmu_slaves_.size(), nullptr};
-
-        for_each_reverse(datagram_tuple, functor);
-    }
-
-    // Последовательность чтения-записи PDO замапленных каждым слейвом
-    template<typename... DataTypes>
-    void read_write(DataTypes&... data) {
-        if(sizeof...(data) > pdo_fmmu_slaves_.size()) {
-            return;
-        }
-
-        size_t i = 0;
-        size_t j = 0;
-
-        // custom_tuple с датаграммами и на чтение и на запись в две строки. 1 строка - тип, 2 строка - имя переменной
-        custom_tuple<datagram::EcatDatagram<command::LRD, DataTypes>... , datagram::EcatDatagram<command::LWR, DataTypes>...>
-        datagram_tuple{pdo_fmmu_slaves_[i++].make_datagram_read(data)... , pdo_fmmu_slaves_[j++].make_datagram_write(data)...};
-
-        DatagramFunctor functor{ecat_buffer_.get_ecat().get_telegram(), pdo_fmmu_slaves_.size(), nullptr};
-
-        for_each_reverse(datagram_tuple, functor);
-    }
-*/
-
 };
 
 } // namespace ecat_pdo_fmmu

components/free_rtos/ethernet_industry/CoE/eth_ecat_sdo_mailbox.hpp

@@ -353,7 +353,7 @@ public:
             sdo_read<TypeT, uint16_t>(telegram, pdo_map_index, pdo_map_subindex, pdo_block_index);
             pdo_map.block_index_map[pdo_map_index].emplace(pdo_map_subindex, pdo_block_index);
 
-            //DebugP_log("pdo_block_index = 0x02%x\r\n", pdo_block_index);
+            DebugP_log("pdo_block_index = 0x02%x\r\n", pdo_block_index);
 
             ecat_buffer::PDODescriptor tmp;
             uint8_t& pdo_block_object_count = tmp.size;

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_api.hpp

@@ -18,6 +18,17 @@
 
 namespace free_rtos {
 
+/*
+ * Порядок инициализации:
+ * Инициализировать и открыть драйвер eth_.Init(...), eth_.Open()
+ * Вызвать EthEcatApi::init(eth_);
+ * Создать служебный поток ecat_task_.Create(...) с вызовом EthEcatApi::process()
+ * Вызвать EthEcatApi::config_init(...)
+ * Создать пользовательский поток ecat_task_pdo_.Create(...)
+ * Для чтения/записи данных в пользовательском потоке вызвать
+ * pdo_write(...), pdo_read(...) или pdo_write_async(...), pdo_read_async(...)
+ */
+
 class EthEcatApi {
 public:
     static void init(Eth& eth);

components/free_rtos/ethernet_industry/eth_ecat_buffer.hpp

@@ -18,7 +18,7 @@ namespace free_rtos {
 
 enum {
     ECT_PDOOUTPUTOFFSET    = 0x1100, // 0x1100 write, output, rx buffer offset
-    ECT_PDOINPUTOFFSET     = 0x1400 // 0x1400, 0x1140 read, input, tx buffer offset
+    ECT_PDOINPUTOFFSET     = 0x1180 // 0x1400, 0x1140 read, input, tx buffer offset
 };
 
 enum {
@@ -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

@@ -22,54 +22,6 @@ namespace free_rtos {
 
 namespace custom_promise {
 
-/*
-// Функтор для обхода и упаковки датаграмм в custom_tuple
-struct DatagramPackFunctor : public PackFunctor {
-    DatagramPackFunctor(uint8_t *raw)
-    : PackFunctor{raw} { }
-
-    using PackFunctor::operator ();
-
-    template<typename CommandT, typename... DataTypes>
-    void operator()(datagram::EcatDatagram<CommandT, DataTypes...>& data) {
-        raw = data.pack(raw);
-    }
-
-    template<typename CommandT, typename... DataTypes>
-    void operator()(std::vector< datagram::EcatDatagram<CommandT, DataTypes...> >& data) {
-        for(uint16_t i = 1; i < data.size(); i++) {
-            raw = data[i - 1].pack(raw);
-        }
-
-        raw = data[data.size() - 1].pack(raw);
-    }
-};
-
-// Функтор для обхода и распаковки датаграмм в custom_tuple
-struct DatagramUnpackFunctor : public UnpackFunctor {
-    DatagramUnpackFunctor(uint8_t *raw)
-    : UnpackFunctor{raw} { }
-
-    using UnpackFunctor::operator ();
-
-    template<typename CommandT, typename... DataTypes>
-    void operator()(datagram::EcatDatagram<CommandT, DataTypes...>& data) {
-        raw = data.unpack(raw);
-    }
-
-    template<typename CommandT, typename... DataTypes>
-    void operator()(std::vector< datagram::EcatDatagram<CommandT, DataTypes...> >& data) {
-        for(uint16_t i = 1; i < data.size(); i++) {
-            data[i - 1] + data[i];
-
-            raw = data[i - 1].unpack(raw);
-        }
-
-        raw = data[data.size() - 1].unpack(raw);
-    }
-};
-*/
-
 template<typename... DataTypes>
 class Future {
 public:
@@ -113,35 +65,15 @@ private:
 
 class IPromise {
 public:
+    friend class queue::Queue<IPromise>;
+
     IPromise(address::Offset offset = 0)
     : offset_{offset} { }
-
-    IPromise* get_next() {
-        queue::QueueEntity<IPromise>* next = queue_entity_.get_next();
-
-        if(next == nullptr) {
-            return nullptr;
-        }
-
-        return next->get_data();
+/*
+    queue::Queue<IPromise> operator+(IPromise &next) {
+        return queue::Queue<IPromise>{queue_entity_, next.queue_entity_};
     }
-
-    queue::QueueEntity<IPromise>& get_queue_entity() {
-        return queue_entity_;
-    }
-
-    IPromise& operator+(IPromise &next) {
-        queue_entity_ + next.get_queue_entity();
-
-        return next;
-    }
-
-    IPromise& operator>>(IPromise &next) {
-        queue_entity_ >> next.get_queue_entity();
-
-        return next;
-    }
-
+*/
     virtual void set_value(uint8_t* process_data, uint32_t len) = 0;
 
 protected:

components/free_rtos/ethernet_industry/eth_ecat_custom_tuple.hpp

@@ -114,14 +114,12 @@ struct each_tuple_element< FunctorT, custom_tuple<> > {
 template<typename TupleT, typename FunctorT>
 void for_each(TupleT& t, FunctorT& functor) {
     each_tuple_element<FunctorT, TupleT>::for_each(functor, t);
-    functor();
 }
 
 // Функция для обратного обхода элементов custom_tuple
 template<typename TupleT, typename FunctorT>
 void for_each_reverse(TupleT& t, FunctorT& functor) {
     each_tuple_element<FunctorT, TupleT>::for_each_reverse(functor, t);
-    functor();
 }
 
 }

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,24 +46,10 @@ public:
         return next->get_data();
     }
 
-    queue::QueueEntity<IEcatDatagram>& get_queue_entity() {
-        return queue_entity_;
-    }
-
-    IEcatDatagram& operator+(IEcatDatagram &next) {
+    queue::Queue<IEcatDatagram> operator+(IEcatDatagram &next) {
         more_ = ec_moredatagrams::EC_MOREDATAGRAMS_MORE;
 
-        queue_entity_ + next.get_queue_entity();
-
-        return next;
-    }
-
-    IEcatDatagram& operator>>(IEcatDatagram &next) {
-        more_ = ec_moredatagrams::EC_MOREDATAGRAMS_MORE;
-
-        queue_entity_ >> next.get_queue_entity();
-
-        return next;
+        return queue::Queue<IEcatDatagram>{queue_entity_, next.queue_entity_};
     }
 
     virtual uint8_t* pack(uint8_t *raw) = 0;
@@ -85,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_packer.hpp

@@ -29,8 +29,6 @@ struct PackFunctorBase {
 
         //DebugP_log((char*)"Data packed: %d\r\n", sizeof(DataT));
     }
-
-    void operator()() { }
 };
 
 // Функтор для обхода и упаковки элементов custom_tuple

components/free_rtos/ethernet_industry/eth_ecat_queue.hpp

@@ -14,9 +14,14 @@ namespace free_rtos {
 
 namespace queue {
 
+template<typename DataType>
+class Queue;
+
 template<typename DataType>
 class QueueEntity {
 public:
+    friend class Queue<DataType>;
+
     QueueEntity(DataType *data)
     : data_{data} { }
 
@@ -28,67 +33,97 @@ public:
         return next_;
     }
 
+private:
+    DataType *data_{nullptr};
+    QueueEntity *next_{nullptr};
+};
+
+template<typename DataType>
+class Queue {
+public:
+    Queue() { }
+
+    explicit Queue(QueueEntity<DataType>& first)
+    : first_{&first}
+    , last_{&first} { }
+
+    Queue(QueueEntity<DataType>& first, QueueEntity<DataType>& next)
+    : first_{&first}
+    , last_{&next} {
+        first.next_ = &next;
+    }
+
+    QueueEntity<DataType>* get_first() {
+        return first_;
+    }
+
+    QueueEntity<DataType>* get_last() {
+        return last_;
+    }
+
     size_t get_size() {
         return size_;
     }
 
-    QueueEntity& operator+(QueueEntity& next) {
-        append(next);
-        //set_next(next);
-
-        return next;
+    Queue& operator+(DataType& data) {
+        return append(&data.queue_entity_);
     }
 
-    QueueEntity& operator>>(QueueEntity& next) {
-        attach(next);
+    Queue& operator+(Queue& other) {
+        return append(other.get_first(), other.get_last(), other.get_size());
+    }
 
-        return next;
+    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);
+    }
+
+    void clear() {
+        first_ = nullptr;
+        last_ = nullptr;
+        size_ = 0;
     }
 
 private:
-    DataType *data_{nullptr};
+    QueueEntity<DataType> *first_{nullptr};
+    QueueEntity<DataType> *last_{nullptr};
+    size_t size_{0};
 
-    QueueEntity *next_{nullptr};
-    QueueEntity *first_{this};
-    QueueEntity *last_{this};
-
-    size_t size_{1};
-
-    void set_next(QueueEntity &next) {
-        next_ = &next;
-    }
-
-    QueueEntity* get_last() {
-        return last_;
-    }
-
-    void set_first(QueueEntity* first) {
-        first_ = first;
-    }
-
-    QueueEntity* append(QueueEntity& next) {
-        if(this != first_) {
-            first_ = first_->append(next);
-        }else{
-            last_->set_next(next);
-            last_ = next.get_last();
-
-            next.set_first(first_);
-
-            size_++;
+    Queue& append(QueueEntity<DataType> *other_first, QueueEntity<DataType> *other_last = nullptr, size_t other_size = 1) {
+        if(first_ == nullptr) {
+            first_ = other_first;
         }
 
-        return first_;
-    }
-
-    QueueEntity* attach(QueueEntity& next) {
-        if(this != first_) {
-            first_ = first_->attach(next);
-        }else{
-            last_->set_next(next);
+        if(last_ != nullptr) {
+            last_->next_ = other_first;
         }
 
-        return first_;
+        if(other_last == nullptr) {
+            last_ = other_first;
+        }else{
+            last_ = other_last;
+        }
+
+        size_ += other_size;
+
+        return *this;
     }
 };
 

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