376 lines
11 KiB
C++
376 lines
11 KiB
C++
/*
|
||
* eth_ecat.cpp
|
||
*
|
||
* Created on: 16 апр. 2023 г.
|
||
* Author: sychev
|
||
*/
|
||
|
||
#include "free_rtos/ethernet_industry/eth_ecat.hpp"
|
||
|
||
#include <cstring>
|
||
#include <kernel/dpl/ClockP.h>
|
||
|
||
namespace free_rtos {
|
||
|
||
EthEcat::EthEcat(Eth& eth)
|
||
: eth_{eth}
|
||
, tx_flow_{*eth.getTxFlowPtr()}
|
||
, telegram_{eth, ecat_timer_}
|
||
, eeprom_{telegram_} { }
|
||
|
||
void EthEcat::Init(TEthMacPorts port_id, uint32_t period_microsec = 250) {
|
||
port_id_ = port_id;
|
||
telegram_.init(port_id, period_microsec, 10);
|
||
|
||
Timer::Settings ecat_tmr_sett = {
|
||
.input_clk_Hz = 25000000, // 25MHz
|
||
.base_address = 0x2400000u, // memory mapping,
|
||
.clock_src_mux_addr = 0x430081B0u, // sysconfig
|
||
.int_num = 152u, // sysconfig
|
||
.int_priority = 4, // sysconfig
|
||
.period_us = period_microsec /// microsec
|
||
};
|
||
|
||
ecat_timer_.Init(ecat_tmr_sett);
|
||
ecat_timer_.Start();
|
||
}
|
||
|
||
bool EthEcat::set_slaves_to_default() {
|
||
const datagram::TEcatWkc expected_wkc = 1;
|
||
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}}, 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}}, expected_wkc, 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}}, expected_wkc, c_data_out };
|
||
c_data_out.fill(0x00);
|
||
|
||
/* reset FMMU's */
|
||
std::array<uint8_t, 16*3> d_data_out;
|
||
datagram::EcatDatagram<command::BWR, std::array<uint8_t, 16*3>> d{ {{broadcast, ECT_REG_FMMU0}}, expected_wkc, d_data_out };
|
||
d_data_out.fill(0x00);
|
||
|
||
/* 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}}, expected_wkc, e_data_out };
|
||
e_data_out.fill(0x00);
|
||
|
||
/* reset activation register */
|
||
uint8_t f_data_out{0x00};
|
||
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;
|
||
datagram::EcatDatagram<command::BWR, std::array<uint8_t, 4>> g{ {{broadcast, ECT_REG_DCSYSTIME}}, expected_wkc, g_data_out };
|
||
g_data_out.fill(0x00);
|
||
|
||
/* DC speedstart */
|
||
uint16_t h_data_out{0x1000};
|
||
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}}, 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}}, 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}}, 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}}, 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}}, expected_wkc, m_data_out };
|
||
|
||
auto queue = a + b + c + d + e + f + g + h + i + j + k + l + m;
|
||
|
||
status = telegram_.transfer(queue);
|
||
|
||
//telegram_.transfer(b);
|
||
//telegram_.transfer(c);
|
||
//telegram_.transfer(d);
|
||
//telegram_.transfer(e);
|
||
//telegram_.transfer(f);
|
||
//telegram_.transfer(g);
|
||
//telegram_.transfer(h);
|
||
//telegram_.transfer(i);
|
||
//telegram_.transfer(j);
|
||
//telegram_.transfer(k);
|
||
//telegram_.transfer(l);
|
||
//telegram_.transfer(m);
|
||
|
||
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<command::BRD, uint16_t> datagram{ {{broadcast, ECT_REG_TYPE}}, 1, data };
|
||
|
||
telegram_.transfer(datagram);
|
||
|
||
return datagram.get_wkc();
|
||
}
|
||
|
||
// Setting Station address (FP) of slave via Position addressing (AP)
|
||
// Station address is datagram data
|
||
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);
|
||
|
||
for(uint16_t i = 0; i < number_of_slaves; i++) {
|
||
address::Position position{static_cast<int16_t>(-i)};
|
||
address::Station station{static_cast<uint16_t>(address_base + i)};
|
||
address::SlaveAddresses slave_addresses{position, 0x0000, station, 0x00000000};
|
||
|
||
slaves_.emplace_back(EcatSlave{std::move(slave_addresses)});
|
||
datagrams.emplace_back(TDatagram{ {{position, ECT_REG_STADR}}, expected_wkc, slaves_.back().get_slave_address<command::FP>() });
|
||
}
|
||
|
||
queue::Queue<datagram::IEcatDatagram> queue;
|
||
|
||
for(uint16_t i = 0; i < number_of_slaves; i++) {
|
||
queue + datagrams[i];
|
||
}
|
||
|
||
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;
|
||
}
|
||
|
||
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}}, expected_wkc, slave.get_slave_address<command::FP>() });
|
||
}
|
||
|
||
for(uint16_t i = 0; i < number_of_slaves; i++) {
|
||
queue + datagrams[i];
|
||
}
|
||
|
||
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<command::AP>(), slave.get_slave_address<command::FP>());
|
||
}
|
||
|
||
return status;
|
||
}
|
||
|
||
bool EthEcat::connection_test() {
|
||
const datagram::TEcatWkc expected_wkc = 0;
|
||
bool status;
|
||
|
||
std::array<uint8_t, 110> test_data_out;
|
||
datagram::EcatDatagram<command::NOP, std::array<uint8_t, 110>> datagram{ {{0x0000, 0x0000}}, expected_wkc, test_data_out };
|
||
test_data_out.fill(0x00);
|
||
|
||
while(1) {
|
||
status = telegram_.transfer(datagram);
|
||
|
||
if(status != true) {
|
||
break;
|
||
}
|
||
}
|
||
|
||
return status;
|
||
}
|
||
|
||
bool EthEcat::config_init(uint16_t address_base) {
|
||
uint16_t number_of_slaves;
|
||
bool status;
|
||
|
||
DebugP_log((char*)"EthercatMaster starting. Initializing slaves...\r\n");
|
||
|
||
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);
|
||
|
||
status = (number_of_slaves > 0);
|
||
|
||
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;
|
||
}
|
||
/*
|
||
DebugP_log((char*)"Starting connection test...\r\n");
|
||
|
||
status = connection_test();
|
||
|
||
if(status != true) {
|
||
return status;
|
||
}
|
||
*/
|
||
return status;
|
||
}
|
||
|
||
bool EthEcat::enable_PDI() {
|
||
const datagram::TEcatWkc expected_wkc = 1;
|
||
bool status = true;
|
||
|
||
for(EcatSlave& slave : slaves_) {
|
||
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 = (success && slave.init_to_preop<command::FP>(telegram_, expected_wkc));
|
||
}
|
||
|
||
DebugP_log((char*)"success = %d\r\n", success);
|
||
|
||
return success;
|
||
}
|
||
|
||
bool EthEcat::preop_to_safeop() {
|
||
const datagram::TEcatWkc expected_wkc = 1;
|
||
bool success = true;
|
||
|
||
for(EcatSlave& slave : slaves_) {
|
||
success = (success && slave.preop_to_safeop<command::FP>(telegram_, expected_wkc));
|
||
}
|
||
|
||
DebugP_log((char*)"success = %d\r\n", success);
|
||
|
||
return success;
|
||
}
|
||
|
||
bool EthEcat::safeop_to_op() {
|
||
const datagram::TEcatWkc expected_wkc = slaves_.size();
|
||
address::Broadcast broadcast{0x0000};
|
||
ALSTAT stat{0x0000, 0x0000};
|
||
uint16_t zero{0x00000000};
|
||
bool status;
|
||
|
||
process_sem_.post();
|
||
if(init_sem_.pend(process_timeout_ticks_) != SystemP_SUCCESS) {
|
||
DebugP_log((char*)"Process task timeout !\r\n");
|
||
|
||
return false;
|
||
}
|
||
|
||
{
|
||
uint16_t data{EC_STATE_OPERATIONAL};
|
||
datagram::EcatDatagram<command::BWR, uint16_t> datagram{ {{broadcast, ECT_REG_ALCTL}}, expected_wkc, data };
|
||
|
||
status = telegram_.transfer(datagram);
|
||
|
||
if(status != true) {
|
||
return status;
|
||
}
|
||
}
|
||
|
||
process_sem_.post();
|
||
if(init_sem_.pend(process_timeout_ticks_) != SystemP_SUCCESS) {
|
||
DebugP_log((char*)"Process task timeout !\r\n");
|
||
|
||
return false;
|
||
}
|
||
|
||
{
|
||
datagram::EcatDatagram<command::BRD, ALSTAT, uint16_t> datagram{ {{broadcast, ECT_REG_ALSTAT}}, expected_wkc, stat, zero };
|
||
|
||
status = telegram_.transfer(datagram);
|
||
|
||
if(status != true) {
|
||
return status;
|
||
}
|
||
}
|
||
|
||
status = (stat.state == EC_STATE_OPERATIONAL) && (stat.fault == 0);
|
||
|
||
if(status != true) {
|
||
DebugP_log((char*)"stat.state = %d, stat.fault = %d\r\n", stat.state, stat.fault);
|
||
|
||
return status;
|
||
}
|
||
|
||
//DebugP_log((char*)"success = %d\r\n", success);
|
||
|
||
process_sem_.post();
|
||
|
||
return status;
|
||
}
|
||
|
||
}
|