dev(UML-1462): Доработал чтение pdo map и удалил старый код
This commit is contained in:
parent
95f2cc120e
commit
b2a9b34ddb
@ -22,21 +22,21 @@ void EthEcatSdoMailbox::init() {
|
||||
}
|
||||
}
|
||||
|
||||
void EthEcatSdoMailbox::pdo_map_read() {
|
||||
void EthEcatSdoMailbox::pdo_map_read(uint16_t pdo_map_rx_index, uint16_t pdo_map_tx_index) {
|
||||
datagram::EcatTelegram& telegram = ecat_buffer_.get_ecat().get_telegram();
|
||||
uint16_t pdo_map_rx_index{0x1C12};
|
||||
uint16_t pdo_map_tx_index{0x1C13};
|
||||
uint16_t pdo_rx_data_size{0x0000};
|
||||
uint16_t pdo_tx_data_size{0x0000};
|
||||
|
||||
for(EcatSdoMailboxSlave& sdo_mailbox_slave : sdo_mailbox_slaves_) {
|
||||
ecat_buffer::PDOMap pdo_map;
|
||||
|
||||
DebugP_log("Reading rx pdo map\r\n");
|
||||
pdo_rx_data_size = sdo_mailbox_slave.pdo_map_read<command::FP>(telegram, pdo_map_rx_index);
|
||||
pdo_rx_data_size = sdo_mailbox_slave.pdo_map_read<command::FP>(telegram, pdo_map, pdo_map_rx_index);
|
||||
|
||||
DebugP_log("Reading tx pdo map\r\n");
|
||||
pdo_tx_data_size = sdo_mailbox_slave.pdo_map_read<command::FP>(telegram, pdo_map_tx_index);
|
||||
pdo_tx_data_size = sdo_mailbox_slave.pdo_map_read<command::FP>(telegram, pdo_map, pdo_map_tx_index);
|
||||
|
||||
pdo_map_.push_back(ecat_buffer::PDOMap{pdo_rx_data_size, pdo_tx_data_size});
|
||||
pdo_map_.push_back(std::move(pdo_map));
|
||||
|
||||
DebugP_log("pdo_rx_data_size = %d\r\n", pdo_rx_data_size);
|
||||
DebugP_log("pdo_tx_data_size = %d\r\n", pdo_tx_data_size);
|
||||
|
||||
@ -46,12 +46,6 @@ enum class Service : uint16_t {
|
||||
SDO_INFORMATION
|
||||
};
|
||||
|
||||
struct PDODescriptor {
|
||||
uint8_t size;
|
||||
uint8_t subindex;
|
||||
uint16_t index;
|
||||
} __attribute__ ((packed));
|
||||
|
||||
struct CompleteSize {
|
||||
uint32_t value;
|
||||
} __attribute__ ((packed));
|
||||
@ -116,7 +110,6 @@ struct custom_tuple<ecat_sdo_mailbox::CoEElements&, ecat_sdo_mailbox::CompleteSi
|
||||
|
||||
constexpr static size_t size = sizeof(ecat_sdo_mailbox::CoEElements) + sizeof(ecat_sdo_mailbox::CompleteSize) + TBase::size;
|
||||
|
||||
//TBase& base_ = static_cast<TBase&>(*this);
|
||||
ecat_sdo_mailbox::CoEElements& head_;
|
||||
ecat_sdo_mailbox::CompleteSize& complete_size_;
|
||||
|
||||
@ -333,30 +326,41 @@ public:
|
||||
}
|
||||
|
||||
template<typename TypeT>
|
||||
uint16_t pdo_map_read(datagram::EcatTelegram& telegram, uint16_t pdo_map_index) {
|
||||
uint16_t pdo_map_read(datagram::EcatTelegram& telegram, ecat_buffer::PDOMap& pdo_map, uint16_t pdo_map_index) {
|
||||
uint16_t pdo_data_size{0x0000}; // Размер данных в битах !
|
||||
uint8_t pdo_block_count{0x00};
|
||||
|
||||
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);
|
||||
pdo_map.block_index_map[pdo_map_index].emplace(0, pdo_block_count);
|
||||
|
||||
DebugP_log("pdo_block_count = 0x%01x\r\n", pdo_block_count);
|
||||
|
||||
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);
|
||||
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);
|
||||
|
||||
uint8_t pdo_block_object_count{0x00};
|
||||
ecat_buffer::PDODescriptor tmp;
|
||||
uint8_t& pdo_block_object_count = tmp.size;
|
||||
|
||||
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);
|
||||
pdo_map.object_descriptor_map[pdo_block_index].emplace(0, tmp);
|
||||
|
||||
DebugP_log("pdo_block_object_count = 0x%01x\r\n", pdo_block_object_count);
|
||||
|
||||
for(uint8_t pdo_block_subindex = 1; pdo_block_subindex < (pdo_block_object_count + 1); pdo_block_subindex++) {
|
||||
PDODescriptor descriptor;
|
||||
sdo_read<TypeT, PDODescriptor>(telegram, pdo_block_index, pdo_block_subindex, descriptor);
|
||||
ecat_buffer::PDODescriptor descriptor;
|
||||
|
||||
sdo_read<TypeT, ecat_buffer::PDODescriptor>(telegram, pdo_block_index, pdo_block_subindex, descriptor);
|
||||
pdo_data_size += descriptor.size;
|
||||
pdo_map.object_descriptor_map[pdo_block_index].emplace(pdo_block_subindex, descriptor);
|
||||
|
||||
//DebugP_log("descriptor.size = 0x%01x\r\n", descriptor.size);
|
||||
//DebugP_log("descriptor.subindex = 0x%01x\r\n", descriptor.subindex);
|
||||
@ -364,6 +368,8 @@ 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);
|
||||
|
||||
return pdo_data_size/8;
|
||||
@ -380,7 +386,7 @@ public:
|
||||
EthEcatSdoMailbox(ecat_buffer::EthEcatBuffer& ecat_buffer): ecat_buffer_{ecat_buffer} { }
|
||||
|
||||
void init();
|
||||
void pdo_map_read();
|
||||
void pdo_map_read(uint16_t pdo_map_rx_index, uint16_t pdo_map_tx_index);
|
||||
|
||||
std::vector<ecat_buffer::PDOMap>& get_pdo_map() {
|
||||
return pdo_map_;
|
||||
|
||||
@ -24,47 +24,6 @@ void EthEcat::Init(TEthMacPorts port_id) {
|
||||
port_id_ = port_id;
|
||||
telegram_.init(port_id);
|
||||
}
|
||||
// What's not process you're looking for
|
||||
int32_t EthEcat::Process(uint8_t * p_data, uint32_t len) {
|
||||
p_pkt_next_->length = len + sizeof(TEthFrameHeader);
|
||||
|
||||
memcpy(p_pkt_next_->data, p_data - sizeof(TEthFrameHeader), p_pkt_next_->length);
|
||||
|
||||
++stat_.rx_pkt;
|
||||
|
||||
rx_sem_.post();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
std::vector<uint8_t> EthEcat::receive_datagram() {
|
||||
rx_sem_.pend();
|
||||
|
||||
return std::vector<uint8_t>(p_pkt_next_->data + sizeof(TEthFrameHeader), p_pkt_next_->data + p_pkt_next_->length);
|
||||
}
|
||||
// What's not send you're looking for
|
||||
void EthEcat::send_datagram(const std::vector<uint8_t>& datagram) {
|
||||
TEthFrameHeader *p_eth_hdr = reinterpret_cast<TEthFrameHeader*>(p_pkt_->data);
|
||||
uint8_t *p_eth_data = reinterpret_cast<uint8_t*>(p_pkt_->data + sizeof(TEthFrameHeader));
|
||||
|
||||
uint8_t mac_dest[ETH_FRAME_MAC_ADDR_LEN_BYTES] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
|
||||
uint8_t mac_src[ETH_FRAME_MAC_ADDR_LEN_BYTES] = {0x01, 0x01, 0x01, 0x01, 0x01, 0x01};
|
||||
|
||||
memcpy(p_eth_hdr->mac_dest, mac_dest, sizeof(mac_dest));
|
||||
memcpy(p_eth_hdr->mac_src, mac_src, sizeof(mac_src));
|
||||
p_eth_hdr->prot_id = ETH_PROT_ECAT_LE;
|
||||
|
||||
memcpy(p_eth_data, datagram.data(), datagram.size());
|
||||
|
||||
p_pkt_->length = sizeof(TEthFrameHeader) + datagram.size();
|
||||
|
||||
bool stat = tx_flow_.send(port_id_, p_pkt_->data, p_pkt_->length);
|
||||
|
||||
if (stat)
|
||||
{
|
||||
++stat_.tx_pkt;
|
||||
}
|
||||
}
|
||||
|
||||
void EthEcat::set_slaves_to_default() {
|
||||
address::Broadcast broadcast{0x0000};
|
||||
|
||||
@ -195,7 +195,7 @@ private:
|
||||
address::SlaveAddresses slave_addresses_;
|
||||
};
|
||||
|
||||
class EthEcat : public Handler {
|
||||
class EthEcat {
|
||||
public:
|
||||
struct Statistic {
|
||||
uint32_t rx_pkt;
|
||||
@ -206,9 +206,6 @@ public:
|
||||
|
||||
void Init(TEthMacPorts port_id);
|
||||
|
||||
std::vector<uint8_t> receive_datagram();
|
||||
void send_datagram(const std::vector<std::uint8_t>& datagram);
|
||||
|
||||
void set_slaves_to_default();
|
||||
uint16_t slaves_detecting();
|
||||
void set_addresses_of_slaves(uint16_t number_of_slaves, uint16_t address_base);
|
||||
@ -258,7 +255,6 @@ public:
|
||||
|
||||
Statistic getStat() { return stat_;}
|
||||
|
||||
virtual int32_t Process(uint8_t * p_data, uint32_t len) override; /// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> ecat
|
||||
private:
|
||||
enum {
|
||||
e_pktFirst,
|
||||
|
||||
@ -9,17 +9,57 @@
|
||||
#define FREE_RTOS_ETHERNET_INDUSTRY_ETH_ECAT_BUFFER_HPP_
|
||||
|
||||
#include <vector>
|
||||
#include <map>
|
||||
#include <kernel/dpl/ClockP.h>
|
||||
|
||||
#include "ethernet_industry/eth_ecat.hpp"
|
||||
|
||||
namespace free_rtos {
|
||||
|
||||
enum {
|
||||
ECT_PDOOUTPUTOFFSET = 0x1100,
|
||||
ECT_PDOINPUTOFFSET = 0x1400
|
||||
};
|
||||
|
||||
enum {
|
||||
ECT_RXPDOMAPINDEX = 0x1C12,
|
||||
ECT_TXPDOMAPINDEX = 0x1C13
|
||||
};
|
||||
|
||||
namespace ecat_buffer {
|
||||
|
||||
/*
|
||||
index[0x1C..]
|
||||
subindex[0x00] = block_count (1 byte)
|
||||
subindex[0x01] = block_0_index (2 bytes)
|
||||
...
|
||||
subindex[n] = block_n-1_index (2 bytes)
|
||||
|
||||
block_index
|
||||
block_subindex[0x00] = object_count (1 byte / 4 bytes)
|
||||
block_subindex[0x01] = object_0_descriptor (4 bytes)
|
||||
...
|
||||
block_subindex[m] = object_m_descriptor (4 bytes)
|
||||
|
||||
object_descriptor
|
||||
size (1 byte)
|
||||
object_subindex (1 byte)
|
||||
object_index (2 bytes)
|
||||
*/
|
||||
|
||||
struct PDODescriptor {
|
||||
uint8_t size;
|
||||
uint8_t subindex;
|
||||
uint16_t index;
|
||||
} __attribute__ ((packed));
|
||||
|
||||
struct PDOMap {
|
||||
uint16_t rx_data_size;
|
||||
uint16_t tx_data_size;
|
||||
std::map<uint16_t, std::map<uint8_t, uint16_t>> block_index_map;
|
||||
std::map<uint16_t, std::map<uint8_t, PDODescriptor>> object_descriptor_map;
|
||||
std::map<uint16_t, uint16_t> data_size_map; // Размеры в байтах !
|
||||
|
||||
uint16_t pdo_output_offset{ECT_PDOOUTPUTOFFSET};
|
||||
uint16_t pdo_input_offset{ECT_PDOINPUTOFFSET};
|
||||
};
|
||||
|
||||
struct FMMUSettings {
|
||||
@ -311,18 +351,20 @@ public:
|
||||
|
||||
void init(uint16_t rx_eeprom_addr, uint16_t tx_eeprom_addr);
|
||||
|
||||
void set_buffer_offset(uint16_t rx_offset, uint16_t tx_offset) {
|
||||
void set_buffer_offset(std::vector<PDOMap>& pdo_map) {
|
||||
uint32_t i = 0;
|
||||
|
||||
for(EcatBufferSlave& buffer_slave : buffer_slaves_) {
|
||||
buffer_slave.set_buffer_offset(rx_offset, tx_offset);
|
||||
buffer_slave.set_buffer_offset(pdo_map[i].pdo_output_offset, pdo_map[i].pdo_input_offset);
|
||||
i++;
|
||||
}
|
||||
}
|
||||
|
||||
// Размер в байтах !
|
||||
void set_buffer_length(std::vector<PDOMap>& pdo_map) {
|
||||
uint32_t i = 0;
|
||||
|
||||
for(EcatBufferSlave& buffer_slave : buffer_slaves_) {
|
||||
buffer_slave.set_buffer_length(pdo_map[i].rx_data_size, pdo_map[i].tx_data_size);
|
||||
buffer_slave.set_buffer_length(pdo_map[i].data_size_map[ECT_RXPDOMAPINDEX], pdo_map[i].data_size_map[ECT_TXPDOMAPINDEX]);
|
||||
i++;
|
||||
}
|
||||
}
|
||||
|
||||
@ -30,7 +30,6 @@ struct custom_tuple<HeadT, TailT...> : custom_tuple<TailT...> {
|
||||
|
||||
constexpr static size_t size = sizeof(THeadDeref) + TBase::size;
|
||||
|
||||
//TBase& base_ = static_cast<TBase&>(*this);
|
||||
THead head_;
|
||||
|
||||
uint8_t* pack(uint8_t *raw) {
|
||||
|
||||
Loading…
Reference in New Issue
Block a user