290 lines
8.3 KiB
C++
290 lines
8.3 KiB
C++
/*
|
||
* eth_ecat.hpp
|
||
*
|
||
* Created on: 16 <20><><EFBFBD>. 2023 <20>.
|
||
* Author: sychev, algin
|
||
*/
|
||
|
||
#ifndef FREE_RTOS_ETHERNET_INDUSTRY_ETH_ECAT_HPP_
|
||
#define FREE_RTOS_ETHERNET_INDUSTRY_ETH_ECAT_HPP_
|
||
|
||
#include <cstdint>
|
||
#include <utility>
|
||
#include <kernel/dpl/ClockP.h>
|
||
|
||
#include "handler_store/handler.hpp"
|
||
#include "ethernet/eth_frame.h"
|
||
#include "mutex/mutex.hpp"
|
||
#include "semaphore/semaphore.hpp"
|
||
#include "ethernet/eth.hpp"
|
||
#include "ethernet_industry/ethercattype.hpp"
|
||
#include "ethernet_industry/eth_ecat_types.h"
|
||
#include "ethernet_industry/eth_ecat_command.hpp"
|
||
#include "ethernet_industry/eth_ecat_datagram.hpp"
|
||
#include "ethernet_industry/eth_ecat_eeprom.hpp"
|
||
|
||
namespace free_rtos {
|
||
|
||
struct ALSTAT {
|
||
uint16_t state;
|
||
uint16_t fault;
|
||
};
|
||
|
||
class EcatSlave {
|
||
public:
|
||
EcatSlave(address::SlaveAddresses&& slave_addresses)
|
||
: slave_addresses_{slave_addresses} {
|
||
|
||
}
|
||
|
||
EcatSlave() {
|
||
|
||
}
|
||
|
||
template<typename TypeT>
|
||
void set_slave_address(typename TypeT::TSlaveAddress&& slave_address) {
|
||
std::get<static_cast<size_t>(TypeT::type)>(slave_addresses_) = slave_address;
|
||
}
|
||
|
||
template<typename TypeT>
|
||
typename TypeT::TSlaveAddress& get_slave_address() {
|
||
return std::get<static_cast<size_t>(TypeT::type)>(slave_addresses_);
|
||
}
|
||
|
||
template<typename TypeT>
|
||
void enable_PDI(datagram::EcatTelegram& telegram) {
|
||
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 };
|
||
|
||
telegram.transfer(datagram);
|
||
|
||
DebugP_log("datagram.get_wkc() = %d\r\n", datagram.get_wkc());
|
||
}
|
||
|
||
template<typename TypeT>
|
||
bool init_to_preop(datagram::EcatTelegram& telegram) {
|
||
auto slave_address = get_slave_address<TypeT>();
|
||
ALSTAT stat{0x0000, 0x0000};
|
||
|
||
{
|
||
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 };
|
||
|
||
telegram.transfer(datagram);
|
||
|
||
DebugP_log("datagram.get_wkc() = %d\r\n", datagram.get_wkc());
|
||
}
|
||
|
||
ClockP_usleep(5000000ul);
|
||
|
||
{
|
||
using TCommand = command::EcatCommand<TypeT, command::RD>;
|
||
datagram::EcatDatagram<TCommand, ALSTAT> datagram{ {{slave_address, ECT_REG_ALSTAT}}, stat };
|
||
|
||
telegram.transfer(datagram);
|
||
|
||
DebugP_log("datagram.get_wkc() = %d\r\n", datagram.get_wkc());
|
||
}
|
||
|
||
DebugP_log("stat.state = %d, stat.fault = %d\r\n", stat.state, stat.fault);
|
||
|
||
if((stat.state & 0x0010) != 0) {
|
||
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};
|
||
|
||
telegram.transfer(datagram);
|
||
|
||
DebugP_log("stat_code = 0x%02x\r\n", stat_code);
|
||
}
|
||
|
||
return (stat.state == EC_STATE_PRE_OP) && (stat.fault == 0);
|
||
}
|
||
|
||
template<typename TypeT>
|
||
bool preop_to_safeop(datagram::EcatTelegram& telegram) {
|
||
auto slave_address = get_slave_address<TypeT>();
|
||
ALSTAT stat{0x0000, 0x0000};
|
||
uint32_t zero{0x00000000};
|
||
|
||
{
|
||
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 };
|
||
|
||
telegram.transfer(datagram);
|
||
|
||
DebugP_log("datagram.get_wkc() = %d\r\n", datagram.get_wkc());
|
||
}
|
||
|
||
ClockP_usleep(1000000ul);
|
||
|
||
{
|
||
using TCommand = command::EcatCommand<TypeT, command::RD>;
|
||
datagram::EcatDatagram<TCommand, ALSTAT, uint32_t> datagram{ {{slave_address, ECT_REG_ALSTAT}}, stat, zero };
|
||
|
||
telegram.transfer(datagram);
|
||
|
||
DebugP_log("datagram.get_wkc() = %d\r\n", datagram.get_wkc());
|
||
}
|
||
|
||
DebugP_log("stat.state = %d, stat.fault = %d\r\n", stat.state, stat.fault);
|
||
|
||
if((stat.state & 0x0010) != 0) {
|
||
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};
|
||
|
||
telegram.transfer(datagram);
|
||
|
||
DebugP_log("stat_code = 0x%02x\r\n", stat_code);
|
||
}
|
||
|
||
return (stat.state == EC_STATE_SAFE_OP) && (stat.fault == 0);
|
||
}
|
||
|
||
template<typename TypeT>
|
||
bool safeop_to_op(datagram::EcatTelegram& telegram) {
|
||
auto slave_address = get_slave_address<TypeT>();
|
||
ALSTAT stat{0x0000, 0x0000};
|
||
uint16_t zero{0x00000000};
|
||
|
||
{
|
||
using TCommand = command::EcatCommand<TypeT, command::WR>;
|
||
uint16_t data{EC_STATE_OPERATIONAL};
|
||
datagram::EcatDatagram<TCommand, uint16_t> datagram{ {{slave_address, ECT_REG_ALCTL}}, data };
|
||
|
||
telegram.transfer(datagram);
|
||
|
||
DebugP_log("datagram.get_wkc() = %d\r\n", datagram.get_wkc());
|
||
}
|
||
|
||
//ClockP_usleep(3000000ul);
|
||
|
||
{
|
||
using TCommand = command::EcatCommand<TypeT, command::RD>;
|
||
datagram::EcatDatagram<TCommand, ALSTAT, uint16_t> datagram{ {{slave_address, ECT_REG_ALSTAT}}, stat, zero };
|
||
|
||
telegram.transfer(datagram);
|
||
|
||
DebugP_log("datagram.get_wkc() = %d\r\n", datagram.get_wkc());
|
||
}
|
||
|
||
DebugP_log("stat.state = %d, stat.fault = %d\r\n", stat.state, stat.fault);
|
||
|
||
if((stat.state & 0x0010) != 0) {
|
||
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};
|
||
|
||
telegram.transfer(datagram);
|
||
|
||
DebugP_log("stat_code = 0x%02x\r\n", stat_code);
|
||
}
|
||
|
||
return (stat.state == EC_STATE_OPERATIONAL) && (stat.fault == 0);
|
||
}
|
||
|
||
private:
|
||
address::SlaveAddresses slave_addresses_;
|
||
};
|
||
|
||
class EthEcat : public Handler {
|
||
public:
|
||
struct Statistic {
|
||
uint32_t rx_pkt;
|
||
uint32_t tx_pkt;
|
||
};
|
||
|
||
EthEcat(Eth& eth);
|
||
|
||
void Init(TEthMacPorts port_id);
|
||
|
||
std::vector<uint8_t> receive_datagram();
|
||
void send_datagram(const std::vector<std::uint8_t>& datagram);
|
||
|
||
void 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();
|
||
|
||
uint16_t config_init();
|
||
|
||
void enable_PDI();
|
||
|
||
bool init_to_preop();
|
||
bool preop_to_safeop();
|
||
bool safeop_to_op();
|
||
|
||
/*
|
||
* Тип адресации slave_address зависит от типа команды CommandT
|
||
* Может иметь тип Position, Broadcast, Node
|
||
*/
|
||
template<typename TypeT, typename DirT, typename EcatDgDataT>
|
||
void simple_send_datagram(typename TypeT::TSlaveAddress& slave_address, uint16_t offset, EcatDgDataT& data) {
|
||
using TCommand = command::EcatCommand<TypeT, DirT>;
|
||
datagram::EcatDatagram<TCommand, EcatDgDataT> datagram{ {{slave_address, offset}}, data };
|
||
|
||
telegram_.transfer(datagram);
|
||
}
|
||
|
||
datagram::EcatTelegram& get_telegram() {
|
||
return telegram_;
|
||
}
|
||
|
||
eeprom::EEPROM& get_eeprom() {
|
||
return eeprom_;
|
||
}
|
||
|
||
std::vector<EcatSlave>& get_slaves() {
|
||
return slaves_;
|
||
}
|
||
|
||
bool is_ready() { return ready_; }
|
||
|
||
Statistic getStat() { return stat_;}
|
||
|
||
virtual int32_t Process(uint8_t * p_data, uint32_t len) override; /// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> ecat
|
||
private:
|
||
enum {
|
||
e_pktFirst,
|
||
e_pktSecond,
|
||
e_pktTotal
|
||
};
|
||
|
||
static constexpr uint16_t max_number_of_slaves = 32;
|
||
|
||
TEthPkt * p_pkt_;
|
||
TEthPkt * p_pkt_next_;
|
||
|
||
TEthPkt pkt_[e_pktTotal];
|
||
|
||
Mutex mut_;
|
||
|
||
free_rtos::Semaphore rx_sem_;
|
||
|
||
Eth& eth_;
|
||
EthTxFlowIface& tx_flow_;
|
||
|
||
datagram::EcatTelegram telegram_;
|
||
eeprom::EEPROM eeprom_;
|
||
|
||
TEthMacPorts port_id_; /// <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
|
||
bool ready_;
|
||
|
||
Statistic stat_;
|
||
|
||
std::vector<EcatSlave> slaves_;
|
||
};
|
||
|
||
}
|
||
|
||
#endif /* FREE_RTOS_ETHERNET_INDUSTRY_ETH_ECAT_HPP_ */
|