fix(UML-1582): Багфиксы

components/free_rtos/ethernet_industry/CoE/eth_ecat_pdo_fmmu.cpp

@@ -101,6 +101,7 @@ void EthEcatPdoFMMU::process() {
 
     while(1) {
         ecat_timer.Wait();
+        //DebugP_log("Tick !\r\n");
 
         read(0, process_data);
 /*

components/free_rtos/ethernet_industry/eth_ecat.cpp

@@ -94,7 +94,7 @@ void EthEcat::set_slaves_to_default() {
     uint8_t m_data_out{0x00};
     datagram::EcatDatagram<command::BWR, uint8_t> m{ {{broadcast, ECT_REG_EEPCFG}}, m_data_out };
 
-    a + b + c + d + e + f + g + h + i + j + k + l + m;
+    auto queue = a + b + c + d + e + f + g + h + i + j + k + l + m;
 
     do {
         telegram_.transfer(a);
@@ -156,15 +156,20 @@ void EthEcat::set_addresses_of_slaves(uint16_t number_of_slaves, uint16_t addres
         slaves_.emplace_back(EcatSlave{std::move(slave_addresses)});
 
         datagrams.emplace_back(TDatagram{ {{position, ECT_REG_STADR}},  slaves_.back().get_slave_address<command::FP>() });
+        datagrams.back().update();
     }
 
-    for(uint16_t i = 1; i < number_of_slaves; i++) {
+    queue::Queue<datagram::IEcatDatagram> queue;
-        datagrams[i - 1] + datagrams[i];
+
+    for(uint16_t i = 0; i < number_of_slaves; i++) {
+        queue + datagrams[i];
     }
 
     do {
         telegram_.transfer(datagrams[0]);
     } while(datagrams[0].get_all_wkc() < number_of_slaves);
+
+    DebugP_log((char*)"Slave addresses set\r\n");
 }
 
 void EthEcat::get_addresses_of_slaves() {
@@ -177,12 +182,13 @@ void EthEcat::get_addresses_of_slaves() {
 
     for(EcatSlave& slave : slaves_) {
         datagrams.emplace_back(TDatagram{ {{slave.get_slave_address<command::AP>(), ECT_REG_STADR}}, slave.get_slave_address<command::FP>() });
+        datagrams.back().update();
     }
 
-    queue::Queue<TDatagram> queue;
+    queue::Queue<datagram::IEcatDatagram> queue;
 
-    for(uint16_t i = 1; i < number_of_slaves; i++) {
+    for(uint16_t i = 0; i < number_of_slaves; i++) {
-        datagrams[i - 1] + datagrams[i];
+        queue + datagrams[i];
     }
 
     do {

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 {

components/free_rtos/ethernet_industry/eth_ecat_datagram.hpp

@@ -27,19 +27,22 @@ class IEcatDatagram {
 public:
     friend class queue::Queue<IEcatDatagram>;
 
-    IEcatDatagram(ec_moredatagrams more, TEcatWkc wkc)
+    IEcatDatagram()
-    : more_{more}
+    : queue_entity_{this}
-    , wkc_{wkc}
     { }
 
-    IEcatDatagram() { }
-
     virtual ~IEcatDatagram() { };
 
+    void update() {
+        queue_entity_.set_data(this);
+    }
+
     IEcatDatagram* get_next() {
         queue::QueueEntity<IEcatDatagram>* next = queue_entity_.get_next();
 
         if(next == nullptr) {
+            //DebugP_log((char*)"No next datagram\r\n");
+
             return nullptr;
         }
 
@@ -47,7 +50,7 @@ public:
     }
 
     queue::Queue<IEcatDatagram> operator+(IEcatDatagram &next) {
-        more_ = ec_moredatagrams::EC_MOREDATAGRAMS_MORE;
+        //DebugP_log((char*)"Coupling first pair\r\n");
 
         return queue::Queue<IEcatDatagram>{queue_entity_, next.queue_entity_};
     }
@@ -73,13 +76,11 @@ public:
         return wkc_ + next->get_data()->get_all_wkc();
     }
 
-    queue::QueueEntity<IEcatDatagram> queue_entity_{this};
-
 protected:
-    ec_moredatagrams more_;
     TEcatDgHeader header_;
-    TEcatWkc wkc_;
+    TEcatWkc wkc_{0x0000};
 
+    queue::QueueEntity<IEcatDatagram> queue_entity_;
 private:
 
 };
@@ -90,7 +91,7 @@ class EcatDatagram : public IEcatDatagram {
 
 public:
     EcatDatagram(CommandT&& command, DataTypes&... data)
-    : IEcatDatagram{ec_moredatagrams::EC_MOREDATAGRAMS_LAST, 0x0000}
+    : IEcatDatagram{}
     , command_{command}
     , data_tuple_{data...} { }
 
@@ -127,9 +128,25 @@ private:
     }
 
     uint8_t* pack_header(uint8_t *raw) {
-        uint8_t *data_raw = raw + sizeof(TEcatDgHeader);
+        uint8_t *data_raw = raw + sizeof(TEcatDgHeader);/*
+    address::Station station_0{static_cast<uint16_t>(address_base + 0)};
+    address::Station station_1{static_cast<uint16_t>(address_base + 1)};
+
+    TDatagram datagram_0{ {{address::Position{0}, ECT_REG_STADR}},   station_0};
+    TDatagram datagram_1{ {{address::Position{-1}, ECT_REG_STADR}},  station_1 };
+
+    queue = datagram_0 + datagram_1;
+*/
         uint8_t *wkc_raw = pack_data(data_raw); // сначала упаковываем  все данные для вычисления их размера
         uint16_t len = wkc_raw - data_raw;      // вычисляем размер данных
+        ec_moredatagrams more;
+
+        if(queue_entity_.get_next() != nullptr) {
+            more = ec_moredatagrams::EC_MOREDATAGRAMS_MORE;
+        }else{
+            more = ec_moredatagrams::EC_MOREDATAGRAMS_LAST;
+        }
+
         TEcatDgHeader *header_ = new(raw) TEcatDgHeader{
             command_.get_cmd(),
             0x00,
@@ -137,7 +154,7 @@ private:
             {.bits = {
                       .len = len,
                       .c = static_cast<uint16_t>(ec_circframe::EC_CIRCFRAME_NOT),
-                      .m = static_cast<uint16_t>(more_)}},
+                      .m = static_cast<uint16_t>(more)}},
             0x0000};
 
         (void)header_;

