dev(SF-33): Добавлен контроль соединения

components/free_rtos/ethernet_industry/CoE/eth_ecat_pdo_fmmu.cpp

@@ -25,7 +25,7 @@ void EthEcatPdoFMMU::init(ProcessCallback callback) {
     callback_ = callback;
 }
 
-void EthEcatPdoFMMU::wait_op() {
+bool EthEcatPdoFMMU::wait_op() {
     free_rtos::Semaphore& init_sem = ecat_buffer_.get_ecat().get_init_sem();
     free_rtos::Semaphore& process_sem = ecat_buffer_.get_ecat().get_process_sem();
 
@@ -37,35 +37,34 @@ void EthEcatPdoFMMU::wait_op() {
     std::vector<uint8_t> pdo_write(logical_full_length_write, 0x00);
     custom_tuple<std::vector<uint8_t>&> data_tuple_read{pdo_read};
     custom_tuple<std::vector<uint8_t>&> data_tuple_write{pdo_write};
+    bool status;
 
-    read(0, pdo_write); // read to pdo_write !
+    status = read(0, pdo_write); // read to pdo_write !
 
-/*
+    if(status != true) {
-    for(uint8_t& byte : process_data) {
+        return status;
-        DebugP_log("0x%01x", byte);
+    }
+
+    status = write(0, pdo_write);
+
+    if(status != true) {
+        return status;
     }
-    DebugP_log("\r\n");
-*/
-    write(0, pdo_write);
 
     init_sem.post();
     process_sem.pend();
 
     for(uint32_t i = 0; i < 250; i++) {
-        //ClockP_usleep(125);
+        status = read_write(0, data_tuple_read, 0, data_tuple_write);
-        read_write(0, data_tuple_read, 0, data_tuple_write);
 
-        //read(0, process_data);
+        if(status != true) {
-/*
+            return status;
-        for(uint8_t& byte : process_data) {
-            DebugP_log("0x%01x", byte);
         }
-        DebugP_log("\r\n");
-*/
-        //write(0, process_data);
     }
 
     init_sem.post();
+
+    return status;
 }
 
 void EthEcatPdoFMMU::process_write_queue(uint8_t* process_data, uint32_t len) {
@@ -128,7 +127,13 @@ void EthEcatPdoFMMU::process_read_queue(uint8_t* process_data, uint32_t len) {
 }
 
 void EthEcatPdoFMMU::process() {
-    wait_op();
+    bool status;
+
+    status = wait_op();
+
+    if(status != true) {
+        return;
+    }
 
     uint32_t logical_full_length_write = ecat_buffer_.get_fmmu_global_properties().logical_full_length_write;
     uint32_t logical_full_length_read = ecat_buffer_.get_fmmu_global_properties().logical_full_length_read;
@@ -140,16 +145,27 @@ void EthEcatPdoFMMU::process() {
 
     ecat_timer.Wait();
 
-    read(0, pdo_write); // read to pdo_write !
+    status = read(0, pdo_write); // read to pdo_write !
-    write(0, pdo_write);
+
+    if(status != true) {
+        return;
+    }
+
+    status = write(0, pdo_write);
+
+    if(status != true) {
+        return;
+    }
 
     while(1) {
         ecat_timer.Wait();
         //DebugP_log("Tick !\r\n");
 
-        read_write(0, data_tuple_read, 0, data_tuple_write);
+        status = read_write(0, data_tuple_read, 0, data_tuple_write);
 
-        //read(0, process_data);
+        if(status != true) {
+            return;
+        }
 
         #if PROFILE_LEVEL > 0
         {
@@ -191,8 +207,6 @@ void EthEcatPdoFMMU::process() {
         process_write_queue(pdo_write.data(), pdo_write.size());
         #endif
 
-        //write(0, process_data);
-
         pdo_counter_++;
     }
 }

components/free_rtos/ethernet_industry/CoE/eth_ecat_pdo_fmmu.hpp

@@ -141,58 +141,55 @@ private:
 
     uint32_t pdo_counter_{0};
 
-    void wait_op();
+    bool wait_op();
 
     void process_write_queue(uint8_t* process_data, uint32_t len);
     void process_read_queue(uint8_t* process_data, uint32_t len);
 
     template<typename... DataTypes>
-    void write(address::Offset offset, DataTypes&... data) {
+    bool write(address::Offset offset, DataTypes&... data) {
         using TDatagram = datagram::EcatDatagram<command::LWR, DataTypes...>;
 
         telegram::EcatTelegram& telegram = ecat_buffer_.get_ecat().get_telegram();
         ecat_buffer::FMMUGlobalProperties& fmmu_global_properties = ecat_buffer_.get_fmmu_global_properties();
         address::Logical logical = fmmu_global_properties.logical_start_address + offset;
-        TDatagram datagram{ {{logical}}, data... };
+        const datagram::TEcatWkc expected_wkc = pdo_fmmu_slaves_.size();
+        TDatagram datagram{ {{logical}}, expected_wkc, data... };
 
-        do {
+        return telegram.transfer(datagram);
-            telegram.transfer(datagram);
-        } while(datagram.get_wkc() < 0x0001);
     }
 
     template<typename... DataTypes>
-    void read(address::Offset offset, DataTypes&... data) {
+    bool read(address::Offset offset, DataTypes&... data) {
         using TDatagram = datagram::EcatDatagram<command::LRD, DataTypes...>;
 
         telegram::EcatTelegram& telegram = ecat_buffer_.get_ecat().get_telegram();
         ecat_buffer::FMMUGlobalProperties& fmmu_global_properties = ecat_buffer_.get_fmmu_global_properties();
         address::Logical logical = fmmu_global_properties.logical_start_address + fmmu_global_properties.logical_full_length_write + offset;
-        TDatagram datagram{ {{logical}}, data... };
+        const datagram::TEcatWkc expected_wkc = pdo_fmmu_slaves_.size();
+        TDatagram datagram{ {{logical}}, expected_wkc, data... };
 
-        do {
+        return telegram.transfer(datagram);
-            telegram.transfer(datagram);
-        } while(datagram.get_wkc() < 0x0001);
     }
 
     template<typename... DataTypes>
-    void read_write(address::Offset offset_read, custom_tuple<DataTypes&...> data_read, address::Offset offset_write, custom_tuple<DataTypes&...> data_write) {
+    bool read_write(address::Offset offset_read, custom_tuple<DataTypes&...> data_read, address::Offset offset_write, custom_tuple<DataTypes&...> data_write) {
         using TDatagramRead = datagram::EcatDatagram<command::LRD, DataTypes...>;
         using TDatagramWrite = datagram::EcatDatagram<command::LWR, DataTypes...>;
 
         telegram::EcatTelegram& telegram = ecat_buffer_.get_ecat().get_telegram();
         ecat_buffer::FMMUGlobalProperties& fmmu_global_properties = ecat_buffer_.get_fmmu_global_properties();
+        const datagram::TEcatWkc expected_wkc = pdo_fmmu_slaves_.size();
 
         address::Logical logical_read = fmmu_global_properties.logical_start_address + fmmu_global_properties.logical_full_length_write + offset_read;
-        TDatagramRead datagram_read{ {{logical_read}}, data_read };
+        TDatagramRead datagram_read{ {{logical_read}}, expected_wkc, data_read };
 
         address::Logical logical_write = fmmu_global_properties.logical_start_address + offset_write;
-        TDatagramWrite datagram_write{ {{logical_write}}, data_write };
+        TDatagramWrite datagram_write{ {{logical_write}}, expected_wkc, data_write };
 
         auto queue = datagram_read + datagram_write;
 
-        do {
+        return telegram.transfer(queue);
-            telegram.transfer(queue);
-        } while(datagram_read.get_all_wkc() < 0x0001);
     }
 
 };

components/free_rtos/ethernet_industry/CoE/eth_ecat_sdo_mailbox.cpp

@@ -22,8 +22,10 @@ void EthEcatSdoMailbox::init() {
     }
 }
 
-void EthEcatSdoMailbox::read_pdo_map(uint16_t pdo_map_rx_index, uint16_t pdo_map_tx_index) {
+bool EthEcatSdoMailbox::read_pdo_map(uint16_t pdo_map_rx_index, uint16_t pdo_map_tx_index) {
+    const datagram::TEcatWkc expected_wkc = 1;
     telegram::EcatTelegram& telegram = ecat_buffer_.get_ecat().get_telegram();
+    bool status = true;
 
     for(EcatSdoMailboxSlave& sdo_mailbox_slave : sdo_mailbox_slaves_) {
         uint16_t pdo_rx_data_size{0x0000};
@@ -31,16 +33,15 @@ void EthEcatSdoMailbox::read_pdo_map(uint16_t pdo_map_rx_index, uint16_t pdo_map
         ecat_buffer::PDOMap pdo_map;
 
         DebugP_log((char*)"Reading rx pdo map\r\n");
-        pdo_rx_data_size = sdo_mailbox_slave.pdo_map_read<command::FP>(telegram, pdo_map, pdo_map_rx_index);
+        status = (status && sdo_mailbox_slave.pdo_map_read<command::FP>(telegram, expected_wkc, pdo_map, pdo_map_rx_index));
 
         DebugP_log((char*)"Reading tx pdo map\r\n");
-        pdo_tx_data_size = sdo_mailbox_slave.pdo_map_read<command::FP>(telegram, pdo_map, pdo_map_tx_index);
+        status = (status && sdo_mailbox_slave.pdo_map_read<command::FP>(telegram, expected_wkc, pdo_map, pdo_map_tx_index));
 
         pdo_map_.emplace_back(std::move(pdo_map));
-
-        DebugP_log((char*)"pdo_rx_data_size = %d\r\n", pdo_rx_data_size);
-        DebugP_log((char*)"pdo_tx_data_size = %d\r\n", pdo_tx_data_size);
     }
+
+    return status;
 }
 
 }

components/free_rtos/ethernet_industry/CoE/eth_ecat_sdo_mailbox.hpp

@@ -147,45 +147,49 @@ public:
     : buffer_slave_(mailbox_slave) { }
 
     template<typename TypeT>
-    void wait_available(telegram::EcatTelegram& telegram) {
+    bool wait_available(telegram::EcatTelegram& telegram, const datagram::TEcatWkc expected_wkc) {
         using TCommand = command::EcatCommand<TypeT, command::RD>;
 
         auto slave_address = buffer_slave_.get_slave().get_slave_address<TypeT>();
         std::array<address::Offset, 4>& buffer_regs = buffer_slave_.get_buffer_regs();
         uint8_t sm_status{0x00};
-        datagram::EcatDatagram<TCommand, uint8_t> datagram{ {{slave_address, buffer_regs[MailboxesRegs::AVAILABLE]}}, sm_status };
+        datagram::EcatDatagram<TCommand, uint8_t> datagram{ {{slave_address, buffer_regs[MailboxesRegs::AVAILABLE]}}, expected_wkc, sm_status };
+        bool status;
 
         do {
-            telegram.transfer(datagram);
+            status = telegram.transfer(datagram);
-        } while((datagram.get_wkc() < 0x0001) || ((sm_status & 0x08) == 0));
+        } while((status == true) && ((sm_status & 0x08) == 0));
+
+        return status;
     }
 
     template<typename TypeT>
-    void wait_empty(telegram::EcatTelegram& telegram) {
+    bool wait_empty(telegram::EcatTelegram& telegram, const datagram::TEcatWkc expected_wkc) {
         using TCommand = command::EcatCommand<TypeT, command::RD>;
 
         auto slave_address = buffer_slave_.get_slave().get_slave_address<TypeT>();
         std::array<address::Offset, 4>& buffer_regs = buffer_slave_.get_buffer_regs();
         uint8_t sm_status{0x00};
-        datagram::EcatDatagram<TCommand, uint8_t> datagram{ {{slave_address, buffer_regs[MailboxesRegs::EMPTY]}}, sm_status };
+        datagram::EcatDatagram<TCommand, uint8_t> datagram{ {{slave_address, buffer_regs[MailboxesRegs::EMPTY]}}, expected_wkc, sm_status };
+        bool status;
 
         do {
-            telegram.transfer(datagram);
+            status = telegram.transfer(datagram);
-        } while((datagram.get_wkc() < 0x0001) || ((sm_status & 0x08) != 0));
+        } while((status == true) && ((sm_status & 0x08) != 0));
+
+        return status;
     }
 
     template<typename TypeT>
-    void empty(telegram::EcatTelegram& telegram) {
+    bool empty(telegram::EcatTelegram& telegram, const datagram::TEcatWkc expected_wkc) {
         using TCommand = command::EcatCommand<TypeT, command::RD>;
 
         auto slave_address = buffer_slave_.get_slave().get_slave_address<TypeT>();
         std::array<address::Offset, 4>& buffer_regs = buffer_slave_.get_buffer_regs();
         uint8_t sm_status{0x00};
-        datagram::EcatDatagram<TCommand, uint8_t> datagram{ {{slave_address, buffer_regs[MailboxesRegs::AVAILABLE]}}, sm_status };
+        datagram::EcatDatagram<TCommand, uint8_t> datagram{ {{slave_address, buffer_regs[MailboxesRegs::AVAILABLE]}}, expected_wkc, sm_status };
 
-        do {
+        return telegram.transfer(datagram);
-            telegram.transfer(datagram);
-        } while(datagram.get_wkc() < 0x0001);
     }
 
     void increment_counter() {
@@ -197,7 +201,7 @@ public:
     }
 
     template<typename TypeT, typename... DataTypes>
-    void send_data(telegram::EcatTelegram& telegram, uint16_t channel, uint16_t priority, ProtocolType type, DataTypes&... data) {
+    bool send_data(telegram::EcatTelegram& telegram, const datagram::TEcatWkc expected_wkc, uint16_t channel, uint16_t priority, ProtocolType type, DataTypes&... data) {
         using TCommand = command::EcatCommand<TypeT, command::WR>;
         using TDatagram = datagram::EcatDatagram<TCommand, MailboxHeader, DataTypes... , Padding>;
 
@@ -213,17 +217,22 @@ public:
             .cnt = static_cast<uint16_t>(counter_)
         };
         Padding padding{buffer_slave_.get_buffer_properties_write().length - sizeof(MailboxHeader) - custom_tuple<DataTypes...>::size};
-        TDatagram datagram{ {{slave_address, buffer_regs[MailboxesRegs::WRITE]}}, header, data... , padding};
+        TDatagram datagram{ {{slave_address, buffer_regs[MailboxesRegs::WRITE]}}, expected_wkc, header, data... , padding};
+        bool status;
 
-        do {
+        status = telegram.transfer(datagram);
-            telegram.transfer(datagram);
+
-        } while(datagram.get_wkc() < 0x0001);
+        if(status != true) {
+            return status;
+        }
 
         increment_counter();
+
+        return status;
     }
 
     template<typename TypeT, typename... DataTypes>
-    void receive_data(telegram::EcatTelegram& telegram, DataTypes&... data) {
+    bool receive_data(telegram::EcatTelegram& telegram, const datagram::TEcatWkc expected_wkc, DataTypes&... data) {
         using TCommand = command::EcatCommand<TypeT, command::RD>;
         using TDatagram = datagram::EcatDatagram<TCommand, MailboxHeader, DataTypes... , Padding>;
 
@@ -231,11 +240,9 @@ public:
         std::array<address::Offset, 4>& buffer_regs = buffer_slave_.get_buffer_regs();
         MailboxHeader header;
         Padding padding{buffer_slave_.get_buffer_properties_read().length - sizeof(MailboxHeader) - custom_tuple<DataTypes...>::size};
-        TDatagram datagram{ {{slave_address, buffer_regs[MailboxesRegs::READ]}}, header, data... , padding};
+        TDatagram datagram{ {{slave_address, buffer_regs[MailboxesRegs::READ]}}, expected_wkc, header, data... , padding};
 
-        do {
+        return telegram.transfer(datagram);
-            telegram.transfer(datagram);
-        } while(datagram.get_wkc() < 0x0001);
 
         //DebugP_log("header.length = %d\r\n", header.length);
         //DebugP_log("header.address = %d\r\n", header.address);
@@ -246,21 +253,21 @@ public:
     }
 
     template<typename TypeT, typename... DataTypes>
-    void receive(telegram::EcatTelegram& telegram, DataTypes&... data) {
+    bool receive(telegram::EcatTelegram& telegram, const datagram::TEcatWkc expected_wkc, DataTypes&... data) {
-        wait_available<TypeT>(telegram);
+        if(wait_available<TypeT>(telegram, expected_wkc) != true) return false;
-        receive_data<TypeT, DataTypes...>(telegram, data...);
+        if(receive_data<TypeT, DataTypes...>(telegram, expected_wkc, data...) != true) return false;
     }
 
     template<typename TypeT, typename... DataTypes>
-    void send(telegram::EcatTelegram& telegram, uint16_t channel, uint16_t priority, ProtocolType type, DataTypes&... data) {
+    bool send(telegram::EcatTelegram& telegram, const datagram::TEcatWkc expected_wkc, uint16_t channel, uint16_t priority, ProtocolType type, DataTypes&... data) {
-        empty<TypeT>(telegram);
+        if(empty<TypeT>(telegram, expected_wkc) != true) return false;
-        wait_empty<TypeT>(telegram);
+        if(wait_empty<TypeT>(telegram, expected_wkc) != true) return false;
-        send_data<TypeT, DataTypes...>(telegram, channel, priority, type, data...);
+        if(send_data<TypeT, DataTypes...>(telegram, expected_wkc, channel, priority, type, data...) != true) return false;
-        wait_empty<TypeT>(telegram);
+        if(wait_empty<TypeT>(telegram, expected_wkc) != true) return false;
     }
 
     template<typename TypeT, typename... DataTypes>
-    CompleteSize sdo_write(telegram::EcatTelegram& telegram, uint16_t index, uint8_t subindex, DataTypes&... data) {
+    bool sdo_write(telegram::EcatTelegram& telegram, const datagram::TEcatWkc expected_wkc, uint16_t index, uint8_t subindex, DataTypes&... data) {
         CoEElements elements{
             .coe_header = {
                 .number = 0x00,
@@ -274,12 +281,23 @@ public:
             .index = index,
             .subindex = subindex };
         CompleteSize complete_size{custom_tuple<DataTypes...>::size};
+        bool status;
 
-        send<TypeT, CoEElements, CompleteSize>(telegram, 0, 0, ProtocolType::CoE, elements, complete_size, data...);
+        status = send<TypeT, CoEElements, CompleteSize>(telegram, expected_wkc, 0, 0, ProtocolType::CoE, elements, complete_size, data...);
-        receive<TypeT, CoEElements, CompleteSize, DataTypes...>(telegram, elements, complete_size);
+
+        if(status != true) {
+            return status;
+        }
+
+        status = receive<TypeT, CoEElements, CompleteSize, DataTypes...>(telegram, expected_wkc, elements, complete_size);
+
+        if(status != true) {
+            return status;
+        }
 
         if( (elements.coe_header.service != static_cast<uint8_t>(Service::SDO_RESPONSE)) ||
             (elements.command_specifier.command_spec != static_cast<uint8_t>(SDOReqCommandSpecifier::UPLOAD)) ) {
+
             DebugP_log((char*)"CoE error: = 0x%04x\r\n", complete_size.value); // 0x601004 - The object cannot be accessed via complete access
         }
 
@@ -294,11 +312,11 @@ public:
         //DebugP_log("elements.subindex = %d\r\n", elements.subindex);
         //DebugP_log("complete_size = %d\r\n", complete_size);
 
-        return complete_size;
+        return status;
     }
 
     template<typename TypeT, typename... DataTypes>
-    CompleteSize sdo_read(telegram::EcatTelegram& telegram, uint16_t index, uint8_t subindex, DataTypes&... data) {
+    bool sdo_read(telegram::EcatTelegram& telegram, const datagram::TEcatWkc expected_wkc, uint16_t index, uint8_t subindex, DataTypes&... data) {
         CoEElements elements{
             .coe_header = {
                 .number = 0x00,
@@ -312,12 +330,23 @@ public:
             .index = index,
             .subindex = subindex };
         CompleteSize complete_size{0x00000000};
+        bool status;
 
-        send<TypeT, CoEElements, CompleteSize>(telegram, 0, 0, ProtocolType::CoE, elements, complete_size);
+        status = send<TypeT, CoEElements, CompleteSize>(telegram, expected_wkc, 0, 0, ProtocolType::CoE, elements, complete_size);
-        receive<TypeT, CoEElements, CompleteSize, DataTypes...>(telegram, elements, complete_size, data...);
+
+        if(status != true) {
+            return status;
+        }
+
+        status = receive<TypeT, CoEElements, CompleteSize, DataTypes...>(telegram, expected_wkc, elements, complete_size, data...);
+
+        if(status != true) {
+            return status;
+        }
 
         if( (elements.coe_header.service != static_cast<uint8_t>(Service::SDO_RESPONSE)) ||
             (elements.command_specifier.command_spec != static_cast<uint8_t>(SDOReqCommandSpecifier::UPLOAD)) ) {
+
             DebugP_log((char*)"CoE error: = 0x%04x\r\n", complete_size.value); // 0x601004 - The object cannot be accessed via complete access
         }
 
@@ -332,17 +361,23 @@ public:
         //DebugP_log("elements.subindex = %d\r\n", elements.subindex);
         //DebugP_log("complete_size = %d\r\n", complete_size);
 
-        return complete_size;
+        return status;
     }
 
     template<typename TypeT>
-    uint16_t pdo_map_read(telegram::EcatTelegram& telegram, ecat_buffer::PDOMap& pdo_map, uint16_t pdo_map_index) {
+    bool pdo_map_read(telegram::EcatTelegram& telegram, const datagram::TEcatWkc expected_wkc, ecat_buffer::PDOMap& pdo_map, uint16_t pdo_map_index) {
         uint16_t pdo_data_size{0x0000}; // Размер данных в битах !
         uint8_t pdo_block_count{0x00};
+        bool status;
 
         pdo_map.block_index_map.emplace(pdo_map_index, std::map<uint8_t, uint16_t>{});
 
-        sdo_read<TypeT, uint8_t>(telegram, pdo_map_index, 0x00, pdo_block_count);
+        status = sdo_read<TypeT, uint8_t>(telegram, expected_wkc, pdo_map_index, 0x00, pdo_block_count);
+
+        if(status != true) {
+            return status;
+        }
+
         pdo_map.block_index_map[pdo_map_index].emplace(0, pdo_block_count);
 
         DebugP_log((char*)"pdo_block_count = 0x%01x\r\n", pdo_block_count);
@@ -350,7 +385,12 @@ public:
         for(uint8_t pdo_map_subindex = 1; pdo_map_subindex < (pdo_block_count + 1); pdo_map_subindex++) {
             uint16_t pdo_block_index{0x0000};
 
-            sdo_read<TypeT, uint16_t>(telegram, pdo_map_index, pdo_map_subindex, pdo_block_index);
+            status = sdo_read<TypeT, uint16_t>(telegram, expected_wkc, pdo_map_index, pdo_map_subindex, pdo_block_index);
+
+            if(status != true) {
+                return status;
+            }
+
             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);
@@ -360,7 +400,12 @@ public:
 
             pdo_map.object_descriptor_map.emplace(pdo_block_index, std::map<uint8_t, ecat_buffer::PDODescriptor>{});
 
-            sdo_read<TypeT, uint8_t>(telegram, pdo_block_index, 0, pdo_block_object_count);
+            status = sdo_read<TypeT, uint8_t>(telegram, expected_wkc, pdo_block_index, 0, pdo_block_object_count);
+
+            if(status != true) {
+                return status;
+            }
+
             pdo_map.object_descriptor_map[pdo_block_index].emplace(0, tmp);
 
             DebugP_log((char*)"pdo_block_object_count = 0x%01x\r\n", pdo_block_object_count);
@@ -368,7 +413,12 @@ public:
             for(uint8_t pdo_block_subindex = 1; pdo_block_subindex < (pdo_block_object_count + 1); pdo_block_subindex++) {
                 ecat_buffer::PDODescriptor descriptor;
 
-                sdo_read<TypeT, ecat_buffer::PDODescriptor>(telegram, pdo_block_index, pdo_block_subindex, descriptor);
+                status = sdo_read<TypeT, ecat_buffer::PDODescriptor>(telegram, expected_wkc, pdo_block_index, pdo_block_subindex, descriptor);
+
+                if(status != true) {
+                    return status;
+                }
+
                 pdo_data_size += descriptor.size;
                 pdo_map.object_descriptor_map[pdo_block_index].emplace(pdo_block_subindex, descriptor);
 
@@ -380,9 +430,9 @@ public:
 
         pdo_map.data_size_map.emplace(pdo_map_index, pdo_data_size/8);
 
-        //DebugP_log("pdo_data_size = %d\r\n", pdo_data_size);
+        DebugP_log("pdo_data_size = %d\r\n", pdo_data_size/8);
 
-        return pdo_data_size/8;
+        return status;
     }
 
 private:
@@ -396,7 +446,7 @@ public:
     EthEcatSdoMailbox(ecat_buffer::EthEcatBuffer& ecat_buffer): ecat_buffer_{ecat_buffer} { }
 
     void init();
-    void read_pdo_map(uint16_t pdo_map_rx_index, uint16_t pdo_map_tx_index);
+    bool read_pdo_map(uint16_t pdo_map_rx_index, uint16_t pdo_map_tx_index);
 
     std::vector<ecat_buffer::PDOMap>& get_pdo_map() {
         return pdo_map_;

components/free_rtos/ethernet_industry/eth_ecat.cpp

@@ -35,70 +35,70 @@ void EthEcat::Init(TEthMacPorts port_id, uint32_t period_microsec = 250) {
     ecat_timer_.Stop();
 }
 
-void EthEcat::set_slaves_to_default() {
+bool EthEcat::set_slaves_to_default() {
+    const datagram::TEcatWkc expected_wkc = 0;
     address::Broadcast broadcast{0x0000};
+    bool status;
 
 /* deact loop manual */
     uint8_t a_data_out{0x00};
-    datagram::EcatDatagram<command::BWR, uint8_t> a{ {{broadcast, ECT_REG_DLPORT}}, a_data_out };
+    datagram::EcatDatagram<command::BWR, uint8_t> a{ {{broadcast, ECT_REG_DLPORT}}, expected_wkc, a_data_out };
 
 /* set IRQ mask */
     uint16_t b_data_out{0x0C04};
-    datagram::EcatDatagram<command::BWR, uint16_t> b{ {{broadcast, ECT_REG_IRQMASK}}, b_data_out };
+    datagram::EcatDatagram<command::BWR, uint16_t> b{ {{broadcast, ECT_REG_IRQMASK}}, expected_wkc, b_data_out };
 
 /* reset CRC counters */
     std::array<uint8_t, 8> c_data_out;
     c_data_out.fill(0x00);
-    datagram::EcatDatagram<command::BWR, std::array<uint8_t, 8>> c{ {{broadcast, ECT_REG_RXERR}}, c_data_out };
+    datagram::EcatDatagram<command::BWR, std::array<uint8_t, 8>> c{ {{broadcast, ECT_REG_RXERR}}, expected_wkc, c_data_out };
 
 /* reset FMMU's */
     std::array<uint8_t, 16*3> d_data_out;
     d_data_out.fill(0x00);
-    datagram::EcatDatagram<command::BWR, std::array<uint8_t, 16*3>> d{ {{broadcast, ECT_REG_FMMU0}}, d_data_out };
+    datagram::EcatDatagram<command::BWR, std::array<uint8_t, 16*3>> d{ {{broadcast, ECT_REG_FMMU0}}, expected_wkc, d_data_out };
 
 /* reset SyncM */
     std::array<uint8_t, 8*4> e_data_out;
     e_data_out.fill(0x00);
-    datagram::EcatDatagram<command::BWR, std::array<uint8_t, 8*4>> e{ {{broadcast, ECT_REG_SM0}}, e_data_out };
+    datagram::EcatDatagram<command::BWR, std::array<uint8_t, 8*4>> e{ {{broadcast, ECT_REG_SM0}}, expected_wkc, e_data_out };
 
 /* reset activation register */
     uint8_t f_data_out{0x00};
-    datagram::EcatDatagram<command::BWR, uint8_t> f{ {{broadcast, ECT_REG_DCSYNCACT}}, f_data_out };
+    datagram::EcatDatagram<command::BWR, uint8_t> f{ {{broadcast, ECT_REG_DCSYNCACT}}, expected_wkc, f_data_out };
 
 /* reset system time+ofs */
     std::array<uint8_t, 4> g_data_out;
     g_data_out.fill(0x00);
-    datagram::EcatDatagram<command::BWR, std::array<uint8_t, 4>> g{ {{broadcast, ECT_REG_DCSYSTIME}}, g_data_out };
+    datagram::EcatDatagram<command::BWR, std::array<uint8_t, 4>> g{ {{broadcast, ECT_REG_DCSYSTIME}}, expected_wkc, g_data_out };
 
 /* DC speedstart */
     uint16_t h_data_out{0x1000};
-    datagram::EcatDatagram<command::BWR, uint16_t> h{ {{broadcast, ECT_REG_DCSPEEDCNT}}, h_data_out };
+    datagram::EcatDatagram<command::BWR, uint16_t> h{ {{broadcast, ECT_REG_DCSPEEDCNT}}, expected_wkc, h_data_out };
 
 /* DC filt expr */
     uint16_t i_data_out{0x0C00};
-    datagram::EcatDatagram<command::BWR, uint16_t> i{ {{broadcast, ECT_REG_DCTIMEFILT}}, i_data_out };
+    datagram::EcatDatagram<command::BWR, uint16_t> i{ {{broadcast, ECT_REG_DCTIMEFILT}}, expected_wkc, i_data_out };
 
 /* Ignore Alias register */
     uint8_t j_data_out{0x00};
-    datagram::EcatDatagram<command::BWR, uint8_t> j{ {{broadcast, ECT_REG_DLALIAS}}, j_data_out };
+    datagram::EcatDatagram<command::BWR, uint8_t> j{ {{broadcast, ECT_REG_DLALIAS}}, expected_wkc, j_data_out };
 
 /* Reset all slaves to Init */
     uint16_t k_data_out{EC_STATE_INIT|EC_STATE_ACK};
-    datagram::EcatDatagram<command::BWR, uint16_t> k{ {{broadcast, ECT_REG_ALCTL}}, k_data_out };
+    datagram::EcatDatagram<command::BWR, uint16_t> k{ {{broadcast, ECT_REG_ALCTL}}, expected_wkc, k_data_out };
 
 /* force Eeprom from PDI */
     uint8_t l_data_out{0x02};
-    datagram::EcatDatagram<command::BWR, uint8_t> l{ {{broadcast, ECT_REG_EEPCFG}}, l_data_out };
+    datagram::EcatDatagram<command::BWR, uint8_t> l{ {{broadcast, ECT_REG_EEPCFG}}, expected_wkc, l_data_out };
 
 /* set Eeprom to master */
     uint8_t m_data_out{0x00};
-    datagram::EcatDatagram<command::BWR, uint8_t> m{ {{broadcast, ECT_REG_EEPCFG}}, m_data_out };
+    datagram::EcatDatagram<command::BWR, uint8_t> m{ {{broadcast, ECT_REG_EEPCFG}}, expected_wkc, m_data_out };
 
     auto queue = a + b + c + d + e + f + g + h + i + j + k + l + m;
 
-    do {
+    status = telegram_.transfer(queue);
-        telegram_.transfer(queue);
-    } while(a.get_all_wkc() < 0x0001);
 
     //telegram_.transfer(b);
     //telegram_.transfer(c);
@@ -113,25 +113,27 @@ void EthEcat::set_slaves_to_default() {
     //telegram_.transfer(l);
     //telegram_.transfer(m);
 
-    DebugP_log((char*)"a.get_wkc() = %d\r\n", a.get_wkc());
+    DebugP_log((char*)"Deact loop manual wkc = %d\r\n",         a.get_wkc());
-    DebugP_log((char*)"b.get_wkc() = %d\r\n", b.get_wkc());
+    DebugP_log((char*)"Set IRQ mask wkc = %d\r\n",              b.get_wkc());
-    DebugP_log((char*)"c.get_wkc() = %d\r\n", c.get_wkc());
+    DebugP_log((char*)"Reset CRC counters wkc = %d\r\n",        c.get_wkc());
-    DebugP_log((char*)"d.get_wkc() = %d\r\n", d.get_wkc());
+    DebugP_log((char*)"Reset FMMU's wkc = %d\r\n",              d.get_wkc());
-    DebugP_log((char*)"e.get_wkc() = %d\r\n", e.get_wkc());
+    DebugP_log((char*)"Reset SyncM wkc = %d\r\n",               e.get_wkc());
-    DebugP_log((char*)"f.get_wkc() = %d\r\n", f.get_wkc());
+    DebugP_log((char*)"Reset activation register wkc = %d\r\n", f.get_wkc());
-    DebugP_log((char*)"g.get_wkc() = %d\r\n", g.get_wkc());
+    DebugP_log((char*)"Reset system time+ofs wkc = %d\r\n",     g.get_wkc());
-    DebugP_log((char*)"h.get_wkc() = %d\r\n", h.get_wkc());
+    DebugP_log((char*)"DC speedstart wkc = %d\r\n",             h.get_wkc());
-    DebugP_log((char*)"i.get_wkc() = %d\r\n", i.get_wkc());
+    DebugP_log((char*)"DC filt expr wkc = %d\r\n",              i.get_wkc());
-    DebugP_log((char*)"j.get_wkc() = %d\r\n", j.get_wkc());
+    DebugP_log((char*)"Ignore Alias register wkc = %d\r\n",     j.get_wkc());
-    DebugP_log((char*)"k.get_wkc() = %d\r\n", k.get_wkc());
+    DebugP_log((char*)"Reset all slaves to Init wkc = %d\r\n",  k.get_wkc());
-    DebugP_log((char*)"l.get_wkc() = %d\r\n", l.get_wkc());
+    DebugP_log((char*)"Force Eeprom from PDI wkc = %d\r\n",     l.get_wkc());
-    DebugP_log((char*)"m.get_wkc() = %d\r\n", m.get_wkc());
+    DebugP_log((char*)"Set Eeprom to master wkc = %d\r\n",      m.get_wkc());
+
+    return status;
 }
 
 uint16_t EthEcat::slaves_detecting() {
     address::Broadcast broadcast{0x0000};
     uint16_t data{0x0000};
-    datagram::EcatDatagram<command::BRD, uint16_t> datagram{ {{broadcast, ECT_REG_TYPE}}, data };
+    datagram::EcatDatagram<command::BRD, uint16_t> datagram{ {{broadcast, ECT_REG_TYPE}}, 1, data };
 
     telegram_.transfer(datagram);
 
@@ -140,10 +142,12 @@ uint16_t EthEcat::slaves_detecting() {
 
 // Setting Station address (FP) of slave via Position addressing (AP)
 // Station address is datagram data
-void EthEcat::set_addresses_of_slaves(uint16_t number_of_slaves, uint16_t address_base) {
+bool EthEcat::set_addresses_of_slaves(uint16_t number_of_slaves, uint16_t address_base) {
     using TDatagram = datagram::EcatDatagram<command::APWR, address::Station>;
 
+    const datagram::TEcatWkc expected_wkc = 1;
     std::vector<TDatagram> datagrams;
+    bool status;
 
     slaves_.reserve(number_of_slaves);
     datagrams.reserve(number_of_slaves);
@@ -154,7 +158,7 @@ void EthEcat::set_addresses_of_slaves(uint16_t number_of_slaves, uint16_t addres
         address::SlaveAddresses slave_addresses{position, 0x0000, station, 0x00000000};
 
         slaves_.emplace_back(EcatSlave{std::move(slave_addresses)});
-        datagrams.emplace_back(TDatagram{ {{position, ECT_REG_STADR}},  slaves_.back().get_slave_address<command::FP>() });
+        datagrams.emplace_back(TDatagram{ {{position, ECT_REG_STADR}}, expected_wkc,  slaves_.back().get_slave_address<command::FP>() });
     }
 
     queue::Queue<datagram::IEcatDatagram> queue;
@@ -163,64 +167,107 @@ void EthEcat::set_addresses_of_slaves(uint16_t number_of_slaves, uint16_t addres
         queue + datagrams[i];
     }
 
-    do {
+    status = telegram_.transfer(queue);
-        telegram_.transfer(queue);
+
-    } while(datagrams[0].get_all_wkc() < number_of_slaves);
+    if(status != true) {
+        DebugP_log((char*)"Slave addresses not set\r\n");
+
+        return status;
+    }
 
     DebugP_log((char*)"Slave addresses set\r\n");
+
+    return status;
 }
 
-void EthEcat::get_addresses_of_slaves() {
+bool EthEcat::get_addresses_of_slaves() {
     using TDatagram = datagram::EcatDatagram<command::APRD, address::Station>;
 
+    const datagram::TEcatWkc expected_wkc = 1;
     std::vector<TDatagram> datagrams;
     uint16_t number_of_slaves = slaves_.size();
+    queue::Queue<datagram::IEcatDatagram> queue;
+    bool status;
 
     datagrams.reserve(number_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.emplace_back(TDatagram{ {{slave.get_slave_address<command::AP>(), ECT_REG_STADR}}, expected_wkc, slave.get_slave_address<command::FP>() });
     }
 
-    queue::Queue<datagram::IEcatDatagram> queue;
-
     for(uint16_t i = 0; i < number_of_slaves; i++) {
         queue + datagrams[i];
     }
 
-    do {
+    status = telegram_.transfer(queue);
-        telegram_.transfer(queue);
+
-    } while(datagrams[0].get_all_wkc() < number_of_slaves);
+    if(status != true) {
+        DebugP_log((char*)"Slave addresses not read\r\n");
+
+        return status;
+    }
 
     for(EcatSlave& slave : slaves_) {
         DebugP_log((char*)"Slave %d address = %d\r\n", -slave.get_slave_address<command::AP>(), slave.get_slave_address<command::FP>());
     }
+
+    return status;
 }
 
-uint16_t EthEcat::config_init(uint16_t address_base) {
+bool EthEcat::config_init(uint16_t address_base) {
+    uint16_t number_of_slaves;
+    bool status;
+
     DebugP_log((char*)"Initializing slaves...\r\n");
 
-    set_slaves_to_default();
+    status = set_slaves_to_default();
-    uint16_t number_of_slaves = slaves_detecting();
+
+    if(status != true) {
+        return status;
+    }
+
+    number_of_slaves = slaves_detecting();
+
     DebugP_log((char*)"number_of_slaves = %d\r\n", number_of_slaves);
 
-    set_addresses_of_slaves(number_of_slaves, address_base);
+    status = (number_of_slaves > 0);
-    get_addresses_of_slaves();
 
-    return number_of_slaves;
+    if(status != true) {
+        return status;
+    }
+
+    status = set_addresses_of_slaves(number_of_slaves, address_base);
+
+    if(status != true) {
+        return status;
+    }
+
+    status = get_addresses_of_slaves();
+
+    if(status != true) {
+        return status;
+    }
+
+    return status;
 }
 
-void EthEcat::enable_PDI() {
+bool EthEcat::enable_PDI() {
+    const datagram::TEcatWkc expected_wkc = 1;
+    bool status = true;
+
     for(EcatSlave& slave : slaves_) {
-        slave.enable_PDI<command::FP>(telegram_);
+        status = (status && slave.enable_PDI<command::FP>(telegram_, expected_wkc));
     }
+
+    return status;
 }
 
 bool EthEcat::init_to_preop() {
+    const datagram::TEcatWkc expected_wkc = 1;
     bool success = true;
 
     for(EcatSlave& slave : slaves_) {
-        success &= slave.init_to_preop<command::FP>(telegram_);
+        success = (success && slave.init_to_preop<command::FP>(telegram_, expected_wkc));
     }
 
     DebugP_log((char*)"success = %d\r\n", success);
@@ -229,10 +276,11 @@ bool EthEcat::init_to_preop() {
 }
 
 bool EthEcat::preop_to_safeop() {
+    const datagram::TEcatWkc expected_wkc = 1;
     bool success = true;
 
     for(EcatSlave& slave : slaves_) {
-        success &= slave.preop_to_safeop<command::FP>(telegram_);
+        success = (success && slave.preop_to_safeop<command::FP>(telegram_, expected_wkc));
     }
 
     DebugP_log((char*)"success = %d\r\n", success);
@@ -241,8 +289,8 @@ bool EthEcat::preop_to_safeop() {
 }
 
 bool EthEcat::safeop_to_op() {
-    uint16_t number_of_slaves = slaves_.size();
+    const uint16_t number_of_slaves = slaves_.size();
-    bool success;
+    bool status;
 
     address::Broadcast broadcast{0x0000};
     ALSTAT stat{0x0000, 0x0000};
@@ -253,34 +301,39 @@ bool EthEcat::safeop_to_op() {
 
     {
         uint16_t data{EC_STATE_OPERATIONAL};
-        datagram::EcatDatagram<command::BWR, uint16_t> datagram{ {{broadcast, ECT_REG_ALCTL}}, data };
+        datagram::EcatDatagram<command::BWR, uint16_t> datagram{ {{broadcast, ECT_REG_ALCTL}}, number_of_slaves, data };
 
-        do {
+        status = telegram_.transfer(datagram);
-            telegram_.transfer(datagram);
+
-        } while(datagram.get_wkc() < number_of_slaves);
+        if(status != true) {
+            return status;
+        }
     }
 
     process_sem_.post();
     init_sem_.pend();
 
     {
-        datagram::EcatDatagram<command::BRD, ALSTAT, uint16_t> datagram{ {{broadcast, ECT_REG_ALSTAT}}, stat, zero };
+        datagram::EcatDatagram<command::BRD, ALSTAT, uint16_t> datagram{ {{broadcast, ECT_REG_ALSTAT}}, number_of_slaves, stat, zero };
 
-        do {
+        status = telegram_.transfer(datagram);
-            telegram_.transfer(datagram);
-        } while(datagram.get_wkc() < number_of_slaves);
-    }
 
-    success = (stat.state == EC_STATE_OPERATIONAL) && (stat.fault == 0);
+        if(status != true) {
-
+            return status;
-    if(success == true) {
+        }
-        ecat_timer_.Start();
     }
 
     //DebugP_log((char*)"stat.state = %d, stat.fault = %d\r\n", stat.state, stat.fault);
+
+    status = (stat.state == EC_STATE_OPERATIONAL) && (stat.fault == 0);
+
+    if(status == true) {
+        ecat_timer_.Start();
+    }
+
     //DebugP_log((char*)"success = %d\r\n", success);
 
-    return success;
+    return status;
 }
 
 }

components/free_rtos/ethernet_industry/eth_ecat.hpp

@@ -53,41 +53,44 @@ public:
     }
 
     template<typename TypeT>
-    void enable_PDI(telegram::EcatTelegram& telegram) {
+    bool enable_PDI(telegram::EcatTelegram& telegram, const datagram::TEcatWkc expected_wkc) {
         using TCommand = command::EcatCommand<TypeT, command::WR>;
         auto slave_address = get_slave_address<TypeT>();
         uint8_t data{0x01};
-        datagram::EcatDatagram<TCommand, uint8_t> datagram{ {{slave_address, ECT_REG_EEPCFG}}, data };
+        datagram::EcatDatagram<TCommand, uint8_t> datagram{ {{slave_address, ECT_REG_EEPCFG}}, expected_wkc, data };
 
-        do {
+        return telegram.transfer(datagram);
-            telegram.transfer(datagram);
-        } while(datagram.get_wkc() < 0x0001);
     }
 
     template<typename TypeT>
-    bool init_to_preop(telegram::EcatTelegram& telegram) {
+    bool init_to_preop(telegram::EcatTelegram& telegram, const datagram::TEcatWkc expected_wkc) {
         auto slave_address = get_slave_address<TypeT>();
         ALSTAT stat{0x0000, 0x0000};
+        bool status;
 
         {
             using TCommand = command::EcatCommand<TypeT, command::WR>;
             uint16_t data{EC_STATE_PRE_OP|EC_STATE_ACK};
-            datagram::EcatDatagram<TCommand, uint16_t> datagram{ {{slave_address, ECT_REG_ALCTL}}, data };
+            datagram::EcatDatagram<TCommand, uint16_t> datagram{ {{slave_address, ECT_REG_ALCTL}}, expected_wkc, data };
 
-            do {
+            status = telegram.transfer(datagram);
-                telegram.transfer(datagram);
+
-            } while(datagram.get_wkc() < 0x0001);
+            if(status != true) {
+                return status;
+            }
         }
 
         ClockP_usleep(5000000ul);
 
         {
             using TCommand = command::EcatCommand<TypeT, command::RD>;
-            datagram::EcatDatagram<TCommand, ALSTAT> datagram{ {{slave_address, ECT_REG_ALSTAT}}, stat };
+            datagram::EcatDatagram<TCommand, ALSTAT> datagram{ {{slave_address, ECT_REG_ALSTAT}}, expected_wkc, stat };
 
-            do {
+            status = telegram.transfer(datagram);
-                telegram.transfer(datagram);
+
-            } while(datagram.get_wkc() < 0x0001);
+            if(status != true) {
+                return status;
+            }
         }
 
         DebugP_log((char*)"stat.state = %d, stat.fault = %d\r\n", stat.state, stat.fault);
@@ -96,41 +99,52 @@ public:
             using TCommand = command::EcatCommand<TypeT, command::RD>;
 
             uint16_t stat_code{0x0000};
-            datagram::EcatDatagram<TCommand, uint16_t> datagram{ {{slave_address, ECT_REG_ALSTATCODE}}, stat_code};
+            datagram::EcatDatagram<TCommand, uint16_t> datagram{ {{slave_address, ECT_REG_ALSTATCODE}}, expected_wkc, stat_code};
 
-            do {
+            status = telegram.transfer(datagram);
-                telegram.transfer(datagram);
+
-            } while(datagram.get_wkc() < 0x0001);
+            if(status != true) {
+                return status;
+            }
+
+            DebugP_log((char*)"stat_code = 0x%02x\r\n", stat_code);
         }
 
-        return (stat.state == EC_STATE_PRE_OP) && (stat.fault == 0);
+        status = ((stat.state == EC_STATE_PRE_OP) && (stat.fault == 0));
+
+        return status;
     }
 
     template<typename TypeT>
-    bool preop_to_safeop(telegram::EcatTelegram& telegram) {
+    bool preop_to_safeop(telegram::EcatTelegram& telegram, const datagram::TEcatWkc expected_wkc) {
         auto slave_address = get_slave_address<TypeT>();
         ALSTAT stat{0x0000, 0x0000};
         uint32_t zero{0x00000000};
+        bool status;
 
         {
             using TCommand = command::EcatCommand<TypeT, command::WR>;
             uint16_t data{EC_STATE_SAFE_OP};
-            datagram::EcatDatagram<TCommand, uint16_t> datagram{ {{slave_address, ECT_REG_ALCTL}}, data };
+            datagram::EcatDatagram<TCommand, uint16_t> datagram{ {{slave_address, ECT_REG_ALCTL}}, expected_wkc, data };
 
-            do {
+            status = telegram.transfer(datagram);
-                telegram.transfer(datagram);
+
-            } while(datagram.get_wkc() < 0x0001);
+            if(status != true) {
+                return status;
+            }
         }
 
         ClockP_usleep(1000000ul);
 
         {
             using TCommand = command::EcatCommand<TypeT, command::RD>;
-            datagram::EcatDatagram<TCommand, ALSTAT, uint32_t> datagram{ {{slave_address, ECT_REG_ALSTAT}}, stat, zero };
+            datagram::EcatDatagram<TCommand, ALSTAT, uint32_t> datagram{ {{slave_address, ECT_REG_ALSTAT}}, expected_wkc, stat, zero };
 
-            do {
+            status = telegram.transfer(datagram);
-                telegram.transfer(datagram);
+
-            } while(datagram.get_wkc() < 0x0001);
+            if(status != true) {
+                return status;
+            }
         }
 
         DebugP_log((char*)"stat.state = %d, stat.fault = %d\r\n", stat.state, stat.fault);
@@ -139,16 +153,20 @@ public:
             using TCommand = command::EcatCommand<TypeT, command::RD>;
 
             uint16_t stat_code{0x0000};
-            datagram::EcatDatagram<TCommand, uint16_t> datagram{ {{slave_address, ECT_REG_ALSTATCODE}}, stat_code};
+            datagram::EcatDatagram<TCommand, uint16_t> datagram{ {{slave_address, ECT_REG_ALSTATCODE}}, expected_wkc, stat_code};
 
-            do {
+            status = telegram.transfer(datagram);
-                telegram.transfer(datagram);
+
-            } while(datagram.get_wkc() < 0x0001);
+            if(status != true) {
+                return status;
+            }
 
             DebugP_log((char*)"stat_code = 0x%02x\r\n", stat_code);
         }
 
-        return (stat.state == EC_STATE_SAFE_OP) && (stat.fault == 0);
+        status = ((stat.state == EC_STATE_SAFE_OP) && (stat.fault == 0));
+
+        return status;
     }
 
 private:
@@ -166,14 +184,14 @@ public:
 
     void Init(TEthMacPorts port_id, uint32_t period_microsec);
 
-    void set_slaves_to_default();
+    bool set_slaves_to_default();
     uint16_t slaves_detecting();
-    void set_addresses_of_slaves(uint16_t number_of_slaves, uint16_t address_base);
+    bool set_addresses_of_slaves(uint16_t number_of_slaves, uint16_t address_base);
-    void get_addresses_of_slaves();
+    bool get_addresses_of_slaves();
 
-    uint16_t config_init(uint16_t address_base);
+    bool config_init(uint16_t address_base);
 
-    void enable_PDI();
+    bool enable_PDI();
 
     bool init_to_preop();
     bool preop_to_safeop();
@@ -195,6 +213,10 @@ public:
         return telegram_;
     }
 
+    const telegram::EcatTelegramStatus& get_telegram_status() {
+        return telegram_.get_status();
+    }
+
     eeprom::EEPROM& get_eeprom() {
         return eeprom_;
     }

components/free_rtos/ethernet_industry/eth_ecat_api.cpp

@@ -10,18 +10,40 @@
 namespace free_rtos {
 
 bool EthEcatApi::config_init(TEthMacPorts port_id, uint16_t address_base, uint32_t period_microsec, ecat_pdo_fmmu::ProcessCallback callback) {
+    uint16_t number_of_slaves;
     bool status = false;
 
     ecat_.Init(port_id, period_microsec);
 
-    ecat_.config_init(address_base);
+    status = ecat_.config_init(address_base);
 
-    ecat_buffer_sdo_.init(ECT_SII_RXMBXADR, ECT_SII_TXMBXADR);
+    if(status != true) {
-    ecat_buffer_sdo_.init_sync_manager(sync_manager::SYNC_M0, sync_manager::SYNC_M1);
+        return status;
+    }
 
-    ecat_buffer_pdo_.init(ECT_PDOOUTPUTADR, ECT_PDOINPUTADR);
+    status = ecat_buffer_sdo_.init(ECT_SII_RXMBXADR, ECT_SII_TXMBXADR);
 
-    ecat_.enable_PDI();
+    if(status != true) {
+        return status;
+    }
+
+    status = ecat_buffer_sdo_.init_sync_manager(sync_manager::SYNC_M0, sync_manager::SYNC_M1);
+
+    if(status != true) {
+        return status;
+    }
+
+    status = ecat_buffer_pdo_.init(ECT_PDOOUTPUTADR, ECT_PDOINPUTADR);
+
+    if(status != true) {
+        return status;
+    }
+
+    status = ecat_.enable_PDI();
+
+    if(status != true) {
+        return status;
+    }
 
     status = ecat_.init_to_preop();
 
@@ -30,7 +52,11 @@ bool EthEcatApi::config_init(TEthMacPorts port_id, uint16_t address_base, uint32
     }
 
     ecat_sdo_mailbox_.init();
-    ecat_sdo_mailbox_.read_pdo_map(ECT_RXPDOMAPINDEX, ECT_TXPDOMAPINDEX);
+    status = ecat_sdo_mailbox_.read_pdo_map(ECT_RXPDOMAPINDEX, ECT_TXPDOMAPINDEX);
+
+    if(status != true) {
+        return status;
+    }
 
     // Override buffer properties from eeprom for PDO
     #ifdef COMX
@@ -38,8 +64,17 @@ bool EthEcatApi::config_init(TEthMacPorts port_id, uint16_t address_base, uint32
     #endif
     ecat_buffer_pdo_.set_buffer_length(ecat_sdo_mailbox_.get_pdo_map());
 
-    ecat_buffer_pdo_.init_sync_manager(sync_manager::SYNC_M2, sync_manager::SYNC_M3);
+    status = ecat_buffer_pdo_.init_sync_manager(sync_manager::SYNC_M2, sync_manager::SYNC_M3);
-    ecat_buffer_pdo_.init_fmmu(fmmu::FMMU0, fmmu::FMMU1);
+
+    if(status != true) {
+        return status;
+    }
+
+    status = ecat_buffer_pdo_.init_fmmu(fmmu::FMMU0, fmmu::FMMU1);
+
+    if(status != true) {
+        return status;
+    }
 
     ecat_pdo_fmmu_.init(callback);
 

components/free_rtos/ethernet_industry/eth_ecat_api.hpp

@@ -37,6 +37,10 @@ public:
         return ecat_pdo_fmmu_.get_pdo_counter();
     }
 
+    const telegram::EcatTelegramStatus& get_telegram_status() {
+        return ecat_.get_telegram_status();
+    }
+
     template<typename... DataTypes>
     ecat_sdo_mailbox::CompleteSize sdo_write(size_t slave_index, uint16_t index, uint8_t subindex, DataTypes&... data) {
         return ecat_sdo_mailbox_.sdo_write<command::FP, DataTypes...>(slave_index, index, subindex, data...);

components/free_rtos/ethernet_industry/eth_ecat_buffer.cpp

@@ -14,9 +14,11 @@ namespace ecat_buffer {
 constexpr std::array<SyncManager, 4> EcatBufferSlave::sync_managers_;
 constexpr std::array<address::Offset, 4> EcatBufferSlave::fmmu_regs_;
 
-void EthEcatBuffer::init(uint16_t rx_eeprom_addr, uint16_t tx_eeprom_addr) {
+bool EthEcatBuffer::init(uint16_t rx_eeprom_addr, uint16_t tx_eeprom_addr) {
+    const datagram::TEcatWkc expected_wkc = 1;
     std::vector<EcatSlave>& slaves = ecat_.get_slaves();
     eeprom::EEPROM& eeprom = ecat_.get_eeprom();
+    bool status = true;
 
     buffer_slaves_.reserve(slaves.size());
 
@@ -29,28 +31,42 @@ void EthEcatBuffer::init(uint16_t rx_eeprom_addr, uint16_t tx_eeprom_addr) {
     }
 */
     for(EcatBufferSlave& buffer_slave : buffer_slaves_) {
-        buffer_slave.read_buffer_info_from_eeprom<command::FP>(eeprom, rx_eeprom_addr, tx_eeprom_addr);
+        status = (status && buffer_slave.read_buffer_info_from_eeprom<command::FP>(expected_wkc, eeprom, rx_eeprom_addr, tx_eeprom_addr));
     }
+
+    return status;
 }
 
-void EthEcatBuffer::init_sync_manager(sync_manager sm_write, sync_manager sm_read) {
+bool EthEcatBuffer::init_sync_manager(sync_manager sm_write, sync_manager sm_read) {
+    const datagram::TEcatWkc expected_wkc = 1;
     telegram::EcatTelegram& telegram = ecat_.get_telegram();
+    bool status = true;
 
     for(EcatBufferSlave& buffer_slave : buffer_slaves_) {
-        buffer_slave.init_sync_manager<command::FP>(telegram, sm_write, sm_read);
+        status = (status && buffer_slave.init_sync_manager<command::FP>(telegram, expected_wkc, sm_write, sm_read));
     }
+
+    return status;
 }
 
-void EthEcatBuffer::init_fmmu(fmmu fmmu_write, fmmu fmmu_read) {
+bool EthEcatBuffer::init_fmmu(fmmu fmmu_write, fmmu fmmu_read) {
+    const datagram::TEcatWkc expected_wkc = 1;
     telegram::EcatTelegram& telegram = ecat_.get_telegram();
+    bool status = true;
 
     for(EcatBufferSlave& buffer_slave : buffer_slaves_) {
-        buffer_slave.init_fmmu_write<command::FP>(telegram, fmmu_write, fmmu_global_properties_);
+        status = (status && buffer_slave.init_fmmu_write<command::FP>(telegram, expected_wkc, fmmu_write, fmmu_global_properties_));
+    }
+
+    if(status != true) {
+        return status;
     }
 
     for(EcatBufferSlave& buffer_slave : buffer_slaves_) {
-        buffer_slave.init_fmmu_read<command::FP>(telegram, fmmu_read, fmmu_global_properties_);
+        status = (status && buffer_slave.init_fmmu_read<command::FP>(telegram, expected_wkc, fmmu_read, fmmu_global_properties_));
     }
+
+    return status;
 }
 
 }

components/free_rtos/ethernet_industry/eth_ecat_buffer.hpp

@@ -168,71 +168,74 @@ public:
     }
 
     template<typename TypeT>
-    void read_buffer_info_from_eeprom(eeprom::EEPROM& eeprom, uint16_t rx_eeprom_addr, uint16_t tx_eeprom_addr) {
+    bool read_buffer_info_from_eeprom(const datagram::TEcatWkc expected_wkc, eeprom::EEPROM& eeprom, uint16_t rx_eeprom_addr, uint16_t tx_eeprom_addr) {
         auto slave_address = slave_.get_slave_address<TypeT>();
+        bool status;
 
-        eeprom.read<TypeT>(slave_address, rx_eeprom_addr, buffer_properties_write_);
+        status = eeprom.read<TypeT>(expected_wkc, slave_address, rx_eeprom_addr, buffer_properties_write_);
-        eeprom.read<TypeT>(slave_address, tx_eeprom_addr, buffer_properties_read_);
+
+        if(status != true) {
+            return status;
+        }
+
+        status = eeprom.read<TypeT>(expected_wkc, slave_address, tx_eeprom_addr, buffer_properties_read_);
+
+        if(status != true) {
+            return status;
+        }
 
         buffer_regs_[MailboxesRegs::WRITE] = buffer_properties_write_.offset;
         buffer_regs_[MailboxesRegs::READ] = buffer_properties_read_.offset;
 
-        DebugP_log((char*)"buffer_properties_write_ = 0x%04x\r\n", buffer_properties_write_);
+        //DebugP_log((char*)"buffer_properties_write_ = 0x%04x\r\n", buffer_properties_write_);
-        DebugP_log((char*)"buffer_properties_read_ = 0x%04x\r\n", buffer_properties_read_);
+        //DebugP_log((char*)"buffer_properties_read_ = 0x%04x\r\n", buffer_properties_read_);
+
+        return status;
     }
 
     template<typename TypeT>
     datagram::EcatDatagram<command::EcatCommand<TypeT, command::WR>, BufferProperties, uint32_t>
-    make_sync_manager_datagram(SyncManager& sync_manager, BufferProperties& buffer) {
+    make_sync_manager_datagram(const datagram::TEcatWkc expected_wkc, SyncManager& sync_manager, BufferProperties& buffer) {
         using TDatagram = datagram::EcatDatagram<command::EcatCommand<TypeT, command::WR>, BufferProperties, uint32_t>;
 
         auto slave_address = slave_.get_slave_address<TypeT>();
 
-        return TDatagram{ {{slave_address, sync_manager.offset}}, buffer, sync_manager.default_setting };
+        return TDatagram{ {{slave_address, sync_manager.offset}}, expected_wkc, buffer, sync_manager.default_setting };
     }
 
     template<typename TypeT>
-    void init_sync_manager(telegram::EcatTelegram& telegram, sync_manager sm_write, sync_manager sm_read) {
+    bool init_sync_manager(telegram::EcatTelegram& telegram, const datagram::TEcatWkc expected_wkc, sync_manager sm_write, sync_manager sm_read) {
         SyncManager sync_manager_write = sync_managers_[static_cast<size_t>(sm_write)];
-        auto datagram_write = make_sync_manager_datagram<TypeT>(sync_manager_write, buffer_properties_write_);
+        auto datagram_write = make_sync_manager_datagram<TypeT>(expected_wkc, sync_manager_write, buffer_properties_write_);
 
         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 datagram_read = make_sync_manager_datagram<TypeT>(expected_wkc, sync_manager_read, buffer_properties_read_);
 
         auto queue = datagram_write + datagram_read;
 
-        do {
-            telegram.transfer(queue);
-        } while(datagram_write.get_all_wkc() < 0x0002);
-/*
-        do {
-            telegram.transfer(datagram_write);
-        } while(datagram_write.get_wkc() < 0x0001);
-
-        do {
-            telegram.transfer(datagram_read);
-        } while(datagram_read.get_wkc() < 0x0001);
-*/
         buffer_regs_[MailboxesRegs::EMPTY] = sync_manager_write.offset + 0x05;
         buffer_regs_[MailboxesRegs::AVAILABLE] = sync_manager_read.offset + 0x05;
 
-        DebugP_log((char*)"datagram_write.get_wkc() = %d\r\n", datagram_write.get_wkc());
+        return telegram.transfer(queue);
-        DebugP_log((char*)"datagram_read.get_wkc() = %d\r\n", datagram_read.get_wkc());
+/*
+        telegram.transfer(datagram_write);
+        telegram.transfer(datagram_read);
+*/
     }
 
     template<typename TypeT>
     datagram::EcatDatagram<command::EcatCommand<TypeT, command::WR>, FMMUSettings>
-    make_fmmu_datagram(fmmu fmmu_x, FMMUSettings& settings) {
+    make_fmmu_datagram(const datagram::TEcatWkc expected_wkc, fmmu fmmu_x, FMMUSettings& settings) {
         using TDatagram = datagram::EcatDatagram<command::EcatCommand<TypeT, command::WR>, FMMUSettings>;
 
         auto slave_address = slave_.get_slave_address<TypeT>();
         address::Offset offset = fmmu_regs_[static_cast<size_t>(fmmu_x)];
 
-        return TDatagram{ {{slave_address, offset}}, settings};
+        return TDatagram{ {{slave_address, offset}}, expected_wkc, settings};
     }
 
     template<typename TypeT>
-    void init_fmmu_write(telegram::EcatTelegram& telegram, fmmu fmmu, FMMUGlobalProperties& fmmu_global_properties) {
+    bool init_fmmu_write(telegram::EcatTelegram& telegram, const datagram::TEcatWkc expected_wkc, fmmu fmmu, FMMUGlobalProperties& fmmu_global_properties) {
         fmmu_write_ = fmmu;
 
         FMMUSettings settings {
@@ -246,7 +249,7 @@ public:
             .activate = 0x01
         };
 
-        auto datagram = make_fmmu_datagram<TypeT>(fmmu, settings);
+        auto datagram = make_fmmu_datagram<TypeT>(expected_wkc, fmmu, settings);
 
         fmmu_properties_write_.address = fmmu_global_properties.logical_end_address;
         fmmu_properties_write_.length = buffer_properties_write_.length;
@@ -254,13 +257,11 @@ public:
         fmmu_global_properties.logical_end_address += buffer_properties_write_.length;
         fmmu_global_properties.logical_full_length_write += buffer_properties_write_.length;
 
-        do {
+        return telegram.transfer(datagram);
-            telegram.transfer(datagram);
-        } while(datagram.get_wkc() < 0x0001);
     }
 
     template<typename TypeT>
-    void init_fmmu_read(telegram::EcatTelegram& telegram, fmmu fmmu, FMMUGlobalProperties& fmmu_global_properties) {
+    bool init_fmmu_read(telegram::EcatTelegram& telegram, const datagram::TEcatWkc expected_wkc, fmmu fmmu, FMMUGlobalProperties& fmmu_global_properties) {
         fmmu_read_ = fmmu;
 
         FMMUSettings settings {
@@ -274,7 +275,7 @@ public:
             .activate = 0x01
         };
 
-        auto datagram = make_fmmu_datagram<TypeT>(fmmu, settings);
+        auto datagram = make_fmmu_datagram<TypeT>(expected_wkc, fmmu, settings);
 
         fmmu_properties_read_.address = fmmu_global_properties.logical_end_address;
         fmmu_properties_read_.length = buffer_properties_read_.length;
@@ -282,13 +283,11 @@ public:
         fmmu_global_properties.logical_end_address += buffer_properties_read_.length;
         fmmu_global_properties.logical_full_length_read += buffer_properties_read_.length;
 
-        do {
+        return telegram.transfer(datagram);
-            telegram.transfer(datagram);
-        } while(datagram.get_wkc() < 0x0001);
     }
 
     template<typename TypeT>
-    void init_fmmu(telegram::EcatTelegram& telegram, fmmu fmmu_write, fmmu fmmu_read, FMMUGlobalProperties& fmmu_global_properties) {
+    bool init_fmmu(telegram::EcatTelegram& telegram, const datagram::TEcatWkc expected_wkc, fmmu fmmu_write, fmmu fmmu_read, FMMUGlobalProperties& fmmu_global_properties) {
         fmmu_write_ = fmmu_write;
         fmmu_read_ = fmmu_read;
 
@@ -303,7 +302,7 @@ public:
             .activate = 0x01
         };
 
-        auto datagram_write = make_fmmu_datagram<TypeT>(fmmu_write, settings_write);
+        auto datagram_write = make_fmmu_datagram<TypeT>(expected_wkc, fmmu_write, settings_write);
 
         fmmu_properties_write_.address = fmmu_global_properties.logical_end_address;
         fmmu_properties_write_.length = buffer_properties_write_.length;
@@ -322,7 +321,7 @@ public:
             .activate = 0x01
         };
 
-        auto datagram_read = make_fmmu_datagram<TypeT>(fmmu_read, settings_read);
+        auto datagram_read = make_fmmu_datagram<TypeT>(expected_wkc, fmmu_read, settings_read);
 
         fmmu_properties_read_.address = fmmu_global_properties.logical_end_address;
         fmmu_properties_read_.length = buffer_properties_read_.length;
@@ -330,22 +329,13 @@ public:
         fmmu_global_properties.logical_end_address += buffer_properties_read_.length;
         fmmu_global_properties.logical_full_length_read += buffer_properties_read_.length;
 
-        datagram_write + datagram_read;
+        auto queue = datagram_write + datagram_read;
 
-        do {
+        return telegram.transfer(queue);
-            telegram.transfer(datagram_write);
-        } while(datagram_write.get_all_wkc() < 0x0002);
 /*
-        do {
+        telegram.transfer(datagram_write);
-            telegram.transfer(datagram_write);
+        telegram.transfer(datagram_read);
-        } while(datagram_write.get_wkc() < 0x0001);
-
-        do {
-            telegram.transfer(datagram_read);
-        } while(datagram_read.get_wkc() < 0x0001);
 */
-        DebugP_log("datagram_read.get_wkc() = %d\r\n", datagram_read.get_wkc());
-        DebugP_log("datagram_write.get_wkc() = %d\r\n", datagram_write.get_wkc());
     }
 
 private:
@@ -387,7 +377,7 @@ class EthEcatBuffer {
 public:
     EthEcatBuffer(EthEcat& ecat): ecat_{ecat} { }
 
-    void init(uint16_t rx_eeprom_addr, uint16_t tx_eeprom_addr);
+    bool init(uint16_t rx_eeprom_addr, uint16_t tx_eeprom_addr);
 
     EthEcat& get_ecat() {
         return ecat_;
@@ -419,8 +409,8 @@ public:
         }
     }
 
-    void init_sync_manager(sync_manager sm_write, sync_manager sm_read);
+    bool init_sync_manager(sync_manager sm_write, sync_manager sm_read);
-    void init_fmmu(fmmu fmmu_write, fmmu fmmu_read);
+    bool init_fmmu(fmmu fmmu_write, fmmu fmmu_read);
 
 private:
     EthEcat& ecat_;

components/free_rtos/ethernet_industry/eth_ecat_datagram.hpp

@@ -23,11 +23,16 @@ namespace datagram {
 
 using TEcatWkc = uint16_t;
 
-class IEcatDatagram : protected queue::QueueEntity {
+class IEcatDatagram : public queue::QueueEntity {
     friend class queue::Queue<IEcatDatagram>;
 
 public:
     IEcatDatagram()
+    : expected_wkc_{0x0001}
+    { }
+
+    IEcatDatagram(TEcatWkc expected_wkc)
+    : expected_wkc_{expected_wkc}
     { }
 
     virtual ~IEcatDatagram() { };
@@ -41,6 +46,9 @@ public:
     virtual uint8_t* pack(uint8_t *raw) = 0;
     virtual uint8_t* unpack(uint8_t *raw) = 0;
 
+    virtual uint8_t* pack_all(uint8_t *raw) = 0;
+    virtual uint8_t* unpack_all(uint8_t *raw) = 0;
+
     TEcatDgHeader& get_header() {
         return header_;
     }
@@ -49,19 +57,54 @@ public:
         return wkc_;
     }
 
+    void reset_wkc() {
+        wkc_ = 0x0000;
+    }
+
+    bool as_expected() {
+        return (wkc_ >= expected_wkc_);
+    }
+
     TEcatWkc get_all_wkc() {
-        IEcatDatagram* next = static_cast<IEcatDatagram*>(get_next());
+        IEcatDatagram* next = this;
+        TEcatWkc wkc = 0x0000;
 
-        if(next == nullptr) {
+        do {
-            return wkc_;
+            wkc += next->get_wkc();
-        }
 
-        return wkc_ + next->get_all_wkc();
+            next = static_cast<IEcatDatagram*>(next->get_next());
+        } while(next != nullptr);
+
+        return wkc;
+    }
+
+    void reset_all_wkc() {
+        IEcatDatagram* next = this;
+
+        do {
+            next->reset_wkc();
+
+            next = static_cast<IEcatDatagram*>(next->get_next());
+        } while(next != nullptr);
+    }
+
+    bool all_as_expected() {
+        IEcatDatagram* next = this;
+        bool as_expected = true;
+
+        do {
+            as_expected = (as_expected && next->as_expected());
+
+            next = static_cast<IEcatDatagram*>(next->get_next());
+        } while(next != nullptr);
+
+        return as_expected;
     }
 
 protected:
     TEcatDgHeader header_;
     TEcatWkc wkc_{0x0000};
+    const TEcatWkc expected_wkc_{0x0001};
 
 private:
 
@@ -72,6 +115,16 @@ class EcatDatagram : public IEcatDatagram {
     static_assert(std::is_base_of<command::EcatCommandBase, CommandT>::value == true, "CommandT should be derived from ECatCommandBase");
 
 public:
+    EcatDatagram(CommandT&& command, const TEcatWkc expected_wkc, DataTypes&... data)
+    : IEcatDatagram{expected_wkc}
+    , command_{command}
+    , data_tuple_{data...} { }
+
+    EcatDatagram(CommandT&& command, const TEcatWkc expected_wkc, custom_tuple<DataTypes&...> data_tuple)
+    : IEcatDatagram{expected_wkc}
+    , command_{command}
+    , data_tuple_{data_tuple} { }
+
     EcatDatagram(CommandT&& command, DataTypes&... data)
     : IEcatDatagram{}
     , command_{command}
@@ -85,13 +138,45 @@ public:
     ~EcatDatagram() { }
 
     virtual uint8_t* pack(uint8_t *raw) override {
+        if(as_expected() == true) {
+            return raw;
+        }
+
         return pack_header(raw);
     }
 
     virtual uint8_t* unpack(uint8_t *raw) override {
+        if(as_expected() == true) {
+            return raw;
+        }
+
         return unpack_header(raw);
     }
 
+    virtual uint8_t* pack_all(uint8_t *raw) override {
+        IEcatDatagram* next = this;
+
+        do {
+            raw = next->pack(raw);
+
+            next = static_cast<IEcatDatagram*>(next->get_next());
+        } while(next != nullptr);
+
+        return raw;
+    }
+
+    virtual uint8_t* unpack_all(uint8_t *raw) override {
+        IEcatDatagram* next = this;
+
+        do {
+            raw = next->unpack(raw);
+
+            next = static_cast<IEcatDatagram*>(next->get_next());
+        } while(next != nullptr);
+
+        return raw;
+    }
+
 private:
     CommandT command_;
     custom_tuple<DataTypes&...> data_tuple_;

components/free_rtos/ethernet_industry/eth_ecat_eeprom.hpp

@@ -23,54 +23,55 @@ public:
     : telegram_{telegram} { }
 
     template<typename TypeT>
-    void wait_busy(typename TypeT::TSlaveAddress& slave_address) {
+    bool wait_busy(const datagram::TEcatWkc expected_wkc, typename TypeT::TSlaveAddress& slave_address) {
         using TCommand= command::EcatCommand<TypeT, command::RD>;
+
         std::array<uint16_t, 2> eeprom_config_status{0x0000, 0x0000};
-        datagram::EcatDatagram<TCommand, std::array<uint16_t, 2>> datagram{ {{slave_address, ECT_REG_EEPCFG}}, eeprom_config_status };
+        datagram::EcatDatagram<TCommand, std::array<uint16_t, 2>> datagram{ {{slave_address, ECT_REG_EEPCFG}}, expected_wkc, eeprom_config_status };
+        bool status;
 
         do {
-            telegram_.transfer(datagram);
+            status = telegram_.transfer(datagram);
-        }while((datagram.get_wkc() < 0x0001) || ((eeprom_config_status[0] & 0xFF00) != 0) || ((eeprom_config_status[1] & EC_ESTAT_BUSY) != 0));
+        } while((status == true) && ( ((eeprom_config_status[0] & 0xFF00) != 0) || ((eeprom_config_status[1] & EC_ESTAT_BUSY) != 0) ) );
+
+        return status;
     }
 
     template<typename TypeT>
-    void control_register(typename TypeT::TSlaveAddress& slave_address, ec_ecmdtype eeprom_cmd, uint16_t eeprom_address) {
+    bool control_register(const datagram::TEcatWkc expected_wkc, typename TypeT::TSlaveAddress& slave_address, ec_ecmdtype eeprom_cmd, uint16_t eeprom_address) {
         using TCommand = command::EcatCommand<TypeT, command::WR>;
+
         std::array<uint16_t, 2> request{eeprom_cmd, eeprom_address};
+        datagram::EcatDatagram<TCommand, std::array<uint16_t, 2>> datagram{ {{slave_address, ECT_REG_EEPCTL}}, expected_wkc, request };
 
-        datagram::EcatDatagram<TCommand, std::array<uint16_t, 2>> datagram{ {{slave_address, ECT_REG_EEPCTL}}, request };
+        return telegram_.transfer(datagram);
-
-        do {
-            telegram_.transfer(datagram);
-        } while(datagram.get_wkc() < 0x0001);
     }
 
     template<typename TypeT, typename DirT, typename... DataTypes>
-    void data_register(typename TypeT::TSlaveAddress& slave_address, DataTypes&... data) {
+    bool data_register(const datagram::TEcatWkc expected_wkc, typename TypeT::TSlaveAddress& slave_address, DataTypes&... data) {
         using TCommand = command::EcatCommand<TypeT, DirT>;
-        datagram::EcatDatagram<TCommand, DataTypes...> datagram{ {{slave_address, ECT_REG_EEPDAT}}, data... };
 
-        do {
+        datagram::EcatDatagram<TCommand, DataTypes...> datagram{ {{slave_address, ECT_REG_EEPDAT}}, expected_wkc, data... };
-            telegram_.transfer(datagram);
+
-        } while(datagram.get_wkc() < 0x0001);
+        return telegram_.transfer(datagram);
     }
 
     template<typename TypeT, typename... DataTypes>
-    void read(typename TypeT::TSlaveAddress& slave_address, uint16_t eeprom_address, DataTypes&... data) {
+    bool read(const datagram::TEcatWkc expected_wkc, typename TypeT::TSlaveAddress& slave_address, uint16_t eeprom_address, DataTypes&... data) {
-        wait_busy<TypeT>(slave_address);
+        if(wait_busy<TypeT>(expected_wkc, slave_address) != true) return false;
-        control_register<TypeT>(slave_address, EC_ECMD_READ, eeprom_address);
+        if(control_register<TypeT>(expected_wkc, slave_address, EC_ECMD_READ, eeprom_address) != true) return false;
-        wait_busy<TypeT>(slave_address);
+        if(wait_busy<TypeT>(expected_wkc, slave_address) != true) return false;
-        data_register<TypeT, command::RD, DataTypes...>(slave_address, data...);
+        if(data_register<TypeT, command::RD, DataTypes...>(expected_wkc, slave_address, data...) != true) return false;
-        wait_busy<TypeT>(slave_address);
+        if(wait_busy<TypeT>(expected_wkc, slave_address) != true) return false;
     }
     // 2 bytes (1 word) max
     template<typename TypeT, typename... DataTypes>
-    void write(typename TypeT::TSlaveAddress& slave_address, uint16_t eeprom_address, DataTypes&... data) {
+    bool write(const datagram::TEcatWkc expected_wkc, typename TypeT::TSlaveAddress& slave_address, uint16_t eeprom_address, DataTypes&... data) {
-        wait_busy<TypeT>(slave_address);
+        if(wait_busy<TypeT>(expected_wkc, slave_address) != true) return false;
-        data_register<TypeT, command::WR, DataTypes...>(slave_address, data...);
+        if(data_register<TypeT, command::WR, DataTypes...>(expected_wkc, slave_address, data...) != true) return false;
-        wait_busy<TypeT>(slave_address);
+        if(wait_busy<TypeT>(expected_wkc, slave_address) != true) return false;
-        control_register<TypeT>(slave_address, EC_ECMD_WRITE, eeprom_address);
+        if(control_register<TypeT>( expected_wkc, slave_address, EC_ECMD_WRITE, eeprom_address) != true) return false;
-        wait_busy<TypeT>(slave_address);
+        if(wait_busy<TypeT>(expected_wkc, slave_address) != true) return false;
     }
 
 private:

components/free_rtos/ethernet_industry/eth_ecat_queue.hpp

@@ -16,12 +16,11 @@ namespace queue {
 
 class QueueEntity {
 public:
-
-protected:
     QueueEntity* get_next() {
         return next_;
     }
 
+protected:
     void set_next(QueueEntity *next) {
         next_ = next;
     }

components/free_rtos/ethernet_industry/eth_ecat_telegram.cpp

@@ -11,8 +11,20 @@ namespace free_rtos {
 
 namespace telegram {
 
+const char EcatTelegramStatus::SuccessString[]       = "Success !";
+const char EcatTelegramStatus::BusyString[]          = "Busy. Transfer in progress...";
+const char EcatTelegramStatus::WarningString[]       = "Warning ! Check error counters";
+const char EcatTelegramStatus::FatalErrorString[]    = "Fatal error ! Transfer attempts exceeded";
+
+const EcatDescription EcatTelegramStatus::descriptions[] = {
+                                         {EcatTelegramStatus::SuccessString, sizeof(EcatTelegramStatus::SuccessString)},
+                                         {EcatTelegramStatus::BusyString, sizeof(EcatTelegramStatus::BusyString)},
+                                         {EcatTelegramStatus::WarningString, sizeof(EcatTelegramStatus::WarningString)},
+                                         {EcatTelegramStatus::FatalErrorString, sizeof(EcatTelegramStatus::FatalErrorString)}
+};
+
 int32_t EcatTelegram::Process(uint8_t *p_data, uint32_t len) {
-    // TODO: Не забывать вычитать из указателя sizeof(TEthFrameHeader) !
+    // TODO: Don't forget to subtract sizeof(TEthFrameHeader) !
     unpack(p_data - sizeof(TEthFrameHeader));
 
 /*
@@ -46,19 +58,12 @@ 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;
+    auto first = datagram_queue_.get_first();
 
     (void)p_eth_hdr;
     (void)p_hdr;
 
-    auto queue = datagram_queue_;  // Копия очереди
+    p_datagram_last = first->pack_all(p_datagram_first);
-    auto next = queue.dequeue();
-
-    while(next != nullptr) {
-        //DebugP_log((char*)"Packet packed\r\n");
-
-        p_datagram_last = next->pack(p_datagram_last);
-        next = queue.dequeue();
-    }
 
     p_hdr->bits.length = p_datagram_last - p_datagram_first;
 
@@ -70,59 +75,82 @@ 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;
+    auto first = datagram_queue_.get_first();
 
-    if(p_eth_hdr->prot_id != ETH_PROT_ECAT_LE) {
+    p_datagram_last = first->unpack_all(p_datagram_first);
-        DebugP_log((char*)"Error: wrong protocol ID\r\n");
-
-        return raw;
-    }
-
-    if(p_hdr->bits.type != static_cast<uint16_t>(ec_network::PROTOCOL_TYPE)) {
-        DebugP_log((char*)"Error: wrong ethercat protocol type\r\n");
-
-        return raw;
-    }
-
-    auto queue = datagram_queue_; // Копия очереди
-    auto next = queue.dequeue();
-
-    while(next != nullptr) {
-        //DebugP_log((char*)"Packet unpacked\r\n");
-
-        p_datagram_last = next->unpack(p_datagram_last);
-        next = queue.dequeue();
-    }
 
     return p_datagram_last;
 }
 
-void EcatTelegram::transfer() {
+bool EcatTelegram::transfer() {
-    //bool stat = eth_stack_.send_pkt(port_id_, ETH_PROT_ECAT_LE, 1);
+    auto first = datagram_queue_.get_first();
-    bool stat = tx_flow_.send(port_id_, this, 1);
+    uint32_t transfer_attempts = 0;
+    bool stat;
+    bool as_expected;
 
-    if(stat == false) {
+    if(first == nullptr) {
-        DebugP_log((char*)"telegram transfer error !\r\n");
+        status_.result = EcatTelegramResult::SUCCESS;
 
-        datagram_queue_.clear();
+        return true;
-
-        return;
     }
 
-    rx_sem_.pend();
-
     datagram_queue_.clear();
+    status_.result = EcatTelegramResult::BUSY;
+
+    first->reset_all_wkc();
+
+    while(1) {
+        transfer_attempts++;
+
+        if(transfer_attempts > max_transfer_attempts_) {
+            status_.attempts_exceeded_errors++;
+            status_.result = EcatTelegramResult::FATAL_ERROR;
+
+            DebugP_log((char*)"Transfer attempts exceeded !\r\n");
+            break;
+        }
+
+        // stat = eth_stack_.send_pkt(port_id_, ETH_PROT_ECAT_LE, 1);
+        stat = tx_flow_.send(port_id_, this, 1);
+
+        if(stat == false) {
+            status_.transfer_errors++;
+            status_.result = EcatTelegramResult::WARNING;
+
+            continue;
+        }
+
+        rx_sem_.pend();
+
+        as_expected = first->all_as_expected();
+
+        if(as_expected == false) {
+            status_.expected_wkc_errors++;
+            status_.result = EcatTelegramResult::WARNING;
+
+            continue;
+        }
+
+        if(status_.result == EcatTelegramResult::BUSY) {
+            status_.result = EcatTelegramResult::SUCCESS;
+        }
+
+        break;
+    }
+
+    return (status_.result != EcatTelegramResult::FATAL_ERROR);
 }
 
-void EcatTelegram::transfer(datagram::IEcatDatagram& next) {
+bool EcatTelegram::transfer(datagram::IEcatDatagram& next) {
     datagram_queue_ + next;
 
-    transfer();
+    return transfer();
 }
 
-void EcatTelegram::transfer(queue::Queue<datagram::IEcatDatagram>& next) {
+bool EcatTelegram::transfer(queue::Queue<datagram::IEcatDatagram>& next) {
     datagram_queue_ + next;
 
-    transfer();
+    return transfer();
 }
 
 }

components/free_rtos/ethernet_industry/eth_ecat_telegram.hpp

@@ -16,6 +16,37 @@ namespace free_rtos {
 
 namespace telegram {
 
+enum class EcatTelegramResult : uint16_t {
+    SUCCESS = 0,
+    BUSY,
+    WARNING,
+    FATAL_ERROR
+};
+
+struct EcatDescription {
+    const char* string;
+    const size_t length;
+};
+
+struct EcatTelegramStatus {
+    uint16_t transfer_errors;
+    uint16_t expected_wkc_errors;
+    uint16_t attempts_exceeded_errors;
+    EcatTelegramResult result = EcatTelegramResult::SUCCESS;
+
+    const EcatDescription& get_description() {
+        return descriptions[static_cast<size_t>(result)];
+    }
+
+private:
+    static const char SuccessString[];
+    static const char BusyString[];
+    static const char WarningString[];
+    static const char FatalErrorString[];
+
+    static const EcatDescription descriptions[];
+};
+
 class EcatTelegram : public Handler {
 public:
     EcatTelegram(Eth& eth)
@@ -23,6 +54,10 @@ public:
     , tx_flow_{*eth.getTxFlowPtr()}
     , eth_stack_{*eth.getEthStackPtr()} { }
 
+    const EcatTelegramStatus& get_status() {
+        return status_;
+    }
+
     virtual int32_t Process(uint8_t *p_data, uint32_t len) override;
     virtual uint32_t Sender(uint8_t *p_data, size_t scatter_segment) override;
 
@@ -31,10 +66,12 @@ public:
         eth_.getEthStackPtr()->Register(ETH_PROT_ECAT_LE, this);
     }
 
-    void transfer(datagram::IEcatDatagram& next);
+    bool transfer(datagram::IEcatDatagram& next);
-    void transfer(queue::Queue<datagram::IEcatDatagram>& next);
+    bool transfer(queue::Queue<datagram::IEcatDatagram>& next);
 
 private:
+    static constexpr uint32_t max_transfer_attempts_ = 3;
+
     Eth& eth_;
     EthTxFlowIface& tx_flow_;
     EthStackIface& eth_stack_;
@@ -45,7 +82,9 @@ private:
     queue::Queue<datagram::IEcatDatagram> datagram_queue_;
     //datagram::IEcatDatagram *datagram_queue_{nullptr};
 
-    void transfer();
+    EcatTelegramStatus status_;
+
+    bool transfer();
 
     uint8_t* pack(uint8_t *raw);
     uint8_t* unpack(uint8_t *raw);