229 lines
7.2 KiB
C++
229 lines
7.2 KiB
C++
/*
|
||
* eth_ecat.cpp
|
||
*
|
||
* Created on: 16 <20><><EFBFBD>. 2023 <20>.
|
||
* Author: sychev
|
||
*/
|
||
|
||
#include "ethernet_industry/eth_ecat.hpp"
|
||
|
||
#include <cstring>
|
||
#include <kernel/dpl/ClockP.h>
|
||
|
||
namespace free_rtos {
|
||
|
||
EthEcat::EthEcat(Eth& eth)
|
||
: p_pkt_{&pkt_[e_pktFirst]}
|
||
, p_pkt_next_{&pkt_[e_pktSecond]}
|
||
, eth_{eth}
|
||
, tx_flow_{*eth.getTxFlowPtr()}
|
||
, telegram_{eth}
|
||
, eeprom_{telegram_} { }
|
||
|
||
void EthEcat::Init(TEthMacPorts port_id) {
|
||
port_id_ = port_id;
|
||
telegram_.init(port_id);
|
||
}
|
||
|
||
int32_t EthEcat::Process(uint8_t * p_data, uint32_t len) {
|
||
p_pkt_next_->length = len + sizeof(TEthFrameHeader);
|
||
|
||
memcpy(p_pkt_next_->data, p_data - sizeof(TEthFrameHeader), p_pkt_next_->length);
|
||
|
||
++stat_.rx_pkt;
|
||
|
||
rx_sem_.post();
|
||
|
||
return 0;
|
||
}
|
||
|
||
std::vector<uint8_t> EthEcat::receive_datagram() {
|
||
rx_sem_.pend();
|
||
|
||
return std::vector<uint8_t>(p_pkt_next_->data + sizeof(TEthFrameHeader), p_pkt_next_->data + p_pkt_next_->length);
|
||
}
|
||
|
||
void EthEcat::send_datagram(const std::vector<uint8_t>& datagram) {
|
||
TEthFrameHeader *p_eth_hdr = reinterpret_cast<TEthFrameHeader*>(p_pkt_->data);
|
||
uint8_t *p_eth_data = reinterpret_cast<uint8_t*>(p_pkt_->data + sizeof(TEthFrameHeader));
|
||
|
||
uint8_t mac_dest[ETH_FRAME_MAC_ADDR_LEN_BYTES] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
|
||
uint8_t mac_src[ETH_FRAME_MAC_ADDR_LEN_BYTES] = {0x01, 0x01, 0x01, 0x01, 0x01, 0x01};
|
||
|
||
memcpy(p_eth_hdr->mac_dest, mac_dest, sizeof(mac_dest));
|
||
memcpy(p_eth_hdr->mac_src, mac_src, sizeof(mac_src));
|
||
p_eth_hdr->prot_id = ETH_PROT_ECAT_LE;
|
||
|
||
memcpy(p_eth_data, datagram.data(), datagram.size());
|
||
|
||
p_pkt_->length = sizeof(TEthFrameHeader) + datagram.size();
|
||
|
||
bool stat = tx_flow_.send(port_id_, p_pkt_->data, p_pkt_->length);
|
||
|
||
if (stat)
|
||
{
|
||
++stat_.tx_pkt;
|
||
}
|
||
}
|
||
|
||
void EthEcat::set_slaves_to_default() {
|
||
address::Broadcast broadcast{0x0000};
|
||
|
||
/* deact loop manual */
|
||
uint8_t a_data_out{0x00};
|
||
datagram::EcatDatagram<command::BWR, uint8_t> a{ {{broadcast, ECT_REG_DLPORT}}, 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 };
|
||
|
||
/* reset CRC counters */
|
||
std::array<uint8_t, 8> c_data_out;
|
||
datagram::EcatDatagram<command::BWR, std::array<uint8_t, 8>> c{ {{broadcast, ECT_REG_RXERR}}, c_data_out };
|
||
|
||
/* reset FMMU's */
|
||
std::array<uint8_t, 16*3> d_data_out;
|
||
std::array<uint8_t, 16*3> d_data_in;
|
||
datagram::EcatDatagram<command::BWR, std::array<uint8_t, 16*3>> d{ {{broadcast, ECT_REG_FMMU0}}, d_data_out };
|
||
|
||
/* reset SyncM */
|
||
std::array<uint8_t, 8*4> e_data_out;
|
||
datagram::EcatDatagram<command::BWR, std::array<uint8_t, 8*4>> e{ {{broadcast, ECT_REG_SM0}}, 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 };
|
||
|
||
/* reset system time+ofs */
|
||
std::array<uint8_t, 4> g_data_out;
|
||
datagram::EcatDatagram<command::BWR, std::array<uint8_t, 4>> g{ {{broadcast, ECT_REG_DCSYSTIME}}, 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 };
|
||
|
||
/* DC filt expr */
|
||
uint16_t i_data_out{0x0C00};
|
||
datagram::EcatDatagram<command::BWR, uint16_t> i{ {{broadcast, ECT_REG_DCTIMEFILT}}, 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 };
|
||
|
||
/* Reset all slaves to Init */
|
||
uint8_t k_data_out{EC_STATE_INIT};
|
||
datagram::EcatDatagram<command::BWR, uint8_t> k{ {{broadcast, ECT_REG_ALCTL}}, 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 };
|
||
|
||
/* set Eeprom to master */
|
||
uint8_t m_data_out{0x00};
|
||
datagram::EcatDatagram<command::BWR, uint8_t> m{ {{broadcast, ECT_REG_EEPCFG}}, m_data_out };
|
||
|
||
a + b + c + d + e + f + g + h + i + j + k + l + m;
|
||
telegram_.transfer(a);
|
||
|
||
DebugP_log("a.get_wkc() = %d\r\n", a.get_wkc());
|
||
DebugP_log("b.get_wkc() = %d\r\n", b.get_wkc());
|
||
DebugP_log("c.get_wkc() = %d\r\n", c.get_wkc());
|
||
DebugP_log("d.get_wkc() = %d\r\n", d.get_wkc());
|
||
DebugP_log("e.get_wkc() = %d\r\n", e.get_wkc());
|
||
DebugP_log("f.get_wkc() = %d\r\n", f.get_wkc());
|
||
DebugP_log("g.get_wkc() = %d\r\n", g.get_wkc());
|
||
DebugP_log("h.get_wkc() = %d\r\n", h.get_wkc());
|
||
DebugP_log("i.get_wkc() = %d\r\n", i.get_wkc());
|
||
DebugP_log("j.get_wkc() = %d\r\n", j.get_wkc());
|
||
DebugP_log("k.get_wkc() = %d\r\n", k.get_wkc());
|
||
DebugP_log("l.get_wkc() = %d\r\n", l.get_wkc());
|
||
DebugP_log("m.get_wkc() = %d\r\n", m.get_wkc());
|
||
}
|
||
|
||
uint16_t EthEcat::slaves_detecting() {
|
||
address::Broadcast broadcast{0x0000};
|
||
uint16_t data_out{0x0000};
|
||
datagram::EcatDatagram<command::BRD, uint16_t> datagram{ {{broadcast, ECT_REG_TYPE}}, data_out };
|
||
|
||
telegram_.transfer(datagram);
|
||
|
||
return datagram.get_wkc();
|
||
}
|
||
|
||
void EthEcat::set_addresses_of_slaves(uint16_t number_of_slaves, uint16_t address_base) {
|
||
// Setting Node address (FP) of slave via Position addressing (AP)
|
||
std::vector<datagram::EcatDatagram<command::APWR, address::Node>> datagrams;
|
||
|
||
slaves_.reserve(number_of_slaves);
|
||
datagrams.reserve(number_of_slaves);
|
||
|
||
for(uint16_t i = 0; i < number_of_slaves; i++) {
|
||
address::Position position{static_cast<int16_t>(-i)};
|
||
address::Node node{static_cast<uint16_t>(address_base + i)};
|
||
address::SlaveAddresses slave_addresses{position, 0x0000, node, 0x00000000};
|
||
|
||
slaves_.push_back(EcatSlave{std::move(slave_addresses)});
|
||
|
||
datagrams.push_back({ {{position, ECT_REG_STADR}}, slaves_.back().get_slave_address<command::FP>() });
|
||
}
|
||
|
||
for(uint16_t i = 1; i < number_of_slaves; i++) {
|
||
datagrams[i - 1] + datagrams[i];
|
||
}
|
||
|
||
telegram_.transfer(datagrams[0]);
|
||
}
|
||
|
||
void EthEcat::get_addresses_of_slaves() {
|
||
std::vector<datagram::EcatDatagram<command::APWR, address::Node>> datagrams;
|
||
|
||
datagrams.reserve(slaves_.size());
|
||
|
||
for(EcatSlave& slave : slaves_) {
|
||
datagrams.push_back({ {{slave.get_slave_address<command::AP>(), ECT_REG_STADR}}, slave.get_slave_address<command::FP>() });
|
||
}
|
||
|
||
for(uint16_t i = 1; i < slaves_.size(); i++) {
|
||
datagrams[i - 1] + datagrams[i];
|
||
}
|
||
|
||
telegram_.transfer(datagrams[0]);
|
||
|
||
for(EcatSlave& slave : slaves_) {
|
||
DebugP_log("Slave %d address = %d\r\n", -slave.get_slave_address<command::AP>(), slave.get_slave_address<command::FP>());
|
||
}
|
||
}
|
||
|
||
uint16_t EthEcat::config_init() {
|
||
DebugP_log("Initializing slaves...\r\n");
|
||
|
||
set_slaves_to_default();
|
||
uint16_t number_of_slaves = slaves_detecting();
|
||
DebugP_log("number_of_slaves = %d\r\n", number_of_slaves);
|
||
|
||
set_addresses_of_slaves(number_of_slaves, 0x1000);
|
||
get_addresses_of_slaves();
|
||
|
||
return number_of_slaves;
|
||
}
|
||
|
||
void EthEcat::enable_PDI() {
|
||
for(EcatSlave& slave : slaves_) {
|
||
slave.enable_PDI<command::FP>(telegram_);
|
||
}
|
||
}
|
||
|
||
bool EthEcat::init_to_preop() {
|
||
bool success = true;
|
||
|
||
for(EcatSlave& slave : slaves_) {
|
||
success &= slave.init_to_preop<command::FP>(telegram_);
|
||
}
|
||
|
||
DebugP_log("success = %d\r\n", success);
|
||
|
||
return success;
|
||
}
|
||
|
||
}
|