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

fix<UML-1780>: Изменен аллокатор ethercat PDO. Относительно стабильный.

Browse Source
This commit is contained in:
algin 2023-10-02 18:01:35 +03:00
parent b2af36ccca
commit d2c09001a2
5 changed files with 216 additions and 203 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

184
components/free_rtos/ethernet_industry/CoE/eth_ecat_pdo_fmmu.cpp
View File

@ -25,49 +25,6 @@ void EthEcatPdoFMMU::init(ProcessCallback callback) {
callback_ = callback;
}
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();
process_sem.pend();
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;
std::vector<uint8_t> pdo_read(logical_full_length_read, 0x00);
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;
status = read(0, pdo_write); // read to pdo_write !
if(status != true) {
return status;
}
status = write(0, pdo_write);
if(status != true) {
return status;
}
init_sem.post();
process_sem.pend();
for(uint32_t i = 0; i < 250; i++) {
status = read_write(0, data_tuple_read, 0, data_tuple_write);
if(status != true) {
return status;
}
}
init_sem.post();
process_sem.pend();
return status;
}
void EthEcatPdoFMMU::process_write_queue(uint8_t* process_data, uint32_t len) {
mutex_write_.lock();
@ -127,147 +84,6 @@ void EthEcatPdoFMMU::process_read_queue(uint8_t* process_data, uint32_t len) {
}
}
void EthEcatPdoFMMU::process() {
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;
std::vector<uint8_t> pdo_read(logical_full_length_read, 0x00);
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};
status = read(0, pdo_write); // read to pdo_write !
if(status != true) {
return;
}
status = write(0, pdo_write);
if(status != true) {
return;
}
while(1) {
status = read_write(0, data_tuple_read, 0, data_tuple_write);
if(status != true) {
return;
}
#if PROFILE_LEVEL > 0
{
#warning ( "Profiling for RPDO subscribers is enabled !" )
PROFILE(process_read_queue, PROFILE_SAMPLES, 0);
process_read_queue(pdo_read.data(), pdo_read.size());
}
#else
process_read_queue(pdo_read.data(), pdo_read.size());
#endif
#if PROFILE_LEVEL > 0
{
#warning ( "Profiling for process callback is enabled !" )
PROFILE(callback_, PROFILE_SAMPLES, 0);
if(callback_ != nullptr) {
callback_();
}
}
#else
if(callback_ != nullptr) {
callback_();
}
#endif
#if PROFILE_LEVEL > 0
{
#warning ( "Profiling for TPDO subscribers is enabled !" )
PROFILE(process_write_queue, PROFILE_SAMPLES, 0);
process_write_queue(pdo_write.data(), pdo_write.size());
}
#else
process_write_queue(pdo_write.data(), pdo_write.size());
#endif
pdo_counter_++;
}
}
void EthEcatPdoFMMU::process_fake(uint32_t period_microsec, ecat_pdo_fmmu::ProcessCallback callback) {
std::vector<uint8_t> pdo_read(110, 0x00);
std::vector<uint8_t> pdo_write(110, 0x00);
free_rtos::Timer& ecat_timer = ecat_buffer_.get_ecat().get_ecat_timer();
Timer::Settings ecat_tmr_sett = {
.input_clk_Hz = 25000000, // 25MHz
.base_address = 0x2400000u, // memory mapping,
.clock_src_mux_addr = 0x430081B0u, // sysconfig
.int_num = 152u, // sysconfig
.int_priority = 4, // sysconfig
.period_us = period_microsec /// microsec
};
ecat_timer.Init(ecat_tmr_sett);
ecat_timer.Start();
while(1) {
#if PROFILE_LEVEL > 0
{
#warning ( "Profiling for RPDO subscribers is enabled !" )
PROFILE(process_read_queue, PROFILE_SAMPLES, 0);
process_read_queue(pdo_read.data(), pdo_read.size());
}
#else
process_read_queue(pdo_read.data(), pdo_read.size());
#endif
#if PROFILE_LEVEL > 0
{
#warning ( "Profiling for process callback is enabled !" )
PROFILE(callback_, PROFILE_SAMPLES, 0);
if(callback_ != nullptr) {
callback_();
}
}
#else
if(callback_ != nullptr) {
callback_();
}
#endif
#if PROFILE_LEVEL > 0
{
#warning ( "Profiling for TPDO subscribers is enabled !" )
PROFILE(process_write_queue, PROFILE_SAMPLES, 0);
process_write_queue(pdo_write.data(), pdo_write.size());
}
#else
process_write_queue(pdo_write.data(), pdo_write.size());
#endif
pdo_counter_++;
}
}
}
}

206
components/free_rtos/ethernet_industry/CoE/eth_ecat_pdo_fmmu.hpp
View File

