Welding/sitara_depot
25
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

dev: Mailboxes basic implementation...

Browse Source
This commit is contained in:
algin 2023-05-04 18:00:43 +03:00
parent 50076e8c05
commit 1e0fcf04c3
6 changed files with 297 additions and 77 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

97
components/free_rtos/ethernet_industry/eth_ecat.cpp
View File

@ -67,58 +67,60 @@ void EthEcat::send_datagram(const std::vector<uint8_t>& datagram) {
}
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{ {{0x0000, ECT_REG_DLPORT}}, a_data_out };
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{ {{0x0000, ECT_REG_IRQMASK}}, b_data_out };
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{ {{0x0000, ECT_REG_RXERR}}, 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{ {{0x0000, ECT_REG_FMMU0}}, d_data_out };
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{ {{0x0000, ECT_REG_SM0}}, 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{ {{0x0000, ECT_REG_DCSYNCACT}}, f_data_out };
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{ {{0x0000, ECT_REG_DCSYSTIME}}, 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{ {{0x0000, ECT_REG_DCSPEEDCNT}}, h_data_out };
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{ {{0x0000, ECT_REG_DCTIMEFILT}}, i_data_out };
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{ {{0x0000, ECT_REG_DLALIAS}}, j_data_out };
datagram::EcatDatagram<command::BWR, uint8_t> j{ {{broadcast, ECT_REG_DLALIAS}}, j_data_out };
/* Reset all slaves to Init */
uint8_t k_data_out{0x00};
datagram::EcatDatagram<command::BWR, uint8_t> k{ {{0x0000, ECT_REG_ALCTL}}, k_data_out };
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{ {{0x0000, ECT_REG_EEPCFG}}, l_data_out };
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{ {{0x0000, ECT_REG_EEPCFG}}, m_data_out };
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);
@ -139,25 +141,33 @@ void EthEcat::set_slaves_to_default() {
}
uint16_t EthEcat::slaves_detecting() {
address::Broadcast broadcast{0x0000};
uint16_t data_out{0x0000};
datagram::EcatDatagram<command::BRD, uint16_t> datagram{ {{0x0000, ECT_REG_TYPE}}, data_out };
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 address_base) {
std::array<datagram::EcatDatagram<command::APWR, address::Node>, max_number_of_slaves> datagrams;
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;
for(uint16_t i = 0; i < number_of_slaves_; i++) {
slaves_[i].set_slave_address<command::FP>(static_cast<address::Node>(address_base + i));
slaves_[i].set_slave_address<command::AP>(static_cast<address::Position>(-i));
slaves_.reserve(number_of_slaves);
datagrams.reserve(number_of_slaves);
datagrams[i] = { {{address::Position{static_cast<int16_t>(-i)}, ECT_REG_STADR}}, slaves_[i].get_slave_address<command::FP>() };
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++) {
for(uint16_t i = 1; i < number_of_slaves; i++) {
datagrams[i - 1] + datagrams[i];
}
@ -165,22 +175,22 @@ void EthEcat::set_addresses_of_slaves(uint16_t address_base) {
}
void EthEcat::get_addresses_of_slaves() {
std::array<datagram::EcatDatagram<command::APRD, address::Node>, max_number_of_slaves> datagrams;
std::vector<datagram::EcatDatagram<command::APWR, address::Node>> datagrams;
for(uint16_t i = 0; i < number_of_slaves_; i++) {
slaves_[i].set_slave_address<command::AP>(static_cast<address::Position>(-i));
datagrams.reserve(slaves_.size());
datagrams[i] = { {{address::Position{static_cast<int16_t>(-i)}, ECT_REG_STADR}}, slaves_[i].get_slave_address<command::FP>() };
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 < number_of_slaves_; i++) {
for(uint16_t i = 1; i < slaves_.size(); i++) {
datagrams[i - 1] + datagrams[i];
}
telegram_.transfer(datagrams[0]);
for(uint16_t i = 0; i < number_of_slaves_; i++) {
DebugP_log("Slave %d address = %d\r\n", i, slaves_[i].get_slave_address<command::FP>());
for(EcatSlave& slave : slaves_) {
DebugP_log("Slave %d address = %d\r\n", -slave.get_slave_address<command::AP>(), slave.get_slave_address<command::FP>());
}
}
@ -188,24 +198,31 @@ uint16_t EthEcat::config_init() {
DebugP_log("Initializing slaves...\r\n");
set_slaves_to_default();
number_of_slaves_ = slaves_detecting();
DebugP_log("number_of_slaves = %d\r\n", number_of_slaves_);
uint16_t number_of_slaves = slaves_detecting();
DebugP_log("number_of_slaves = %d\r\n", number_of_slaves);
set_addresses_of_slaves(0x1000);
set_addresses_of_slaves(number_of_slaves, 0x1000);
get_addresses_of_slaves();
address::Node node = 4096;
return number_of_slaves;
}
BufferProperties mbx_write_adr_len;
eeprom_.read<command::FP>(node, ECT_SII_RXMBXADR, mbx_write_adr_len);
void EthEcat::enable_PDI() {
for(EcatSlave& slave : slaves_) {
slave.enable_PDI<command::FP>(telegram_);
}
}
BufferProperties mbx_read_adr_len;
eeprom_.read<command::FP>(node, ECT_SII_TXMBXADR, mbx_read_adr_len);
bool EthEcat::init_to_preop() {
bool success = true;
DebugP_log("response = 0x%04x\r\n", mbx_write_adr_len);
DebugP_log("response = 0x%04x\r\n", mbx_read_adr_len);
for(EcatSlave& slave : slaves_) {
success &= slave.init_to_preop<command::FP>(telegram_);
}
return number_of_slaves_;
DebugP_log("success = %d\r\n", success);
return success;
}
}

84
components/free_rtos/ethernet_industry/eth_ecat.hpp
View File

@ -2,14 +2,15 @@
* eth_ecat.hpp
*
* Created on: 16 <EFBFBD><EFBFBD><EFBFBD>. 2023 <EFBFBD>.
* Author: sychev
* Author: sychev, algin
*/
#ifndef FREE_RTOS_ETHERNET_INDUSTRY_ETH_ECAT_HPP_
#define FREE_RTOS_ETHERNET_INDUSTRY_ETH_ECAT_HPP_
#include <ethernet_industry/eth_ecat_datagram.hpp>
#include <cstdint>
#include <utility>
#include <kernel/dpl/ClockP.h>
#include "handler_store/handler.hpp"
#include "ethernet/eth_frame.h"
@ -19,17 +20,23 @@
#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 BufferProperties {
uint16_t address;
uint16_t length;
struct ALSTAT {
uint16_t state;
uint16_t fault;
};
class EcatSlave {
public:
EcatSlave(address::SlaveAddresses&& slave_addresses)
: slave_addresses_{slave_addresses} {
}
EcatSlave() {
}
@ -44,14 +51,51 @@ public:
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>;
uint8_t data{EC_STATE_PRE_OP};
datagram::EcatDatagram<TCommand, uint8_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> datagram{ {{slave_address, ECT_REG_ALCTL}}, 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);
return (stat.state == EC_STATE_PRE_OP) && (stat.fault == 0);
}
private:
address::SlaveTypes slave_addresses_;
BufferProperties mbx_write_;
BufferProperties mbx_read_;
BufferProperties pdo_output_;
BufferProperties mbx_input_;
address::SlaveAddresses slave_addresses_;
};
class EthEcat : public Handler {
@ -70,13 +114,17 @@ public:
void set_slaves_to_default();
uint16_t slaves_detecting();
void set_addresses_of_slaves(uint16_t address_base);
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();
/*
* Тип адресации slave_address зависит от типа команды CommandT
* Может иметь тип Position, Broadcast, Node
* Logical не поддерживается
*/
template<typename TypeT, typename DirT, typename EcatDgDataT>
void simple_send_datagram(typename TypeT::TSlaveAddress& slave_address, uint16_t offset, EcatDgDataT& data) {
@ -94,7 +142,9 @@ public:
return eeprom_;
}
uint16_t config_init();
std::vector<EcatSlave>& get_slaves() {
return slaves_;
}
bool is_ready() { return ready_; }
@ -131,9 +181,7 @@ private:
Statistic stat_;
std::array<EcatSlave, max_number_of_slaves> slaves_;
uint16_t number_of_slaves_ = 0;
std::vector<EcatSlave> slaves_;
};
}

26
components/free_rtos/ethernet_industry/eth_ecat_command.hpp
View File

@ -55,17 +55,17 @@ struct Logical {
uint32_t val_;
};
*/
using SlaveTypes = std::tuple<Position, Broadcast, Node, Logical>;
using SlaveAddresses = std::tuple<Position, Broadcast, Node, Logical>;
// Register offset
using Offset = uint16_t;
using Offset = ec_reg_offset;
using PositionAddress = std::tuple<Position, Offset>;
using BroadcastAddress = std::tuple<Broadcast, Offset>;
using NodeAddress = std::tuple<Node, Offset>;
using LogicalAddress = std::tuple<Logical>;
using Types = std::tuple<PositionAddress, BroadcastAddress, NodeAddress, LogicalAddress>;
using Addresses = std::tuple<PositionAddress, BroadcastAddress, NodeAddress, LogicalAddress>;
} // namespace address
@ -108,7 +108,7 @@ struct TypeBase {
template<TYPE_INDEX type_index>
struct Type : public TypeBase {
using TAddress = typename std::tuple_element<static_cast<size_t>(type_index), address::Types>::type;
using TAddress = typename std::tuple_element<static_cast<size_t>(type_index), address::Addresses>::type;
using TSlaveAddress = typename std::tuple_element<0, TAddress>::type;
static constexpr TYPE_INDEX type = type_index;
@ -148,30 +148,30 @@ private:
template<typename TypeT, typename DirT>
class EcatCommand : public EcatCommandBase {
static_assert(std::is_base_of<TypeBase, TypeT>::value == true, "TypeT should be derived from CMD::TypeBase");
static_assert(std::is_base_of<DirBase, DirT>::value == true, "DirT should be derived from CMD::DirBase");
static_assert(std::is_base_of<TypeBase, TypeT>::value == true, "TypeT should be derived from command::TypeBase");
static_assert(std::is_base_of<DirBase, DirT>::value == true, "DirT should be derived from command::DirBase");
public:
EcatCommand(typename TypeT::TAddress fields)
: EcatCommandBase{TypeT::get_cmd(DirT::dir)}
, address_{fields} { }
, address_union_{fields} { }
EcatCommand() { }
uint32_t get_address() {
return address_.dword_;
return address_union_.dword_;
}
private:
template<typename FieldsT>
union Address
union AddressUnion
{
Address(FieldsT fields)
AddressUnion(FieldsT fields)
: fields_{fields} { }
Address() { }
AddressUnion() { }
Address& operator=(const Address& other) {
AddressUnion& operator=(const AddressUnion& other) {
fields_ = other.fields_;
return *this;
@ -181,7 +181,7 @@ private:
uint32_t dword_;
};
Address<typename TypeT::TAddress> address_;
AddressUnion<typename TypeT::TAddress> address_union_;
};
/*

6
components/free_rtos/ethernet_industry/eth_ecat_eeprom.hpp
View File

@ -50,22 +50,16 @@ public:
template<typename TypeT, typename EcatDgDataT>
void read(typename TypeT::TSlaveAddress& slave_address, uint16_t eeprom_address, EcatDgDataT& data) {
wait_busy<TypeT>(slave_address);
control_register<TypeT, command::WR>(slave_address, EC_ECMD_READ, eeprom_address);
wait_busy<TypeT>(slave_address);
data_register<TypeT, command::RD, EcatDgDataT>(slave_address, data);
}
template<typename TypeT, typename EcatDgDataT>
void write(typename TypeT::TSlaveAddress& slave_address, uint16_t eeprom_address, EcatDgDataT& data) {
wait_busy<TypeT>(slave_address);
data_register<TypeT, command::WR, EcatDgDataT>(slave_address, data);
wait_busy<TypeT>(slave_address);
control_register<TypeT, command::WR>(slave_address, EC_ECMD_WRITE, eeprom_address);
}

47
components/free_rtos/ethernet_industry/eth_ecat_mailbox.cpp Normal file
View File

@ -0,0 +1,47 @@
/*
* eth_ecat_mailbox.cpp
*
* Created on: May 3, 2023
* Author: algin
*/
#include "eth_ecat_mailbox.hpp"
namespace free_rtos {
void EthEcatMailbox::mailbox_registration() {
std::vector<EcatSlave>& slaves = ecat_.get_slaves();
mailbox_slaves_.reserve(slaves.size());
for(EcatSlave& slave : slaves) {
mailbox_slaves_.push_back(EcatMailboxSlave{slave});
}
for(EcatMailboxSlave& mailbox_slave : mailbox_slaves_) {
mailbox_slave.read_mailbox_info_from_eeprom<command::FP>(ecat_.get_eeprom());
}
}
void EthEcatMailbox::sync_manager_registration() {
datagram::EcatTelegram& telegram = ecat_.get_telegram();
for(EcatMailboxSlave& mailbox_slave : mailbox_slaves_) {
mailbox_slave.register_sync_manager<command::FP>(telegram, sync_manager::SYNC_M1, sync_manager::SYNC_M0);
}
}
uint16_t EthEcatMailbox::config_init() {
uint16_t number_of_slaves = ecat_.config_init();
bool status;
mailbox_registration();
sync_manager_registration();
ecat_.enable_PDI();
status = ecat_.init_to_preop();
return number_of_slaves;
}
}

114
components/free_rtos/ethernet_industry/eth_ecat_mailbox.hpp Normal file
View File

@ -0,0 +1,114 @@
/*
* eth_ecat_mailbox.hpp
*
* Created on: May 3, 2023
* Author: algin
*/
#ifndef FREE_RTOS_ETHERNET_INDUSTRY_ETH_ECAT_MAILBOX_HPP_
#define FREE_RTOS_ETHERNET_INDUSTRY_ETH_ECAT_MAILBOX_HPP_
#include <vector>
#include "ethernet_industry/eth_ecat.hpp"
namespace free_rtos {
struct BufferProperties {
uint16_t address;
uint16_t length;
};
struct SyncManager {
address::Offset offset;
uint32_t default_setting;
};
struct SyncManagerProperties {
BufferProperties mbx;
uint32_t setting;
};
enum class SyncManagerDirection : uint8_t {
READ = 0,
WRITE
};
class EcatMailboxSlave {
public:
EcatMailboxSlave(EcatSlave& slave)
: slave_(slave) { }
template<typename TypeT>
void read_mailbox_info_from_eeprom(eeprom::EEPROM& eeprom) {
auto slave_address = slave_.get_slave_address<TypeT>();
eeprom.read<TypeT>(slave_address, ECT_SII_TXMBXADR, mbx_[static_cast<size_t>(SyncManagerDirection::READ)]);
eeprom.read<TypeT>(slave_address, ECT_SII_RXMBXADR, mbx_[static_cast<size_t>(SyncManagerDirection::WRITE)]);
eeprom.read<TypeT>(slave_address, ECT_PDOOUTPUTADR, pdo_[0]);
eeprom.read<TypeT>(slave_address, ECT_PDOINPUTADR, pdo_[1]);
DebugP_log("mbx_[READ] = 0x%04x\r\n", mbx_[0]);
DebugP_log("mbx_[WRITE] = 0x%04x\r\n", mbx_[1]);
DebugP_log("pdo_[OUTPUT] = 0x%04x\r\n", pdo_[0]);
DebugP_log("pdo_[INPUT] = 0x%04x\r\n", pdo_[1]);
}
template<typename TypeT>
void register_sync_manager(datagram::EcatTelegram& telegram, sync_manager sm_read, sync_manager sm_write) {
using TCommand = command::EcatCommand<TypeT, command::WR>;
static constexpr std::array<SyncManager, 4> syncManagers = {{
{ECT_REG_SM0, EC_DEFAULTMBXSM0},
{ECT_REG_SM1, EC_DEFAULTMBXSM1},
{ECT_REG_SM2, EC_DEFAULTMBXSM2},
{ECT_REG_SM3, EC_DEFAULTMBXSM3}
}};
auto slave_address = slave_.get_slave_address<TypeT>();
BufferProperties& mbx_read = mbx_[static_cast<size_t>(SyncManagerDirection::READ)];
SyncManager sync_manager_read = syncManagers[static_cast<size_t>(sm_read)];
SyncManagerProperties properties_read{mbx_read, sync_manager_read.default_setting};
datagram::EcatDatagram<TCommand, SyncManagerProperties> datagram_read{ {{slave_address, sync_manager_read.offset}}, properties_read };
BufferProperties& mbx_write = mbx_[static_cast<size_t>(SyncManagerDirection::WRITE)];
SyncManager sync_manager_write = syncManagers[static_cast<size_t>(sm_write)];
SyncManagerProperties properties_write{mbx_write, sync_manager_write.default_setting};
datagram::EcatDatagram<TCommand, SyncManagerProperties> datagram_write{ {{slave_address, sync_manager_write.offset}}, properties_write };
datagram_read + 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:
std::array<BufferProperties, 2> mbx_;
std::array<BufferProperties, 2> pdo_;
EcatSlave& slave_;
};
class EthEcatMailbox {
public:
EthEcatMailbox(EthEcat& ecat): ecat_{ecat} { }
void mailbox_registration();
void sync_manager_registration();
uint16_t config_init();
private:
EthEcat& ecat_;
std::vector<EcatMailboxSlave> mailbox_slaves_;
};
}
#endif /* FREE_RTOS_ETHERNET_INDUSTRY_ETH_ECAT_MAILBOX_HPP_ */