diff --git a/CMakeLists.txt b/CMakeLists.txt index 7d49c0ec..ce3c92cc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -14,6 +14,7 @@ set(LIB_SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src/Gateway.cc ${CMAKE_CURRENT_SOURCE_DIR}/src/Link.cc ${CMAKE_CURRENT_SOURCE_DIR}/src/Mailbox.cc + ${CMAKE_CURRENT_SOURCE_DIR}/src/MasterMailbox.cc ${CMAKE_CURRENT_SOURCE_DIR}/src/Prints.cc ${CMAKE_CURRENT_SOURCE_DIR}/src/protocol.cc ${CMAKE_CURRENT_SOURCE_DIR}/src/Slave.cc diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 2ab22d0f..498e7f5e 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -1,5 +1,5 @@ add_subdirectory(common) add_subdirectory(easycat) -add_subdirectory(test) +add_subdirectory(gateway) add_subdirectory(elmo_control) add_subdirectory(ingenia_control) diff --git a/examples/test/CMakeLists.txt b/examples/gateway/CMakeLists.txt similarity index 100% rename from examples/test/CMakeLists.txt rename to examples/gateway/CMakeLists.txt diff --git a/examples/gateway/emitter.cc b/examples/gateway/emitter.cc new file mode 100644 index 00000000..7db799a2 --- /dev/null +++ b/examples/gateway/emitter.cc @@ -0,0 +1,156 @@ +#include "kickcat/protocol.h" +#include "kickcat/Mailbox.h" + +#ifdef __linux__ + #include "kickcat/OS/Linux/Socket.h" +#elif __PikeOS__ + #include "kickcat/OS/PikeOS/Socket.h" +#else + #error "Unknown platform" +#endif + + +#include +#include + + +using namespace kickcat; + +int main(int argc, char* argv[]) +{ + (void) argc; + (void) argv; + printf("Start\n"); + + int fd = ::socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); + if (fd < 0) + { + perror("socket()"); + return -1; + } + + // Destination + struct sockaddr_in addr; + socklen_t addr_size; + std::memset(&addr, 0, sizeof(addr)); + + addr.sin_family = AF_INET; // IPv4 + addr.sin_addr.s_addr = inet_addr("127.0.0.1"); + addr.sin_port = hton(0x88A4); // Port is defined in ETG 8200 + + + // Serial number storage +// uint32_t sn = 0; +// uint32_t sn_size = 4; + + // Local mailbox to generate and process messages + Mailbox mailbox; + mailbox.recv_size = 128; + + // Frame to send/rec on the UDP socket + uint8_t frame[ETH_MTU_SIZE]; + EthercatHeader* header = reinterpret_cast(frame); + header->type = EthercatType::MAILBOX; + +// for (int32_t i = 0; i < 256; ++i) +// { +// sleep(2s); +// +// // Target is an Elmo Gold device +// mailbox.createSDO(0x1018, 4, false, CoE::SDO::request::UPLOAD, &sn, &sn_size); +// auto msg = mailbox.send(); +// msg->setAddress(1001); +// +// std::memcpy(frame + sizeof(EthercatHeader), msg->data(), msg->size()); +// header->len = msg->size(); +// +// int32_t sent = ::sendto(fd, &frame, header->len + sizeof(EthercatHeader), MSG_DONTWAIT, (struct sockaddr*)&addr, sizeof(addr)); +// if (sent < 0) +// { +// perror("sendto()"); +// continue; +// } +// +// int rec = ::recvfrom(fd, frame, ETH_MTU_SIZE, 0, (struct sockaddr*)&addr, &addr_size); +// if (rec < 0) +// { +// continue; +// } +// +// if (mailbox.receive(frame + sizeof(EthercatHeader)) == false) +// { +// printf("Mailbox didn't process this message\n"); +// continue; +// } +// +// printf("Serial number %d\n", sn); +// sn = 0; +// } + + // Try to access to master mailbox data + + //Random SDO, (Target is an Elmo Gold device) + //mailbox.createSDO(0x1018, 4, false, CoE::SDO::request::UPLOAD, &sn, &sn_size); + + + + + + + + + +// uint32_t device_type = 5; +// uint32_t device_type_size = 4; +// mailbox.createSDO(0x1000, 0, false, CoE::SDO::request::UPLOAD, &device_type, &device_type_size); + +// CoE::IdentityObject identity{1,2,3,4,5}; +// uint32_t identity_size = sizeof(identity); +// printf("client identity size %i \n", identity_size); +// mailbox.createSDO(0x1018, 1, true, CoE::SDO::request::UPLOAD, &identity.vendor_id, &identity_size); + + + std::string device_name; + device_name.resize(50); + uint32_t device_name_size = device_name.size(); + mailbox.createSDO(0x1008, 0, false, CoE::SDO::request::UPLOAD, device_name.data(), &device_name_size); + + auto msg = mailbox.send(); + msg->setAddress(0); // target master + + std::memcpy(frame + sizeof(EthercatHeader), msg->data(), msg->size()); + header->len = msg->size(); + + int32_t sent = ::sendto(fd, &frame, header->len + sizeof(EthercatHeader), MSG_DONTWAIT, (struct sockaddr*)&addr, sizeof(addr)); + if (sent < 0) + { + perror("sendto()"); + } + + int rec = ::recvfrom(fd, frame, ETH_MTU_SIZE, 0, (struct sockaddr*)&addr, &addr_size); + if (rec < 0) + { + printf("Nothing to read \n"); + } + + if (mailbox.receive(frame + sizeof(EthercatHeader)) == false) + { + printf("Mailbox didn't process this message\n"); + } + + + mailbox::ServiceData* coe = reinterpret_cast(frame + sizeof(EthercatHeader) + sizeof(mailbox::Header)); + + if (coe->service == CoE::Service::SDO_RESPONSE) + { +// printf("Device type received %i \n", device_type); + +// printf("Identity number_of_entries %i \n", identity.number_of_entries); +// printf("Identity vendor_id %i \n", identity.vendor_id); +// printf("Identity serial_number %i \n", identity.serial_number); + + printf("Device name received %s \n", device_name.c_str()); + } + + return 0; +} diff --git a/examples/test/server.cc b/examples/gateway/server.cc similarity index 94% rename from examples/test/server.cc rename to examples/gateway/server.cc index b8da916e..c0437421 100644 --- a/examples/test/server.cc +++ b/examples/gateway/server.cc @@ -26,8 +26,8 @@ int main(int argc, char* argv[]) { if (argc != 3 and argc != 2) { - printf("usage redundancy mode : ./test NIC_nominal NIC_redundancy\n"); - printf("usage no redundancy mode : ./test NIC_nominal\n"); + printf("usage redundancy mode : ./server NIC_nominal NIC_redundancy\n"); + printf("usage no redundancy mode : ./server NIC_nominal\n"); return 1; } @@ -82,8 +82,6 @@ int main(int argc, char* argv[]) try { bus.init(); - - printf("Init done \n"); print_current_state(); } catch (ErrorCode const& e) diff --git a/examples/ingenia_control/ingenia_control.cc b/examples/ingenia_control/ingenia_control.cc index afa29210..3b8d98e7 100644 --- a/examples/ingenia_control/ingenia_control.cc +++ b/examples/ingenia_control/ingenia_control.cc @@ -101,6 +101,11 @@ int main(int argc, char *argv[]) // Map RXPDO mapPDO(0, 0x1600, pdo::rx_mapping, pdo::rx_mapping_count, 0x1C12); + // Map TXPDO + mapPDO(1, 0x1A00, pdo::tx_mapping, pdo::tx_mapping_count, 0x1C13); + // Map RXPDO + mapPDO(1, 0x1600, pdo::rx_mapping, pdo::rx_mapping_count, 0x1C12); + bus.createMapping(io_buffer); bus.requestState(State::SAFE_OP); @@ -158,10 +163,25 @@ int main(int argc, char *argv[]) // Setting a small torque output_pdo->mode_of_operation = 0x5; - output_pdo->target_torque = 3; + output_pdo->target_torque = 0.01; output_pdo->max_current = 3990; output_pdo->target_position = 0; + uint32_t dataSize = 256; + uint8_t buffer[256]; + + bus.readSDO(ingenia, 0x1018, 0x1, Bus::Access::COMPLETE, &buffer, &dataSize); + CoE::IdentityObject* identity = reinterpret_cast(buffer); + printf("identity number of entries %u vendor id %x \n", identity->number_of_entries, identity->vendor_id); + + std::abort(); + + uint32_t size = 4; + uint32_t vendorID; + bus.readSDO(ingenia, 0x1018, 0x01, Bus::Access::PARTIAL, &vendorID, &size); + printf("Direct vendor id %x \n", vendorID); + + constexpr int64_t LOOP_NUMBER = 12 * 3600 * 1000; // 12h int64_t last_error = 0; for (int64_t i = 0; i < LOOP_NUMBER; ++i) diff --git a/examples/test/emitter.cc b/examples/test/emitter.cc deleted file mode 100644 index 04e7902c..00000000 --- a/examples/test/emitter.cc +++ /dev/null @@ -1,91 +0,0 @@ -#include "kickcat/protocol.h" -#include "kickcat/Mailbox.h" - -#ifdef __linux__ - #include "kickcat/OS/Linux/Socket.h" -#elif __PikeOS__ - #include "kickcat/OS/PikeOS/Socket.h" -#else - #error "Unknown platform" -#endif - - -#include -#include - - -using namespace kickcat; - -int main(int argc, char* argv[]) -{ - (void) argc; - (void) argv; - printf("Start\n"); - - int fd = ::socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); - if (fd < 0) - { - perror("socket()"); - return -1; - } - - // Destination - struct sockaddr_in addr; - socklen_t addr_size; - std::memset(&addr, 0, sizeof(addr)); - - addr.sin_family = AF_INET; // IPv4 - addr.sin_addr.s_addr = inet_addr("127.0.0.1"); - addr.sin_port = hton(0x88A4); // Port is defined in ETG 8200 - - - // Serial number storage - uint32_t sn; - uint32_t sn_size = 4; - - // Local mailbox to generate and process messages - Mailbox mailbox; - mailbox.recv_size = 128; - - // Frame to send/rec on the UDP socket - uint8_t frame[ETH_MTU_SIZE]; - EthercatHeader* header = reinterpret_cast(frame); - header->type = EthercatType::MAILBOX; - - for (int32_t i = 0; i < 256; ++i) - { - sleep(2s); - - // Target is an Elmo Gold device - mailbox.createSDO(0x1018, 4, false, CoE::SDO::request::UPLOAD, &sn, &sn_size); - auto msg = mailbox.send(); - msg->setAddress(1001); - - std::memcpy(frame + sizeof(EthercatHeader), msg->data(), msg->size()); - header->len = msg->size(); - - int32_t sent = ::sendto(fd, &frame, header->len + sizeof(EthercatHeader), MSG_DONTWAIT, (struct sockaddr*)&addr, sizeof(addr)); - if (sent < 0) - { - perror("sendto()"); - continue; - } - - int rec = ::recvfrom(fd, frame, ETH_MTU_SIZE, 0, (struct sockaddr*)&addr, &addr_size); - if (rec < 0) - { - continue; - } - - if (mailbox.receive(frame + sizeof(EthercatHeader)) == false) - { - printf("Mailbox didn't process this message\n"); - continue; - } - - printf("Serial number %d\n", sn); - sn = 0; - } - - return 0; -} diff --git a/include/kickcat/Bus.h b/include/kickcat/Bus.h index 4069b5b2..c0bb7154 100644 --- a/include/kickcat/Bus.h +++ b/include/kickcat/Bus.h @@ -10,6 +10,8 @@ #include "Error.h" #include "Frame.h" #include "Link.h" +#include "MasterMailbox.h" +#include "protocol.h" #include "Slave.h" namespace kickcat @@ -107,6 +109,7 @@ namespace kickcat void clearErrorCounters(); + void setMasterDeviceIdentity(CoE::MasterDeviceDescription description); protected: // for unit testing @@ -167,6 +170,9 @@ namespace kickcat nanoseconds big_wait{10ms}; uint16_t irq_mask_{0}; + + CoE::MasterDeviceDescription deviceDescription_{1, "my_name", "hw version", "soft version", {4, 11, 12, 13, 14}}; + MasterMailbox mailbox_gateway_{}; }; } diff --git a/include/kickcat/MasterMailbox.h b/include/kickcat/MasterMailbox.h new file mode 100644 index 00000000..9a4c2874 --- /dev/null +++ b/include/kickcat/MasterMailbox.h @@ -0,0 +1,75 @@ +#ifndef KICKCAT_MASTER_MAILBOX +#define KICKCAT_MASTER_MAILBOX + +#include "Mailbox.h" +#include "protocol.h" +#include +#include +#include + + +namespace kickcat +{ +#define CREATE_SDO_FIELD(field) SDOField{&field, sizeof(field)} +#define CREATE_STRING_FIELD(field) SDOField{field.data(), static_cast(field.size())} +#define CREATE_UNITARY_SDO_OBJECT(field) {{{0, CREATE_SDO_FIELD(field)}}, false, nullptr, 0} +#define CREATE_UNITARY_SDO_OBJECT_STRING(field) {{{0, CREATE_STRING_FIELD(field)}}, false, nullptr, 0} + + struct SDOFrame + { + SDOFrame(int32_t payload_size) + { + data_.resize(sizeof(mailbox::Header) + sizeof(mailbox::ServiceData) + payload_size); + header_ = reinterpret_cast(data_.data()); + coe_ = reinterpret_cast(data_.data() + sizeof(mailbox::Header)); + payload_ = data_.data() + sizeof(mailbox::Header) + sizeof(mailbox::ServiceData); + } + + std::vector data_; + mailbox::Header* header_; + mailbox::ServiceData* coe_; + void* payload_; + }; + + + struct SDOField + { + void* payload; + uint32_t size; // byte + }; + + struct SDOObject + { + std::unordered_map fields; /// key:subindex, value: associated field. + bool complete_access_enable; + void* payload_complete_access; /// ETG 1006: subindex of zero implies subindex 0 included. + uint32_t size_complete_access; + }; + + ///< key: index of sdo, value: SDO object. + typedef std::unordered_map MasterObjectDictionary; + + ///< Brief + class MasterMailbox + { + public: + MasterMailbox(); + ~MasterMailbox() = default; + + void init(CoE::MasterDeviceDescription& master_description); + + /// Brief Process a canOpen message (SDO) aimed at the master. Other protocols are not supported. + std::shared_ptr replyGatewayMessage(uint8_t const* raw_message, int32_t raw_message_size, uint16_t gateway_index); + + private: + std::vector replyUploadSDO(uint16_t address, uint16_t index, uint8_t subindex, bool complete_access); + + std::vector createAbortSDO(uint16_t address, uint16_t index, uint8_t subindex, uint32_t abort_code); + + MasterObjectDictionary objectDictionary_; + }; +} + + + +#endif diff --git a/include/kickcat/protocol.h b/include/kickcat/protocol.h index fe91d7e7..ed0b5c05 100644 --- a/include/kickcat/protocol.h +++ b/include/kickcat/protocol.h @@ -423,8 +423,8 @@ namespace kickcat reserved : 3, service : 4; // i.e. request, response uint8_t size_indicator : 1, - transfer_type : 1, // expedited or not - block_size : 2, + transfer_type : 1, // expedited (1) or not (0) + block_size : 2, // data set size in ETG1000_6 complete_access : 1, command : 3; // i.e. upload uint16_t index; @@ -501,8 +501,147 @@ namespace kickcat constexpr uint8_t SDO_INFO_ERROR_REQ = 0x07; } - char const* abort_to_str(uint32_t abort_code); + namespace abortcode + { + constexpr uint32_t TOGGLE_BIT_NOT_CHANGED = 0x05030000; + constexpr uint32_t TIMEOUT = 0x05040000; + constexpr uint32_t INVALID_COMMAND = 0x05040001; + constexpr uint32_t OUT_OF_MEMORY = 0x05040005; + constexpr uint32_t UNSUPPORTED_ACCESS = 0x06010000; + constexpr uint32_t WRITE_ONLY = 0x06010001; + constexpr uint32_t READ_ONLY = 0x06010002; + constexpr uint32_t CANT_WRITE_SI = 0x06010003; + constexpr uint32_t CA_NOT_SUPPORTED = 0x06010004; + constexpr uint32_t OBJECT_TOO_BIG = 0x06010005; + constexpr uint32_t OBJECT_MAPPED_TO_RXPDO = 0x06010006; + constexpr uint32_t OBJECT_DOES_NOT_EXIST = 0x06020000; + constexpr uint32_t CANT_MAP_OBJECT_TO_PDO = 0x06040041; + constexpr uint32_t EXCEED_PDO_LENGHT = 0x06040042; + constexpr uint32_t PARAMETER_INCOMPATIBILITY = 0x06040043; + constexpr uint32_t INTERNAL_INCOMPATIBILITY = 0x06040047; + constexpr uint32_t HARDWARE_ERROR = 0x06060000; + constexpr uint32_t SERVICE_PARAMETER_LENGHT_NOT_MATCH = 0x06070010; + constexpr uint32_t SERVICE_PARAMETER_LENGHT_TOO_HIGH = 0x06070012; + constexpr uint32_t SERVICE_PARAMETER_LENGHT_TOO_LOW = 0x06070013; + constexpr uint32_t SUBINDEX_DOES_NOT_EXIST = 0x06090011; + constexpr uint32_t PARAMETER_RANGE_EXCEEDED = 0x06090030; + constexpr uint32_t PARAMATER_VALUE_TOO_HIGH = 0x06090031; + constexpr uint32_t PARAMATER_VALUE_TOO_LOW = 0x06090032; + constexpr uint32_t MODULE_LIST_DOES_NOT_MATCH = 0x06090033; + constexpr uint32_t MAX_VALUE_SMALLER_THAN_MIN = 0x06090036; + constexpr uint32_t GENERAL_ERROR = 0x08000000; + constexpr uint32_t CANT_STORE_DATA = 0x08000020; + constexpr uint32_t CANT_STORE_DATA_LOCAL_CONTROL = 0x08000021; + constexpr uint32_t CANT_STORE_DATA_DEVICE_STATE = 0x08000022; + constexpr uint32_t MISSING_OBJECT_DICTIONNARY = 0x08000023; + + char const* abort_to_str(uint32_t abort_code); + } + } + + // ETG1000.6 + struct IdentityObject + { + uint8_t number_of_entries = 4; + uint32_t vendor_id = 42; + uint32_t product_code = 43; + uint32_t revision_number = 44; + uint32_t serial_number = 45; + } __attribute__((__packed__)); + + struct MasterDeviceDescription + { + uint32_t device_type = 1; + std::string device_name; + std::string hardware_version; + std::string software_version; + IdentityObject identity; + }; + + // ETG1510 + struct ConfigurationData + { + uint8_t number_of_entries; + uint16_t fixed_station_address; + std::string type; // CA ? + std::string name; + uint32_t device_type; + uint32_t vendor_id; + uint32_t product_code; + uint32_t revision_number; + uint32_t serial_number; + uint16_t mailbox_out_size; + uint16_t mailbox_in_size; + uint8_t link_status; + uint8_t link_preset; + uint8_t flags; + uint16_t port_physics; + uint16_t mailbox_protocol_supported; + bool diag_history_object_supported; + }; + + // ETG1510 + struct InformationData + { + uint8_t number_of_entries; + uint16_t station_address; + uint32_t vendor_id; + uint32_t product_code; + uint32_t revision_number; + uint32_t serial_number; + uint16_t dl_status_register; + }; + + // ETG 1510 + struct DiagnosisData + { + uint8_t number_of_entries; + uint16_t al_status; + uint16_t al_control; /// RW / RO ? + uint16_t last_al_status_code; + uint8_t link_conn_status; + uint8_t link_control; /// RW / RO ? + uint16_t fixed_address_conn_port_0; + uint16_t fixed_address_conn_port_1; + uint16_t fixed_address_conn_port_2; + uint16_t fixed_address_conn_port_3; + uint32_t frame_error_counter_port_0; + uint32_t frame_error_counter_port_1; + uint32_t frame_error_counter_port_2; + uint32_t frame_error_counter_port_3; + uint32_t cyclic_wc_error_counter; + uint32_t slave_not_present_counter; + uint32_t abnormal_state_change_counter; + bool disable_automatic_link_control; /// RW /RO ? + uint32_t last_coe_soe_protocol_error; + bool new_diag_message_available; + }; + + struct DetectModulesCommand + { + uint8_t number_of_entries; + char scan_command_request[2]; /// RW ? + uint8_t scan_command_status; + char scan_command_response[6]; + }; + + struct MasterDiagData + { + uint8_t number_of_entries; + uint32_t cyclic_lost_frames; + uint32_t acyclic_lost_frames; + uint32_t cyclic_frames_per_second; + uint32_t acyclic_frame_per_second; + uint16_t master_state; + }; + + struct DiagInterfaceControl + { + uint8_t number_of_entries; + uint16_t reset_diag_info; /// RW + }; + } // MAC addresses are not used by EtherCAT but set them helps the debug easier when following a network trace. diff --git a/src/Bus.cc b/src/Bus.cc index 51aaac92..27e2bac7 100644 --- a/src/Bus.cc +++ b/src/Bus.cc @@ -108,6 +108,8 @@ namespace kickcat slave.mailbox.to_process.push_back(emg); } } + + mailbox_gateway_.init(deviceDescription_); } @@ -945,14 +947,21 @@ namespace kickcat { mailbox::Header const* const mbx_header = reinterpret_cast(raw_message); - // Try to associate the request with a destination + // Address 0 is a message for the master. if (mbx_header->address == 0) { - // Master is the destination, unsupported for now (ETG 1510) - DEBUG_PRINT("Master mailbox not implemented"); - return nullptr; + // handle only CoE SDO otherwise drop. + if (mbx_header->type != mailbox::CoE) + { + printf("ABORT unsupported protocol\n"); + return nullptr; + } + + std::shared_ptr response = mailbox_gateway_.replyGatewayMessage(raw_message, raw_message_size, gateway_index); + return response; } + // Non zero address is for a slave. auto it = std::find_if(slaves_.begin(), slaves_.end(), [&](Slave const& slave) { return slave.address == mbx_header->address; }); if (it == slaves_.end()) { @@ -1010,5 +1019,4 @@ namespace kickcat link_->addDatagram(Command::FPRD, createAddress(slave.address, reg::ESC_DL_STATUS), nullptr, 2, process, error); link_->processDatagrams(); } - } diff --git a/src/Gateway.cc b/src/Gateway.cc index afc8e58e..623077dc 100644 --- a/src/Gateway.cc +++ b/src/Gateway.cc @@ -69,6 +69,7 @@ namespace kickcat header->type = EthercatType::MAILBOX; header->len = msg->size() & 0x7ff; std::memcpy(frame + sizeof(EthercatHeader), msg->data(), msg->size()); + socket_->sendTo(frame, static_cast(msg->size() + sizeof(EthercatHeader)), msg->gatewayIndex()); return true; diff --git a/src/Mailbox.cc b/src/Mailbox.cc index 7613e276..0a29060f 100644 --- a/src/Mailbox.cc +++ b/src/Mailbox.cc @@ -200,6 +200,7 @@ namespace kickcat ProcessingResult SDOMessage::process(uint8_t const* received) { + mailbox::Header const* header = reinterpret_cast(received); mailbox::ServiceData const* coe = reinterpret_cast(received + sizeof(mailbox::Header)); uint8_t const* payload = received + sizeof(mailbox::Header) + sizeof(mailbox::ServiceData); @@ -236,7 +237,7 @@ namespace kickcat { uint32_t code = *reinterpret_cast(payload); // TODO: let client display itself the message - DEBUG_PRINT("Abort requested for %x:%d ! code %08x - %s\n", coe->index, coe->subindex, code, CoE::SDO::abort_to_str(code)); + DEBUG_PRINT("Abort requested for %x:%d ! code %08x - %s\n", coe->index, coe->subindex, code, CoE::SDO::abortcode::abort_to_str(code)); status_ = code; return ProcessingResult::FINALIZE; } @@ -285,6 +286,8 @@ namespace kickcat uint32_t const complete_size = *reinterpret_cast(payload); payload += 4; + printf("Complete size %i client data size %i\n", complete_size, *client_data_size_); + if (*client_data_size_ < complete_size) { status_ = MessageStatus::COE_CLIENT_BUFFER_TOO_SMALL; @@ -425,6 +428,7 @@ namespace kickcat // Switch address field with gateway index and identifier address_ = header_->address; + header_->address = mailbox::GATEWAY_MESSAGE_MASK | gateway_index; } @@ -441,9 +445,13 @@ namespace kickcat // It is the reply to this request: store the result and set back the address field int32_t size = header->len + sizeof(mailbox::Header); + data_.resize(size); + printf("Gateway process size %i \n", size); + header_ = reinterpret_cast(data_.data()); std::memcpy(data_.data(), received, size); + header_->address = address_; status_ = MessageStatus::SUCCESS; diff --git a/src/MasterMailbox.cc b/src/MasterMailbox.cc new file mode 100644 index 00000000..5f5af167 --- /dev/null +++ b/src/MasterMailbox.cc @@ -0,0 +1,169 @@ +#include "MasterMailbox.h" + +namespace kickcat +{ + MasterMailbox::MasterMailbox() + { + } + + + void MasterMailbox::init(CoE::MasterDeviceDescription& master_description) + { + // Associate pointers to master data to their SDO index / subindex. + objectDictionary_.insert({0x1000, CREATE_UNITARY_SDO_OBJECT(master_description.device_type)}); + objectDictionary_.insert({0x1008, CREATE_UNITARY_SDO_OBJECT_STRING(master_description.device_name)}); + objectDictionary_.insert({0x1009, CREATE_UNITARY_SDO_OBJECT_STRING(master_description.hardware_version)}); + objectDictionary_.insert({0x100A, CREATE_UNITARY_SDO_OBJECT_STRING(master_description.software_version)}); + objectDictionary_.insert({0x1018, + { + { + {0, CREATE_SDO_FIELD(master_description.identity.number_of_entries)}, + {1, CREATE_SDO_FIELD(master_description.identity.vendor_id)}, + {2, CREATE_SDO_FIELD(master_description.identity.product_code)}, + {3, CREATE_SDO_FIELD(master_description.identity.revision_number)}, + {4, CREATE_SDO_FIELD(master_description.identity.serial_number)} + }, true, &master_description.identity.number_of_entries, sizeof(CoE::IdentityObject) + } + }); + } + + + std::shared_ptr MasterMailbox::replyGatewayMessage(uint8_t const* raw_message, int32_t raw_message_size, uint16_t gateway_index) + { + auto msg = std::make_shared(raw_message_size, raw_message, gateway_index, 0ns); + + //Fill message. + + mailbox::Header const* header = reinterpret_cast(msg->data()); + mailbox::ServiceData const* coe = reinterpret_cast(msg->data() + sizeof(mailbox::Header)); + + std::vector response; //contains mailbox header + serviceData + data + + if (header->type == mailbox::Type::CoE) + { + DEBUG_PRINT("VALID message type \n"); + } + + switch (coe->command) + { + case CoE::SDO::request::UPLOAD: + { + response = replyUploadSDO(header->address, coe->index, coe->subindex, coe->complete_access); + break; + } + case CoE::SDO::request::DOWNLOAD: + case CoE::SDO::request::DOWNLOAD_SEGMENTED: + case CoE::SDO::request::UPLOAD_SEGMENTED: + default: + { + uint32_t abort_code = CoE::SDO::abortcode::UNSUPPORTED_ACCESS; //unsupported access + response = createAbortSDO(header->address, coe->index, coe->subindex, abort_code); + } + } + + msg->process(response.data()); + return msg; + } + + + std::vector MasterMailbox::createAbortSDO(uint16_t address, uint16_t index, uint8_t subindex, uint32_t abort_code) + { + // ETG1000_6 Table 40 – Abort SDO Transfer Request + SDOFrame sdo(sizeof(abort_code)); + + sdo.header_->len = sizeof(mailbox::ServiceData) + sizeof(abort_code); + sdo.header_->address = address; + sdo.header_->channel = 0; // unused; + sdo.header_->priority = 0; // unused; + sdo.header_->type = mailbox::CoE; + sdo.header_->count = 0; // unused; + + sdo.coe_->number = 0; + sdo.coe_->reserved = 0; + sdo.coe_->service = CoE::Service::SDO_REQUEST; + + sdo.coe_->size_indicator = 0; + sdo.coe_->transfer_type = 0; + sdo.coe_->block_size = 0; + sdo.coe_->complete_access = 0; + sdo.coe_->command = CoE::SDO::request::ABORT; + + sdo.coe_->index = index; + sdo.coe_->subindex = subindex; + + memcpy(sdo.payload_, &abort_code, sizeof(abort_code)); + + return sdo.data_; + } + + + std::vector MasterMailbox::replyUploadSDO(uint16_t address, uint16_t index, uint8_t subindex, bool complete_access) + { + auto const& entryIt = objectDictionary_.find(index); + SDOObject const& entry = entryIt->second; + + if (entryIt == objectDictionary_.end()) + { + uint32_t abort_code = CoE::SDO::abortcode::OBJECT_DOES_NOT_EXIST; + return createAbortSDO(address, index, subindex, abort_code); + } + + if (entry.fields.size() <= subindex) + { + uint32_t abort_code = CoE::SDO::abortcode::SUBINDEX_DOES_NOT_EXIST; + return createAbortSDO(address, index, subindex, abort_code); + } + + SDOField data; + if (complete_access) + { + if (subindex > 1 or not entry.complete_access_enable) + { + uint32_t abort_code = CoE::SDO::abortcode::UNSUPPORTED_ACCESS; + return createAbortSDO(address, index, subindex, abort_code); + } + + if (subindex == 1) + { + // skip subindex 0 + uint32_t sizeSubindex0 = entry.fields.at(0).size; + data.payload = (uint8_t*)entry.payload_complete_access + sizeSubindex0; + data.size = entry.size_complete_access - sizeSubindex0; + } + else + { + data.payload = entry.payload_complete_access; + data.size = entry.size_complete_access; + } + } + else + { + data = entry.fields.at(subindex); + } + + SDOFrame sdo(data.size + 4); + sdo.header_->len = sizeof(mailbox::ServiceData) + data.size + 4; + sdo.header_->address = address; + sdo.header_->channel = 0; // unused; + sdo.header_->priority = 0; // unused; + sdo.header_->type = mailbox::CoE; + sdo.header_->count = 0; // unused; + + sdo.coe_->number = 0; + sdo.coe_->reserved = 0; + sdo.coe_->service = CoE::Service::SDO_RESPONSE; + + sdo.coe_->size_indicator = 1; + sdo.coe_->transfer_type = 0; + sdo.coe_->block_size = 0; + sdo.coe_->complete_access = uint8_t (complete_access); + sdo.coe_->command = CoE::SDO::response::UPLOAD; + + sdo.coe_->index = index; + sdo.coe_->subindex = subindex; + memcpy(sdo.payload_, &data.size, 4); // fill complete size + memcpy((uint8_t*)sdo.payload_ + 4, data.payload, data.size); // data + + return sdo.data_; + } +} diff --git a/src/protocol.cc b/src/protocol.cc index 46ae42a4..f66f84c9 100644 --- a/src/protocol.cc +++ b/src/protocol.cc @@ -4,41 +4,42 @@ namespace kickcat { - char const* CoE::SDO::abort_to_str(uint32_t abort_code) + using namespace CoE::SDO::abortcode; + char const* CoE::SDO::abortcode::abort_to_str(uint32_t abort_code) { switch (abort_code) { - case 0x05030000: { return "Toggle bit not changed"; } - case 0x05040000: { return "SDO protocol timeout"; } - case 0x05040001: { return "Client/Server command specifier not valid or unknown"; } - case 0x05040005: { return "Out of memory"; } - case 0x06010000: { return "Unsupported access to object"; } - case 0x06010001: { return "Attempt to read a write only object "; } - case 0x06010002: { return "Attempt to write a read only object"; } - case 0x06010003: { return "Subindex cannot be written, SI0 must be 0 for write access"; } - case 0x06010004: { return "SDO Complete access not supported for objects of variable length"; } - case 0x06010005: { return "Object length exceeds mailbox size"; } - case 0x06010006: { return "Object mapped to RxPDO, SDO Download blocked"; } - case 0x06020000: { return "The object does not exist in the object dictionnary"; } - case 0x06040041: { return "The object cannot be mapped into the PDO"; } - case 0x06040042: { return "The number and length of the objects to be mapped would exceed the PDO lenght"; } - case 0x06040043: { return "General parameter incompatibility reason"; } - case 0x06040047: { return "General internal incompatibility in the device"; } - case 0x06060000: { return "Access failed due to a hardware error"; } - case 0x06070010: { return "Data type does not match, length of service parameter does not match"; } - case 0x06070012: { return "Data type does not match, length of service parameter too high"; } - case 0x06070013: { return "Data type does not match, length of service parameter too low"; } - case 0x06090011: { return "Subindex does not exist"; } - case 0x06090030: { return "Value range of parameter exceeded"; } - case 0x06090031: { return "Value of parameter written too high"; } - case 0x06090032: { return "Value of parameter written too low"; } - case 0x06090033: { return "Configured module list does not match detected module list"; } - case 0x06090036: { return "Maximum value is less than minimum value"; } - case 0x08000000: { return "General error"; } - case 0x08000020: { return "Data cannot be transferred or stored to the application"; } - case 0x08000021: { return "Data cannot be transferred or stored to the application because of local control"; } - case 0x08000022: { return "Data cannot be transferred or stored to the application because of the present device state";} - case 0x08000023: { return "Object dictionnary dynamic generation fails or no object dictionnary is present"; } + case TOGGLE_BIT_NOT_CHANGED: { return "Toggle bit not changed"; } + case TIMEOUT: { return "SDO protocol timeout"; } + case INVALID_COMMAND: { return "Client/Server command specifier not valid or unknown"; } + case OUT_OF_MEMORY: { return "Out of memory"; } + case UNSUPPORTED_ACCESS: { return "Unsupported access to object"; } + case WRITE_ONLY: { return "Attempt to read a write only object "; } + case READ_ONLY: { return "Attempt to write a read only object"; } + case CANT_WRITE_SI: { return "Subindex cannot be written, SI0 must be 0 for write access"; } + case CA_NOT_SUPPORTED: { return "SDO Complete access not supported for objects of variable length"; } + case OBJECT_TOO_BIG: { return "Object length exceeds mailbox size"; } + case OBJECT_MAPPED_TO_RXPDO: { return "Object mapped to RxPDO, SDO Download blocked"; } + case OBJECT_DOES_NOT_EXIST: { return "The object does not exist in the object dictionnary"; } + case CANT_MAP_OBJECT_TO_PDO: { return "The object cannot be mapped into the PDO"; } + case EXCEED_PDO_LENGHT: { return "The number and length of the objects to be mapped would exceed the PDO lenght"; } + case PARAMETER_INCOMPATIBILITY: { return "General parameter incompatibility reason"; } + case INTERNAL_INCOMPATIBILITY: { return "General internal incompatibility in the device"; } + case HARDWARE_ERROR: { return "Access failed due to a hardware error"; } + case SERVICE_PARAMETER_LENGHT_NOT_MATCH: { return "Data type does not match, length of service parameter does not match"; } + case SERVICE_PARAMETER_LENGHT_TOO_HIGH: { return "Data type does not match, length of service parameter too high"; } + case SERVICE_PARAMETER_LENGHT_TOO_LOW: { return "Data type does not match, length of service parameter too low"; } + case SUBINDEX_DOES_NOT_EXIST: { return "Subindex does not exist"; } + case PARAMETER_RANGE_EXCEEDED: { return "Value range of parameter exceeded"; } + case PARAMATER_VALUE_TOO_HIGH: { return "Value of parameter written too high"; } + case PARAMATER_VALUE_TOO_LOW: { return "Value of parameter written too low"; } + case MODULE_LIST_DOES_NOT_MATCH: { return "Configured module list does not match detected module list"; } + case MAX_VALUE_SMALLER_THAN_MIN: { return "Maximum value is less than minimum value"; } + case GENERAL_ERROR: { return "General error"; } + case CANT_STORE_DATA: { return "Data cannot be transferred or stored to the application"; } + case CANT_STORE_DATA_LOCAL_CONTROL: { return "Data cannot be transferred or stored to the application because of local control"; } + case CANT_STORE_DATA_DEVICE_STATE: { return "Data cannot be transferred or stored to the application because of the present device state";} + case MISSING_OBJECT_DICTIONNARY: { return "Object dictionnary dynamic generation fails or no object dictionnary is present"; } default: { diff --git a/unit/mailbox-t.cc b/unit/mailbox-t.cc index d6e8f1a6..bbc09394 100644 --- a/unit/mailbox-t.cc +++ b/unit/mailbox-t.cc @@ -299,10 +299,10 @@ TEST_F(MailboxTest, SDO_download_abort) sdo->service = CoE::Service::SDO_RESPONSE; sdo->index = 0x1018; sdo->subindex = 1; - *static_cast(payload) = 0x06010000; + *static_cast(payload) = CoE::SDO::abortcode::UNSUPPORTED_ACCESS; ASSERT_TRUE(mailbox.receive(raw_message)); - ASSERT_EQ(0x06010000, message->status()); + ASSERT_EQ(CoE::SDO::abortcode::UNSUPPORTED_ACCESS, message->status()); } TEST_F(MailboxTest, SDO_timedout) diff --git a/unit/protocol-t.cc b/unit/protocol-t.cc index 6c35cb73..1ae26fff 100644 --- a/unit/protocol-t.cc +++ b/unit/protocol-t.cc @@ -9,19 +9,19 @@ TEST(Protocol, SDO_abort_to_str) { for (uint32_t i = 0x05000000; i < 0x05050000; ++i) { - char const* text = CoE::SDO::abort_to_str(i); + char const* text = CoE::SDO::abortcode::abort_to_str(i); ASSERT_EQ(5, strnlen(text, 5)); } for (uint32_t i = 0x06000000; i < 0x060A0000; ++i) { - char const* text = CoE::SDO::abort_to_str(i); + char const* text = CoE::SDO::abortcode::abort_to_str(i); ASSERT_EQ(5, strnlen(text, 5)); } for (uint32_t i = 0x08000000; i < 0x08001000; ++i) { - char const* text = CoE::SDO::abort_to_str(i); + char const* text = CoE::SDO::abortcode::abort_to_str(i); ASSERT_EQ(5, strnlen(text, 5)); } } @@ -58,4 +58,4 @@ TEST(Protocol, hton) ASSERT_EQ(0xFECA, network_16); ASSERT_EQ(0xCADEFECA, network_32); ASSERT_THROW(hton(host_64), kickcat::Error); -} \ No newline at end of file +}