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

dev(UML-1462): Доработал custom tuple и мелкие улучшения

Browse Source
This commit is contained in:
algin 2023-05-25 10:20:33 +03:00
parent d356b80aac
commit 95f2cc120e
8 changed files with 171 additions and 134 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -21,7 +21,7 @@ void EthEcatPdoFMMU::init() {
}
}
void EthEcatPdoFMMU::process() {
void 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();
std::array<std::array<uint8_t, 55>, 2> process_data;
@ -31,44 +31,50 @@ void EthEcatPdoFMMU::process() {
process_data[0].fill(0x00);
process_data[1].fill(0x00);
read(0, process_data[0], process_data[1]);
read(process_data[0], process_data[1]);
/*
for(uint8_t& byte : process_data[0]) {
DebugP_log("0x%01x", byte);
}
DebugP_log("\r\n");
write(0, process_data[0], process_data[1]);
*/
write(process_data[0], process_data[1]);
init_sem.post();
process_sem.pend();
for(uint32_t i = 0; i < 250; i++) {
read(0, process_data[0], process_data[1]);
read(process_data[0], process_data[1]);
/*
for(uint8_t& byte : process_data[0]) {
DebugP_log("0x%01x", byte);
}
DebugP_log("\r\n");
write(0, process_data[0], process_data[1]);
*/
write(process_data[0], process_data[1]);
//ClockP_usleep(125ul);
}
init_sem.post();
process_sem.pend();
}
void EthEcatPdoFMMU::process() {
std::array<std::array<uint8_t, 55>, 2> process_data;
wait_op();
while(1)
{
read(0, process_data[0], process_data[1]);
read(process_data[0], process_data[1]);
/*
for(uint8_t& byte : process_data[0]) {
DebugP_log("0x%01x", byte);
}
DebugP_log("\r\n");
write(0, process_data[0], process_data[1]);
*/
write(process_data[0], process_data[1]);
//ClockP_usleep(125ul);
}

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

@ -48,55 +48,71 @@ private:
};
class EthEcatPdoFMMU {
public:
EthEcatPdoFMMU(ecat_buffer::EthEcatBuffer& ecat_mailbox): ecat_buffer_{ecat_mailbox} { }
void init();
private:
template<typename... DataTypes>
void write(size_t slave_index, DataTypes&... data);
void write_recursion(datagram::EcatTelegram& telegram, size_t slave_index, DataTypes&... data);
template<typename DataType, typename... DataTypes>
void write(size_t slave_index, DataType& head, DataTypes&... tail) {
datagram::EcatTelegram& telegram = ecat_buffer_.get_ecat().get_telegram();
void write_recursion(datagram::EcatTelegram& telegram, size_t slave_index, DataType& head, DataTypes&... tail) {
pdo_fmmu_slaves_[slave_index].write(telegram, head);
slave_index++;
if(slave_index < pdo_fmmu_slaves_.size()) {
write(slave_index, tail...);
}
write_recursion(telegram, slave_index, tail...);
}
template<>
void write(size_t slave_index) { }
void write_recursion(datagram::EcatTelegram& telegram, size_t slave_index) { }
template<typename... DataTypes>
void read(size_t slave_index, DataTypes&... data);
void read_recursion(datagram::EcatTelegram& telegram, size_t slave_index, DataTypes&... data);
template<typename DataType, typename... DataTypes>
void read(size_t slave_index, DataType& head, DataTypes&... tail) {
datagram::EcatTelegram& telegram = ecat_buffer_.get_ecat().get_telegram();
void read_recursion(datagram::EcatTelegram& telegram, size_t slave_index, DataType& head, DataTypes&... tail) {
pdo_fmmu_slaves_[slave_index].read(telegram, head);
slave_index++;
if(slave_index < pdo_fmmu_slaves_.size()) {
read(slave_index, tail...);
}
read_recursion(telegram, slave_index, tail...);
}
template<>
void read(size_t slave_index) { }
void read_recursion(datagram::EcatTelegram& telegram, size_t slave_index) { }
public:
EthEcatPdoFMMU(ecat_buffer::EthEcatBuffer& ecat_mailbox): ecat_buffer_{ecat_mailbox} { }
void init();
void process();
template<typename... DataTypes>
void write(DataTypes&... data) {
datagram::EcatTelegram& telegram = ecat_buffer_.get_ecat().get_telegram();
if(sizeof...(data) > pdo_fmmu_slaves_.size()) {
return;
}
write_recursion(telegram, 0, data...);
}
template<typename... DataTypes>
void read(DataTypes&... data) {
datagram::EcatTelegram& telegram = ecat_buffer_.get_ecat().get_telegram();
if(sizeof...(data) > pdo_fmmu_slaves_.size()) {
return;
}
read_recursion(telegram, 0, data...);
}
private:
ecat_buffer::EthEcatBuffer& ecat_buffer_;
std::vector<EcatPdoFMMUSlave> pdo_fmmu_slaves_;
void wait_op();
};
} // namespace ecat_pdo_fmmu

24
components/free_rtos/ethernet_industry/CoE/eth_ecat_sdo_mailbox.hpp
View File

@ -106,17 +106,17 @@ struct MailboxHeader {
// Специализация шаблона для распаковки протокола CoE
template<typename... TailT>
struct CustomTuple<ecat_sdo_mailbox::CoEElements, ecat_sdo_mailbox::CompleteSize, TailT...> : CustomTuple<TailT...> {
CustomTuple(ecat_sdo_mailbox::CoEElements& head, ecat_sdo_mailbox::CompleteSize& complete_size, TailT&... tail)
: CustomTuple<TailT...>(tail...)
struct custom_tuple<ecat_sdo_mailbox::CoEElements&, ecat_sdo_mailbox::CompleteSize&, TailT...> : custom_tuple<TailT...> {
custom_tuple(ecat_sdo_mailbox::CoEElements& head, ecat_sdo_mailbox::CompleteSize& complete_size, TailT... tail)
: custom_tuple<TailT...>(tail...)
, head_(head)
, complete_size_(complete_size) { }
using TBase = CustomTuple<TailT...>;
using TBase = custom_tuple<TailT...>;
constexpr static size_t size = sizeof(ecat_sdo_mailbox::CoEElements) + sizeof(ecat_sdo_mailbox::CompleteSize) + TBase::size;
TBase& base_ = static_cast<TBase&>(*this);
//TBase& base_ = static_cast<TBase&>(*this);
ecat_sdo_mailbox::CoEElements& head_;
ecat_sdo_mailbox::CompleteSize& complete_size_;
@ -127,13 +127,13 @@ struct CustomTuple<ecat_sdo_mailbox::CoEElements, ecat_sdo_mailbox::CompleteSize
(void)complete_size;
raw += sizeof(ecat_sdo_mailbox::CompleteSize);
raw = base_.pack(raw);
raw = TBase::pack(raw);
}
if(head_.command_specifier.transfer_type == static_cast<uint8_t>(ecat_sdo_mailbox::TransferType::EXPEDITED)) {
// По-нормальному мы не должны сюда попадать, т.к. в expedited транзакции не должно быть блока CompleteSize,
// значит либо пользователь ошибся, либо после предыдущей отправки датаграммы слейв указал тип expedited
raw = base_.pack(raw);
raw = TBase::pack(raw);
raw += sizeof(ecat_sdo_mailbox::CompleteSize);
}
@ -155,11 +155,11 @@ struct CustomTuple<ecat_sdo_mailbox::CoEElements, ecat_sdo_mailbox::CompleteSize
complete_size_ = *complete_size;
raw += sizeof(ecat_sdo_mailbox::CompleteSize);
raw = base_.unpack(raw);
raw = TBase::unpack(raw);
}
if(head_.command_specifier.transfer_type == static_cast<uint8_t>(ecat_sdo_mailbox::TransferType::EXPEDITED)) {
raw = base_.unpack(raw);
raw = TBase::unpack(raw);
raw += sizeof(ecat_sdo_mailbox::CompleteSize);
}
@ -239,7 +239,7 @@ public:
auto slave_address = slave.get_slave_address<TypeT>();
std::array<address::Offset, 4>& buffer_regs = buffer_slave_.get_buffer_regs();
MailboxHeader header{
.length = CustomTuple<DataTypes...>::size,
.length = custom_tuple<DataTypes...>::size,
.address = 0, // slave.get_slave_address<command::FP>()
.channel = channel,
.priority = priority,
@ -247,7 +247,7 @@ public:
.cnt = static_cast<uint16_t>(counter_)
};
datagram::EcatDatagram<TCommand, MailboxHeader, DataTypes...> datagram{ {{slave_address, buffer_regs[MailboxesRegs::WRITE]}}, header, data... };
uint16_t padding = buffer_slave_.get_buffer_properties_write().length - sizeof(MailboxHeader) - CustomTuple<DataTypes...>::size;
uint16_t padding = buffer_slave_.get_buffer_properties_write().length - sizeof(MailboxHeader) - custom_tuple<DataTypes...>::size;
datagram.set_padding(padding);
@ -266,7 +266,7 @@ public:
std::array<address::Offset, 4>& buffer_regs = buffer_slave_.get_buffer_regs();
MailboxHeader header;
datagram::EcatDatagram<TCommand, MailboxHeader, DataTypes...> datagram{ {{slave_address, buffer_regs[MailboxesRegs::READ]}}, header, data... };
uint16_t padding = buffer_slave_.get_buffer_properties_read().length - sizeof(MailboxHeader) - CustomTuple<DataTypes...>::size;
uint16_t padding = buffer_slave_.get_buffer_properties_read().length - sizeof(MailboxHeader) - custom_tuple<DataTypes...>::size;
datagram.set_padding(padding);

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

@ -125,20 +125,20 @@ void EthEcat::set_slaves_to_default() {
uint8_t m_data_out{0x00};
datagram::EcatDatagram<command::BWR, uint8_t> m{ {{broadcast, ECT_REG_EEPCFG}}, m_data_out };
//a + b + c + d + e + f + g + h + i + j + k + l + m;
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);
//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());
@ -157,8 +157,8 @@ void EthEcat::set_slaves_to_default() {
uint16_t EthEcat::slaves_detecting() {
address::Broadcast broadcast{0x0000};
uint16_t data_out{0x0000};
datagram::EcatDatagram<command::BRD, uint16_t> datagram{ {{broadcast, ECT_REG_TYPE}}, data_out };
uint16_t data{0x0000};
datagram::EcatDatagram<command::BRD, uint16_t> datagram{ {{broadcast, ECT_REG_TYPE}}, data };
telegram_.transfer(datagram);

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

@ -43,12 +43,12 @@ public:
template<typename TypeT>
void set_slave_address(typename TypeT::TSlaveAddress&& slave_address) {
std::get<static_cast<size_t>(TypeT::type)>(slave_addresses_) = slave_address;
free_rtos::get<static_cast<size_t>(TypeT::type)>(slave_addresses_) = slave_address;
}
template<typename TypeT>
typename TypeT::TSlaveAddress& get_slave_address() {
return std::get<static_cast<size_t>(TypeT::type)>(slave_addresses_);
return free_rtos::get<static_cast<size_t>(TypeT::type)>(slave_addresses_);
}
template<typename TypeT>
@ -266,8 +266,6 @@ private:
e_pktTotal
};
static constexpr uint16_t max_number_of_slaves = 32;
TEthPkt * p_pkt_;
TEthPkt * p_pkt_next_;

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

@ -10,11 +10,11 @@
#include <cstdint>
#include <array>
#include <tuple>
#include <type_traits>
#include "ethernet_industry/ethercattype.hpp"
#include "ethernet_industry/eth_ecat_types.h"
#include "ethernet_industry/eth_ecat_custom_tuple.hpp"
namespace free_rtos {
@ -26,17 +26,17 @@ using Broadcast = uint16_t;
using Station = uint16_t;
using Logical = uint32_t;
using SlaveAddresses = std::tuple<Position, Broadcast, Station, Logical>;
using SlaveAddresses = free_rtos::custom_tuple<Position, Broadcast, Station, Logical>;
// Register offset
using Offset = uint16_t;
using PositionAddress = std::tuple<Position, Offset>;
using BroadcastAddress = std::tuple<Broadcast, Offset>;
using StationAddress = std::tuple<Station, Offset>;
using LogicalAddress = std::tuple<Logical>;
using PositionAddress = free_rtos::custom_tuple<Position, Offset>;
using BroadcastAddress = free_rtos::custom_tuple<Broadcast, Offset>;
using StationAddress = free_rtos::custom_tuple<Station, Offset>;
using LogicalAddress = free_rtos::custom_tuple<Logical>;
using Addresses = std::tuple<PositionAddress, BroadcastAddress, StationAddress, LogicalAddress>;
using Addresses = free_rtos::custom_tuple<PositionAddress, BroadcastAddress, StationAddress, LogicalAddress>;
} // namespace address
@ -79,12 +79,33 @@ struct TypeBase {
template<TYPE_INDEX type_index>
struct Type : public TypeBase {
using TAddress = typename std::tuple_element<static_cast<size_t>(type_index), address::Addresses>::type;
using TSlaveAddress = typename std::tuple_element<0, TAddress>::type;
using TAddress = typename free_rtos::custom_tuple_element<static_cast<size_t>(type_index), address::Addresses>::type;
using TSlaveAddress = typename free_rtos::custom_tuple_element<0, TAddress>::type;
static constexpr TYPE_INDEX type = type_index;
};
static constexpr ec_cmdtype get_cmd(DIR_INDEX dir) {
using AP = Type<TYPE_INDEX::AP>;
using B = Type<TYPE_INDEX::B>;
using FP = Type<TYPE_INDEX::FP>;
using L = Type<TYPE_INDEX::L>;
class EcatCommandBase {
public:
EcatCommandBase(TYPE_INDEX type, DIR_INDEX dir)
: cmd_{get_cmd_from_table(type, dir)} { }
EcatCommandBase()
: cmd_{ec_cmdtype::EC_CMD_NOP} { }
uint8_t get_cmd() {
return cmd_;
}
private:
ec_cmdtype cmd_;
static constexpr ec_cmdtype get_cmd_from_table(TYPE_INDEX type, DIR_INDEX dir) {
std::array<std::array<ec_cmdtype, 4>, 4> commands = {{
{{EC_CMD_APRD, EC_CMD_APWR, EC_CMD_APRW, EC_CMD_ARMW}},
{{EC_CMD_BRD, EC_CMD_BWR, EC_CMD_BRW, EC_CMD_NOP}},
@ -96,63 +117,29 @@ struct Type : public TypeBase {
}
};
using AP = Type<TYPE_INDEX::AP>;
using B = Type<TYPE_INDEX::B>;
using FP = Type<TYPE_INDEX::FP>;
using L = Type<TYPE_INDEX::L>;
class EcatCommandBase {
public:
EcatCommandBase(ec_cmdtype cmd)
: cmd_{cmd} { }
EcatCommandBase()
: cmd_{ec_cmdtype::EC_CMD_NOP} { }
uint8_t get_cmd() {
return cmd_;
}
private:
ec_cmdtype cmd_;
};
template<typename TypeT, typename DirT>
class EcatCommand : public EcatCommandBase {
static_assert(std::is_base_of<TypeBase, TypeT>::value == true, "TypeT should be derived from command::TypeBase");
static_assert(std::is_base_of<DirBase, DirT>::value == true, "DirT should be derived from command::DirBase");
public:
EcatCommand(typename TypeT::TAddress fields)
: EcatCommandBase{TypeT::get_cmd(DirT::dir)}
, address_union_{fields} { }
EcatCommand(typename TypeT::TAddress address)
: EcatCommandBase{TypeT::type, DirT::dir}
, address_{address} { }
EcatCommand() { }
uint32_t get_address() {
return address_union_.dword_;
uint32_t address{0x00000000};
address_.pack(reinterpret_cast<uint8_t*>(&address));
return address;
}
private:
template<typename FieldsT>
union AddressUnion
{
AddressUnion(FieldsT fields)
: fields_{fields} { }
AddressUnion() { }
AddressUnion& operator=(const AddressUnion& other) {
fields_ = other.fields_;
return *this;
}
FieldsT fields_;
uint32_t dword_;
};
AddressUnion<typename TypeT::TAddress> address_union_;
typename TypeT::TAddress address_;
};
/*

58
components/free_rtos/ethernet_industry/eth_ecat_custom_tuple.hpp
View File

@ -12,42 +12,47 @@ namespace free_rtos {
// Базовый шаблон класса, никогда не инстанциируется, поэтому без тела
template<typename... Args>
struct CustomTuple;
struct custom_tuple;
// Основная специализация шаблона. Есть еще одна для протокола CoE.
template<typename HeadT, typename... TailT>
struct CustomTuple<HeadT, TailT...> : CustomTuple<TailT...> {
CustomTuple(HeadT& head, TailT&... tail)
: CustomTuple<TailT...>(tail...)
struct custom_tuple<HeadT, TailT...> : custom_tuple<TailT...> {
custom_tuple(HeadT head, TailT... tail)
: custom_tuple<TailT...>(tail...)
, head_(head) { }
using TBase = CustomTuple<TailT...>;
custom_tuple()
: custom_tuple<TailT...>() { }
constexpr static size_t size = sizeof(HeadT) + TBase::size;
using TBase = custom_tuple<TailT...>;
using THead = HeadT;
using THeadDeref = typename std::remove_reference<THead>::type;
TBase& base_ = static_cast<TBase&>(*this);
HeadT& head_;
constexpr static size_t size = sizeof(THeadDeref) + TBase::size;
//TBase& base_ = static_cast<TBase&>(*this);
THead head_;
uint8_t* pack(uint8_t *raw) {
HeadT *head = new(raw) HeadT{head_};
THeadDeref *head = new(raw) THeadDeref{head_};
(void)head;
return base_.pack(raw + sizeof(HeadT));
return TBase::pack(raw + sizeof(THeadDeref));
}
uint8_t* unpack(uint8_t *raw) {
HeadT *head = reinterpret_cast<HeadT*>(raw);
THeadDeref *head = reinterpret_cast<THeadDeref*>(raw);
head_ = *head;
return base_.unpack(raw + sizeof(HeadT));
return TBase::unpack(raw + sizeof(THeadDeref));
}
};
// Специализация завершения рекурсии
template<>
struct CustomTuple<> {
struct custom_tuple<> {
constexpr static size_t size = 0;
uint8_t* pack(uint8_t *raw) {
@ -59,8 +64,33 @@ struct CustomTuple<> {
}
};
template<size_t index, typename TupleT>
struct custom_tuple_element;
template<size_t index, typename HeadT, typename... TailT>
struct custom_tuple_element<index, custom_tuple<HeadT, TailT...>> {
using TBase = custom_tuple_element<index - 1, custom_tuple<TailT...>>;
using type = typename TBase::type;
static type& get(custom_tuple<HeadT, TailT...>& t) {
return TBase::get(t);
}
};
template<typename HeadT, typename... TailT>
struct custom_tuple_element<0, custom_tuple<HeadT, TailT...>> {
using type = HeadT;
static type& get(custom_tuple<HeadT, TailT...>& t) {
return t.head_;
}
};
template<size_t index, typename TupleT>
typename custom_tuple_element<index, TupleT>::type& get(TupleT& t) {
return custom_tuple_element<index, TupleT>::get(t);
}
}
#endif /* FREE_RTOS_ETHERNET_INDUSTRY_ETH_ECAT_CUSTOM_TUPLE_HPP_ */

4
components/free_rtos/ethernet_industry/eth_ecat_datagram.hpp
View File

@ -13,8 +13,8 @@
#include "ethernet/eth.hpp"
#include "ethernet_industry/ethercattype.hpp"
#include "ethernet_industry/eth_ecat_types.h"
#include "ethernet_industry/eth_ecat_command.hpp"
#include "ethernet_industry/eth_ecat_custom_tuple.hpp"
#include "ethernet_industry/eth_ecat_command.hpp"
namespace free_rtos {
@ -95,7 +95,7 @@ public:
private:
CommandT command_;
CustomTuple<DataTypes...> data_;
custom_tuple<DataTypes&...> data_;
uint8_t* pack_wkc(uint8_t *raw) {
TEcatWkc *wkc = new(raw) TEcatWkc{0x0000};