From c9aa8f9b4c5a40f49a1870029eac4dc61f0c1add Mon Sep 17 00:00:00 2001 From: algin Date: Wed, 6 Sep 2023 18:09:23 +0300 Subject: [PATCH] =?UTF-8?q?dev(SF-33):=20=D0=94=D0=BE=D0=B1=D0=B0=D0=B2?= =?UTF-8?q?=D0=BB=D0=B5=D0=BD=20=D0=BA=D0=BE=D0=BD=D1=82=D1=80=D0=BE=D0=BB?= =?UTF-8?q?=D1=8C=20=D1=81=D0=BE=D0=B5=D0=B4=D0=B8=D0=BD=D0=B5=D0=BD=D0=B8?= =?UTF-8?q?=D1=8F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../CoE/eth_ecat_pdo_fmmu.cpp | 62 +++--- .../CoE/eth_ecat_pdo_fmmu.hpp | 31 ++- .../CoE/eth_ecat_sdo_mailbox.cpp | 13 +- .../CoE/eth_ecat_sdo_mailbox.hpp | 144 ++++++++----- .../free_rtos/ethernet_industry/eth_ecat.cpp | 191 +++++++++++------- .../free_rtos/ethernet_industry/eth_ecat.hpp | 98 +++++---- .../ethernet_industry/eth_ecat_api.cpp | 51 ++++- .../ethernet_industry/eth_ecat_api.hpp | 4 + .../ethernet_industry/eth_ecat_buffer.cpp | 30 ++- .../ethernet_industry/eth_ecat_buffer.hpp | 100 +++++---- .../ethernet_industry/eth_ecat_datagram.hpp | 97 ++++++++- .../ethernet_industry/eth_ecat_eeprom.hpp | 55 ++--- .../ethernet_industry/eth_ecat_queue.hpp | 3 +- .../ethernet_industry/eth_ecat_telegram.cpp | 118 ++++++----- .../ethernet_industry/eth_ecat_telegram.hpp | 45 ++++- 15 files changed, 688 insertions(+), 354 deletions(-) diff --git a/components/free_rtos/ethernet_industry/CoE/eth_ecat_pdo_fmmu.cpp b/components/free_rtos/ethernet_industry/CoE/eth_ecat_pdo_fmmu.cpp index c537a8d..e684dd4 100644 --- a/components/free_rtos/ethernet_industry/CoE/eth_ecat_pdo_fmmu.cpp +++ b/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 pdo_write(logical_full_length_write, 0x00); custom_tuple&> data_tuple_read{pdo_read}; custom_tuple&> data_tuple_write{pdo_write}; + bool status; - read(0, pdo_write); // read to pdo_write ! + status = read(0, pdo_write); // read to pdo_write ! -/* - for(uint8_t& byte : process_data) { - DebugP_log("0x%01x", byte); + if(status != true) { + return status; + } + + 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); - 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); -/* - for(uint8_t& byte : process_data) { - DebugP_log("0x%01x", byte); + if(status != true) { + return status; } - 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 ! - write(0, pdo_write); + status = read(0, pdo_write); // read to 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_++; } } diff --git a/components/free_rtos/ethernet_industry/CoE/eth_ecat_pdo_fmmu.hpp b/components/free_rtos/ethernet_industry/CoE/eth_ecat_pdo_fmmu.hpp index 4c042c3..bfc00e8 100644 --- a/components/free_rtos/ethernet_industry/CoE/eth_ecat_pdo_fmmu.hpp +++ b/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 - void write(address::Offset offset, DataTypes&... data) { + bool write(address::Offset offset, DataTypes&... data) { using TDatagram = datagram::EcatDatagram; 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 { - telegram.transfer(datagram); - } while(datagram.get_wkc() < 0x0001); + return telegram.transfer(datagram); } template - void read(address::Offset offset, DataTypes&... data) { + bool read(address::Offset offset, DataTypes&... data) { using TDatagram = datagram::EcatDatagram; 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 { - telegram.transfer(datagram); - } while(datagram.get_wkc() < 0x0001); + return telegram.transfer(datagram); } template - void read_write(address::Offset offset_read, custom_tuple data_read, address::Offset offset_write, custom_tuple data_write) { + bool read_write(address::Offset offset_read, custom_tuple data_read, address::Offset offset_write, custom_tuple data_write) { using TDatagramRead = datagram::EcatDatagram; using TDatagramWrite = datagram::EcatDatagram; 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 { - telegram.transfer(queue); - } while(datagram_read.get_all_wkc() < 0x0001); + return telegram.transfer(queue); } }; diff --git a/components/free_rtos/ethernet_industry/CoE/eth_ecat_sdo_mailbox.cpp b/components/free_rtos/ethernet_industry/CoE/eth_ecat_sdo_mailbox.cpp index 94b2c0d..716bad1 100644 --- a/components/free_rtos/ethernet_industry/CoE/eth_ecat_sdo_mailbox.cpp +++ b/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(telegram, pdo_map, pdo_map_rx_index); + status = (status && sdo_mailbox_slave.pdo_map_read(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(telegram, pdo_map, pdo_map_tx_index); + status = (status && sdo_mailbox_slave.pdo_map_read(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; } } diff --git a/components/free_rtos/ethernet_industry/CoE/eth_ecat_sdo_mailbox.hpp b/components/free_rtos/ethernet_industry/CoE/eth_ecat_sdo_mailbox.hpp index 569dc21..c944634 100644 --- a/components/free_rtos/ethernet_industry/CoE/eth_ecat_sdo_mailbox.hpp +++ b/components/free_rtos/ethernet_industry/CoE/eth_ecat_sdo_mailbox.hpp @@ -147,45 +147,49 @@ public: : buffer_slave_(mailbox_slave) { } template - void wait_available(telegram::EcatTelegram& telegram) { + bool wait_available(telegram::EcatTelegram& telegram, const datagram::TEcatWkc expected_wkc) { using TCommand = command::EcatCommand; auto slave_address = buffer_slave_.get_slave().get_slave_address(); std::array& buffer_regs = buffer_slave_.get_buffer_regs(); uint8_t sm_status{0x00}; - datagram::EcatDatagram datagram{ {{slave_address, buffer_regs[MailboxesRegs::AVAILABLE]}}, sm_status }; + datagram::EcatDatagram datagram{ {{slave_address, buffer_regs[MailboxesRegs::AVAILABLE]}}, expected_wkc, sm_status }; + bool status; do { - telegram.transfer(datagram); - } while((datagram.get_wkc() < 0x0001) || ((sm_status & 0x08) == 0)); + status = telegram.transfer(datagram); + } while((status == true) && ((sm_status & 0x08) == 0)); + + return status; } template - void wait_empty(telegram::EcatTelegram& telegram) { + bool wait_empty(telegram::EcatTelegram& telegram, const datagram::TEcatWkc expected_wkc) { using TCommand = command::EcatCommand; auto slave_address = buffer_slave_.get_slave().get_slave_address(); std::array& buffer_regs = buffer_slave_.get_buffer_regs(); uint8_t sm_status{0x00}; - datagram::EcatDatagram datagram{ {{slave_address, buffer_regs[MailboxesRegs::EMPTY]}}, sm_status }; + datagram::EcatDatagram datagram{ {{slave_address, buffer_regs[MailboxesRegs::EMPTY]}}, expected_wkc, sm_status }; + bool status; do { - telegram.transfer(datagram); - } while((datagram.get_wkc() < 0x0001) || ((sm_status & 0x08) != 0)); + status = telegram.transfer(datagram); + } while((status == true) && ((sm_status & 0x08) != 0)); + + return status; } template - void empty(telegram::EcatTelegram& telegram) { + bool empty(telegram::EcatTelegram& telegram, const datagram::TEcatWkc expected_wkc) { using TCommand = command::EcatCommand; auto slave_address = buffer_slave_.get_slave().get_slave_address(); std::array& buffer_regs = buffer_slave_.get_buffer_regs(); uint8_t sm_status{0x00}; - datagram::EcatDatagram datagram{ {{slave_address, buffer_regs[MailboxesRegs::AVAILABLE]}}, sm_status }; + datagram::EcatDatagram datagram{ {{slave_address, buffer_regs[MailboxesRegs::AVAILABLE]}}, expected_wkc, sm_status }; - do { - telegram.transfer(datagram); - } while(datagram.get_wkc() < 0x0001); + return telegram.transfer(datagram); } void increment_counter() { @@ -197,7 +201,7 @@ public: } template - 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; using TDatagram = datagram::EcatDatagram; @@ -213,17 +217,22 @@ public: .cnt = static_cast(counter_) }; Padding padding{buffer_slave_.get_buffer_properties_write().length - sizeof(MailboxHeader) - custom_tuple::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 { - telegram.transfer(datagram); - } while(datagram.get_wkc() < 0x0001); + status = telegram.transfer(datagram); + + if(status != true) { + return status; + } increment_counter(); + + return status; } template - 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; using TDatagram = datagram::EcatDatagram; @@ -231,11 +240,9 @@ public: std::array& buffer_regs = buffer_slave_.get_buffer_regs(); MailboxHeader header; Padding padding{buffer_slave_.get_buffer_properties_read().length - sizeof(MailboxHeader) - custom_tuple::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 { - telegram.transfer(datagram); - } while(datagram.get_wkc() < 0x0001); + return telegram.transfer(datagram); //DebugP_log("header.length = %d\r\n", header.length); //DebugP_log("header.address = %d\r\n", header.address); @@ -246,21 +253,21 @@ public: } template - void receive(telegram::EcatTelegram& telegram, DataTypes&... data) { - wait_available(telegram); - receive_data(telegram, data...); + bool receive(telegram::EcatTelegram& telegram, const datagram::TEcatWkc expected_wkc, DataTypes&... data) { + if(wait_available(telegram, expected_wkc) != true) return false; + if(receive_data(telegram, expected_wkc, data...) != true) return false; } template - void send(telegram::EcatTelegram& telegram, uint16_t channel, uint16_t priority, ProtocolType type, DataTypes&... data) { - empty(telegram); - wait_empty(telegram); - send_data(telegram, channel, priority, type, data...); - wait_empty(telegram); + bool send(telegram::EcatTelegram& telegram, const datagram::TEcatWkc expected_wkc, uint16_t channel, uint16_t priority, ProtocolType type, DataTypes&... data) { + if(empty(telegram, expected_wkc) != true) return false; + if(wait_empty(telegram, expected_wkc) != true) return false; + if(send_data(telegram, expected_wkc, channel, priority, type, data...) != true) return false; + if(wait_empty(telegram, expected_wkc) != true) return false; } template - 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::size}; + bool status; - send(telegram, 0, 0, ProtocolType::CoE, elements, complete_size, data...); - receive(telegram, elements, complete_size); + status = send(telegram, expected_wkc, 0, 0, ProtocolType::CoE, elements, complete_size, data...); + + if(status != true) { + return status; + } + + status = receive(telegram, expected_wkc, elements, complete_size); + + if(status != true) { + return status; + } if( (elements.coe_header.service != static_cast(Service::SDO_RESPONSE)) || (elements.command_specifier.command_spec != static_cast(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 - 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(telegram, 0, 0, ProtocolType::CoE, elements, complete_size); - receive(telegram, elements, complete_size, data...); + status = send(telegram, expected_wkc, 0, 0, ProtocolType::CoE, elements, complete_size); + + if(status != true) { + return status; + } + + status = receive(telegram, expected_wkc, elements, complete_size, data...); + + if(status != true) { + return status; + } if( (elements.coe_header.service != static_cast(Service::SDO_RESPONSE)) || (elements.command_specifier.command_spec != static_cast(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 - 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{}); - sdo_read(telegram, pdo_map_index, 0x00, pdo_block_count); + status = sdo_read(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(telegram, pdo_map_index, pdo_map_subindex, pdo_block_index); + status = sdo_read(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{}); - sdo_read(telegram, pdo_block_index, 0, pdo_block_object_count); + status = sdo_read(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(telegram, pdo_block_index, pdo_block_subindex, descriptor); + status = sdo_read(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& get_pdo_map() { return pdo_map_; diff --git a/components/free_rtos/ethernet_industry/eth_ecat.cpp b/components/free_rtos/ethernet_industry/eth_ecat.cpp index d656609..72a3a65 100644 --- a/components/free_rtos/ethernet_industry/eth_ecat.cpp +++ b/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 a{ {{broadcast, ECT_REG_DLPORT}}, a_data_out }; + datagram::EcatDatagram a{ {{broadcast, ECT_REG_DLPORT}}, expected_wkc, a_data_out }; /* set IRQ mask */ uint16_t b_data_out{0x0C04}; - datagram::EcatDatagram b{ {{broadcast, ECT_REG_IRQMASK}}, b_data_out }; + datagram::EcatDatagram b{ {{broadcast, ECT_REG_IRQMASK}}, expected_wkc, b_data_out }; /* reset CRC counters */ std::array c_data_out; c_data_out.fill(0x00); - datagram::EcatDatagram> c{ {{broadcast, ECT_REG_RXERR}}, c_data_out }; + datagram::EcatDatagram> c{ {{broadcast, ECT_REG_RXERR}}, expected_wkc, c_data_out }; /* reset FMMU's */ std::array d_data_out; d_data_out.fill(0x00); - datagram::EcatDatagram> d{ {{broadcast, ECT_REG_FMMU0}}, d_data_out }; + datagram::EcatDatagram> d{ {{broadcast, ECT_REG_FMMU0}}, expected_wkc, d_data_out }; /* reset SyncM */ std::array e_data_out; e_data_out.fill(0x00); - datagram::EcatDatagram> e{ {{broadcast, ECT_REG_SM0}}, e_data_out }; + datagram::EcatDatagram> e{ {{broadcast, ECT_REG_SM0}}, expected_wkc, e_data_out }; /* reset activation register */ uint8_t f_data_out{0x00}; - datagram::EcatDatagram f{ {{broadcast, ECT_REG_DCSYNCACT}}, f_data_out }; + datagram::EcatDatagram f{ {{broadcast, ECT_REG_DCSYNCACT}}, expected_wkc, f_data_out }; /* reset system time+ofs */ std::array g_data_out; g_data_out.fill(0x00); - datagram::EcatDatagram> g{ {{broadcast, ECT_REG_DCSYSTIME}}, g_data_out }; + datagram::EcatDatagram> g{ {{broadcast, ECT_REG_DCSYSTIME}}, expected_wkc, g_data_out }; /* DC speedstart */ uint16_t h_data_out{0x1000}; - datagram::EcatDatagram h{ {{broadcast, ECT_REG_DCSPEEDCNT}}, h_data_out }; + datagram::EcatDatagram h{ {{broadcast, ECT_REG_DCSPEEDCNT}}, expected_wkc, h_data_out }; /* DC filt expr */ uint16_t i_data_out{0x0C00}; - datagram::EcatDatagram i{ {{broadcast, ECT_REG_DCTIMEFILT}}, i_data_out }; + datagram::EcatDatagram i{ {{broadcast, ECT_REG_DCTIMEFILT}}, expected_wkc, i_data_out }; /* Ignore Alias register */ uint8_t j_data_out{0x00}; - datagram::EcatDatagram j{ {{broadcast, ECT_REG_DLALIAS}}, j_data_out }; + datagram::EcatDatagram 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 k{ {{broadcast, ECT_REG_ALCTL}}, k_data_out }; + datagram::EcatDatagram k{ {{broadcast, ECT_REG_ALCTL}}, expected_wkc, k_data_out }; /* force Eeprom from PDI */ uint8_t l_data_out{0x02}; - datagram::EcatDatagram l{ {{broadcast, ECT_REG_EEPCFG}}, l_data_out }; + datagram::EcatDatagram l{ {{broadcast, ECT_REG_EEPCFG}}, expected_wkc, l_data_out }; /* set Eeprom to master */ uint8_t m_data_out{0x00}; - datagram::EcatDatagram m{ {{broadcast, ECT_REG_EEPCFG}}, m_data_out }; + datagram::EcatDatagram 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 { - telegram_.transfer(queue); - } while(a.get_all_wkc() < 0x0001); + status = telegram_.transfer(queue); //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*)"b.get_wkc() = %d\r\n", b.get_wkc()); - DebugP_log((char*)"c.get_wkc() = %d\r\n", c.get_wkc()); - DebugP_log((char*)"d.get_wkc() = %d\r\n", d.get_wkc()); - DebugP_log((char*)"e.get_wkc() = %d\r\n", e.get_wkc()); - DebugP_log((char*)"f.get_wkc() = %d\r\n", f.get_wkc()); - DebugP_log((char*)"g.get_wkc() = %d\r\n", g.get_wkc()); - DebugP_log((char*)"h.get_wkc() = %d\r\n", h.get_wkc()); - DebugP_log((char*)"i.get_wkc() = %d\r\n", i.get_wkc()); - DebugP_log((char*)"j.get_wkc() = %d\r\n", j.get_wkc()); - DebugP_log((char*)"k.get_wkc() = %d\r\n", k.get_wkc()); - DebugP_log((char*)"l.get_wkc() = %d\r\n", l.get_wkc()); - DebugP_log((char*)"m.get_wkc() = %d\r\n", m.get_wkc()); + DebugP_log((char*)"Deact loop manual wkc = %d\r\n", a.get_wkc()); + DebugP_log((char*)"Set IRQ mask wkc = %d\r\n", b.get_wkc()); + DebugP_log((char*)"Reset CRC counters wkc = %d\r\n", c.get_wkc()); + DebugP_log((char*)"Reset FMMU's wkc = %d\r\n", d.get_wkc()); + DebugP_log((char*)"Reset SyncM wkc = %d\r\n", e.get_wkc()); + DebugP_log((char*)"Reset activation register wkc = %d\r\n", f.get_wkc()); + DebugP_log((char*)"Reset system time+ofs wkc = %d\r\n", g.get_wkc()); + DebugP_log((char*)"DC speedstart wkc = %d\r\n", h.get_wkc()); + DebugP_log((char*)"DC filt expr wkc = %d\r\n", i.get_wkc()); + DebugP_log((char*)"Ignore Alias register wkc = %d\r\n", j.get_wkc()); + DebugP_log((char*)"Reset all slaves to Init wkc = %d\r\n", k.get_wkc()); + DebugP_log((char*)"Force Eeprom from PDI wkc = %d\r\n", l.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 datagram{ {{broadcast, ECT_REG_TYPE}}, data }; + datagram::EcatDatagram 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; + const datagram::TEcatWkc expected_wkc = 1; std::vector 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() }); + datagrams.emplace_back(TDatagram{ {{position, ECT_REG_STADR}}, expected_wkc, slaves_.back().get_slave_address() }); } queue::Queue queue; @@ -163,64 +167,107 @@ void EthEcat::set_addresses_of_slaves(uint16_t number_of_slaves, uint16_t addres queue + datagrams[i]; } - do { - telegram_.transfer(queue); - } while(datagrams[0].get_all_wkc() < number_of_slaves); + status = telegram_.transfer(queue); + + 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; + const datagram::TEcatWkc expected_wkc = 1; std::vector datagrams; uint16_t number_of_slaves = slaves_.size(); + queue::Queue queue; + bool status; datagrams.reserve(number_of_slaves); for(EcatSlave& slave : slaves_) { - datagrams.emplace_back(TDatagram{ {{slave.get_slave_address(), ECT_REG_STADR}}, slave.get_slave_address() }); + datagrams.emplace_back(TDatagram{ {{slave.get_slave_address(), ECT_REG_STADR}}, expected_wkc, slave.get_slave_address() }); } - queue::Queue queue; - for(uint16_t i = 0; i < number_of_slaves; i++) { queue + datagrams[i]; } - do { - telegram_.transfer(queue); - } while(datagrams[0].get_all_wkc() < number_of_slaves); + status = telegram_.transfer(queue); + + 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(), slave.get_slave_address()); } + + 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(); - uint16_t number_of_slaves = slaves_detecting(); + status = set_slaves_to_default(); + + 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); - get_addresses_of_slaves(); + status = (number_of_slaves > 0); - 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(telegram_); + status = (status && slave.enable_PDI(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(telegram_); + success = (success && slave.init_to_preop(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(telegram_); + success = (success && slave.preop_to_safeop(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(); - bool success; + const uint16_t number_of_slaves = slaves_.size(); + 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 datagram{ {{broadcast, ECT_REG_ALCTL}}, data }; + datagram::EcatDatagram datagram{ {{broadcast, ECT_REG_ALCTL}}, number_of_slaves, data }; - do { - telegram_.transfer(datagram); - } while(datagram.get_wkc() < number_of_slaves); + status = telegram_.transfer(datagram); + + if(status != true) { + return status; + } } process_sem_.post(); init_sem_.pend(); { - datagram::EcatDatagram datagram{ {{broadcast, ECT_REG_ALSTAT}}, stat, zero }; + datagram::EcatDatagram datagram{ {{broadcast, ECT_REG_ALSTAT}}, number_of_slaves, stat, zero }; - do { - telegram_.transfer(datagram); - } while(datagram.get_wkc() < number_of_slaves); - } + status = telegram_.transfer(datagram); - success = (stat.state == EC_STATE_OPERATIONAL) && (stat.fault == 0); - - if(success == true) { - ecat_timer_.Start(); + if(status != true) { + return status; + } } //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; } } diff --git a/components/free_rtos/ethernet_industry/eth_ecat.hpp b/components/free_rtos/ethernet_industry/eth_ecat.hpp index 43ea62f..9842ee0 100644 --- a/components/free_rtos/ethernet_industry/eth_ecat.hpp +++ b/components/free_rtos/ethernet_industry/eth_ecat.hpp @@ -53,41 +53,44 @@ public: } template - void enable_PDI(telegram::EcatTelegram& telegram) { + bool enable_PDI(telegram::EcatTelegram& telegram, const datagram::TEcatWkc expected_wkc) { using TCommand = command::EcatCommand; auto slave_address = get_slave_address(); uint8_t data{0x01}; - datagram::EcatDatagram datagram{ {{slave_address, ECT_REG_EEPCFG}}, data }; + datagram::EcatDatagram datagram{ {{slave_address, ECT_REG_EEPCFG}}, expected_wkc, data }; - do { - telegram.transfer(datagram); - } while(datagram.get_wkc() < 0x0001); + return telegram.transfer(datagram); } template - 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(); ALSTAT stat{0x0000, 0x0000}; + bool status; { using TCommand = command::EcatCommand; uint16_t data{EC_STATE_PRE_OP|EC_STATE_ACK}; - datagram::EcatDatagram datagram{ {{slave_address, ECT_REG_ALCTL}}, data }; + datagram::EcatDatagram datagram{ {{slave_address, ECT_REG_ALCTL}}, expected_wkc, data }; - do { - telegram.transfer(datagram); - } while(datagram.get_wkc() < 0x0001); + status = telegram.transfer(datagram); + + if(status != true) { + return status; + } } ClockP_usleep(5000000ul); { using TCommand = command::EcatCommand; - datagram::EcatDatagram datagram{ {{slave_address, ECT_REG_ALSTAT}}, stat }; + datagram::EcatDatagram datagram{ {{slave_address, ECT_REG_ALSTAT}}, expected_wkc, stat }; - do { - telegram.transfer(datagram); - } while(datagram.get_wkc() < 0x0001); + status = telegram.transfer(datagram); + + 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; uint16_t stat_code{0x0000}; - datagram::EcatDatagram datagram{ {{slave_address, ECT_REG_ALSTATCODE}}, stat_code}; + datagram::EcatDatagram datagram{ {{slave_address, ECT_REG_ALSTATCODE}}, expected_wkc, stat_code}; - do { - telegram.transfer(datagram); - } while(datagram.get_wkc() < 0x0001); + status = telegram.transfer(datagram); + + 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 - 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(); ALSTAT stat{0x0000, 0x0000}; uint32_t zero{0x00000000}; + bool status; { using TCommand = command::EcatCommand; uint16_t data{EC_STATE_SAFE_OP}; - datagram::EcatDatagram datagram{ {{slave_address, ECT_REG_ALCTL}}, data }; + datagram::EcatDatagram datagram{ {{slave_address, ECT_REG_ALCTL}}, expected_wkc, data }; - do { - telegram.transfer(datagram); - } while(datagram.get_wkc() < 0x0001); + status = telegram.transfer(datagram); + + if(status != true) { + return status; + } } ClockP_usleep(1000000ul); { using TCommand = command::EcatCommand; - datagram::EcatDatagram datagram{ {{slave_address, ECT_REG_ALSTAT}}, stat, zero }; + datagram::EcatDatagram datagram{ {{slave_address, ECT_REG_ALSTAT}}, expected_wkc, stat, zero }; - do { - telegram.transfer(datagram); - } while(datagram.get_wkc() < 0x0001); + status = telegram.transfer(datagram); + + 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; uint16_t stat_code{0x0000}; - datagram::EcatDatagram datagram{ {{slave_address, ECT_REG_ALSTATCODE}}, stat_code}; + datagram::EcatDatagram datagram{ {{slave_address, ECT_REG_ALSTATCODE}}, expected_wkc, stat_code}; - do { - telegram.transfer(datagram); - } while(datagram.get_wkc() < 0x0001); + status = telegram.transfer(datagram); + + 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); - void get_addresses_of_slaves(); + bool set_addresses_of_slaves(uint16_t number_of_slaves, uint16_t address_base); + 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_; } diff --git a/components/free_rtos/ethernet_industry/eth_ecat_api.cpp b/components/free_rtos/ethernet_industry/eth_ecat_api.cpp index 8f45106..5aa12e4 100644 --- a/components/free_rtos/ethernet_industry/eth_ecat_api.cpp +++ b/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); - ecat_buffer_sdo_.init_sync_manager(sync_manager::SYNC_M0, sync_manager::SYNC_M1); + if(status != true) { + 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); - ecat_buffer_pdo_.init_fmmu(fmmu::FMMU0, fmmu::FMMU1); + status = ecat_buffer_pdo_.init_sync_manager(sync_manager::SYNC_M2, sync_manager::SYNC_M3); + + 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); diff --git a/components/free_rtos/ethernet_industry/eth_ecat_api.hpp b/components/free_rtos/ethernet_industry/eth_ecat_api.hpp index 37477b0..e6e3567 100644 --- a/components/free_rtos/ethernet_industry/eth_ecat_api.hpp +++ b/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 ecat_sdo_mailbox::CompleteSize sdo_write(size_t slave_index, uint16_t index, uint8_t subindex, DataTypes&... data) { return ecat_sdo_mailbox_.sdo_write(slave_index, index, subindex, data...); diff --git a/components/free_rtos/ethernet_industry/eth_ecat_buffer.cpp b/components/free_rtos/ethernet_industry/eth_ecat_buffer.cpp index a663ab3..c48d069 100644 --- a/components/free_rtos/ethernet_industry/eth_ecat_buffer.cpp +++ b/components/free_rtos/ethernet_industry/eth_ecat_buffer.cpp @@ -14,9 +14,11 @@ namespace ecat_buffer { constexpr std::array EcatBufferSlave::sync_managers_; constexpr std::array 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& 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(eeprom, rx_eeprom_addr, tx_eeprom_addr); + status = (status && buffer_slave.read_buffer_info_from_eeprom(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(telegram, sm_write, sm_read); + status = (status && buffer_slave.init_sync_manager(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(telegram, fmmu_write, fmmu_global_properties_); + status = (status && buffer_slave.init_fmmu_write(telegram, expected_wkc, fmmu_write, fmmu_global_properties_)); + } + + if(status != true) { + return status; } for(EcatBufferSlave& buffer_slave : buffer_slaves_) { - buffer_slave.init_fmmu_read(telegram, fmmu_read, fmmu_global_properties_); + status = (status && buffer_slave.init_fmmu_read(telegram, expected_wkc, fmmu_read, fmmu_global_properties_)); } + + return status; } } diff --git a/components/free_rtos/ethernet_industry/eth_ecat_buffer.hpp b/components/free_rtos/ethernet_industry/eth_ecat_buffer.hpp index 451ccaa..0233e61 100644 --- a/components/free_rtos/ethernet_industry/eth_ecat_buffer.hpp +++ b/components/free_rtos/ethernet_industry/eth_ecat_buffer.hpp @@ -168,71 +168,74 @@ public: } template - 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(); + bool status; - eeprom.read(slave_address, rx_eeprom_addr, buffer_properties_write_); - eeprom.read(slave_address, tx_eeprom_addr, buffer_properties_read_); + status = eeprom.read(expected_wkc, slave_address, rx_eeprom_addr, buffer_properties_write_); + + if(status != true) { + return status; + } + + status = eeprom.read(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_read_ = 0x%04x\r\n", buffer_properties_read_); + //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_); + + return status; } template datagram::EcatDatagram, 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, BufferProperties, uint32_t>; auto slave_address = slave_.get_slave_address(); - 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 - 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(sm_write)]; - auto datagram_write = make_sync_manager_datagram(sync_manager_write, buffer_properties_write_); + auto datagram_write = make_sync_manager_datagram(expected_wkc, sync_manager_write, buffer_properties_write_); SyncManager sync_manager_read = sync_managers_[static_cast(sm_read)]; - auto datagram_read = make_sync_manager_datagram(sync_manager_read, buffer_properties_read_); + auto datagram_read = make_sync_manager_datagram(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()); - DebugP_log((char*)"datagram_read.get_wkc() = %d\r\n", datagram_read.get_wkc()); + return telegram.transfer(queue); +/* + telegram.transfer(datagram_write); + telegram.transfer(datagram_read); +*/ } template datagram::EcatDatagram, 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, FMMUSettings>; auto slave_address = slave_.get_slave_address(); address::Offset offset = fmmu_regs_[static_cast(fmmu_x)]; - return TDatagram{ {{slave_address, offset}}, settings}; + return TDatagram{ {{slave_address, offset}}, expected_wkc, settings}; } template - 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(fmmu, settings); + auto datagram = make_fmmu_datagram(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 { - telegram.transfer(datagram); - } while(datagram.get_wkc() < 0x0001); + return telegram.transfer(datagram); } template - 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(fmmu, settings); + auto datagram = make_fmmu_datagram(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 { - telegram.transfer(datagram); - } while(datagram.get_wkc() < 0x0001); + return telegram.transfer(datagram); } template - 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(fmmu_write, settings_write); + auto datagram_write = make_fmmu_datagram(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(fmmu_read, settings_read); + auto datagram_read = make_fmmu_datagram(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 { - telegram.transfer(datagram_write); - } while(datagram_write.get_all_wkc() < 0x0002); + return telegram.transfer(queue); /* - do { - telegram.transfer(datagram_write); - } while(datagram_write.get_wkc() < 0x0001); - - do { - telegram.transfer(datagram_read); - } while(datagram_read.get_wkc() < 0x0001); + telegram.transfer(datagram_write); + telegram.transfer(datagram_read); */ - 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); - void init_fmmu(fmmu fmmu_write, fmmu fmmu_read); + bool init_sync_manager(sync_manager sm_write, sync_manager sm_read); + bool init_fmmu(fmmu fmmu_write, fmmu fmmu_read); private: EthEcat& ecat_; diff --git a/components/free_rtos/ethernet_industry/eth_ecat_datagram.hpp b/components/free_rtos/ethernet_industry/eth_ecat_datagram.hpp index 6ae3337..720556c 100644 --- a/components/free_rtos/ethernet_industry/eth_ecat_datagram.hpp +++ b/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; 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(get_next()); + IEcatDatagram* next = this; + TEcatWkc wkc = 0x0000; - if(next == nullptr) { - return wkc_; - } + do { + wkc += next->get_wkc(); - return wkc_ + next->get_all_wkc(); + next = static_cast(next->get_next()); + } while(next != nullptr); + + return wkc; + } + + void reset_all_wkc() { + IEcatDatagram* next = this; + + do { + next->reset_wkc(); + + next = static_cast(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(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::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 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(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(next->get_next()); + } while(next != nullptr); + + return raw; + } + private: CommandT command_; custom_tuple data_tuple_; diff --git a/components/free_rtos/ethernet_industry/eth_ecat_eeprom.hpp b/components/free_rtos/ethernet_industry/eth_ecat_eeprom.hpp index 1acd494..73b057c 100644 --- a/components/free_rtos/ethernet_industry/eth_ecat_eeprom.hpp +++ b/components/free_rtos/ethernet_industry/eth_ecat_eeprom.hpp @@ -23,54 +23,55 @@ public: : telegram_{telegram} { } template - 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; + std::array eeprom_config_status{0x0000, 0x0000}; - datagram::EcatDatagram> datagram{ {{slave_address, ECT_REG_EEPCFG}}, eeprom_config_status }; + datagram::EcatDatagram> datagram{ {{slave_address, ECT_REG_EEPCFG}}, expected_wkc, eeprom_config_status }; + bool status; do { - telegram_.transfer(datagram); - }while((datagram.get_wkc() < 0x0001) || ((eeprom_config_status[0] & 0xFF00) != 0) || ((eeprom_config_status[1] & EC_ESTAT_BUSY) != 0)); + status = telegram_.transfer(datagram); + } while((status == true) && ( ((eeprom_config_status[0] & 0xFF00) != 0) || ((eeprom_config_status[1] & EC_ESTAT_BUSY) != 0) ) ); + + return status; } template - 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; + std::array request{eeprom_cmd, eeprom_address}; + datagram::EcatDatagram> datagram{ {{slave_address, ECT_REG_EEPCTL}}, expected_wkc, request }; - datagram::EcatDatagram> datagram{ {{slave_address, ECT_REG_EEPCTL}}, request }; - - do { - telegram_.transfer(datagram); - } while(datagram.get_wkc() < 0x0001); + return telegram_.transfer(datagram); } template - 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; - datagram::EcatDatagram datagram{ {{slave_address, ECT_REG_EEPDAT}}, data... }; - do { - telegram_.transfer(datagram); - } while(datagram.get_wkc() < 0x0001); + datagram::EcatDatagram datagram{ {{slave_address, ECT_REG_EEPDAT}}, expected_wkc, data... }; + + return telegram_.transfer(datagram); } template - void read(typename TypeT::TSlaveAddress& slave_address, uint16_t eeprom_address, DataTypes&... data) { - wait_busy(slave_address); - control_register(slave_address, EC_ECMD_READ, eeprom_address); - wait_busy(slave_address); - data_register(slave_address, data...); - wait_busy(slave_address); + bool read(const datagram::TEcatWkc expected_wkc, typename TypeT::TSlaveAddress& slave_address, uint16_t eeprom_address, DataTypes&... data) { + if(wait_busy(expected_wkc, slave_address) != true) return false; + if(control_register(expected_wkc, slave_address, EC_ECMD_READ, eeprom_address) != true) return false; + if(wait_busy(expected_wkc, slave_address) != true) return false; + if(data_register(expected_wkc, slave_address, data...) != true) return false; + if(wait_busy(expected_wkc, slave_address) != true) return false; } // 2 bytes (1 word) max template - void write(typename TypeT::TSlaveAddress& slave_address, uint16_t eeprom_address, DataTypes&... data) { - wait_busy(slave_address); - data_register(slave_address, data...); - wait_busy(slave_address); - control_register(slave_address, EC_ECMD_WRITE, eeprom_address); - wait_busy(slave_address); + bool write(const datagram::TEcatWkc expected_wkc, typename TypeT::TSlaveAddress& slave_address, uint16_t eeprom_address, DataTypes&... data) { + if(wait_busy(expected_wkc, slave_address) != true) return false; + if(data_register(expected_wkc, slave_address, data...) != true) return false; + if(wait_busy(expected_wkc, slave_address) != true) return false; + if(control_register( expected_wkc, slave_address, EC_ECMD_WRITE, eeprom_address) != true) return false; + if(wait_busy(expected_wkc, slave_address) != true) return false; } private: diff --git a/components/free_rtos/ethernet_industry/eth_ecat_queue.hpp b/components/free_rtos/ethernet_industry/eth_ecat_queue.hpp index 356eb5d..b37edab 100644 --- a/components/free_rtos/ethernet_industry/eth_ecat_queue.hpp +++ b/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; } diff --git a/components/free_rtos/ethernet_industry/eth_ecat_telegram.cpp b/components/free_rtos/ethernet_industry/eth_ecat_telegram.cpp index 6eb92f7..7edff8e 100644 --- a/components/free_rtos/ethernet_industry/eth_ecat_telegram.cpp +++ b/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(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_; // Копия очереди - 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_datagram_last = first->pack_all(p_datagram_first); 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(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) { - DebugP_log((char*)"Error: wrong protocol ID\r\n"); - - return raw; - } - - if(p_hdr->bits.type != static_cast(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(); - } + p_datagram_last = first->unpack_all(p_datagram_first); return p_datagram_last; } -void EcatTelegram::transfer() { - //bool stat = eth_stack_.send_pkt(port_id_, ETH_PROT_ECAT_LE, 1); - bool stat = tx_flow_.send(port_id_, this, 1); +bool EcatTelegram::transfer() { + auto first = datagram_queue_.get_first(); + uint32_t transfer_attempts = 0; + bool stat; + bool as_expected; - if(stat == false) { - DebugP_log((char*)"telegram transfer error !\r\n"); + if(first == nullptr) { + status_.result = EcatTelegramResult::SUCCESS; - datagram_queue_.clear(); - - return; + return true; } - 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& next) { +bool EcatTelegram::transfer(queue::Queue& next) { datagram_queue_ + next; - transfer(); + return transfer(); } } diff --git a/components/free_rtos/ethernet_industry/eth_ecat_telegram.hpp b/components/free_rtos/ethernet_industry/eth_ecat_telegram.hpp index 42245c3..eb7e21d 100644 --- a/components/free_rtos/ethernet_industry/eth_ecat_telegram.hpp +++ b/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(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); - void transfer(queue::Queue& next); + bool transfer(datagram::IEcatDatagram& next); + bool transfer(queue::Queue& 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_queue_; //datagram::IEcatDatagram *datagram_queue_{nullptr}; - void transfer(); + EcatTelegramStatus status_; + + bool transfer(); uint8_t* pack(uint8_t *raw); uint8_t* unpack(uint8_t *raw);