@ -8,6 +8,8 @@
#ifndef FREE_RTOS_ETHERNET_INDUSTRY_COE_ETH_ECAT_PDO_FMMU_HPP_
#define FREE_RTOS_ETHERNET_INDUSTRY_COE_ETH_ECAT_PDO_FMMU_HPP_
#include <vector>
#include "free_rtos/semaphore/semaphore.hpp"
#include "free_rtos/mutex/mutex.hpp"
@ -53,14 +55,18 @@ private:
ecat_buffer::EcatBufferSlave& buffer_slave_;
};
class EthEcatPdoFMMU {
public:
EthEcatPdoFMMU(ecat_buffer::EthEcatBuffer& ecat_buffer): ecat_buffer_{ecat_buffer} { }
EthEcatPdoFMMU(ecat_buffer::EthEcatBuffer& ecat_buffer): ecat_buffer_{ecat_buffer}
{ }
void init(ProcessCallback callback = nullptr);
void process();
void process_fake(uint32_t period_microsec = 250, ProcessCallback callback = nullptr);
template<class DataT, class Allocator>
void process(Allocator& allocator);
template<class DataT, class Allocator>
void process_fake(Allocator& allocator, uint32_t period_microsec = 250, ProcessCallback callback = nullptr);
uint32_t get_pdo_counter() {
return pdo_counter_;
@ -141,7 +147,8 @@ private:
uint32_t pdo_counter_{0};
bool wait_op();
template<class DataT, class Allocator>
bool wait_op(std::vector<DataT, Allocator>& pdo_read, std::vector<DataT, Allocator>& pdo_write);
void process_write_queue(uint8_t* process_data, uint32_t len);
void process_read_queue(uint8_t* process_data, uint32_t len);
@ -194,6 +201,195 @@ private:
};
template<class DataT, class Allocator>
bool EthEcatPdoFMMU::wait_op(std::vector<DataT, Allocator>& pdo_read, std::vector<DataT, Allocator>& pdo_write) {
free_rtos::Semaphore& init_sem = ecat_buffer_.get_ecat().get_init_sem();
free_rtos::Semaphore& process_sem = ecat_buffer_.get_ecat().get_process_sem();
process_sem.pend();
uint32_t logical_full_length_write = ecat_buffer_.get_fmmu_global_properties().logical_full_length_write + sizeof(DataT) - 1;
uint32_t logical_full_length_read = ecat_buffer_.get_fmmu_global_properties().logical_full_length_read + sizeof(DataT) - 1;
custom_tuple< std::vector<DataT, Allocator>& > data_tuple_read{pdo_read};
custom_tuple< std::vector<DataT, Allocator>& > data_tuple_write{pdo_write};
bool status;
pdo_read.reserve(logical_full_length_read/sizeof(DataT));
pdo_read.resize(logical_full_length_read/sizeof(DataT), 0);
pdo_write.reserve(logical_full_length_write/sizeof(DataT));
pdo_write.resize(logical_full_length_write/sizeof(DataT), 0);
status = read(0, pdo_write); // read to pdo_write !
if(status != true) {
return status;
}
status = write(0, pdo_write);
if(status != true) {
return status;
}
init_sem.post();
process_sem.pend();
for(uint32_t i = 0; i < 250; i++) {
status = read_write(0, data_tuple_read, 0, data_tuple_write);
if(status != true) {
return status;
}
}
init_sem.post();
process_sem.pend();
return status;
}
template<class DataT, class Allocator>
void EthEcatPdoFMMU::process(Allocator& allocator) {
std::vector<DataT, Allocator> pdo_read{allocator};
std::vector<DataT, Allocator> pdo_write{allocator};
bool status;
status = wait_op(pdo_read, pdo_write);
if(status != true) {
return;
}
custom_tuple< std::vector<DataT, Allocator>& > data_tuple_read{pdo_read};
custom_tuple< std::vector<DataT, Allocator>& > data_tuple_write{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) {
status = read_write(0, data_tuple_read, 0, data_tuple_write);
if(status != true) {
return;
}
#if PROFILE_LEVEL > 0
{
#warning ( "Profiling for RPDO subscribers is enabled !" )
PROFILE(process_read_queue, PROFILE_SAMPLES, 0);
process_read_queue(pdo_read.data(), pdo_read.size());
}
#else
process_read_queue(pdo_read.data(), pdo_read.size());
#endif
#if PROFILE_LEVEL > 0
{
#warning ( "Profiling for process callback is enabled !" )
PROFILE(callback_, PROFILE_SAMPLES, 0);
if(callback_ != nullptr) {
callback_();
}
}
#else
if(callback_ != nullptr) {
callback_();
}
#endif
#if PROFILE_LEVEL > 0
{
#warning ( "Profiling for TPDO subscribers is enabled !" )
PROFILE(process_write_queue, PROFILE_SAMPLES, 0);
process_write_queue(pdo_write.data(), pdo_write.size());
}
#else
process_write_queue(pdo_write.data(), pdo_write.size());
#endif
pdo_counter_++;
}
}
template<class DataT, class Allocator>
void EthEcatPdoFMMU::process_fake(Allocator& allocator, uint32_t period_microsec, ecat_pdo_fmmu::ProcessCallback callback) {
std::vector<DataT, Allocator> pdo_read(110, 0, allocator);
std::vector<DataT, Allocator> pdo_write(110, 0, allocator);
free_rtos::Timer& ecat_timer = ecat_buffer_.get_ecat().get_ecat_timer();
Timer::Settings ecat_tmr_sett = {
.input_clk_Hz = 25000000, // 25MHz
.base_address = 0x2400000u, // memory mapping,
.clock_src_mux_addr = 0x430081B0u, // sysconfig
.int_num = 152u, // sysconfig
.int_priority = 4, // sysconfig
.period_us = period_microsec /// microsec
};
ecat_timer.Init(ecat_tmr_sett);
ecat_timer.Start();
while(1) {
#if PROFILE_LEVEL > 0
{
#warning ( "Profiling for RPDO subscribers is enabled !" )
PROFILE(process_read_queue, PROFILE_SAMPLES, 0);
process_read_queue(pdo_read.data(), pdo_read.size());
}
#else
process_read_queue(pdo_read.data(), pdo_read.size());
#endif
#if PROFILE_LEVEL > 0
{
#warning ( "Profiling for process callback is enabled !" )
PROFILE(callback_, PROFILE_SAMPLES, 0);
if(callback_ != nullptr) {
callback_();
}
}
#else
if(callback_ != nullptr) {
callback_();
}
#endif
#if PROFILE_LEVEL > 0
{
#warning ( "Profiling for TPDO subscribers is enabled !" )
PROFILE(process_write_queue, PROFILE_SAMPLES, 0);
process_write_queue(pdo_write.data(), pdo_write.size());
}
#else
process_write_queue(pdo_write.data(), pdo_write.size());
#endif
pdo_counter_++;
}
}
} // namespace ecat_pdo_fmmu
} // namespace free_rtos