components/free_rtos/ethernet_industry/eth_ecat_queue.hpp

@@ -25,6 +25,10 @@ public:
     QueueEntity(DataType *data)
     : data_{data} { }
 
+    void set_data(DataType* data) {
+        data_ = data;
+    }
+
     DataType* get_data() {
         return data_;
     }
@@ -38,19 +42,24 @@ private:
     QueueEntity *next_{nullptr};
 };
 
+
 template<typename DataType>
 class Queue {
 public:
     Queue() { }
 
-    explicit Queue(QueueEntity<DataType>& first)
+    explicit Queue(QueueEntity<DataType>& first) {
-    : first_{&first}
+        //DebugP_log((char*)"Constructor coupling first data\r\n");
-    , last_{&first} { }
+
+        append(&first);
+    }
+
+    Queue(QueueEntity<DataType>& first, QueueEntity<DataType>& next) {
+        //DebugP_log((char*)"Constructor coupling first and next data\r\n");
 
-    Queue(QueueEntity<DataType>& first, QueueEntity<DataType>& next)
-    : first_{&first}
-    , last_{&next} {
         first.next_ = &next;
+
+        append(&first, &next, 2);
     }
 
     QueueEntity<DataType>* get_first() {
@@ -66,10 +75,14 @@ public:
     }
 
     Queue& operator+(DataType& data) {
+        //DebugP_log((char*)"Coupling next data\r\n");
+
         return append(&data.queue_entity_);
     }
 
     Queue& operator+(Queue& other) {
+        //DebugP_log((char*)"Coupling with other queue\r\n");
+
         return append(other.get_first(), other.get_last(), other.get_size());
     }
 
@@ -123,6 +136,8 @@ private:
 
         size_ += other_size;
 
+        //DebugP_log((char*)"size_ = %d\r\n", size_);
+
         return *this;
     }
 };

components/free_rtos/ethernet_industry/eth_ecat_telegram.cpp

@@ -52,10 +52,20 @@ uint8_t* EcatTelegram::pack(uint8_t *raw) {
 
     queue::Queue<datagram::IEcatDatagram> queue = datagram_queue_;
     auto next = queue.dequeue();
+/*
+    if(datagram_queue_ == nullptr) {
+        DebugP_log((char*)"Warning ! Empty queue !\r\n");
 
+        return p_datagram_last;
+    }
+    auto next = datagram_queue_;
+*/
     while(next != nullptr) {
+        //DebugP_log((char*)"Packet packed\r\n");
+
         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;
@@ -83,10 +93,20 @@ uint8_t* EcatTelegram::unpack(uint8_t *raw) {
 
     queue::Queue<datagram::IEcatDatagram> queue = datagram_queue_;
     auto next = queue.dequeue();
+/*
+    if(datagram_queue_ == nullptr) {
+        DebugP_log((char*)"Warning ! Empty queue !\r\n");
 
+        return p_datagram_last;
+    }
+    auto next = datagram_queue_;
+*/
     while(next != nullptr) {
+        //DebugP_log((char*)"Packet unpacked\r\n");
+
         p_datagram_last = next->unpack(p_datagram_last);
         next = queue.dequeue();
+        //next = next->get_next();
     }
 
     return p_datagram_last;
@@ -94,6 +114,7 @@ uint8_t* EcatTelegram::unpack(uint8_t *raw) {
 
 void EcatTelegram::transfer(datagram::IEcatDatagram& next) {
     datagram_queue_ + next;
+    //datagram_queue_ = &next;
 
     bool stat = eth_stack_.send_pkt(port_id_, ETH_PROT_ECAT_LE, 1);
 
@@ -106,6 +127,7 @@ void EcatTelegram::transfer(datagram::IEcatDatagram& next) {
     rx_sem_.pend();
 
     datagram_queue_.clear();
+    //datagram_queue_ = nullptr;
 }
 /*
 void EcatTelegram::transfer(queue::Queue<datagram::IEcatDatagram>& next) {

components/free_rtos/ethernet_industry/eth_ecat_telegram.hpp

@@ -41,6 +41,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);