dev: Релизация ecat mailbox
This commit is contained in:
parent
1e0fcf04c3
commit
1323d61b47
@ -79,15 +79,17 @@ void EthEcat::set_slaves_to_default() {
|
||||
|
||||
/* reset CRC counters */
|
||||
std::array<uint8_t, 8> c_data_out;
|
||||
c_data_out.fill(0x00);
|
||||
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;
|
||||
d_data_out.fill(0x00);
|
||||
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;
|
||||
e_data_out.fill(0x00);
|
||||
datagram::EcatDatagram<command::BWR, std::array<uint8_t, 8*4>> e{ {{broadcast, ECT_REG_SM0}}, e_data_out };
|
||||
|
||||
/* reset activation register */
|
||||
@ -96,6 +98,7 @@ void EthEcat::set_slaves_to_default() {
|
||||
|
||||
/* reset system time+ofs */
|
||||
std::array<uint8_t, 4> g_data_out;
|
||||
g_data_out.fill(0x00);
|
||||
datagram::EcatDatagram<command::BWR, std::array<uint8_t, 4>> g{ {{broadcast, ECT_REG_DCSYSTIME}}, g_data_out };
|
||||
|
||||
/* DC speedstart */
|
||||
@ -111,8 +114,8 @@ void EthEcat::set_slaves_to_default() {
|
||||
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 };
|
||||
uint16_t k_data_out{EC_STATE_INIT|EC_STATE_ACK};
|
||||
datagram::EcatDatagram<command::BWR, uint16_t> k{ {{broadcast, ECT_REG_ALCTL}}, k_data_out };
|
||||
|
||||
/* force Eeprom from PDI */
|
||||
uint8_t l_data_out{0x02};
|
||||
@ -124,6 +127,18 @@ void EthEcat::set_slaves_to_default() {
|
||||
|
||||
a + b + c + d + e + f + g + h + i + j + k + l + m;
|
||||
telegram_.transfer(a);
|
||||
//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("a.get_wkc() = %d\r\n", a.get_wkc());
|
||||
DebugP_log("b.get_wkc() = %d\r\n", b.get_wkc());
|
||||
|
||||
@ -70,8 +70,8 @@ public:
|
||||
|
||||
{
|
||||
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 };
|
||||
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);
|
||||
|
||||
@ -82,7 +82,7 @@ public:
|
||||
|
||||
{
|
||||
using TCommand = command::EcatCommand<TypeT, command::RD>;
|
||||
datagram::EcatDatagram<TCommand, ALSTAT> datagram{ {{slave_address, ECT_REG_ALCTL}}, stat };
|
||||
datagram::EcatDatagram<TCommand, ALSTAT> datagram{ {{slave_address, ECT_REG_ALSTAT}}, stat };
|
||||
|
||||
telegram.transfer(datagram);
|
||||
|
||||
|
||||
@ -26,35 +26,6 @@ using Broadcast = uint16_t;
|
||||
using Node = uint16_t;
|
||||
using Logical = uint32_t;
|
||||
|
||||
/*
|
||||
struct Position {
|
||||
Position(int16_t val): val_{val} { }
|
||||
Position() { }
|
||||
|
||||
int16_t val_;
|
||||
};
|
||||
|
||||
struct Broadcast {
|
||||
Broadcast(uint16_t val): val_{val} { }
|
||||
Broadcast() { }
|
||||
|
||||
uint16_t val_;
|
||||
};
|
||||
|
||||
struct Node {
|
||||
Node(uint16_t val): val_{val} { }
|
||||
Node() { }
|
||||
|
||||
uint16_t val_;
|
||||
};
|
||||
|
||||
struct Logical {
|
||||
Logical(uint32_t val): val_{val} { }
|
||||
Logical() { }
|
||||
|
||||
uint32_t val_;
|
||||
};
|
||||
*/
|
||||
using SlaveAddresses = std::tuple<Position, Broadcast, Node, Logical>;
|
||||
|
||||
// Register offset
|
||||
@ -204,8 +175,6 @@ using BRD = EcatCommand<B, RD>;
|
||||
using BRW = EcatCommand<B, RW>;
|
||||
using BWR = EcatCommand<B, WR>;
|
||||
|
||||
// Node addressing shortcuts
|
||||
|
||||
/*
|
||||
* arg0: Slave is addressed if Address matches Configured Station Address or Configured Station Alias (if enabled).
|
||||
* arg1: Local register or memory address of the ESC
|
||||
|
||||
@ -0,0 +1,62 @@
|
||||
/*
|
||||
* eth_ecat_custom_tuple.hpp
|
||||
*
|
||||
* Created on: May 5, 2023
|
||||
* Author: algin
|
||||
*/
|
||||
|
||||
#ifndef FREE_RTOS_ETHERNET_INDUSTRY_ETH_ECAT_CUSTOM_TUPLE_HPP_
|
||||
#define FREE_RTOS_ETHERNET_INDUSTRY_ETH_ECAT_CUSTOM_TUPLE_HPP_
|
||||
|
||||
namespace free_rtos {
|
||||
|
||||
// Базовый шаблон класса, никогда не инстанциируется, поэтому без тела
|
||||
template<typename... Args>
|
||||
struct CustomTuple;
|
||||
|
||||
// Основная специализация шаблона
|
||||
template<typename HeadT, typename... TailT>
|
||||
struct CustomTuple<HeadT, TailT...> : CustomTuple<TailT...> {
|
||||
CustomTuple(HeadT& head, TailT&... tail)
|
||||
: CustomTuple<TailT...>(tail...)
|
||||
, head_(head) { }
|
||||
|
||||
using TBase = CustomTuple<TailT...>;
|
||||
|
||||
TBase& base_ = static_cast<TBase&>(*this);
|
||||
HeadT& head_;
|
||||
|
||||
uint8_t* pack(uint8_t *raw) {
|
||||
HeadT* data = new(raw) HeadT{head_};
|
||||
|
||||
(void)data;
|
||||
|
||||
return base_.pack(raw + sizeof(HeadT));
|
||||
}
|
||||
|
||||
uint8_t* unpack(uint8_t *raw) {
|
||||
HeadT *head = reinterpret_cast<HeadT*>(raw);
|
||||
|
||||
head_ = *head;
|
||||
|
||||
return base_.unpack(raw + sizeof(HeadT));
|
||||
}
|
||||
};
|
||||
|
||||
// Специализация завершения рекурсии
|
||||
template<>
|
||||
struct CustomTuple<> {
|
||||
uint8_t* pack(uint8_t *raw) {
|
||||
return raw;
|
||||
}
|
||||
|
||||
uint8_t* unpack(uint8_t *raw) {
|
||||
return raw;
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
#endif /* FREE_RTOS_ETHERNET_INDUSTRY_ETH_ECAT_CUSTOM_TUPLE_HPP_ */
|
||||
@ -31,6 +31,9 @@ void EcatTelegram::pack(IEcatDatagram& first) {
|
||||
uint8_t *p_datagram_last = p_datagram_first;
|
||||
IEcatDatagram *next = &first;
|
||||
|
||||
(void)p_eth_hdr;
|
||||
(void)p_hdr;
|
||||
|
||||
while(next != nullptr) {
|
||||
p_datagram_last = next->pack(p_datagram_last);
|
||||
next = next->get_next();
|
||||
@ -47,6 +50,9 @@ void EcatTelegram::unpack(IEcatDatagram& first) {
|
||||
uint8_t *p_datagram_last = p_datagram_first;
|
||||
IEcatDatagram *next = &first;
|
||||
|
||||
(void)p_eth_hdr;
|
||||
(void)p_hdr;
|
||||
|
||||
while(next != nullptr) {
|
||||
p_datagram_last = next->unpack(p_datagram_last);
|
||||
next = next->get_next();
|
||||
|
||||
@ -14,6 +14,7 @@
|
||||
#include "ethernet_industry/ethercattype.hpp"
|
||||
#include "ethernet_industry/eth_ecat_types.h"
|
||||
#include "ethernet_industry/eth_ecat_command.hpp"
|
||||
#include "ethernet_industry/eth_ecat_custom_tuple.hpp"
|
||||
|
||||
namespace free_rtos {
|
||||
|
||||
@ -21,55 +22,6 @@ namespace datagram {
|
||||
|
||||
using TEcatWkc = uint16_t;
|
||||
|
||||
template<typename EcatDgDataT>
|
||||
struct EcatDg {
|
||||
public:
|
||||
EcatDg(uint8_t cmd, uint32_t address, EcatDgDataT& data, ec_moredatagrams more = ec_moredatagrams::EC_MOREDATAGRAMS_LAST, uint8_t idx = 0, uint16_t irq = 0)
|
||||
: header_{
|
||||
cmd,
|
||||
idx,
|
||||
address,
|
||||
{.bits = {
|
||||
.len = sizeof(EcatDgDataT),
|
||||
.c = static_cast<uint16_t>(ec_circframe::EC_CIRCFRAME_NOT),
|
||||
.m = static_cast<uint16_t>(more)}},
|
||||
irq}
|
||||
, data_{data}
|
||||
, wkc_{0} { }
|
||||
|
||||
EcatDg(uint8_t cmd, uint32_t address, ec_moredatagrams more = ec_moredatagrams::EC_MOREDATAGRAMS_LAST, uint8_t idx = 0, uint16_t irq = 0)
|
||||
: header_{
|
||||
cmd,
|
||||
idx,
|
||||
address,
|
||||
{.bits = {
|
||||
.len = sizeof(EcatDgDataT),
|
||||
.c = static_cast<uint16_t>(ec_circframe::EC_CIRCFRAME_NOT),
|
||||
.m = static_cast<uint16_t>(more)}},
|
||||
irq}
|
||||
, wkc_{0} { }
|
||||
|
||||
EcatDg() { }
|
||||
|
||||
TEcatDgHeader& get_header() {
|
||||
return header_;
|
||||
}
|
||||
|
||||
EcatDgDataT& get_data() {
|
||||
return data_;
|
||||
}
|
||||
|
||||
TEcatWkc get_wkc() {
|
||||
return wkc_;
|
||||
}
|
||||
|
||||
private:
|
||||
TEcatDgHeader header_;
|
||||
EcatDgDataT data_;
|
||||
TEcatWkc wkc_;
|
||||
|
||||
} __attribute__ ((packed));
|
||||
|
||||
class IEcatDatagram {
|
||||
public:
|
||||
IEcatDatagram(ec_moredatagrams more, TEcatWkc wkc)
|
||||
@ -98,71 +50,99 @@ public:
|
||||
virtual uint8_t* pack(uint8_t *raw) = 0;
|
||||
virtual uint8_t* unpack(uint8_t *raw) = 0;
|
||||
|
||||
TEcatWkc get_wkc() {
|
||||
TEcatDgHeader& get_header() {
|
||||
return header_;
|
||||
}
|
||||
|
||||
TEcatWkc& get_wkc() {
|
||||
return wkc_;
|
||||
}
|
||||
|
||||
protected:
|
||||
ec_moredatagrams more_;
|
||||
|
||||
TEcatDgHeader header_;
|
||||
TEcatWkc wkc_;
|
||||
|
||||
private:
|
||||
IEcatDatagram *next_{nullptr};
|
||||
};
|
||||
|
||||
template<typename CommandT, typename EcatDgDataT>
|
||||
template<typename CommandT, typename... DataTypes>
|
||||
class EcatDatagram : public IEcatDatagram {
|
||||
static_assert(std::is_base_of<command::EcatCommandBase, CommandT>::value == true, "CommandT should be derived from ECatCommandBase");
|
||||
|
||||
public:
|
||||
EcatDatagram(CommandT command, EcatDgDataT &data_out, EcatDgDataT &data_in)
|
||||
EcatDatagram(CommandT command, DataTypes&... data)
|
||||
: IEcatDatagram{ec_moredatagrams::EC_MOREDATAGRAMS_LAST, 0x0000}
|
||||
, command_{command}
|
||||
, data_out_{&data_out}
|
||||
, data_in_{&data_in} { }
|
||||
|
||||
EcatDatagram(CommandT command, EcatDgDataT &data)
|
||||
: IEcatDatagram{ec_moredatagrams::EC_MOREDATAGRAMS_LAST, 0x0000}
|
||||
, command_{command}
|
||||
, data_out_{&data}
|
||||
, data_in_{&data} { }
|
||||
|
||||
EcatDatagram(CommandT command)
|
||||
: IEcatDatagram{ec_moredatagrams::EC_MOREDATAGRAMS_LAST, 0x0000}
|
||||
, command_{command} { }
|
||||
, data_{data...} { }
|
||||
|
||||
EcatDatagram() { }
|
||||
|
||||
virtual uint8_t* pack(uint8_t *raw) override {
|
||||
EcatDg<EcatDgDataT> *p_dg;
|
||||
|
||||
if(data_out_ != nullptr) {
|
||||
p_dg = new(raw) EcatDg<EcatDgDataT> {command_.get_cmd(), command_.get_address(), *data_out_, more_};
|
||||
}else{
|
||||
p_dg = new(raw) EcatDg<EcatDgDataT> {command_.get_cmd(), command_.get_address(), more_};
|
||||
}
|
||||
|
||||
(void)p_dg;
|
||||
|
||||
return (raw + sizeof(EcatDg<EcatDgDataT>));
|
||||
return pack_header(raw);
|
||||
}
|
||||
|
||||
virtual uint8_t* unpack(uint8_t *raw) override {
|
||||
EcatDg<EcatDgDataT> *p_dg = reinterpret_cast<EcatDg<EcatDgDataT>*>(raw);
|
||||
|
||||
if(data_in_ != nullptr) {
|
||||
*data_in_ = p_dg->get_data();
|
||||
}
|
||||
|
||||
wkc_ = p_dg->get_wkc();
|
||||
|
||||
return (raw + sizeof(EcatDg<EcatDgDataT>));
|
||||
return unpack_header(raw);
|
||||
}
|
||||
|
||||
private:
|
||||
CommandT command_;
|
||||
EcatDgDataT *data_out_{nullptr};
|
||||
EcatDgDataT *data_in_{nullptr};
|
||||
CustomTuple<DataTypes...> data_;
|
||||
|
||||
uint8_t* pack_wkc(uint8_t *raw) {
|
||||
TEcatWkc *wkc = new(raw) TEcatWkc{0x0000};
|
||||
|
||||
(void)wkc;
|
||||
|
||||
return raw + sizeof(TEcatWkc);
|
||||
}
|
||||
|
||||
uint8_t* pack_data(uint8_t *raw) {
|
||||
return data_.pack(raw);
|
||||
}
|
||||
|
||||
uint8_t* pack_header(uint8_t *raw) {
|
||||
uint8_t *data_raw = raw + sizeof(TEcatDgHeader);
|
||||
uint8_t *wkc_raw = pack_data(data_raw); // сначала упаковываем все данные для вычислением их размера
|
||||
uint16_t len = wkc_raw - data_raw;
|
||||
TEcatDgHeader *header_ = new(raw) TEcatDgHeader{
|
||||
command_.get_cmd(),
|
||||
0x00,
|
||||
command_.get_address(),
|
||||
{.bits = {
|
||||
.len = len,
|
||||
.c = static_cast<uint16_t>(ec_circframe::EC_CIRCFRAME_NOT),
|
||||
.m = static_cast<uint16_t>(more_)}},
|
||||
0x0000};
|
||||
|
||||
(void)header_;
|
||||
|
||||
return pack_wkc(wkc_raw);
|
||||
}
|
||||
|
||||
|
||||
uint8_t* unpack_wkc(uint8_t *raw) {
|
||||
TEcatWkc *wkc = reinterpret_cast<TEcatWkc*>(raw);
|
||||
|
||||
wkc_ = *wkc;
|
||||
|
||||
return raw + sizeof(TEcatWkc);
|
||||
}
|
||||
|
||||
uint8_t* unpack_data(uint8_t *raw) {
|
||||
return unpack_wkc(data_.unpack(raw));
|
||||
}
|
||||
|
||||
uint8_t* unpack_header(uint8_t *raw) {
|
||||
TEcatDgHeader *header = reinterpret_cast<TEcatDgHeader*>(raw);
|
||||
|
||||
header_ = *header;
|
||||
|
||||
return unpack_data(raw + sizeof(TEcatDgHeader));
|
||||
}
|
||||
};
|
||||
|
||||
class EcatTelegram : public Handler {
|
||||
|
||||
@ -9,6 +9,8 @@
|
||||
|
||||
namespace free_rtos {
|
||||
|
||||
namespace ecat_mailbox {
|
||||
|
||||
void EthEcatMailbox::mailbox_registration() {
|
||||
std::vector<EcatSlave>& slaves = ecat_.get_slaves();
|
||||
|
||||
@ -45,3 +47,5 @@ uint16_t EthEcatMailbox::config_init() {
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@ -9,13 +9,41 @@
|
||||
#define FREE_RTOS_ETHERNET_INDUSTRY_ETH_ECAT_MAILBOX_HPP_
|
||||
|
||||
#include <vector>
|
||||
#include <kernel/dpl/ClockP.h>
|
||||
|
||||
#include "ethernet_industry/eth_ecat.hpp"
|
||||
|
||||
namespace free_rtos {
|
||||
|
||||
struct BufferProperties {
|
||||
namespace ecat_mailbox {
|
||||
|
||||
enum ProtocolType : std::uint16_t {
|
||||
/** Beckhoff: AoE ADS over EtherCAT */
|
||||
VENDOR_SPECIFIC = 1,
|
||||
/** Ethernet over EtherCAT */
|
||||
EoE,
|
||||
/** CanOpen Over EtherCAT */
|
||||
CoE,
|
||||
/** File access over EtherCAT */
|
||||
FoE,
|
||||
/** Servo profile over EtherCAT */
|
||||
SoE,
|
||||
/** Vendor specific (VoE) */
|
||||
VoE = 0xF
|
||||
};
|
||||
|
||||
struct MailboxHeader {
|
||||
uint16_t length;
|
||||
uint16_t address;
|
||||
uint16_t channel : 6;
|
||||
uint16_t priority : 2;
|
||||
ProtocolType type : 4;
|
||||
uint16_t cnt : 3;
|
||||
uint16_t reserved : 1;
|
||||
};
|
||||
|
||||
struct BufferProperties {
|
||||
address::Offset offset;
|
||||
uint16_t length;
|
||||
};
|
||||
|
||||
@ -24,11 +52,6 @@ struct SyncManager {
|
||||
uint32_t default_setting;
|
||||
};
|
||||
|
||||
struct SyncManagerProperties {
|
||||
BufferProperties mbx;
|
||||
uint32_t setting;
|
||||
};
|
||||
|
||||
enum class SyncManagerDirection : uint8_t {
|
||||
READ = 0,
|
||||
WRITE
|
||||
@ -49,8 +72,22 @@ public:
|
||||
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]);
|
||||
#ifndef COMX
|
||||
mbx_[static_cast<size_t>(SyncManagerDirection::READ)].length = read_synch_manager_buffer_size;
|
||||
mbx_[static_cast<size_t>(SyncManagerDirection::WRITE)].length = write_synch_manager_buffer_size;
|
||||
#endif
|
||||
|
||||
#ifdef COMX
|
||||
//todo: костыль под COMX, необходимо исправлять, понять как и где найти эти адреса
|
||||
pdo_[0].address = 0x1100 & 0xFFFF;
|
||||
pdo_[1].address = 0x1400 & 0xFFFF;
|
||||
#endif
|
||||
|
||||
mailbox_regs[READ] = mbx_[static_cast<size_t>(SyncManagerDirection::READ)].offset;
|
||||
mailbox_regs[WRITE] = mbx_[static_cast<size_t>(SyncManagerDirection::WRITE)].offset;
|
||||
|
||||
DebugP_log("mbx_[READ] = 0x%04x\r\n", mbx_[static_cast<size_t>(SyncManagerDirection::READ)]);
|
||||
DebugP_log("mbx_[WRITE] = 0x%04x\r\n", mbx_[static_cast<size_t>(SyncManagerDirection::WRITE)]);
|
||||
|
||||
DebugP_log("pdo_[OUTPUT] = 0x%04x\r\n", pdo_[0]);
|
||||
DebugP_log("pdo_[INPUT] = 0x%04x\r\n", pdo_[1]);
|
||||
@ -71,13 +108,15 @@ public:
|
||||
|
||||
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 };
|
||||
datagram::EcatDatagram<TCommand, BufferProperties, uint32_t> datagram_read{ {{slave_address, sync_manager_read.offset}},
|
||||
mbx_read,
|
||||
sync_manager_read.default_setting };
|
||||
|
||||
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::EcatDatagram<TCommand, BufferProperties, uint32_t> datagram_write{ {{slave_address, sync_manager_write.offset}},
|
||||
mbx_write,
|
||||
sync_manager_write.default_setting };
|
||||
|
||||
datagram_read + datagram_write;
|
||||
|
||||
@ -87,11 +126,126 @@ public:
|
||||
DebugP_log("datagram_write.get_wkc() = %d\r\n", datagram_write.get_wkc());
|
||||
}
|
||||
|
||||
template<typename TypeT>
|
||||
bool is_available(datagram::EcatTelegram& telegram) {
|
||||
using TCommand = command::EcatCommand<TypeT, command::RD>;
|
||||
|
||||
auto slave_address = slave_.get_slave_address<TypeT>();
|
||||
uint16_t status{0x0000};
|
||||
datagram::EcatDatagram<TCommand, uint16_t> datagram{ {{slave_address, mailbox_regs[AVAILABLE]}}, status};
|
||||
|
||||
do{
|
||||
telegram.transfer(datagram);
|
||||
|
||||
DebugP_log("datagram.get_wkc() = %d\r\n", datagram.get_wkc());
|
||||
|
||||
if((status & 0x0008) != 0) {
|
||||
return true;
|
||||
}
|
||||
} while(1);
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
template<typename TypeT>
|
||||
bool is_empty(datagram::EcatTelegram& telegram) {
|
||||
using TCommand = command::EcatCommand<TypeT, command::RD>;
|
||||
|
||||
auto slave_address = slave_.get_slave_address<TypeT>();
|
||||
uint8_t empty{0x00};
|
||||
datagram::EcatDatagram<TCommand, uint16_t> datagram{ {{slave_address, mailbox_regs[EMPTY]}}, empty};
|
||||
|
||||
telegram.transfer(datagram);
|
||||
|
||||
DebugP_log("datagram.get_wkc() = %d\r\n", datagram.get_wkc());
|
||||
|
||||
if((empty & 0x08) != 0 ) {
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
template<typename TypeT>
|
||||
void empty(datagram::EcatTelegram& telegram) {
|
||||
using TCommand = command::EcatCommand<TypeT, command::RD>;
|
||||
|
||||
auto slave_address = slave_.get_slave_address<TypeT>();
|
||||
uint16_t trash{0x0000};
|
||||
datagram::EcatDatagram<TCommand, uint16_t> datagram{ {{slave_address, mailbox_regs[AVAILABLE]}}, trash};
|
||||
|
||||
telegram.transfer(datagram);
|
||||
|
||||
DebugP_log("datagram.get_wkc() = %d\r\n", datagram.get_wkc());
|
||||
}
|
||||
|
||||
template<typename TypeT, typename... DataTypes>
|
||||
void send_data(datagram::EcatTelegram& telegram, uint16_t address, uint16_t priority, ProtocolType type, DataTypes&... data) {
|
||||
using TCommand = command::EcatCommand<TypeT, command::WR>;
|
||||
|
||||
auto slave_address = slave_.get_slave_address<TypeT>();
|
||||
MailboxHeader header = {
|
||||
.length = sizeof...(DataTypes),
|
||||
.address = address,
|
||||
.channel = 0,
|
||||
.priority = priority,
|
||||
.type = type,
|
||||
.cnt = static_cast<uint16_t>(counter % 7)
|
||||
};
|
||||
datagram::EcatDatagram<TCommand, MailboxHeader, DataTypes...> datagram{ {{slave_address, mailbox_regs[WRITE]}}, header, data...};
|
||||
|
||||
telegram.transfer(datagram);
|
||||
|
||||
counter++;
|
||||
|
||||
DebugP_log("datagram.get_wkc() = %d\r\n", datagram.get_wkc());
|
||||
}
|
||||
|
||||
template<typename TypeT, typename... DataTypes>
|
||||
void receive_data(datagram::EcatTelegram& telegram, DataTypes&... data) {
|
||||
using TCommand = command::EcatCommand<TypeT, command::RD>;
|
||||
|
||||
auto slave_address = slave_.get_slave_address<TypeT>();
|
||||
MailboxHeader header;
|
||||
datagram::EcatDatagram<TCommand, MailboxHeader, DataTypes...> datagram{ {{slave_address, mailbox_regs[READ]}}, header, data...};
|
||||
|
||||
telegram.transfer(datagram);
|
||||
|
||||
counter++;
|
||||
|
||||
DebugP_log("datagram.get_wkc() = %d\r\n", datagram.get_wkc());
|
||||
}
|
||||
|
||||
template<typename TypeT, typename... DataTypes>
|
||||
void receive(datagram::EcatTelegram& telegram, DataTypes&... data) {
|
||||
is_available<TypeT>(telegram);
|
||||
receive_data<TypeT, DataTypes...>(telegram, data...);
|
||||
}
|
||||
|
||||
template<typename TypeT, typename... DataTypes>
|
||||
void send(datagram::EcatTelegram& telegram, uint16_t address, uint16_t priority, ProtocolType type, DataTypes&... data) {
|
||||
empty<TypeT>(telegram);
|
||||
bool empty = is_empty<TypeT>(telegram);
|
||||
|
||||
DebugP_log("empty = %d\r\n", empty);
|
||||
|
||||
send_data<TypeT, DataTypes...>(telegram, address, priority, type, data...);
|
||||
}
|
||||
|
||||
private:
|
||||
std::array<address::Offset, 4> mailbox_regs = {
|
||||
static_cast<address::Offset>(0x0000),
|
||||
static_cast<address::Offset>(0x0000),
|
||||
ECT_REG_SM0STAT,
|
||||
ECT_REG_SM1STAT
|
||||
};
|
||||
|
||||
std::array<BufferProperties, 2> mbx_;
|
||||
std::array<BufferProperties, 2> pdo_;
|
||||
|
||||
EcatSlave& slave_;
|
||||
|
||||
uint8_t counter{0};
|
||||
};
|
||||
|
||||
class EthEcatMailbox {
|
||||
@ -111,4 +265,6 @@ private:
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif /* FREE_RTOS_ETHERNET_INDUSTRY_ETH_ECAT_MAILBOX_HPP_ */
|
||||
|
||||
Loading…
Reference in New Issue
Block a user