dev(SF-33): Добавлен контроль соединения
This commit is contained in:
parent
a842451322
commit
c9aa8f9b4c
@ -25,7 +25,7 @@ void EthEcatPdoFMMU::init(ProcessCallback callback) {
|
||||
callback_ = callback;
|
||||
}
|
||||
|
||||
void EthEcatPdoFMMU::wait_op() {
|
||||
bool EthEcatPdoFMMU::wait_op() {
|
||||
free_rtos::Semaphore& init_sem = ecat_buffer_.get_ecat().get_init_sem();
|
||||
free_rtos::Semaphore& process_sem = ecat_buffer_.get_ecat().get_process_sem();
|
||||
|
||||
@ -37,35 +37,34 @@ void EthEcatPdoFMMU::wait_op() {
|
||||
std::vector<uint8_t> pdo_write(logical_full_length_write, 0x00);
|
||||
custom_tuple<std::vector<uint8_t>&> data_tuple_read{pdo_read};
|
||||
custom_tuple<std::vector<uint8_t>&> data_tuple_write{pdo_write};
|
||||
bool status;
|
||||
|
||||
read(0, pdo_write); // read to pdo_write !
|
||||
status = read(0, pdo_write); // read to pdo_write !
|
||||
|
||||
/*
|
||||
for(uint8_t& byte : process_data) {
|
||||
DebugP_log("0x%01x", byte);
|
||||
if(status != true) {
|
||||
return status;
|
||||
}
|
||||
|
||||
status = write(0, pdo_write);
|
||||
|
||||
if(status != true) {
|
||||
return status;
|
||||
}
|
||||
DebugP_log("\r\n");
|
||||
*/
|
||||
write(0, pdo_write);
|
||||
|
||||
init_sem.post();
|
||||
process_sem.pend();
|
||||
|
||||
for(uint32_t i = 0; i < 250; i++) {
|
||||
//ClockP_usleep(125);
|
||||
read_write(0, data_tuple_read, 0, data_tuple_write);
|
||||
status = read_write(0, data_tuple_read, 0, data_tuple_write);
|
||||
|
||||
//read(0, process_data);
|
||||
/*
|
||||
for(uint8_t& byte : process_data) {
|
||||
DebugP_log("0x%01x", byte);
|
||||
if(status != true) {
|
||||
return status;
|
||||
}
|
||||
DebugP_log("\r\n");
|
||||
*/
|
||||
//write(0, process_data);
|
||||
}
|
||||
|
||||
init_sem.post();
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
void EthEcatPdoFMMU::process_write_queue(uint8_t* process_data, uint32_t len) {
|
||||
@ -128,7 +127,13 @@ void EthEcatPdoFMMU::process_read_queue(uint8_t* process_data, uint32_t len) {
|
||||
}
|
||||
|
||||
void EthEcatPdoFMMU::process() {
|
||||
wait_op();
|
||||
bool status;
|
||||
|
||||
status = wait_op();
|
||||
|
||||
if(status != true) {
|
||||
return;
|
||||
}
|
||||
|
||||
uint32_t logical_full_length_write = ecat_buffer_.get_fmmu_global_properties().logical_full_length_write;
|
||||
uint32_t logical_full_length_read = ecat_buffer_.get_fmmu_global_properties().logical_full_length_read;
|
||||
@ -140,16 +145,27 @@ void EthEcatPdoFMMU::process() {
|
||||
|
||||
ecat_timer.Wait();
|
||||
|
||||
read(0, pdo_write); // read to pdo_write !
|
||||
write(0, pdo_write);
|
||||
status = read(0, pdo_write); // read to pdo_write !
|
||||
|
||||
if(status != true) {
|
||||
return;
|
||||
}
|
||||
|
||||
status = write(0, pdo_write);
|
||||
|
||||
if(status != true) {
|
||||
return;
|
||||
}
|
||||
|
||||
while(1) {
|
||||
ecat_timer.Wait();
|
||||
//DebugP_log("Tick !\r\n");
|
||||
|
||||
read_write(0, data_tuple_read, 0, data_tuple_write);
|
||||
status = read_write(0, data_tuple_read, 0, data_tuple_write);
|
||||
|
||||
//read(0, process_data);
|
||||
if(status != true) {
|
||||
return;
|
||||
}
|
||||
|
||||
#if PROFILE_LEVEL > 0
|
||||
{
|
||||
@ -191,8 +207,6 @@ void EthEcatPdoFMMU::process() {
|
||||
process_write_queue(pdo_write.data(), pdo_write.size());
|
||||
#endif
|
||||
|
||||
//write(0, process_data);
|
||||
|
||||
pdo_counter_++;
|
||||
}
|
||||
}
|
||||
|
||||
@ -141,58 +141,55 @@ private:
|
||||
|
||||
uint32_t pdo_counter_{0};
|
||||
|
||||
void wait_op();
|
||||
bool wait_op();
|
||||
|
||||
void process_write_queue(uint8_t* process_data, uint32_t len);
|
||||
void process_read_queue(uint8_t* process_data, uint32_t len);
|
||||
|
||||
template<typename... DataTypes>
|
||||
void write(address::Offset offset, DataTypes&... data) {
|
||||
bool write(address::Offset offset, DataTypes&... data) {
|
||||
using TDatagram = datagram::EcatDatagram<command::LWR, DataTypes...>;
|
||||
|
||||
telegram::EcatTelegram& telegram = ecat_buffer_.get_ecat().get_telegram();
|
||||
ecat_buffer::FMMUGlobalProperties& fmmu_global_properties = ecat_buffer_.get_fmmu_global_properties();
|
||||
address::Logical logical = fmmu_global_properties.logical_start_address + offset;
|
||||
TDatagram datagram{ {{logical}}, data... };
|
||||
const datagram::TEcatWkc expected_wkc = pdo_fmmu_slaves_.size();
|
||||
TDatagram datagram{ {{logical}}, expected_wkc, data... };
|
||||
|
||||
do {
|
||||
telegram.transfer(datagram);
|
||||
} while(datagram.get_wkc() < 0x0001);
|
||||
return telegram.transfer(datagram);
|
||||
}
|
||||
|
||||
template<typename... DataTypes>
|
||||
void read(address::Offset offset, DataTypes&... data) {
|
||||
bool read(address::Offset offset, DataTypes&... data) {
|
||||
using TDatagram = datagram::EcatDatagram<command::LRD, DataTypes...>;
|
||||
|
||||
telegram::EcatTelegram& telegram = ecat_buffer_.get_ecat().get_telegram();
|
||||
ecat_buffer::FMMUGlobalProperties& fmmu_global_properties = ecat_buffer_.get_fmmu_global_properties();
|
||||
address::Logical logical = fmmu_global_properties.logical_start_address + fmmu_global_properties.logical_full_length_write + offset;
|
||||
TDatagram datagram{ {{logical}}, data... };
|
||||
const datagram::TEcatWkc expected_wkc = pdo_fmmu_slaves_.size();
|
||||
TDatagram datagram{ {{logical}}, expected_wkc, data... };
|
||||
|
||||
do {
|
||||
telegram.transfer(datagram);
|
||||
} while(datagram.get_wkc() < 0x0001);
|
||||
return telegram.transfer(datagram);
|
||||
}
|
||||
|
||||
template<typename... DataTypes>
|
||||
void read_write(address::Offset offset_read, custom_tuple<DataTypes&...> data_read, address::Offset offset_write, custom_tuple<DataTypes&...> data_write) {
|
||||
bool read_write(address::Offset offset_read, custom_tuple<DataTypes&...> data_read, address::Offset offset_write, custom_tuple<DataTypes&...> data_write) {
|
||||
using TDatagramRead = datagram::EcatDatagram<command::LRD, DataTypes...>;
|
||||
using TDatagramWrite = datagram::EcatDatagram<command::LWR, DataTypes...>;
|
||||
|
||||
telegram::EcatTelegram& telegram = ecat_buffer_.get_ecat().get_telegram();
|
||||
ecat_buffer::FMMUGlobalProperties& fmmu_global_properties = ecat_buffer_.get_fmmu_global_properties();
|
||||
const datagram::TEcatWkc expected_wkc = pdo_fmmu_slaves_.size();
|
||||
|
||||
address::Logical logical_read = fmmu_global_properties.logical_start_address + fmmu_global_properties.logical_full_length_write + offset_read;
|
||||
TDatagramRead datagram_read{ {{logical_read}}, data_read };
|
||||
TDatagramRead datagram_read{ {{logical_read}}, expected_wkc, data_read };
|
||||
|
||||
address::Logical logical_write = fmmu_global_properties.logical_start_address + offset_write;
|
||||
TDatagramWrite datagram_write{ {{logical_write}}, data_write };
|
||||
TDatagramWrite datagram_write{ {{logical_write}}, expected_wkc, data_write };
|
||||
|
||||
auto queue = datagram_read + datagram_write;
|
||||
|
||||
do {
|
||||
telegram.transfer(queue);
|
||||
} while(datagram_read.get_all_wkc() < 0x0001);
|
||||
return telegram.transfer(queue);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
@ -22,8 +22,10 @@ void EthEcatSdoMailbox::init() {
|
||||
}
|
||||
}
|
||||
|
||||
void EthEcatSdoMailbox::read_pdo_map(uint16_t pdo_map_rx_index, uint16_t pdo_map_tx_index) {
|
||||
bool EthEcatSdoMailbox::read_pdo_map(uint16_t pdo_map_rx_index, uint16_t pdo_map_tx_index) {
|
||||
const datagram::TEcatWkc expected_wkc = 1;
|
||||
telegram::EcatTelegram& telegram = ecat_buffer_.get_ecat().get_telegram();
|
||||
bool status = true;
|
||||
|
||||
for(EcatSdoMailboxSlave& sdo_mailbox_slave : sdo_mailbox_slaves_) {
|
||||
uint16_t pdo_rx_data_size{0x0000};
|
||||
@ -31,16 +33,15 @@ void EthEcatSdoMailbox::read_pdo_map(uint16_t pdo_map_rx_index, uint16_t pdo_map
|
||||
ecat_buffer::PDOMap pdo_map;
|
||||
|
||||
DebugP_log((char*)"Reading rx pdo map\r\n");
|
||||
pdo_rx_data_size = sdo_mailbox_slave.pdo_map_read<command::FP>(telegram, pdo_map, pdo_map_rx_index);
|
||||
status = (status && sdo_mailbox_slave.pdo_map_read<command::FP>(telegram, expected_wkc, pdo_map, pdo_map_rx_index));
|
||||
|
||||
DebugP_log((char*)"Reading tx pdo map\r\n");
|
||||
pdo_tx_data_size = sdo_mailbox_slave.pdo_map_read<command::FP>(telegram, pdo_map, pdo_map_tx_index);
|
||||
status = (status && sdo_mailbox_slave.pdo_map_read<command::FP>(telegram, expected_wkc, pdo_map, pdo_map_tx_index));
|
||||
|
||||
pdo_map_.emplace_back(std::move(pdo_map));
|
||||
|
||||
DebugP_log((char*)"pdo_rx_data_size = %d\r\n", pdo_rx_data_size);
|
||||
DebugP_log((char*)"pdo_tx_data_size = %d\r\n", pdo_tx_data_size);
|
||||
}
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@ -147,45 +147,49 @@ public:
|
||||
: buffer_slave_(mailbox_slave) { }
|
||||
|
||||
template<typename TypeT>
|
||||
void wait_available(telegram::EcatTelegram& telegram) {
|
||||
bool wait_available(telegram::EcatTelegram& telegram, const datagram::TEcatWkc expected_wkc) {
|
||||
using TCommand = command::EcatCommand<TypeT, command::RD>;
|
||||
|
||||
auto slave_address = buffer_slave_.get_slave().get_slave_address<TypeT>();
|
||||
std::array<address::Offset, 4>& buffer_regs = buffer_slave_.get_buffer_regs();
|
||||
uint8_t sm_status{0x00};
|
||||
datagram::EcatDatagram<TCommand, uint8_t> datagram{ {{slave_address, buffer_regs[MailboxesRegs::AVAILABLE]}}, sm_status };
|
||||
datagram::EcatDatagram<TCommand, uint8_t> datagram{ {{slave_address, buffer_regs[MailboxesRegs::AVAILABLE]}}, expected_wkc, sm_status };
|
||||
bool status;
|
||||
|
||||
do {
|
||||
telegram.transfer(datagram);
|
||||
} while((datagram.get_wkc() < 0x0001) || ((sm_status & 0x08) == 0));
|
||||
status = telegram.transfer(datagram);
|
||||
} while((status == true) && ((sm_status & 0x08) == 0));
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
template<typename TypeT>
|
||||
void wait_empty(telegram::EcatTelegram& telegram) {
|
||||
bool wait_empty(telegram::EcatTelegram& telegram, const datagram::TEcatWkc expected_wkc) {
|
||||
using TCommand = command::EcatCommand<TypeT, command::RD>;
|
||||
|
||||
auto slave_address = buffer_slave_.get_slave().get_slave_address<TypeT>();
|
||||
std::array<address::Offset, 4>& buffer_regs = buffer_slave_.get_buffer_regs();
|
||||
uint8_t sm_status{0x00};
|
||||
datagram::EcatDatagram<TCommand, uint8_t> datagram{ {{slave_address, buffer_regs[MailboxesRegs::EMPTY]}}, sm_status };
|
||||
datagram::EcatDatagram<TCommand, uint8_t> datagram{ {{slave_address, buffer_regs[MailboxesRegs::EMPTY]}}, expected_wkc, sm_status };
|
||||
bool status;
|
||||
|
||||
do {
|
||||
telegram.transfer(datagram);
|
||||
} while((datagram.get_wkc() < 0x0001) || ((sm_status & 0x08) != 0));
|
||||
status = telegram.transfer(datagram);
|
||||
} while((status == true) && ((sm_status & 0x08) != 0));
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
template<typename TypeT>
|
||||
void empty(telegram::EcatTelegram& telegram) {
|
||||
bool empty(telegram::EcatTelegram& telegram, const datagram::TEcatWkc expected_wkc) {
|
||||
using TCommand = command::EcatCommand<TypeT, command::RD>;
|
||||
|
||||
auto slave_address = buffer_slave_.get_slave().get_slave_address<TypeT>();
|
||||
std::array<address::Offset, 4>& buffer_regs = buffer_slave_.get_buffer_regs();
|
||||
uint8_t sm_status{0x00};
|
||||
datagram::EcatDatagram<TCommand, uint8_t> datagram{ {{slave_address, buffer_regs[MailboxesRegs::AVAILABLE]}}, sm_status };
|
||||
datagram::EcatDatagram<TCommand, uint8_t> datagram{ {{slave_address, buffer_regs[MailboxesRegs::AVAILABLE]}}, expected_wkc, sm_status };
|
||||
|
||||
do {
|
||||
telegram.transfer(datagram);
|
||||
} while(datagram.get_wkc() < 0x0001);
|
||||
return telegram.transfer(datagram);
|
||||
}
|
||||
|
||||
void increment_counter() {
|
||||
@ -197,7 +201,7 @@ public:
|
||||
}
|
||||
|
||||
template<typename TypeT, typename... DataTypes>
|
||||
void send_data(telegram::EcatTelegram& telegram, uint16_t channel, uint16_t priority, ProtocolType type, DataTypes&... data) {
|
||||
bool send_data(telegram::EcatTelegram& telegram, const datagram::TEcatWkc expected_wkc, uint16_t channel, uint16_t priority, ProtocolType type, DataTypes&... data) {
|
||||
using TCommand = command::EcatCommand<TypeT, command::WR>;
|
||||
using TDatagram = datagram::EcatDatagram<TCommand, MailboxHeader, DataTypes... , Padding>;
|
||||
|
||||
@ -213,17 +217,22 @@ public:
|
||||
.cnt = static_cast<uint16_t>(counter_)
|
||||
};
|
||||
Padding padding{buffer_slave_.get_buffer_properties_write().length - sizeof(MailboxHeader) - custom_tuple<DataTypes...>::size};
|
||||
TDatagram datagram{ {{slave_address, buffer_regs[MailboxesRegs::WRITE]}}, header, data... , padding};
|
||||
TDatagram datagram{ {{slave_address, buffer_regs[MailboxesRegs::WRITE]}}, expected_wkc, header, data... , padding};
|
||||
bool status;
|
||||
|
||||
do {
|
||||
telegram.transfer(datagram);
|
||||
} while(datagram.get_wkc() < 0x0001);
|
||||
status = telegram.transfer(datagram);
|
||||
|
||||
if(status != true) {
|
||||
return status;
|
||||
}
|
||||
|
||||
increment_counter();
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
template<typename TypeT, typename... DataTypes>
|
||||
void receive_data(telegram::EcatTelegram& telegram, DataTypes&... data) {
|
||||
bool receive_data(telegram::EcatTelegram& telegram, const datagram::TEcatWkc expected_wkc, DataTypes&... data) {
|
||||
using TCommand = command::EcatCommand<TypeT, command::RD>;
|
||||
using TDatagram = datagram::EcatDatagram<TCommand, MailboxHeader, DataTypes... , Padding>;
|
||||
|
||||
@ -231,11 +240,9 @@ public:
|
||||
std::array<address::Offset, 4>& buffer_regs = buffer_slave_.get_buffer_regs();
|
||||
MailboxHeader header;
|
||||
Padding padding{buffer_slave_.get_buffer_properties_read().length - sizeof(MailboxHeader) - custom_tuple<DataTypes...>::size};
|
||||
TDatagram datagram{ {{slave_address, buffer_regs[MailboxesRegs::READ]}}, header, data... , padding};
|
||||
TDatagram datagram{ {{slave_address, buffer_regs[MailboxesRegs::READ]}}, expected_wkc, header, data... , padding};
|
||||
|
||||
do {
|
||||
telegram.transfer(datagram);
|
||||
} while(datagram.get_wkc() < 0x0001);
|
||||
return telegram.transfer(datagram);
|
||||
|
||||
//DebugP_log("header.length = %d\r\n", header.length);
|
||||
//DebugP_log("header.address = %d\r\n", header.address);
|
||||
@ -246,21 +253,21 @@ public:
|
||||
}
|
||||
|
||||
template<typename TypeT, typename... DataTypes>
|
||||
void receive(telegram::EcatTelegram& telegram, DataTypes&... data) {
|
||||
wait_available<TypeT>(telegram);
|
||||
receive_data<TypeT, DataTypes...>(telegram, data...);
|
||||
bool receive(telegram::EcatTelegram& telegram, const datagram::TEcatWkc expected_wkc, DataTypes&... data) {
|
||||
if(wait_available<TypeT>(telegram, expected_wkc) != true) return false;
|
||||
if(receive_data<TypeT, DataTypes...>(telegram, expected_wkc, data...) != true) return false;
|
||||
}
|
||||
|
||||
template<typename TypeT, typename... DataTypes>
|
||||
void send(telegram::EcatTelegram& telegram, uint16_t channel, uint16_t priority, ProtocolType type, DataTypes&... data) {
|
||||
empty<TypeT>(telegram);
|
||||
wait_empty<TypeT>(telegram);
|
||||
send_data<TypeT, DataTypes...>(telegram, channel, priority, type, data...);
|
||||
wait_empty<TypeT>(telegram);
|
||||
bool send(telegram::EcatTelegram& telegram, const datagram::TEcatWkc expected_wkc, uint16_t channel, uint16_t priority, ProtocolType type, DataTypes&... data) {
|
||||
if(empty<TypeT>(telegram, expected_wkc) != true) return false;
|
||||
if(wait_empty<TypeT>(telegram, expected_wkc) != true) return false;
|
||||
if(send_data<TypeT, DataTypes...>(telegram, expected_wkc, channel, priority, type, data...) != true) return false;
|
||||
if(wait_empty<TypeT>(telegram, expected_wkc) != true) return false;
|
||||
}
|
||||
|
||||
template<typename TypeT, typename... DataTypes>
|
||||
CompleteSize sdo_write(telegram::EcatTelegram& telegram, uint16_t index, uint8_t subindex, DataTypes&... data) {
|
||||
bool sdo_write(telegram::EcatTelegram& telegram, const datagram::TEcatWkc expected_wkc, uint16_t index, uint8_t subindex, DataTypes&... data) {
|
||||
CoEElements elements{
|
||||
.coe_header = {
|
||||
.number = 0x00,
|
||||
@ -274,12 +281,23 @@ public:
|
||||
.index = index,
|
||||
.subindex = subindex };
|
||||
CompleteSize complete_size{custom_tuple<DataTypes...>::size};
|
||||
bool status;
|
||||
|
||||
send<TypeT, CoEElements, CompleteSize>(telegram, 0, 0, ProtocolType::CoE, elements, complete_size, data...);
|
||||
receive<TypeT, CoEElements, CompleteSize, DataTypes...>(telegram, elements, complete_size);
|
||||
status = send<TypeT, CoEElements, CompleteSize>(telegram, expected_wkc, 0, 0, ProtocolType::CoE, elements, complete_size, data...);
|
||||
|
||||
if(status != true) {
|
||||
return status;
|
||||
}
|
||||
|
||||
status = receive<TypeT, CoEElements, CompleteSize, DataTypes...>(telegram, expected_wkc, elements, complete_size);
|
||||
|
||||
if(status != true) {
|
||||
return status;
|
||||
}
|
||||
|
||||
if( (elements.coe_header.service != static_cast<uint8_t>(Service::SDO_RESPONSE)) ||
|
||||
(elements.command_specifier.command_spec != static_cast<uint8_t>(SDOReqCommandSpecifier::UPLOAD)) ) {
|
||||
|
||||
DebugP_log((char*)"CoE error: = 0x%04x\r\n", complete_size.value); // 0x601004 - The object cannot be accessed via complete access
|
||||
}
|
||||
|
||||
@ -294,11 +312,11 @@ public:
|
||||
//DebugP_log("elements.subindex = %d\r\n", elements.subindex);
|
||||
//DebugP_log("complete_size = %d\r\n", complete_size);
|
||||
|
||||
return complete_size;
|
||||
return status;
|
||||
}
|
||||
|
||||
template<typename TypeT, typename... DataTypes>
|
||||
CompleteSize sdo_read(telegram::EcatTelegram& telegram, uint16_t index, uint8_t subindex, DataTypes&... data) {
|
||||
bool sdo_read(telegram::EcatTelegram& telegram, const datagram::TEcatWkc expected_wkc, uint16_t index, uint8_t subindex, DataTypes&... data) {
|
||||
CoEElements elements{
|
||||
.coe_header = {
|
||||
.number = 0x00,
|
||||
@ -312,12 +330,23 @@ public:
|
||||
.index = index,
|
||||
.subindex = subindex };
|
||||
CompleteSize complete_size{0x00000000};
|
||||
bool status;
|
||||
|
||||
send<TypeT, CoEElements, CompleteSize>(telegram, 0, 0, ProtocolType::CoE, elements, complete_size);
|
||||
receive<TypeT, CoEElements, CompleteSize, DataTypes...>(telegram, elements, complete_size, data...);
|
||||
status = send<TypeT, CoEElements, CompleteSize>(telegram, expected_wkc, 0, 0, ProtocolType::CoE, elements, complete_size);
|
||||
|
||||
if(status != true) {
|
||||
return status;
|
||||
}
|
||||
|
||||
status = receive<TypeT, CoEElements, CompleteSize, DataTypes...>(telegram, expected_wkc, elements, complete_size, data...);
|
||||
|
||||
if(status != true) {
|
||||
return status;
|
||||
}
|
||||
|
||||
if( (elements.coe_header.service != static_cast<uint8_t>(Service::SDO_RESPONSE)) ||
|
||||
(elements.command_specifier.command_spec != static_cast<uint8_t>(SDOReqCommandSpecifier::UPLOAD)) ) {
|
||||
|
||||
DebugP_log((char*)"CoE error: = 0x%04x\r\n", complete_size.value); // 0x601004 - The object cannot be accessed via complete access
|
||||
}
|
||||
|
||||
@ -332,17 +361,23 @@ public:
|
||||
//DebugP_log("elements.subindex = %d\r\n", elements.subindex);
|
||||
//DebugP_log("complete_size = %d\r\n", complete_size);
|
||||
|
||||
return complete_size;
|
||||
return status;
|
||||
}
|
||||
|
||||
template<typename TypeT>
|
||||
uint16_t pdo_map_read(telegram::EcatTelegram& telegram, ecat_buffer::PDOMap& pdo_map, uint16_t pdo_map_index) {
|
||||
bool pdo_map_read(telegram::EcatTelegram& telegram, const datagram::TEcatWkc expected_wkc, ecat_buffer::PDOMap& pdo_map, uint16_t pdo_map_index) {
|
||||
uint16_t pdo_data_size{0x0000}; // Размер данных в битах !
|
||||
uint8_t pdo_block_count{0x00};
|
||||
bool status;
|
||||
|
||||
pdo_map.block_index_map.emplace(pdo_map_index, std::map<uint8_t, uint16_t>{});
|
||||
|
||||
sdo_read<TypeT, uint8_t>(telegram, pdo_map_index, 0x00, pdo_block_count);
|
||||
status = sdo_read<TypeT, uint8_t>(telegram, expected_wkc, pdo_map_index, 0x00, pdo_block_count);
|
||||
|
||||
if(status != true) {
|
||||
return status;
|
||||
}
|
||||
|
||||
pdo_map.block_index_map[pdo_map_index].emplace(0, pdo_block_count);
|
||||
|
||||
DebugP_log((char*)"pdo_block_count = 0x%01x\r\n", pdo_block_count);
|
||||
@ -350,7 +385,12 @@ public:
|
||||
for(uint8_t pdo_map_subindex = 1; pdo_map_subindex < (pdo_block_count + 1); pdo_map_subindex++) {
|
||||
uint16_t pdo_block_index{0x0000};
|
||||
|
||||
sdo_read<TypeT, uint16_t>(telegram, pdo_map_index, pdo_map_subindex, pdo_block_index);
|
||||
status = sdo_read<TypeT, uint16_t>(telegram, expected_wkc, pdo_map_index, pdo_map_subindex, pdo_block_index);
|
||||
|
||||
if(status != true) {
|
||||
return status;
|
||||
}
|
||||
|
||||
pdo_map.block_index_map[pdo_map_index].emplace(pdo_map_subindex, pdo_block_index);
|
||||
|
||||
DebugP_log("pdo_block_index = 0x02%x\r\n", pdo_block_index);
|
||||
@ -360,7 +400,12 @@ public:
|
||||
|
||||
pdo_map.object_descriptor_map.emplace(pdo_block_index, std::map<uint8_t, ecat_buffer::PDODescriptor>{});
|
||||
|
||||
sdo_read<TypeT, uint8_t>(telegram, pdo_block_index, 0, pdo_block_object_count);
|
||||
status = sdo_read<TypeT, uint8_t>(telegram, expected_wkc, pdo_block_index, 0, pdo_block_object_count);
|
||||
|
||||
if(status != true) {
|
||||
return status;
|
||||
}
|
||||
|
||||
pdo_map.object_descriptor_map[pdo_block_index].emplace(0, tmp);
|
||||
|
||||
DebugP_log((char*)"pdo_block_object_count = 0x%01x\r\n", pdo_block_object_count);
|
||||
@ -368,7 +413,12 @@ public:
|
||||
for(uint8_t pdo_block_subindex = 1; pdo_block_subindex < (pdo_block_object_count + 1); pdo_block_subindex++) {
|
||||
ecat_buffer::PDODescriptor descriptor;
|
||||
|
||||
sdo_read<TypeT, ecat_buffer::PDODescriptor>(telegram, pdo_block_index, pdo_block_subindex, descriptor);
|
||||
status = sdo_read<TypeT, ecat_buffer::PDODescriptor>(telegram, expected_wkc, pdo_block_index, pdo_block_subindex, descriptor);
|
||||
|
||||
if(status != true) {
|
||||
return status;
|
||||
}
|
||||
|
||||
pdo_data_size += descriptor.size;
|
||||
pdo_map.object_descriptor_map[pdo_block_index].emplace(pdo_block_subindex, descriptor);
|
||||
|
||||
@ -380,9 +430,9 @@ public:
|
||||
|
||||
pdo_map.data_size_map.emplace(pdo_map_index, pdo_data_size/8);
|
||||
|
||||
//DebugP_log("pdo_data_size = %d\r\n", pdo_data_size);
|
||||
DebugP_log("pdo_data_size = %d\r\n", pdo_data_size/8);
|
||||
|
||||
return pdo_data_size/8;
|
||||
return status;
|
||||
}
|
||||
|
||||
private:
|
||||
@ -396,7 +446,7 @@ public:
|
||||
EthEcatSdoMailbox(ecat_buffer::EthEcatBuffer& ecat_buffer): ecat_buffer_{ecat_buffer} { }
|
||||
|
||||
void init();
|
||||
void read_pdo_map(uint16_t pdo_map_rx_index, uint16_t pdo_map_tx_index);
|
||||
bool read_pdo_map(uint16_t pdo_map_rx_index, uint16_t pdo_map_tx_index);
|
||||
|
||||
std::vector<ecat_buffer::PDOMap>& get_pdo_map() {
|
||||
return pdo_map_;
|
||||
|
||||
@ -35,70 +35,70 @@ void EthEcat::Init(TEthMacPorts port_id, uint32_t period_microsec = 250) {
|
||||
ecat_timer_.Stop();
|
||||
}
|
||||
|
||||
void EthEcat::set_slaves_to_default() {
|
||||
bool EthEcat::set_slaves_to_default() {
|
||||
const datagram::TEcatWkc expected_wkc = 0;
|
||||
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}}, a_data_out };
|
||||
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}}, b_data_out };
|
||||
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;
|
||||
c_data_out.fill(0x00);
|
||||
datagram::EcatDatagram<command::BWR, std::array<uint8_t, 8>> c{ {{broadcast, ECT_REG_RXERR}}, c_data_out };
|
||||
datagram::EcatDatagram<command::BWR, std::array<uint8_t, 8>> c{ {{broadcast, ECT_REG_RXERR}}, expected_wkc, c_data_out };
|
||||
|
||||
/* reset FMMU's */
|
||||
std::array<uint8_t, 16*3> d_data_out;
|
||||
d_data_out.fill(0x00);
|
||||
datagram::EcatDatagram<command::BWR, std::array<uint8_t, 16*3>> d{ {{broadcast, ECT_REG_FMMU0}}, d_data_out };
|
||||
datagram::EcatDatagram<command::BWR, std::array<uint8_t, 16*3>> d{ {{broadcast, ECT_REG_FMMU0}}, expected_wkc, 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 };
|
||||
datagram::EcatDatagram<command::BWR, std::array<uint8_t, 8*4>> e{ {{broadcast, ECT_REG_SM0}}, expected_wkc, 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 };
|
||||
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;
|
||||
g_data_out.fill(0x00);
|
||||
datagram::EcatDatagram<command::BWR, std::array<uint8_t, 4>> g{ {{broadcast, ECT_REG_DCSYSTIME}}, g_data_out };
|
||||
datagram::EcatDatagram<command::BWR, std::array<uint8_t, 4>> g{ {{broadcast, ECT_REG_DCSYSTIME}}, expected_wkc, 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 };
|
||||
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}}, i_data_out };
|
||||
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}}, j_data_out };
|
||||
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}}, k_data_out };
|
||||
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}}, l_data_out };
|
||||
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}}, m_data_out };
|
||||
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;
|
||||
|
||||
do {
|
||||
telegram_.transfer(queue);
|
||||
} while(a.get_all_wkc() < 0x0001);
|
||||
status = telegram_.transfer(queue);
|
||||
|
||||
//telegram_.transfer(b);
|
||||
//telegram_.transfer(c);
|
||||
@ -113,25 +113,27 @@ void EthEcat::set_slaves_to_default() {
|
||||
//telegram_.transfer(l);
|
||||
//telegram_.transfer(m);
|
||||
|
||||
DebugP_log((char*)"a.get_wkc() = %d\r\n", a.get_wkc());
|
||||
DebugP_log((char*)"b.get_wkc() = %d\r\n", b.get_wkc());
|
||||
DebugP_log((char*)"c.get_wkc() = %d\r\n", c.get_wkc());
|
||||
DebugP_log((char*)"d.get_wkc() = %d\r\n", d.get_wkc());
|
||||
DebugP_log((char*)"e.get_wkc() = %d\r\n", e.get_wkc());
|
||||
DebugP_log((char*)"f.get_wkc() = %d\r\n", f.get_wkc());
|
||||
DebugP_log((char*)"g.get_wkc() = %d\r\n", g.get_wkc());
|
||||
DebugP_log((char*)"h.get_wkc() = %d\r\n", h.get_wkc());
|
||||
DebugP_log((char*)"i.get_wkc() = %d\r\n", i.get_wkc());
|
||||
DebugP_log((char*)"j.get_wkc() = %d\r\n", j.get_wkc());
|
||||
DebugP_log((char*)"k.get_wkc() = %d\r\n", k.get_wkc());
|
||||
DebugP_log((char*)"l.get_wkc() = %d\r\n", l.get_wkc());
|
||||
DebugP_log((char*)"m.get_wkc() = %d\r\n", m.get_wkc());
|
||||
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}}, data };
|
||||
datagram::EcatDatagram<command::BRD, uint16_t> datagram{ {{broadcast, ECT_REG_TYPE}}, 1, data };
|
||||
|
||||
telegram_.transfer(datagram);
|
||||
|
||||
@ -140,10 +142,12 @@ uint16_t EthEcat::slaves_detecting() {
|
||||
|
||||
// Setting Station address (FP) of slave via Position addressing (AP)
|
||||
// Station address is datagram data
|
||||
void EthEcat::set_addresses_of_slaves(uint16_t number_of_slaves, uint16_t address_base) {
|
||||
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);
|
||||
@ -154,7 +158,7 @@ void EthEcat::set_addresses_of_slaves(uint16_t number_of_slaves, uint16_t addres
|
||||
address::SlaveAddresses slave_addresses{position, 0x0000, station, 0x00000000};
|
||||
|
||||
slaves_.emplace_back(EcatSlave{std::move(slave_addresses)});
|
||||
datagrams.emplace_back(TDatagram{ {{position, ECT_REG_STADR}}, slaves_.back().get_slave_address<command::FP>() });
|
||||
datagrams.emplace_back(TDatagram{ {{position, ECT_REG_STADR}}, expected_wkc, slaves_.back().get_slave_address<command::FP>() });
|
||||
}
|
||||
|
||||
queue::Queue<datagram::IEcatDatagram> queue;
|
||||
@ -163,64 +167,107 @@ void EthEcat::set_addresses_of_slaves(uint16_t number_of_slaves, uint16_t addres
|
||||
queue + datagrams[i];
|
||||
}
|
||||
|
||||
do {
|
||||
telegram_.transfer(queue);
|
||||
} while(datagrams[0].get_all_wkc() < number_of_slaves);
|
||||
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;
|
||||
}
|
||||
|
||||
void EthEcat::get_addresses_of_slaves() {
|
||||
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}}, slave.get_slave_address<command::FP>() });
|
||||
datagrams.emplace_back(TDatagram{ {{slave.get_slave_address<command::AP>(), ECT_REG_STADR}}, expected_wkc, slave.get_slave_address<command::FP>() });
|
||||
}
|
||||
|
||||
queue::Queue<datagram::IEcatDatagram> queue;
|
||||
|
||||
for(uint16_t i = 0; i < number_of_slaves; i++) {
|
||||
queue + datagrams[i];
|
||||
}
|
||||
|
||||
do {
|
||||
telegram_.transfer(queue);
|
||||
} while(datagrams[0].get_all_wkc() < number_of_slaves);
|
||||
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;
|
||||
}
|
||||
|
||||
uint16_t EthEcat::config_init(uint16_t address_base) {
|
||||
bool EthEcat::config_init(uint16_t address_base) {
|
||||
uint16_t number_of_slaves;
|
||||
bool status;
|
||||
|
||||
DebugP_log((char*)"Initializing slaves...\r\n");
|
||||
|
||||
set_slaves_to_default();
|
||||
uint16_t number_of_slaves = slaves_detecting();
|
||||
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);
|
||||
|
||||
set_addresses_of_slaves(number_of_slaves, address_base);
|
||||
get_addresses_of_slaves();
|
||||
status = (number_of_slaves > 0);
|
||||
|
||||
return number_of_slaves;
|
||||
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;
|
||||
}
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
void EthEcat::enable_PDI() {
|
||||
bool EthEcat::enable_PDI() {
|
||||
const datagram::TEcatWkc expected_wkc = 1;
|
||||
bool status = true;
|
||||
|
||||
for(EcatSlave& slave : slaves_) {
|
||||
slave.enable_PDI<command::FP>(telegram_);
|
||||
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 &= slave.init_to_preop<command::FP>(telegram_);
|
||||
success = (success && slave.init_to_preop<command::FP>(telegram_, expected_wkc));
|
||||
}
|
||||
|
||||
DebugP_log((char*)"success = %d\r\n", success);
|
||||
@ -229,10 +276,11 @@ bool EthEcat::init_to_preop() {
|
||||
}
|
||||
|
||||
bool EthEcat::preop_to_safeop() {
|
||||
const datagram::TEcatWkc expected_wkc = 1;
|
||||
bool success = true;
|
||||
|
||||
for(EcatSlave& slave : slaves_) {
|
||||
success &= slave.preop_to_safeop<command::FP>(telegram_);
|
||||
success = (success && slave.preop_to_safeop<command::FP>(telegram_, expected_wkc));
|
||||
}
|
||||
|
||||
DebugP_log((char*)"success = %d\r\n", success);
|
||||
@ -241,8 +289,8 @@ bool EthEcat::preop_to_safeop() {
|
||||
}
|
||||
|
||||
bool EthEcat::safeop_to_op() {
|
||||
uint16_t number_of_slaves = slaves_.size();
|
||||
bool success;
|
||||
const uint16_t number_of_slaves = slaves_.size();
|
||||
bool status;
|
||||
|
||||
address::Broadcast broadcast{0x0000};
|
||||
ALSTAT stat{0x0000, 0x0000};
|
||||
@ -253,34 +301,39 @@ bool EthEcat::safeop_to_op() {
|
||||
|
||||
{
|
||||
uint16_t data{EC_STATE_OPERATIONAL};
|
||||
datagram::EcatDatagram<command::BWR, uint16_t> datagram{ {{broadcast, ECT_REG_ALCTL}}, data };
|
||||
datagram::EcatDatagram<command::BWR, uint16_t> datagram{ {{broadcast, ECT_REG_ALCTL}}, number_of_slaves, data };
|
||||
|
||||
do {
|
||||
telegram_.transfer(datagram);
|
||||
} while(datagram.get_wkc() < number_of_slaves);
|
||||
status = telegram_.transfer(datagram);
|
||||
|
||||
if(status != true) {
|
||||
return status;
|
||||
}
|
||||
}
|
||||
|
||||
process_sem_.post();
|
||||
init_sem_.pend();
|
||||
|
||||
{
|
||||
datagram::EcatDatagram<command::BRD, ALSTAT, uint16_t> datagram{ {{broadcast, ECT_REG_ALSTAT}}, stat, zero };
|
||||
datagram::EcatDatagram<command::BRD, ALSTAT, uint16_t> datagram{ {{broadcast, ECT_REG_ALSTAT}}, number_of_slaves, stat, zero };
|
||||
|
||||
do {
|
||||
telegram_.transfer(datagram);
|
||||
} while(datagram.get_wkc() < number_of_slaves);
|
||||
}
|
||||
status = telegram_.transfer(datagram);
|
||||
|
||||
success = (stat.state == EC_STATE_OPERATIONAL) && (stat.fault == 0);
|
||||
|
||||
if(success == true) {
|
||||
ecat_timer_.Start();
|
||||
if(status != true) {
|
||||
return status;
|
||||
}
|
||||
}
|
||||
|
||||
//DebugP_log((char*)"stat.state = %d, stat.fault = %d\r\n", stat.state, stat.fault);
|
||||
|
||||
status = (stat.state == EC_STATE_OPERATIONAL) && (stat.fault == 0);
|
||||
|
||||
if(status == true) {
|
||||
ecat_timer_.Start();
|
||||
}
|
||||
|
||||
//DebugP_log((char*)"success = %d\r\n", success);
|
||||
|
||||
return success;
|
||||
return status;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@ -53,41 +53,44 @@ public:
|
||||
}
|
||||
|
||||
template<typename TypeT>
|
||||
void enable_PDI(telegram::EcatTelegram& telegram) {
|
||||
bool enable_PDI(telegram::EcatTelegram& telegram, const datagram::TEcatWkc expected_wkc) {
|
||||
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 };
|
||||
datagram::EcatDatagram<TCommand, uint8_t> datagram{ {{slave_address, ECT_REG_EEPCFG}}, expected_wkc, data };
|
||||
|
||||
do {
|
||||
telegram.transfer(datagram);
|
||||
} while(datagram.get_wkc() < 0x0001);
|
||||
return telegram.transfer(datagram);
|
||||
}
|
||||
|
||||
template<typename TypeT>
|
||||
bool init_to_preop(telegram::EcatTelegram& telegram) {
|
||||
bool init_to_preop(telegram::EcatTelegram& telegram, const datagram::TEcatWkc expected_wkc) {
|
||||
auto slave_address = get_slave_address<TypeT>();
|
||||
ALSTAT stat{0x0000, 0x0000};
|
||||
bool status;
|
||||
|
||||
{
|
||||
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 };
|
||||
datagram::EcatDatagram<TCommand, uint16_t> datagram{ {{slave_address, ECT_REG_ALCTL}}, expected_wkc, data };
|
||||
|
||||
do {
|
||||
telegram.transfer(datagram);
|
||||
} while(datagram.get_wkc() < 0x0001);
|
||||
status = telegram.transfer(datagram);
|
||||
|
||||
if(status != true) {
|
||||
return status;
|
||||
}
|
||||
}
|
||||
|
||||
ClockP_usleep(5000000ul);
|
||||
|
||||
{
|
||||
using TCommand = command::EcatCommand<TypeT, command::RD>;
|
||||
datagram::EcatDatagram<TCommand, ALSTAT> datagram{ {{slave_address, ECT_REG_ALSTAT}}, stat };
|
||||
datagram::EcatDatagram<TCommand, ALSTAT> datagram{ {{slave_address, ECT_REG_ALSTAT}}, expected_wkc, stat };
|
||||
|
||||
do {
|
||||
telegram.transfer(datagram);
|
||||
} while(datagram.get_wkc() < 0x0001);
|
||||
status = telegram.transfer(datagram);
|
||||
|
||||
if(status != true) {
|
||||
return status;
|
||||
}
|
||||
}
|
||||
|
||||
DebugP_log((char*)"stat.state = %d, stat.fault = %d\r\n", stat.state, stat.fault);
|
||||
@ -96,41 +99,52 @@ public:
|
||||
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};
|
||||
datagram::EcatDatagram<TCommand, uint16_t> datagram{ {{slave_address, ECT_REG_ALSTATCODE}}, expected_wkc, stat_code};
|
||||
|
||||
do {
|
||||
telegram.transfer(datagram);
|
||||
} while(datagram.get_wkc() < 0x0001);
|
||||
status = telegram.transfer(datagram);
|
||||
|
||||
if(status != true) {
|
||||
return status;
|
||||
}
|
||||
|
||||
DebugP_log((char*)"stat_code = 0x%02x\r\n", stat_code);
|
||||
}
|
||||
|
||||
return (stat.state == EC_STATE_PRE_OP) && (stat.fault == 0);
|
||||
status = ((stat.state == EC_STATE_PRE_OP) && (stat.fault == 0));
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
template<typename TypeT>
|
||||
bool preop_to_safeop(telegram::EcatTelegram& telegram) {
|
||||
bool preop_to_safeop(telegram::EcatTelegram& telegram, const datagram::TEcatWkc expected_wkc) {
|
||||
auto slave_address = get_slave_address<TypeT>();
|
||||
ALSTAT stat{0x0000, 0x0000};
|
||||
uint32_t zero{0x00000000};
|
||||
bool status;
|
||||
|
||||
{
|
||||
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 };
|
||||
datagram::EcatDatagram<TCommand, uint16_t> datagram{ {{slave_address, ECT_REG_ALCTL}}, expected_wkc, data };
|
||||
|
||||
do {
|
||||
telegram.transfer(datagram);
|
||||
} while(datagram.get_wkc() < 0x0001);
|
||||
status = telegram.transfer(datagram);
|
||||
|
||||
if(status != true) {
|
||||
return status;
|
||||
}
|
||||
}
|
||||
|
||||
ClockP_usleep(1000000ul);
|
||||
|
||||
{
|
||||
using TCommand = command::EcatCommand<TypeT, command::RD>;
|
||||
datagram::EcatDatagram<TCommand, ALSTAT, uint32_t> datagram{ {{slave_address, ECT_REG_ALSTAT}}, stat, zero };
|
||||
datagram::EcatDatagram<TCommand, ALSTAT, uint32_t> datagram{ {{slave_address, ECT_REG_ALSTAT}}, expected_wkc, stat, zero };
|
||||
|
||||
do {
|
||||
telegram.transfer(datagram);
|
||||
} while(datagram.get_wkc() < 0x0001);
|
||||
status = telegram.transfer(datagram);
|
||||
|
||||
if(status != true) {
|
||||
return status;
|
||||
}
|
||||
}
|
||||
|
||||
DebugP_log((char*)"stat.state = %d, stat.fault = %d\r\n", stat.state, stat.fault);
|
||||
@ -139,16 +153,20 @@ public:
|
||||
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};
|
||||
datagram::EcatDatagram<TCommand, uint16_t> datagram{ {{slave_address, ECT_REG_ALSTATCODE}}, expected_wkc, stat_code};
|
||||
|
||||
do {
|
||||
telegram.transfer(datagram);
|
||||
} while(datagram.get_wkc() < 0x0001);
|
||||
status = telegram.transfer(datagram);
|
||||
|
||||
if(status != true) {
|
||||
return status;
|
||||
}
|
||||
|
||||
DebugP_log((char*)"stat_code = 0x%02x\r\n", stat_code);
|
||||
}
|
||||
|
||||
return (stat.state == EC_STATE_SAFE_OP) && (stat.fault == 0);
|
||||
status = ((stat.state == EC_STATE_SAFE_OP) && (stat.fault == 0));
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
private:
|
||||
@ -166,14 +184,14 @@ public:
|
||||
|
||||
void Init(TEthMacPorts port_id, uint32_t period_microsec);
|
||||
|
||||
void set_slaves_to_default();
|
||||
bool 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();
|
||||
bool set_addresses_of_slaves(uint16_t number_of_slaves, uint16_t address_base);
|
||||
bool get_addresses_of_slaves();
|
||||
|
||||
uint16_t config_init(uint16_t address_base);
|
||||
bool config_init(uint16_t address_base);
|
||||
|
||||
void enable_PDI();
|
||||
bool enable_PDI();
|
||||
|
||||
bool init_to_preop();
|
||||
bool preop_to_safeop();
|
||||
@ -195,6 +213,10 @@ public:
|
||||
return telegram_;
|
||||
}
|
||||
|
||||
const telegram::EcatTelegramStatus& get_telegram_status() {
|
||||
return telegram_.get_status();
|
||||
}
|
||||
|
||||
eeprom::EEPROM& get_eeprom() {
|
||||
return eeprom_;
|
||||
}
|
||||
|
||||
@ -10,18 +10,40 @@
|
||||
namespace free_rtos {
|
||||
|
||||
bool EthEcatApi::config_init(TEthMacPorts port_id, uint16_t address_base, uint32_t period_microsec, ecat_pdo_fmmu::ProcessCallback callback) {
|
||||
uint16_t number_of_slaves;
|
||||
bool status = false;
|
||||
|
||||
ecat_.Init(port_id, period_microsec);
|
||||
|
||||
ecat_.config_init(address_base);
|
||||
status = ecat_.config_init(address_base);
|
||||
|
||||
ecat_buffer_sdo_.init(ECT_SII_RXMBXADR, ECT_SII_TXMBXADR);
|
||||
ecat_buffer_sdo_.init_sync_manager(sync_manager::SYNC_M0, sync_manager::SYNC_M1);
|
||||
if(status != true) {
|
||||
return status;
|
||||
}
|
||||
|
||||
ecat_buffer_pdo_.init(ECT_PDOOUTPUTADR, ECT_PDOINPUTADR);
|
||||
status = ecat_buffer_sdo_.init(ECT_SII_RXMBXADR, ECT_SII_TXMBXADR);
|
||||
|
||||
ecat_.enable_PDI();
|
||||
if(status != true) {
|
||||
return status;
|
||||
}
|
||||
|
||||
status = ecat_buffer_sdo_.init_sync_manager(sync_manager::SYNC_M0, sync_manager::SYNC_M1);
|
||||
|
||||
if(status != true) {
|
||||
return status;
|
||||
}
|
||||
|
||||
status = ecat_buffer_pdo_.init(ECT_PDOOUTPUTADR, ECT_PDOINPUTADR);
|
||||
|
||||
if(status != true) {
|
||||
return status;
|
||||
}
|
||||
|
||||
status = ecat_.enable_PDI();
|
||||
|
||||
if(status != true) {
|
||||
return status;
|
||||
}
|
||||
|
||||
status = ecat_.init_to_preop();
|
||||
|
||||
@ -30,7 +52,11 @@ bool EthEcatApi::config_init(TEthMacPorts port_id, uint16_t address_base, uint32
|
||||
}
|
||||
|
||||
ecat_sdo_mailbox_.init();
|
||||
ecat_sdo_mailbox_.read_pdo_map(ECT_RXPDOMAPINDEX, ECT_TXPDOMAPINDEX);
|
||||
status = ecat_sdo_mailbox_.read_pdo_map(ECT_RXPDOMAPINDEX, ECT_TXPDOMAPINDEX);
|
||||
|
||||
if(status != true) {
|
||||
return status;
|
||||
}
|
||||
|
||||
// Override buffer properties from eeprom for PDO
|
||||
#ifdef COMX
|
||||
@ -38,8 +64,17 @@ bool EthEcatApi::config_init(TEthMacPorts port_id, uint16_t address_base, uint32
|
||||
#endif
|
||||
ecat_buffer_pdo_.set_buffer_length(ecat_sdo_mailbox_.get_pdo_map());
|
||||
|
||||
ecat_buffer_pdo_.init_sync_manager(sync_manager::SYNC_M2, sync_manager::SYNC_M3);
|
||||
ecat_buffer_pdo_.init_fmmu(fmmu::FMMU0, fmmu::FMMU1);
|
||||
status = ecat_buffer_pdo_.init_sync_manager(sync_manager::SYNC_M2, sync_manager::SYNC_M3);
|
||||
|
||||
if(status != true) {
|
||||
return status;
|
||||
}
|
||||
|
||||
status = ecat_buffer_pdo_.init_fmmu(fmmu::FMMU0, fmmu::FMMU1);
|
||||
|
||||
if(status != true) {
|
||||
return status;
|
||||
}
|
||||
|
||||
ecat_pdo_fmmu_.init(callback);
|
||||
|
||||
|
||||
@ -37,6 +37,10 @@ public:
|
||||
return ecat_pdo_fmmu_.get_pdo_counter();
|
||||
}
|
||||
|
||||
const telegram::EcatTelegramStatus& get_telegram_status() {
|
||||
return ecat_.get_telegram_status();
|
||||
}
|
||||
|
||||
template<typename... DataTypes>
|
||||
ecat_sdo_mailbox::CompleteSize sdo_write(size_t slave_index, uint16_t index, uint8_t subindex, DataTypes&... data) {
|
||||
return ecat_sdo_mailbox_.sdo_write<command::FP, DataTypes...>(slave_index, index, subindex, data...);
|
||||
|
||||
@ -14,9 +14,11 @@ namespace ecat_buffer {
|
||||
constexpr std::array<SyncManager, 4> EcatBufferSlave::sync_managers_;
|
||||
constexpr std::array<address::Offset, 4> EcatBufferSlave::fmmu_regs_;
|
||||
|
||||
void EthEcatBuffer::init(uint16_t rx_eeprom_addr, uint16_t tx_eeprom_addr) {
|
||||
bool EthEcatBuffer::init(uint16_t rx_eeprom_addr, uint16_t tx_eeprom_addr) {
|
||||
const datagram::TEcatWkc expected_wkc = 1;
|
||||
std::vector<EcatSlave>& slaves = ecat_.get_slaves();
|
||||
eeprom::EEPROM& eeprom = ecat_.get_eeprom();
|
||||
bool status = true;
|
||||
|
||||
buffer_slaves_.reserve(slaves.size());
|
||||
|
||||
@ -29,28 +31,42 @@ void EthEcatBuffer::init(uint16_t rx_eeprom_addr, uint16_t tx_eeprom_addr) {
|
||||
}
|
||||
*/
|
||||
for(EcatBufferSlave& buffer_slave : buffer_slaves_) {
|
||||
buffer_slave.read_buffer_info_from_eeprom<command::FP>(eeprom, rx_eeprom_addr, tx_eeprom_addr);
|
||||
status = (status && buffer_slave.read_buffer_info_from_eeprom<command::FP>(expected_wkc, eeprom, rx_eeprom_addr, tx_eeprom_addr));
|
||||
}
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
void EthEcatBuffer::init_sync_manager(sync_manager sm_write, sync_manager sm_read) {
|
||||
bool EthEcatBuffer::init_sync_manager(sync_manager sm_write, sync_manager sm_read) {
|
||||
const datagram::TEcatWkc expected_wkc = 1;
|
||||
telegram::EcatTelegram& telegram = ecat_.get_telegram();
|
||||
bool status = true;
|
||||
|
||||
for(EcatBufferSlave& buffer_slave : buffer_slaves_) {
|
||||
buffer_slave.init_sync_manager<command::FP>(telegram, sm_write, sm_read);
|
||||
status = (status && buffer_slave.init_sync_manager<command::FP>(telegram, expected_wkc, sm_write, sm_read));
|
||||
}
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
void EthEcatBuffer::init_fmmu(fmmu fmmu_write, fmmu fmmu_read) {
|
||||
bool EthEcatBuffer::init_fmmu(fmmu fmmu_write, fmmu fmmu_read) {
|
||||
const datagram::TEcatWkc expected_wkc = 1;
|
||||
telegram::EcatTelegram& telegram = ecat_.get_telegram();
|
||||
bool status = true;
|
||||
|
||||
for(EcatBufferSlave& buffer_slave : buffer_slaves_) {
|
||||
buffer_slave.init_fmmu_write<command::FP>(telegram, fmmu_write, fmmu_global_properties_);
|
||||
status = (status && buffer_slave.init_fmmu_write<command::FP>(telegram, expected_wkc, fmmu_write, fmmu_global_properties_));
|
||||
}
|
||||
|
||||
if(status != true) {
|
||||
return status;
|
||||
}
|
||||
|
||||
for(EcatBufferSlave& buffer_slave : buffer_slaves_) {
|
||||
buffer_slave.init_fmmu_read<command::FP>(telegram, fmmu_read, fmmu_global_properties_);
|
||||
status = (status && buffer_slave.init_fmmu_read<command::FP>(telegram, expected_wkc, fmmu_read, fmmu_global_properties_));
|
||||
}
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@ -168,71 +168,74 @@ public:
|
||||
}
|
||||
|
||||
template<typename TypeT>
|
||||
void read_buffer_info_from_eeprom(eeprom::EEPROM& eeprom, uint16_t rx_eeprom_addr, uint16_t tx_eeprom_addr) {
|
||||
bool read_buffer_info_from_eeprom(const datagram::TEcatWkc expected_wkc, eeprom::EEPROM& eeprom, uint16_t rx_eeprom_addr, uint16_t tx_eeprom_addr) {
|
||||
auto slave_address = slave_.get_slave_address<TypeT>();
|
||||
bool status;
|
||||
|
||||
eeprom.read<TypeT>(slave_address, rx_eeprom_addr, buffer_properties_write_);
|
||||
eeprom.read<TypeT>(slave_address, tx_eeprom_addr, buffer_properties_read_);
|
||||
status = eeprom.read<TypeT>(expected_wkc, slave_address, rx_eeprom_addr, buffer_properties_write_);
|
||||
|
||||
if(status != true) {
|
||||
return status;
|
||||
}
|
||||
|
||||
status = eeprom.read<TypeT>(expected_wkc, slave_address, tx_eeprom_addr, buffer_properties_read_);
|
||||
|
||||
if(status != true) {
|
||||
return status;
|
||||
}
|
||||
|
||||
buffer_regs_[MailboxesRegs::WRITE] = buffer_properties_write_.offset;
|
||||
buffer_regs_[MailboxesRegs::READ] = buffer_properties_read_.offset;
|
||||
|
||||
DebugP_log((char*)"buffer_properties_write_ = 0x%04x\r\n", buffer_properties_write_);
|
||||
DebugP_log((char*)"buffer_properties_read_ = 0x%04x\r\n", buffer_properties_read_);
|
||||
//DebugP_log((char*)"buffer_properties_write_ = 0x%04x\r\n", buffer_properties_write_);
|
||||
//DebugP_log((char*)"buffer_properties_read_ = 0x%04x\r\n", buffer_properties_read_);
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
template<typename TypeT>
|
||||
datagram::EcatDatagram<command::EcatCommand<TypeT, command::WR>, BufferProperties, uint32_t>
|
||||
make_sync_manager_datagram(SyncManager& sync_manager, BufferProperties& buffer) {
|
||||
make_sync_manager_datagram(const datagram::TEcatWkc expected_wkc, SyncManager& sync_manager, BufferProperties& buffer) {
|
||||
using TDatagram = datagram::EcatDatagram<command::EcatCommand<TypeT, command::WR>, BufferProperties, uint32_t>;
|
||||
|
||||
auto slave_address = slave_.get_slave_address<TypeT>();
|
||||
|
||||
return TDatagram{ {{slave_address, sync_manager.offset}}, buffer, sync_manager.default_setting };
|
||||
return TDatagram{ {{slave_address, sync_manager.offset}}, expected_wkc, buffer, sync_manager.default_setting };
|
||||
}
|
||||
|
||||
template<typename TypeT>
|
||||
void init_sync_manager(telegram::EcatTelegram& telegram, sync_manager sm_write, sync_manager sm_read) {
|
||||
bool init_sync_manager(telegram::EcatTelegram& telegram, const datagram::TEcatWkc expected_wkc, sync_manager sm_write, sync_manager sm_read) {
|
||||
SyncManager sync_manager_write = sync_managers_[static_cast<size_t>(sm_write)];
|
||||
auto datagram_write = make_sync_manager_datagram<TypeT>(sync_manager_write, buffer_properties_write_);
|
||||
auto datagram_write = make_sync_manager_datagram<TypeT>(expected_wkc, sync_manager_write, buffer_properties_write_);
|
||||
|
||||
SyncManager sync_manager_read = sync_managers_[static_cast<size_t>(sm_read)];
|
||||
auto datagram_read = make_sync_manager_datagram<TypeT>(sync_manager_read, buffer_properties_read_);
|
||||
auto datagram_read = make_sync_manager_datagram<TypeT>(expected_wkc, sync_manager_read, buffer_properties_read_);
|
||||
|
||||
auto queue = datagram_write + datagram_read;
|
||||
|
||||
do {
|
||||
telegram.transfer(queue);
|
||||
} while(datagram_write.get_all_wkc() < 0x0002);
|
||||
/*
|
||||
do {
|
||||
telegram.transfer(datagram_write);
|
||||
} while(datagram_write.get_wkc() < 0x0001);
|
||||
|
||||
do {
|
||||
telegram.transfer(datagram_read);
|
||||
} while(datagram_read.get_wkc() < 0x0001);
|
||||
*/
|
||||
buffer_regs_[MailboxesRegs::EMPTY] = sync_manager_write.offset + 0x05;
|
||||
buffer_regs_[MailboxesRegs::AVAILABLE] = sync_manager_read.offset + 0x05;
|
||||
|
||||
DebugP_log((char*)"datagram_write.get_wkc() = %d\r\n", datagram_write.get_wkc());
|
||||
DebugP_log((char*)"datagram_read.get_wkc() = %d\r\n", datagram_read.get_wkc());
|
||||
return telegram.transfer(queue);
|
||||
/*
|
||||
telegram.transfer(datagram_write);
|
||||
telegram.transfer(datagram_read);
|
||||
*/
|
||||
}
|
||||
|
||||
template<typename TypeT>
|
||||
datagram::EcatDatagram<command::EcatCommand<TypeT, command::WR>, FMMUSettings>
|
||||
make_fmmu_datagram(fmmu fmmu_x, FMMUSettings& settings) {
|
||||
make_fmmu_datagram(const datagram::TEcatWkc expected_wkc, fmmu fmmu_x, FMMUSettings& settings) {
|
||||
using TDatagram = datagram::EcatDatagram<command::EcatCommand<TypeT, command::WR>, FMMUSettings>;
|
||||
|
||||
auto slave_address = slave_.get_slave_address<TypeT>();
|
||||
address::Offset offset = fmmu_regs_[static_cast<size_t>(fmmu_x)];
|
||||
|
||||
return TDatagram{ {{slave_address, offset}}, settings};
|
||||
return TDatagram{ {{slave_address, offset}}, expected_wkc, settings};
|
||||
}
|
||||
|
||||
template<typename TypeT>
|
||||
void init_fmmu_write(telegram::EcatTelegram& telegram, fmmu fmmu, FMMUGlobalProperties& fmmu_global_properties) {
|
||||
bool init_fmmu_write(telegram::EcatTelegram& telegram, const datagram::TEcatWkc expected_wkc, fmmu fmmu, FMMUGlobalProperties& fmmu_global_properties) {
|
||||
fmmu_write_ = fmmu;
|
||||
|
||||
FMMUSettings settings {
|
||||
@ -246,7 +249,7 @@ public:
|
||||
.activate = 0x01
|
||||
};
|
||||
|
||||
auto datagram = make_fmmu_datagram<TypeT>(fmmu, settings);
|
||||
auto datagram = make_fmmu_datagram<TypeT>(expected_wkc, fmmu, settings);
|
||||
|
||||
fmmu_properties_write_.address = fmmu_global_properties.logical_end_address;
|
||||
fmmu_properties_write_.length = buffer_properties_write_.length;
|
||||
@ -254,13 +257,11 @@ public:
|
||||
fmmu_global_properties.logical_end_address += buffer_properties_write_.length;
|
||||
fmmu_global_properties.logical_full_length_write += buffer_properties_write_.length;
|
||||
|
||||
do {
|
||||
telegram.transfer(datagram);
|
||||
} while(datagram.get_wkc() < 0x0001);
|
||||
return telegram.transfer(datagram);
|
||||
}
|
||||
|
||||
template<typename TypeT>
|
||||
void init_fmmu_read(telegram::EcatTelegram& telegram, fmmu fmmu, FMMUGlobalProperties& fmmu_global_properties) {
|
||||
bool init_fmmu_read(telegram::EcatTelegram& telegram, const datagram::TEcatWkc expected_wkc, fmmu fmmu, FMMUGlobalProperties& fmmu_global_properties) {
|
||||
fmmu_read_ = fmmu;
|
||||
|
||||
FMMUSettings settings {
|
||||
@ -274,7 +275,7 @@ public:
|
||||
.activate = 0x01
|
||||
};
|
||||
|
||||
auto datagram = make_fmmu_datagram<TypeT>(fmmu, settings);
|
||||
auto datagram = make_fmmu_datagram<TypeT>(expected_wkc, fmmu, settings);
|
||||
|
||||
fmmu_properties_read_.address = fmmu_global_properties.logical_end_address;
|
||||
fmmu_properties_read_.length = buffer_properties_read_.length;
|
||||
@ -282,13 +283,11 @@ public:
|
||||
fmmu_global_properties.logical_end_address += buffer_properties_read_.length;
|
||||
fmmu_global_properties.logical_full_length_read += buffer_properties_read_.length;
|
||||
|
||||
do {
|
||||
telegram.transfer(datagram);
|
||||
} while(datagram.get_wkc() < 0x0001);
|
||||
return telegram.transfer(datagram);
|
||||
}
|
||||
|
||||
template<typename TypeT>
|
||||
void init_fmmu(telegram::EcatTelegram& telegram, fmmu fmmu_write, fmmu fmmu_read, FMMUGlobalProperties& fmmu_global_properties) {
|
||||
bool init_fmmu(telegram::EcatTelegram& telegram, const datagram::TEcatWkc expected_wkc, fmmu fmmu_write, fmmu fmmu_read, FMMUGlobalProperties& fmmu_global_properties) {
|
||||
fmmu_write_ = fmmu_write;
|
||||
fmmu_read_ = fmmu_read;
|
||||
|
||||
@ -303,7 +302,7 @@ public:
|
||||
.activate = 0x01
|
||||
};
|
||||
|
||||
auto datagram_write = make_fmmu_datagram<TypeT>(fmmu_write, settings_write);
|
||||
auto datagram_write = make_fmmu_datagram<TypeT>(expected_wkc, fmmu_write, settings_write);
|
||||
|
||||
fmmu_properties_write_.address = fmmu_global_properties.logical_end_address;
|
||||
fmmu_properties_write_.length = buffer_properties_write_.length;
|
||||
@ -322,7 +321,7 @@ public:
|
||||
.activate = 0x01
|
||||
};
|
||||
|
||||
auto datagram_read = make_fmmu_datagram<TypeT>(fmmu_read, settings_read);
|
||||
auto datagram_read = make_fmmu_datagram<TypeT>(expected_wkc, fmmu_read, settings_read);
|
||||
|
||||
fmmu_properties_read_.address = fmmu_global_properties.logical_end_address;
|
||||
fmmu_properties_read_.length = buffer_properties_read_.length;
|
||||
@ -330,22 +329,13 @@ public:
|
||||
fmmu_global_properties.logical_end_address += buffer_properties_read_.length;
|
||||
fmmu_global_properties.logical_full_length_read += buffer_properties_read_.length;
|
||||
|
||||
datagram_write + datagram_read;
|
||||
auto queue = datagram_write + datagram_read;
|
||||
|
||||
do {
|
||||
telegram.transfer(datagram_write);
|
||||
} while(datagram_write.get_all_wkc() < 0x0002);
|
||||
return telegram.transfer(queue);
|
||||
/*
|
||||
do {
|
||||
telegram.transfer(datagram_write);
|
||||
} while(datagram_write.get_wkc() < 0x0001);
|
||||
|
||||
do {
|
||||
telegram.transfer(datagram_read);
|
||||
} while(datagram_read.get_wkc() < 0x0001);
|
||||
telegram.transfer(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:
|
||||
@ -387,7 +377,7 @@ class EthEcatBuffer {
|
||||
public:
|
||||
EthEcatBuffer(EthEcat& ecat): ecat_{ecat} { }
|
||||
|
||||
void init(uint16_t rx_eeprom_addr, uint16_t tx_eeprom_addr);
|
||||
bool init(uint16_t rx_eeprom_addr, uint16_t tx_eeprom_addr);
|
||||
|
||||
EthEcat& get_ecat() {
|
||||
return ecat_;
|
||||
@ -419,8 +409,8 @@ public:
|
||||
}
|
||||
}
|
||||
|
||||
void init_sync_manager(sync_manager sm_write, sync_manager sm_read);
|
||||
void init_fmmu(fmmu fmmu_write, fmmu fmmu_read);
|
||||
bool init_sync_manager(sync_manager sm_write, sync_manager sm_read);
|
||||
bool init_fmmu(fmmu fmmu_write, fmmu fmmu_read);
|
||||
|
||||
private:
|
||||
EthEcat& ecat_;
|
||||
|
||||
@ -23,11 +23,16 @@ namespace datagram {
|
||||
|
||||
using TEcatWkc = uint16_t;
|
||||
|
||||
class IEcatDatagram : protected queue::QueueEntity {
|
||||
class IEcatDatagram : public queue::QueueEntity {
|
||||
friend class queue::Queue<IEcatDatagram>;
|
||||
|
||||
public:
|
||||
IEcatDatagram()
|
||||
: expected_wkc_{0x0001}
|
||||
{ }
|
||||
|
||||
IEcatDatagram(TEcatWkc expected_wkc)
|
||||
: expected_wkc_{expected_wkc}
|
||||
{ }
|
||||
|
||||
virtual ~IEcatDatagram() { };
|
||||
@ -41,6 +46,9 @@ public:
|
||||
virtual uint8_t* pack(uint8_t *raw) = 0;
|
||||
virtual uint8_t* unpack(uint8_t *raw) = 0;
|
||||
|
||||
virtual uint8_t* pack_all(uint8_t *raw) = 0;
|
||||
virtual uint8_t* unpack_all(uint8_t *raw) = 0;
|
||||
|
||||
TEcatDgHeader& get_header() {
|
||||
return header_;
|
||||
}
|
||||
@ -49,19 +57,54 @@ public:
|
||||
return wkc_;
|
||||
}
|
||||
|
||||
void reset_wkc() {
|
||||
wkc_ = 0x0000;
|
||||
}
|
||||
|
||||
bool as_expected() {
|
||||
return (wkc_ >= expected_wkc_);
|
||||
}
|
||||
|
||||
TEcatWkc get_all_wkc() {
|
||||
IEcatDatagram* next = static_cast<IEcatDatagram*>(get_next());
|
||||
IEcatDatagram* next = this;
|
||||
TEcatWkc wkc = 0x0000;
|
||||
|
||||
if(next == nullptr) {
|
||||
return wkc_;
|
||||
}
|
||||
do {
|
||||
wkc += next->get_wkc();
|
||||
|
||||
return wkc_ + next->get_all_wkc();
|
||||
next = static_cast<IEcatDatagram*>(next->get_next());
|
||||
} while(next != nullptr);
|
||||
|
||||
return wkc;
|
||||
}
|
||||
|
||||
void reset_all_wkc() {
|
||||
IEcatDatagram* next = this;
|
||||
|
||||
do {
|
||||
next->reset_wkc();
|
||||
|
||||
next = static_cast<IEcatDatagram*>(next->get_next());
|
||||
} while(next != nullptr);
|
||||
}
|
||||
|
||||
bool all_as_expected() {
|
||||
IEcatDatagram* next = this;
|
||||
bool as_expected = true;
|
||||
|
||||
do {
|
||||
as_expected = (as_expected && next->as_expected());
|
||||
|
||||
next = static_cast<IEcatDatagram*>(next->get_next());
|
||||
} while(next != nullptr);
|
||||
|
||||
return as_expected;
|
||||
}
|
||||
|
||||
protected:
|
||||
TEcatDgHeader header_;
|
||||
TEcatWkc wkc_{0x0000};
|
||||
const TEcatWkc expected_wkc_{0x0001};
|
||||
|
||||
private:
|
||||
|
||||
@ -72,6 +115,16 @@ 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, const TEcatWkc expected_wkc, DataTypes&... data)
|
||||
: IEcatDatagram{expected_wkc}
|
||||
, command_{command}
|
||||
, data_tuple_{data...} { }
|
||||
|
||||
EcatDatagram(CommandT&& command, const TEcatWkc expected_wkc, custom_tuple<DataTypes&...> data_tuple)
|
||||
: IEcatDatagram{expected_wkc}
|
||||
, command_{command}
|
||||
, data_tuple_{data_tuple} { }
|
||||
|
||||
EcatDatagram(CommandT&& command, DataTypes&... data)
|
||||
: IEcatDatagram{}
|
||||
, command_{command}
|
||||
@ -85,13 +138,45 @@ public:
|
||||
~EcatDatagram() { }
|
||||
|
||||
virtual uint8_t* pack(uint8_t *raw) override {
|
||||
if(as_expected() == true) {
|
||||
return raw;
|
||||
}
|
||||
|
||||
return pack_header(raw);
|
||||
}
|
||||
|
||||
virtual uint8_t* unpack(uint8_t *raw) override {
|
||||
if(as_expected() == true) {
|
||||
return raw;
|
||||
}
|
||||
|
||||
return unpack_header(raw);
|
||||
}
|
||||
|
||||
virtual uint8_t* pack_all(uint8_t *raw) override {
|
||||
IEcatDatagram* next = this;
|
||||
|
||||
do {
|
||||
raw = next->pack(raw);
|
||||
|
||||
next = static_cast<IEcatDatagram*>(next->get_next());
|
||||
} while(next != nullptr);
|
||||
|
||||
return raw;
|
||||
}
|
||||
|
||||
virtual uint8_t* unpack_all(uint8_t *raw) override {
|
||||
IEcatDatagram* next = this;
|
||||
|
||||
do {
|
||||
raw = next->unpack(raw);
|
||||
|
||||
next = static_cast<IEcatDatagram*>(next->get_next());
|
||||
} while(next != nullptr);
|
||||
|
||||
return raw;
|
||||
}
|
||||
|
||||
private:
|
||||
CommandT command_;
|
||||
custom_tuple<DataTypes&...> data_tuple_;
|
||||
|
||||
@ -23,54 +23,55 @@ public:
|
||||
: telegram_{telegram} { }
|
||||
|
||||
template<typename TypeT>
|
||||
void wait_busy(typename TypeT::TSlaveAddress& slave_address) {
|
||||
bool wait_busy(const datagram::TEcatWkc expected_wkc, typename TypeT::TSlaveAddress& slave_address) {
|
||||
using TCommand= command::EcatCommand<TypeT, command::RD>;
|
||||
|
||||
std::array<uint16_t, 2> eeprom_config_status{0x0000, 0x0000};
|
||||
datagram::EcatDatagram<TCommand, std::array<uint16_t, 2>> datagram{ {{slave_address, ECT_REG_EEPCFG}}, eeprom_config_status };
|
||||
datagram::EcatDatagram<TCommand, std::array<uint16_t, 2>> datagram{ {{slave_address, ECT_REG_EEPCFG}}, expected_wkc, eeprom_config_status };
|
||||
bool status;
|
||||
|
||||
do {
|
||||
telegram_.transfer(datagram);
|
||||
}while((datagram.get_wkc() < 0x0001) || ((eeprom_config_status[0] & 0xFF00) != 0) || ((eeprom_config_status[1] & EC_ESTAT_BUSY) != 0));
|
||||
status = telegram_.transfer(datagram);
|
||||
} while((status == true) && ( ((eeprom_config_status[0] & 0xFF00) != 0) || ((eeprom_config_status[1] & EC_ESTAT_BUSY) != 0) ) );
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
template<typename TypeT>
|
||||
void control_register(typename TypeT::TSlaveAddress& slave_address, ec_ecmdtype eeprom_cmd, uint16_t eeprom_address) {
|
||||
bool control_register(const datagram::TEcatWkc expected_wkc, typename TypeT::TSlaveAddress& slave_address, ec_ecmdtype eeprom_cmd, uint16_t eeprom_address) {
|
||||
using TCommand = command::EcatCommand<TypeT, command::WR>;
|
||||
|
||||
std::array<uint16_t, 2> request{eeprom_cmd, eeprom_address};
|
||||
datagram::EcatDatagram<TCommand, std::array<uint16_t, 2>> datagram{ {{slave_address, ECT_REG_EEPCTL}}, expected_wkc, request };
|
||||
|
||||
datagram::EcatDatagram<TCommand, std::array<uint16_t, 2>> datagram{ {{slave_address, ECT_REG_EEPCTL}}, request };
|
||||
|
||||
do {
|
||||
telegram_.transfer(datagram);
|
||||
} while(datagram.get_wkc() < 0x0001);
|
||||
return telegram_.transfer(datagram);
|
||||
}
|
||||
|
||||
template<typename TypeT, typename DirT, typename... DataTypes>
|
||||
void data_register(typename TypeT::TSlaveAddress& slave_address, DataTypes&... data) {
|
||||
bool data_register(const datagram::TEcatWkc expected_wkc, typename TypeT::TSlaveAddress& slave_address, DataTypes&... data) {
|
||||
using TCommand = command::EcatCommand<TypeT, DirT>;
|
||||
datagram::EcatDatagram<TCommand, DataTypes...> datagram{ {{slave_address, ECT_REG_EEPDAT}}, data... };
|
||||
|
||||
do {
|
||||
telegram_.transfer(datagram);
|
||||
} while(datagram.get_wkc() < 0x0001);
|
||||
datagram::EcatDatagram<TCommand, DataTypes...> datagram{ {{slave_address, ECT_REG_EEPDAT}}, expected_wkc, data... };
|
||||
|
||||
return telegram_.transfer(datagram);
|
||||
}
|
||||
|
||||
template<typename TypeT, typename... DataTypes>
|
||||
void read(typename TypeT::TSlaveAddress& slave_address, uint16_t eeprom_address, DataTypes&... data) {
|
||||
wait_busy<TypeT>(slave_address);
|
||||
control_register<TypeT>(slave_address, EC_ECMD_READ, eeprom_address);
|
||||
wait_busy<TypeT>(slave_address);
|
||||
data_register<TypeT, command::RD, DataTypes...>(slave_address, data...);
|
||||
wait_busy<TypeT>(slave_address);
|
||||
bool read(const datagram::TEcatWkc expected_wkc, typename TypeT::TSlaveAddress& slave_address, uint16_t eeprom_address, DataTypes&... data) {
|
||||
if(wait_busy<TypeT>(expected_wkc, slave_address) != true) return false;
|
||||
if(control_register<TypeT>(expected_wkc, slave_address, EC_ECMD_READ, eeprom_address) != true) return false;
|
||||
if(wait_busy<TypeT>(expected_wkc, slave_address) != true) return false;
|
||||
if(data_register<TypeT, command::RD, DataTypes...>(expected_wkc, slave_address, data...) != true) return false;
|
||||
if(wait_busy<TypeT>(expected_wkc, slave_address) != true) return false;
|
||||
}
|
||||
// 2 bytes (1 word) max
|
||||
template<typename TypeT, typename... DataTypes>
|
||||
void write(typename TypeT::TSlaveAddress& slave_address, uint16_t eeprom_address, DataTypes&... data) {
|
||||
wait_busy<TypeT>(slave_address);
|
||||
data_register<TypeT, command::WR, DataTypes...>(slave_address, data...);
|
||||
wait_busy<TypeT>(slave_address);
|
||||
control_register<TypeT>(slave_address, EC_ECMD_WRITE, eeprom_address);
|
||||
wait_busy<TypeT>(slave_address);
|
||||
bool write(const datagram::TEcatWkc expected_wkc, typename TypeT::TSlaveAddress& slave_address, uint16_t eeprom_address, DataTypes&... data) {
|
||||
if(wait_busy<TypeT>(expected_wkc, slave_address) != true) return false;
|
||||
if(data_register<TypeT, command::WR, DataTypes...>(expected_wkc, slave_address, data...) != true) return false;
|
||||
if(wait_busy<TypeT>(expected_wkc, slave_address) != true) return false;
|
||||
if(control_register<TypeT>( expected_wkc, slave_address, EC_ECMD_WRITE, eeprom_address) != true) return false;
|
||||
if(wait_busy<TypeT>(expected_wkc, slave_address) != true) return false;
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
@ -16,12 +16,11 @@ namespace queue {
|
||||
|
||||
class QueueEntity {
|
||||
public:
|
||||
|
||||
protected:
|
||||
QueueEntity* get_next() {
|
||||
return next_;
|
||||
}
|
||||
|
||||
protected:
|
||||
void set_next(QueueEntity *next) {
|
||||
next_ = next;
|
||||
}
|
||||
|
||||
@ -11,8 +11,20 @@ namespace free_rtos {
|
||||
|
||||
namespace telegram {
|
||||
|
||||
const char EcatTelegramStatus::SuccessString[] = "Success !";
|
||||
const char EcatTelegramStatus::BusyString[] = "Busy. Transfer in progress...";
|
||||
const char EcatTelegramStatus::WarningString[] = "Warning ! Check error counters";
|
||||
const char EcatTelegramStatus::FatalErrorString[] = "Fatal error ! Transfer attempts exceeded";
|
||||
|
||||
const EcatDescription EcatTelegramStatus::descriptions[] = {
|
||||
{EcatTelegramStatus::SuccessString, sizeof(EcatTelegramStatus::SuccessString)},
|
||||
{EcatTelegramStatus::BusyString, sizeof(EcatTelegramStatus::BusyString)},
|
||||
{EcatTelegramStatus::WarningString, sizeof(EcatTelegramStatus::WarningString)},
|
||||
{EcatTelegramStatus::FatalErrorString, sizeof(EcatTelegramStatus::FatalErrorString)}
|
||||
};
|
||||
|
||||
int32_t EcatTelegram::Process(uint8_t *p_data, uint32_t len) {
|
||||
// TODO: Не забывать вычитать из указателя sizeof(TEthFrameHeader) !
|
||||
// TODO: Don't forget to subtract sizeof(TEthFrameHeader) !
|
||||
unpack(p_data - sizeof(TEthFrameHeader));
|
||||
|
||||
/*
|
||||
@ -46,19 +58,12 @@ uint8_t* EcatTelegram::pack(uint8_t *raw) {
|
||||
.type = static_cast<uint16_t>(ec_network::PROTOCOL_TYPE) } };
|
||||
uint8_t *p_datagram_first = raw + sizeof(TEthFrameHeader) + sizeof(TEcatFrameHeader);
|
||||
uint8_t *p_datagram_last = p_datagram_first;
|
||||
auto first = datagram_queue_.get_first();
|
||||
|
||||
(void)p_eth_hdr;
|
||||
(void)p_hdr;
|
||||
|
||||
auto queue = datagram_queue_; // Копия очереди
|
||||
auto next = queue.dequeue();
|
||||
|
||||
while(next != nullptr) {
|
||||
//DebugP_log((char*)"Packet packed\r\n");
|
||||
|
||||
p_datagram_last = next->pack(p_datagram_last);
|
||||
next = queue.dequeue();
|
||||
}
|
||||
p_datagram_last = first->pack_all(p_datagram_first);
|
||||
|
||||
p_hdr->bits.length = p_datagram_last - p_datagram_first;
|
||||
|
||||
@ -70,59 +75,82 @@ uint8_t* EcatTelegram::unpack(uint8_t *raw) {
|
||||
TEcatFrameHeader *p_hdr = reinterpret_cast<TEcatFrameHeader*>(raw + sizeof(TEthFrameHeader));
|
||||
uint8_t *p_datagram_first = raw + sizeof(TEthFrameHeader) + sizeof(TEcatFrameHeader);
|
||||
uint8_t *p_datagram_last = p_datagram_first;
|
||||
auto first = datagram_queue_.get_first();
|
||||
|
||||
if(p_eth_hdr->prot_id != ETH_PROT_ECAT_LE) {
|
||||
DebugP_log((char*)"Error: wrong protocol ID\r\n");
|
||||
|
||||
return raw;
|
||||
}
|
||||
|
||||
if(p_hdr->bits.type != static_cast<uint16_t>(ec_network::PROTOCOL_TYPE)) {
|
||||
DebugP_log((char*)"Error: wrong ethercat protocol type\r\n");
|
||||
|
||||
return raw;
|
||||
}
|
||||
|
||||
auto queue = datagram_queue_; // Копия очереди
|
||||
auto next = queue.dequeue();
|
||||
|
||||
while(next != nullptr) {
|
||||
//DebugP_log((char*)"Packet unpacked\r\n");
|
||||
|
||||
p_datagram_last = next->unpack(p_datagram_last);
|
||||
next = queue.dequeue();
|
||||
}
|
||||
p_datagram_last = first->unpack_all(p_datagram_first);
|
||||
|
||||
return p_datagram_last;
|
||||
}
|
||||
|
||||
void EcatTelegram::transfer() {
|
||||
//bool stat = eth_stack_.send_pkt(port_id_, ETH_PROT_ECAT_LE, 1);
|
||||
bool stat = tx_flow_.send(port_id_, this, 1);
|
||||
bool EcatTelegram::transfer() {
|
||||
auto first = datagram_queue_.get_first();
|
||||
uint32_t transfer_attempts = 0;
|
||||
bool stat;
|
||||
bool as_expected;
|
||||
|
||||
if(stat == false) {
|
||||
DebugP_log((char*)"telegram transfer error !\r\n");
|
||||
if(first == nullptr) {
|
||||
status_.result = EcatTelegramResult::SUCCESS;
|
||||
|
||||
datagram_queue_.clear();
|
||||
|
||||
return;
|
||||
return true;
|
||||
}
|
||||
|
||||
rx_sem_.pend();
|
||||
|
||||
datagram_queue_.clear();
|
||||
status_.result = EcatTelegramResult::BUSY;
|
||||
|
||||
first->reset_all_wkc();
|
||||
|
||||
while(1) {
|
||||
transfer_attempts++;
|
||||
|
||||
if(transfer_attempts > max_transfer_attempts_) {
|
||||
status_.attempts_exceeded_errors++;
|
||||
status_.result = EcatTelegramResult::FATAL_ERROR;
|
||||
|
||||
DebugP_log((char*)"Transfer attempts exceeded !\r\n");
|
||||
break;
|
||||
}
|
||||
|
||||
// stat = eth_stack_.send_pkt(port_id_, ETH_PROT_ECAT_LE, 1);
|
||||
stat = tx_flow_.send(port_id_, this, 1);
|
||||
|
||||
if(stat == false) {
|
||||
status_.transfer_errors++;
|
||||
status_.result = EcatTelegramResult::WARNING;
|
||||
|
||||
continue;
|
||||
}
|
||||
|
||||
rx_sem_.pend();
|
||||
|
||||
as_expected = first->all_as_expected();
|
||||
|
||||
if(as_expected == false) {
|
||||
status_.expected_wkc_errors++;
|
||||
status_.result = EcatTelegramResult::WARNING;
|
||||
|
||||
continue;
|
||||
}
|
||||
|
||||
if(status_.result == EcatTelegramResult::BUSY) {
|
||||
status_.result = EcatTelegramResult::SUCCESS;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
return (status_.result != EcatTelegramResult::FATAL_ERROR);
|
||||
}
|
||||
|
||||
void EcatTelegram::transfer(datagram::IEcatDatagram& next) {
|
||||
bool EcatTelegram::transfer(datagram::IEcatDatagram& next) {
|
||||
datagram_queue_ + next;
|
||||
|
||||
transfer();
|
||||
return transfer();
|
||||
}
|
||||
|
||||
void EcatTelegram::transfer(queue::Queue<datagram::IEcatDatagram>& next) {
|
||||
bool EcatTelegram::transfer(queue::Queue<datagram::IEcatDatagram>& next) {
|
||||
datagram_queue_ + next;
|
||||
|
||||
transfer();
|
||||
return transfer();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@ -16,6 +16,37 @@ namespace free_rtos {
|
||||
|
||||
namespace telegram {
|
||||
|
||||
enum class EcatTelegramResult : uint16_t {
|
||||
SUCCESS = 0,
|
||||
BUSY,
|
||||
WARNING,
|
||||
FATAL_ERROR
|
||||
};
|
||||
|
||||
struct EcatDescription {
|
||||
const char* string;
|
||||
const size_t length;
|
||||
};
|
||||
|
||||
struct EcatTelegramStatus {
|
||||
uint16_t transfer_errors;
|
||||
uint16_t expected_wkc_errors;
|
||||
uint16_t attempts_exceeded_errors;
|
||||
EcatTelegramResult result = EcatTelegramResult::SUCCESS;
|
||||
|
||||
const EcatDescription& get_description() {
|
||||
return descriptions[static_cast<size_t>(result)];
|
||||
}
|
||||
|
||||
private:
|
||||
static const char SuccessString[];
|
||||
static const char BusyString[];
|
||||
static const char WarningString[];
|
||||
static const char FatalErrorString[];
|
||||
|
||||
static const EcatDescription descriptions[];
|
||||
};
|
||||
|
||||
class EcatTelegram : public Handler {
|
||||
public:
|
||||
EcatTelegram(Eth& eth)
|
||||
@ -23,6 +54,10 @@ public:
|
||||
, tx_flow_{*eth.getTxFlowPtr()}
|
||||
, eth_stack_{*eth.getEthStackPtr()} { }
|
||||
|
||||
const EcatTelegramStatus& get_status() {
|
||||
return status_;
|
||||
}
|
||||
|
||||
virtual int32_t Process(uint8_t *p_data, uint32_t len) override;
|
||||
virtual uint32_t Sender(uint8_t *p_data, size_t scatter_segment) override;
|
||||
|
||||
@ -31,10 +66,12 @@ public:
|
||||
eth_.getEthStackPtr()->Register(ETH_PROT_ECAT_LE, this);
|
||||
}
|
||||
|
||||
void transfer(datagram::IEcatDatagram& next);
|
||||
void transfer(queue::Queue<datagram::IEcatDatagram>& next);
|
||||
bool transfer(datagram::IEcatDatagram& next);
|
||||
bool transfer(queue::Queue<datagram::IEcatDatagram>& next);
|
||||
|
||||
private:
|
||||
static constexpr uint32_t max_transfer_attempts_ = 3;
|
||||
|
||||
Eth& eth_;
|
||||
EthTxFlowIface& tx_flow_;
|
||||
EthStackIface& eth_stack_;
|
||||
@ -45,7 +82,9 @@ private:
|
||||
queue::Queue<datagram::IEcatDatagram> datagram_queue_;
|
||||
//datagram::IEcatDatagram *datagram_queue_{nullptr};
|
||||
|
||||
void transfer();
|
||||
EcatTelegramStatus status_;
|
||||
|
||||
bool transfer();
|
||||
|
||||
uint8_t* pack(uint8_t *raw);
|
||||
uint8_t* unpack(uint8_t *raw);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user