8
components/free_rtos/ethernet_industry/eth_ecat_api.cpp
View File

@ -114,14 +114,6 @@ bool EthEcatApi::config_init(TEthMacPorts port_id, uint16_t address_base, uint32
return status;
}
void EthEcatApi::process() {
ecat_pdo_fmmu_.process();
}
void EthEcatApi::process_fake(uint32_t period_microsec, ecat_pdo_fmmu::ProcessCallback callback) {
ecat_pdo_fmmu_.process_fake(period_microsec, callback);
}
std::vector<ecat_buffer::PDOMap>& EthEcatApi::get_ecat_pdo_map() {
return ecat_sdo_mailbox_.get_pdo_map();
}

13
components/free_rtos/ethernet_industry/eth_ecat_api.hpp
View File

@ -29,8 +29,17 @@ public:
, ecat_pdo_fmmu_{ecat_buffer_pdo_} { }
bool config_init(TEthMacPorts port_id, uint16_t address_base, uint32_t period_microsec = 250, ecat_pdo_fmmu::ProcessCallback callback = nullptr);
void process();
void process_fake(uint32_t period_microsec = 250, ecat_pdo_fmmu::ProcessCallback callback = nullptr);
template<class DataT, class Allocator>
void process(Allocator& allocator) {
ecat_pdo_fmmu_.process<DataT, Allocator>(allocator);
}
template<class DataT, class Allocator>
void process_fake(Allocator& allocator, uint32_t period_microsec, ecat_pdo_fmmu::ProcessCallback callback) {
ecat_pdo_fmmu_.process_fake<DataT, Allocator>(allocator, period_microsec, callback);
}
std::vector<ecat_buffer::PDOMap>& get_ecat_pdo_map();
uint32_t get_pdo_counter() {

8
components/free_rtos/ethernet_industry/eth_ecat_packer.hpp
View File

@ -45,8 +45,8 @@ struct PackFunctor : public PackFunctorBase {
using PackFunctorBase::operator ();
template<typename DataT>
void operator()(std::vector<DataT>& data) {
template<class DataT, class Allocator>
void operator()(std::vector<DataT, Allocator>& data) {
size_t size = data.size() * sizeof(DataT);
if(skip == false) {
@ -99,8 +99,8 @@ struct UnpackFunctor : public UnpackFunctorBase {
using UnpackFunctorBase::operator ();
template<typename DataT>
void operator()(std::vector<DataT>& data) {
template<class DataT, class Allocator>
void operator()(std::vector<DataT, Allocator>& data) {
size_t size = data.size() * sizeof(DataT);
if(skip == false) {