WIP: Master mailbox

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

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)

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 <arpa/inet.h>
+#include <cstring>
+
+
+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<uint16_t>(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<EthercatHeader*>(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<mailbox::ServiceData*>(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;
+}

examples/test/server.cc → 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)

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<CoE::IdentityObject*>(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)

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_{};
     };
 }