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..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 @@ -25,9 +25,9 @@ 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") { + } 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 f6981d93..0bc59d82 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,14 @@ 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") { + uint32_t raw = EC_READ_U32(domain_address); + 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; } else { @@ -92,6 +100,13 @@ 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") { + 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) { @@ -192,9 +207,9 @@ 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") { + } 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 9f141595..a53f8ce3 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)); @@ -79,7 +79,15 @@ class SdoConfigEntry } // value if (sdo_config["value"]) { - data = sdo_config["value"].as(); + if (data_type == "float" || data_type == "real32") { + 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(); + } } else { std::cerr << "sdo " << index << ": missing sdo value" << std::endl; return false; @@ -105,9 +113,9 @@ 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") { + } else if (type == "int64" || type == "uint64" || type == "double" || type == "real64") { return 8; } }