From 7dc0ccf11d800894f8abe31df5fc8184f6b5e37d Mon Sep 17 00:00:00 2001 From: Jens Vanhooydonck Date: Tue, 25 Jun 2024 16:27:06 +0200 Subject: [PATCH 1/4] Added real32 (Beckhoff float) https://infosys.beckhoff.com/english.php?content=../content/1033/ioanalogmanual/12855866763.html&id= --- .../ethercat_generic_slave/src/generic_ec_slave.cpp | 2 +- .../include/ethercat_interface/ec_pdo_channel_manager.hpp | 4 ++++ .../include/ethercat_interface/ec_sdo_manager.hpp | 4 +++- 3 files changed, 8 insertions(+), 2 deletions(-) diff --git a/ethercat_generic_plugins/ethercat_generic_slave/src/generic_ec_slave.cpp b/ethercat_generic_plugins/ethercat_generic_slave/src/generic_ec_slave.cpp index 7514cf01..c11654a1 100644 --- a/ethercat_generic_plugins/ethercat_generic_slave/src/generic_ec_slave.cpp +++ b/ethercat_generic_plugins/ethercat_generic_slave/src/generic_ec_slave.cpp @@ -25,7 +25,7 @@ size_t type2bytes(std::string type) return 1; } else if (type == "int16" || type == "uint16") { return 2; - } else if (type == "int32" || type == "uint32") { + } else if (type == "int32" || type == "uint32" || type == "float" || type == "real32") { return 4; } else if (type == "int64" || type == "uint64") { return 8; diff --git a/ethercat_interface/include/ethercat_interface/ec_pdo_channel_manager.hpp b/ethercat_interface/include/ethercat_interface/ec_pdo_channel_manager.hpp index f6981d93..6da1650f 100644 --- a/ethercat_interface/include/ethercat_interface/ec_pdo_channel_manager.hpp +++ b/ethercat_interface/include/ethercat_interface/ec_pdo_channel_manager.hpp @@ -65,6 +65,8 @@ class EcPdoChannelManager last_value = static_cast(EC_READ_U64(domain_address)); } else if (data_type == "int64") { last_value = static_cast(EC_READ_S64(domain_address)); + } else if (data_type == "real32" || data_type == "float") { + last_value = static_cast(EC_READ_REAL(domain_address)); } else if (data_type == "bool") { last_value = (EC_READ_U8(domain_address) & data_mask) ? 1 : 0; } else { @@ -92,6 +94,8 @@ class EcPdoChannelManager EC_WRITE_U64(domain_address, static_cast(value)); } else if (data_type == "int64") { EC_WRITE_S64(domain_address, static_cast(value)); + } else if (data_type == "real32" || data_type == "float") { + EC_WRITE_REAL(domain_address, static_cast(value)); } else { buffer_ = EC_READ_U8(domain_address); if (popcount(data_mask) == 1) { diff --git a/ethercat_interface/include/ethercat_interface/ec_sdo_manager.hpp b/ethercat_interface/include/ethercat_interface/ec_sdo_manager.hpp index 9f141595..500f5f49 100644 --- a/ethercat_interface/include/ethercat_interface/ec_sdo_manager.hpp +++ b/ethercat_interface/include/ethercat_interface/ec_sdo_manager.hpp @@ -51,6 +51,8 @@ class SdoConfigEntry EC_WRITE_U64(buffer, static_cast(data)); } else if (data_type == "int64") { EC_WRITE_S64(buffer, static_cast(data)); + } else if (data_type == "real32" || data_type == "float") { + EC_WRITE_REAL(buffer, static_cast(data)); } } @@ -105,7 +107,7 @@ class SdoConfigEntry return 1; } else if (type == "int16" || type == "uint16") { return 2; - } else if (type == "int32" || type == "uint32") { + } else if (type == "int32" || type == "uint32" || type == "float" || type == "real32") { return 4; } else if (type == "int64" || type == "uint64") { return 8; From e554af28ed0f00d8e6e3c4c89342f8c6904e6011 Mon Sep 17 00:00:00 2001 From: Jens Vanhooydonck Date: Tue, 2 Jul 2024 08:47:01 +0200 Subject: [PATCH 2/4] Changed EC_READ_REAL and EC_WRITE_REAL to bitcasting. EC_READ_REAL and EC_WRITE_REAL aren't always available. --- .../ethercat_interface/ec_pdo_channel_manager.hpp | 9 ++++++--- .../include/ethercat_interface/ec_sdo_manager.hpp | 10 ++++++++-- 2 files changed, 14 insertions(+), 5 deletions(-) diff --git a/ethercat_interface/include/ethercat_interface/ec_pdo_channel_manager.hpp b/ethercat_interface/include/ethercat_interface/ec_pdo_channel_manager.hpp index 6da1650f..3c36b7a0 100644 --- a/ethercat_interface/include/ethercat_interface/ec_pdo_channel_manager.hpp +++ b/ethercat_interface/include/ethercat_interface/ec_pdo_channel_manager.hpp @@ -66,7 +66,9 @@ class EcPdoChannelManager } else if (data_type == "int64") { last_value = static_cast(EC_READ_S64(domain_address)); } else if (data_type == "real32" || data_type == "float") { - last_value = static_cast(EC_READ_REAL(domain_address)); + uint32_t raw = EC_READ_U32(domain_address); + float value = *(float *)(const void *)&raw; + last_value = static_cast(value); } else if (data_type == "bool") { last_value = (EC_READ_U8(domain_address) & data_mask) ? 1 : 0; } else { @@ -95,7 +97,8 @@ class EcPdoChannelManager } else if (data_type == "int64") { EC_WRITE_S64(domain_address, static_cast(value)); } else if (data_type == "real32" || data_type == "float") { - EC_WRITE_REAL(domain_address, static_cast(value)); + uint32_t raw = *(uint32_t *)(const void *)&value; + EC_WRITE_U32(domain_address, static_cast(raw)); } else { buffer_ = EC_READ_U8(domain_address); if (popcount(data_mask) == 1) { @@ -196,7 +199,7 @@ class EcPdoChannelManager return 8; } else if (type == "int16" || type == "uint16") { return 16; - } else if (type == "int32" || type == "uint32") { + } else if (type == "int32" || type == "uint32" || type == "float" || type == "real32") { return 32; } else if (type == "int64" || type == "uint64") { return 64; diff --git a/ethercat_interface/include/ethercat_interface/ec_sdo_manager.hpp b/ethercat_interface/include/ethercat_interface/ec_sdo_manager.hpp index 500f5f49..77bd5fa6 100644 --- a/ethercat_interface/include/ethercat_interface/ec_sdo_manager.hpp +++ b/ethercat_interface/include/ethercat_interface/ec_sdo_manager.hpp @@ -52,7 +52,8 @@ class SdoConfigEntry } else if (data_type == "int64") { EC_WRITE_S64(buffer, static_cast(data)); } else if (data_type == "real32" || data_type == "float") { - EC_WRITE_REAL(buffer, static_cast(data)); + uint32_t raw = *(uint32_t *)(const void *)&doubledata; + EC_WRITE_U32(buffer, static_cast(raw)); } } @@ -81,7 +82,11 @@ class SdoConfigEntry } // value if (sdo_config["value"]) { - data = sdo_config["value"].as(); + if (data_type == "float" || data_type == "real32") { + doubledata = sdo_config["value"].as(); + } else { + data = sdo_config["value"].as(); + } } else { std::cerr << "sdo " << index << ": missing sdo value" << std::endl; return false; @@ -99,6 +104,7 @@ class SdoConfigEntry uint8_t sub_index; std::string data_type; int data; + double doubledata; private: size_t type2bytes(std::string type) From 2a5d7764781a15c7ace97c1e6ab775970074e330 Mon Sep 17 00:00:00 2001 From: Jens Vanhooydonck Date: Tue, 2 Jul 2024 11:51:29 +0200 Subject: [PATCH 3/4] Working SDO + altered casting of floats + added doubles --- .../src/generic_ec_slave.cpp | 2 +- .../ethercat_interface/ec_pdo_channel_manager.hpp | 14 +++++++++++--- .../include/ethercat_interface/ec_sdo_manager.hpp | 13 ++++++++----- 3 files changed, 20 insertions(+), 9 deletions(-) diff --git a/ethercat_generic_plugins/ethercat_generic_slave/src/generic_ec_slave.cpp b/ethercat_generic_plugins/ethercat_generic_slave/src/generic_ec_slave.cpp index c11654a1..3aaebdd7 100644 --- a/ethercat_generic_plugins/ethercat_generic_slave/src/generic_ec_slave.cpp +++ b/ethercat_generic_plugins/ethercat_generic_slave/src/generic_ec_slave.cpp @@ -27,7 +27,7 @@ size_t type2bytes(std::string type) return 2; } else if (type == "int32" || type == "uint32" || type == "float" || type == "real32") { return 4; - } else if (type == "int64" || type == "uint64") { + } else if (type == "int64" || type == "uint64" || type == "double" || type == "real64") { return 8; } } diff --git a/ethercat_interface/include/ethercat_interface/ec_pdo_channel_manager.hpp b/ethercat_interface/include/ethercat_interface/ec_pdo_channel_manager.hpp index 3c36b7a0..0bc59d82 100644 --- a/ethercat_interface/include/ethercat_interface/ec_pdo_channel_manager.hpp +++ b/ethercat_interface/include/ethercat_interface/ec_pdo_channel_manager.hpp @@ -67,7 +67,11 @@ class EcPdoChannelManager last_value = static_cast(EC_READ_S64(domain_address)); } else if (data_type == "real32" || data_type == "float") { uint32_t raw = EC_READ_U32(domain_address); - float value = *(float *)(const void *)&raw; + float value = *(float *)&raw; + last_value = static_cast(value); + } else if (data_type == "real64" || data_type == "double") { + uint64_t raw = EC_READ_U64(domain_address); + double value = *(double *)&raw; last_value = static_cast(value); } else if (data_type == "bool") { last_value = (EC_READ_U8(domain_address) & data_mask) ? 1 : 0; @@ -97,8 +101,12 @@ class EcPdoChannelManager } else if (data_type == "int64") { EC_WRITE_S64(domain_address, static_cast(value)); } else if (data_type == "real32" || data_type == "float") { - uint32_t raw = *(uint32_t *)(const void *)&value; + float f = (float)value; + uint32_t raw = *(uint32_t *)&f; EC_WRITE_U32(domain_address, static_cast(raw)); + } else if (data_type == "real64" || data_type == "double") { + uint32_t raw = *(uint32_t *)&value; + EC_WRITE_U64(domain_address, static_cast(raw)); } else { buffer_ = EC_READ_U8(domain_address); if (popcount(data_mask) == 1) { @@ -201,7 +209,7 @@ class EcPdoChannelManager return 16; } else if (type == "int32" || type == "uint32" || type == "float" || type == "real32") { return 32; - } else if (type == "int64" || type == "uint64") { + } else if (type == "int64" || type == "uint64" || type == "double" || type == "real64") { return 64; } else if (type.find("bit") != std::string::npos) { std::string n_bits = type.substr(type.find("bit") + 3); diff --git a/ethercat_interface/include/ethercat_interface/ec_sdo_manager.hpp b/ethercat_interface/include/ethercat_interface/ec_sdo_manager.hpp index 77bd5fa6..46514289 100644 --- a/ethercat_interface/include/ethercat_interface/ec_sdo_manager.hpp +++ b/ethercat_interface/include/ethercat_interface/ec_sdo_manager.hpp @@ -43,11 +43,11 @@ class SdoConfigEntry EC_WRITE_U16(buffer, static_cast(data)); } else if (data_type == "int16") { EC_WRITE_S16(buffer, static_cast(data)); - } else if (data_type == "uint32") { + } else if (data_type == "uint32" || data_type == "real32" || data_type == "float") { EC_WRITE_U32(buffer, static_cast(data)); } else if (data_type == "int32") { EC_WRITE_S32(buffer, static_cast(data)); - } else if (data_type == "uint64") { + } else if (data_type == "uint64" || data_type == "real64" || data_type == "double") { EC_WRITE_U64(buffer, static_cast(data)); } else if (data_type == "int64") { EC_WRITE_S64(buffer, static_cast(data)); @@ -83,7 +83,11 @@ class SdoConfigEntry // value if (sdo_config["value"]) { if (data_type == "float" || data_type == "real32") { - doubledata = sdo_config["value"].as(); + float floatvalue = sdo_config["value"].as(); + data = *(int *)&floatvalue; + } else if (data_type == "double" || data_type == "real64") { + float doublevalue = sdo_config["value"].as(); + data = *(int *)&doublevalue; } else { data = sdo_config["value"].as(); } @@ -104,7 +108,6 @@ class SdoConfigEntry uint8_t sub_index; std::string data_type; int data; - double doubledata; private: size_t type2bytes(std::string type) @@ -115,7 +118,7 @@ class SdoConfigEntry return 2; } else if (type == "int32" || type == "uint32" || type == "float" || type == "real32") { return 4; - } else if (type == "int64" || type == "uint64") { + } else if (type == "int64" || type == "uint64" || type == "double" || type == "real64") { return 8; } } From d80460f9d009d9693007b2b65f0202da8ce27587 Mon Sep 17 00:00:00 2001 From: Jens Vanhooydonck Date: Thu, 4 Jul 2024 12:07:11 +0200 Subject: [PATCH 4/4] Removed unused code part SDO --- .../include/ethercat_interface/ec_sdo_manager.hpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/ethercat_interface/include/ethercat_interface/ec_sdo_manager.hpp b/ethercat_interface/include/ethercat_interface/ec_sdo_manager.hpp index 46514289..a53f8ce3 100644 --- a/ethercat_interface/include/ethercat_interface/ec_sdo_manager.hpp +++ b/ethercat_interface/include/ethercat_interface/ec_sdo_manager.hpp @@ -51,9 +51,6 @@ class SdoConfigEntry EC_WRITE_U64(buffer, static_cast(data)); } else if (data_type == "int64") { EC_WRITE_S64(buffer, static_cast(data)); - } else if (data_type == "real32" || data_type == "float") { - uint32_t raw = *(uint32_t *)(const void *)&doubledata; - EC_WRITE_U32(buffer, static_cast(raw)); } }