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

components/free_rtos/ethernet_industry/CoE/eth_ecat_pdo_fmmu.hpp

@@ -47,7 +47,32 @@ 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} { }
@@ -66,7 +91,8 @@ public:
 
         mutex_write_.lock();
 
-        queue_write_ + promise;
+        custom_promise::IPromise& last_write = (*last_write_) >> promise;
+        last_write_ = &last_write;
 
         mutex_write_.unlock();
 
@@ -82,7 +108,8 @@ public:
 
         mutex_read_.lock();
 
-        queue_read_ + promise;
+        custom_promise::IPromise& last_read = (*last_read_) >> promise;
+        last_read_ = &last_read;
 
         mutex_read_.unlock();
 
@@ -95,7 +122,8 @@ public:
     void pdo_write_async(custom_promise::IPromise& promise) {
         mutex_write_.lock();
 
-        queue_write_ + promise;
+        custom_promise::IPromise& last_write = (*last_write_) >> promise;
+        last_write_ = &last_write;
 
         mutex_write_.unlock();
     }
@@ -103,7 +131,8 @@ public:
     void pdo_read_async(custom_promise::IPromise& promise) {
         mutex_read_.lock();
 
-        queue_read_ + promise;
+        custom_promise::IPromise& last_read = (*last_read_) >> promise;
+        last_read_ = &last_read;
 
         mutex_read_.unlock();
     }
@@ -116,8 +145,11 @@ private:
     Mutex mutex_write_;
     Mutex mutex_read_;
 
-    queue::Queue<custom_promise::IPromise> queue_write_;
-    queue::Queue<custom_promise::IPromise> queue_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_};
 
     uint32_t pdo_counter_{0};
 
@@ -175,6 +207,55 @@ 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,8 +179,6 @@ 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,17 +18,6 @@
 
 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     = 0x1180 // 0x1400, 0x1140 read, input, tx buffer offset
+    ECT_PDOINPUTOFFSET     = 0x1400 // 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_);
 
-        auto queue = datagram_write + datagram_read;
+        datagram_write + datagram_read;
 
         do {
             telegram.transfer(datagram_write);

components/free_rtos/ethernet_industry/eth_ecat_custom_promise.hpp

@@ -22,6 +22,54 @@ 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:
@@ -65,15 +113,35 @@ private:
 
 class IPromise {
 public:
-    friend class queue::Queue<IPromise>;
-
     IPromise(address::Offset offset = 0)
     : offset_{offset} { }
-/*
-    queue::Queue<IPromise> operator+(IPromise &next) {
-        return queue::Queue<IPromise>{queue_entity_, next.queue_entity_};
+
+    IPromise* get_next() {
+        queue::QueueEntity<IPromise>* next = queue_entity_.get_next();
+
+        if(next == nullptr) {
+            return nullptr;
+        }
+
+        return next->get_data();
     }
-*/
+
+    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,12 +114,14 @@ 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,8 +25,6 @@ using TEcatWkc = uint16_t;
 
 class IEcatDatagram {
 public:
-    friend class queue::Queue<IEcatDatagram>;
-
     IEcatDatagram(ec_moredatagrams more, TEcatWkc wkc)
     : more_{more}
     , wkc_{wkc}
@@ -46,10 +44,24 @@ public:
         return next->get_data();
     }
 
-    queue::Queue<IEcatDatagram> operator+(IEcatDatagram &next) {
+    queue::QueueEntity<IEcatDatagram>& get_queue_entity() {
+        return queue_entity_;
+    }
+
+    IEcatDatagram& operator+(IEcatDatagram &next) {
         more_ = ec_moredatagrams::EC_MOREDATAGRAMS_MORE;
 
-        return queue::Queue<IEcatDatagram>{queue_entity_, next.queue_entity_};
+        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;
     }
 
     virtual uint8_t* pack(uint8_t *raw) = 0;
@@ -73,15 +85,13 @@ 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,6 +29,8 @@ 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,14 +14,9 @@ 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} { }
 
@@ -33,97 +28,67 @@ 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_;
     }
 
-    Queue& operator+(DataType& data) {
-        return append(&data.queue_entity_);
+    QueueEntity& operator+(QueueEntity& next) {
+        append(next);
+        //set_next(next);
+
+        return next;
     }
 
-    Queue& operator+(Queue& other) {
-        return append(other.get_first(), other.get_last(), other.get_size());
-    }
+    QueueEntity& operator>>(QueueEntity& next) {
+        attach(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;
+        return next;
     }
 
 private:
-    QueueEntity<DataType> *first_{nullptr};
-    QueueEntity<DataType> *last_{nullptr};
-    size_t size_{0};
+    DataType *data_{nullptr};
 
-    Queue& append(QueueEntity<DataType> *other_first, QueueEntity<DataType> *other_last = nullptr, size_t other_size = 1) {
-        if(first_ == nullptr) {
-            first_ = other_first;
-        }
+    QueueEntity *next_{nullptr};
+    QueueEntity *first_{this};
+    QueueEntity *last_{this};
 
-        if(last_ != nullptr) {
-            last_->next_ = other_first;
-        }
+    size_t size_{1};
 
-        if(other_last == nullptr) {
-            last_ = other_first;
+    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_ = other_last;
+            last_->set_next(next);
+            last_ = next.get_last();
+
+            next.set_first(first_);
+
+            size_++;
         }
 
-        size_ += other_size;
+        return first_;
+    }
 
-        return *this;
+    QueueEntity* attach(QueueEntity& next) {
+        if(this != first_) {
+            first_ = first_->attach(next);
+        }else{
+            last_->set_next(next);
+        }
+
+        return first_;
     }
 };
 

components/free_rtos/ethernet_industry/eth_ecat_telegram.cpp

@@ -46,16 +46,14 @@ 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 = queue.dequeue();
+        next = next->get_next();
     }
 
     p_hdr->bits.length = p_datagram_last - p_datagram_first;
@@ -68,6 +66,7 @@ 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");
@@ -81,19 +80,16 @@ 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 = queue.dequeue();
+        next = next->get_next();
     }
 
     return p_datagram_last;
 }
 
-void EcatTelegram::transfer(datagram::IEcatDatagram& next) {
-    datagram_queue_ + next;
+void EcatTelegram::transfer(datagram::IEcatDatagram& first) {
+    datagram_queue_ = &first;
 
     bool stat = eth_stack_.send_pkt(port_id_, ETH_PROT_ECAT_LE, 1);
 
@@ -105,25 +101,9 @@ void EcatTelegram::transfer(datagram::IEcatDatagram& next) {
 
     rx_sem_.pend();
 
-    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();
-}
-*/
+    datagram_queue_ = nullptr;
+}
+
 }
 
 }

components/free_rtos/ethernet_industry/eth_ecat_telegram.hpp

@@ -30,8 +30,7 @@ public:
         eth_.getEthStackPtr()->Register(ETH_PROT_ECAT_LE, this);
     }
 
-    void transfer(datagram::IEcatDatagram& next);
-    //void transfer(queue::Queue<datagram::IEcatDatagram>& next);
+    void transfer(datagram::IEcatDatagram& first);
 
 private:
     Eth& eth_;
@@ -40,7 +39,7 @@ private:
 
     free_rtos::Semaphore rx_sem_;
 
-    queue::Queue<datagram::IEcatDatagram> datagram_queue_;
+    datagram::IEcatDatagram *datagram_queue_{nullptr};
 
     uint8_t* pack(uint8_t *raw);
     uint8_t* unpack(uint8_t *raw);