Backport SDO info old PR

examples/master/CMakeLists.txt

@@ -1,7 +1,7 @@
 add_subdirectory(common)
 add_subdirectory(easycat)
-add_subdirectory(elmo_control)
-add_subdirectory(ingenia_control)
+add_subdirectory(elmo)
+add_subdirectory(ingenia)
 add_subdirectory(load_esi)
 add_subdirectory(wdc_foot)
 if(UNIX)

examples/master/ingenia_control/CMakeLists.txt → examples/master/ingenia/CMakeLists.txt

@@ -6,3 +6,8 @@ add_executable(ingenia_control ingenia_control.cc ${OS_SRCS})
 target_include_directories(ingenia_control PUBLIC  ${CMAKE_CURRENT_SOURCE_DIR}/include)
 target_link_libraries(ingenia_control common kickcat)
 set_kickcat_properties(ingenia_control)
+
+add_executable(ingenia_info ingenia_info.cc ${OS_SRCS})
+target_include_directories(ingenia_info PUBLIC  ${CMAKE_CURRENT_SOURCE_DIR}/include)
+target_link_libraries(ingenia_info common kickcat)
+set_kickcat_properties(ingenia_info)

examples/master/ingenia/ingenia_info.cc

@@ -0,0 +1,178 @@
+#include <iostream>
+#include <cstring>
+#include <cmath>
+
+#include "kickcat/Bus.h"
+#include "kickcat/Link.h"
+#include "kickcat/Mailbox.h"
+#include "kickcat/Prints.h"
+#include "kickcat/SocketNull.h"
+#include "kickcat/helpers.h"
+
+#include "CanOpenErrors.h"
+#include "CanOpenStateMachine.h"
+#include "IngeniaProtocol.h"
+
+#ifdef __linux__
+    #include "kickcat/OS/Linux/Socket.h"
+#elif __PikeOS__
+    #include "kickcat/OS/PikeOS/Socket.h"
+#elif __MINGW64__
+    #include "kickcat/OS/Windows/Socket.h"
+#else
+#error "Unknown platform"
+#endif
+
+
+using namespace kickcat;
+
+
+void printObjectDictionnaryList(Bus& bus, Slave& slave, CoE::SDO::information::ListType type)
+{
+    uint16_t buffer[2048];
+    uint32_t buffer_size = 4096; // in bytes
+
+    auto sdo = slave.mailbox.createSDOInfoGetODList(type, &buffer, &buffer_size, 100ms);
+
+    bus.waitForMessage(sdo);
+    if (sdo->status() != mailbox::request::MessageStatus::SUCCESS)
+    {
+        THROW_ERROR_CODE("Error while get Object Dictionnary List", sdo->status());
+    }
+
+    printf("Data size received %u \n", buffer_size);
+    std::vector<uint16_t> index_list{buffer + sizeof(type), buffer + buffer_size/2};
+    printf("Object dictionnary list: size: %li\n", index_list.size());
+
+    for (auto const& index : index_list)
+    {
+        printf("index %x \n", index);
+    }
+}
+
+
+void printObjectDescription(Bus& bus, Slave& slave, uint16_t index)
+{
+    char buffer[4096];
+    uint32_t buffer_size = 4096; // in bytes
+
+    auto sdo = slave.mailbox.createSDOInfoGetOD(index, &buffer, &buffer_size, 100ms);
+    bus.waitForMessage(sdo);
+    if (sdo->status() != mailbox::request::MessageStatus::SUCCESS)
+    {
+        THROW_ERROR_CODE("Error while get Object Description", sdo->status());
+    }
+
+    CoE::SDO::information::ObjectDescription* description = reinterpret_cast<CoE::SDO::information::ObjectDescription*>(buffer);
+    std::string name{buffer + sizeof(CoE::SDO::information::ObjectDescription), buffer_size - sizeof(CoE::SDO::information::ObjectDescription)};
+
+    printf("Received object %s desc: %s ", name.c_str(), toString(*description).c_str());
+}
+
+
+void printEntryDescription(Bus& bus, Slave& slave, uint16_t index, uint8_t subindex, uint8_t value_info)
+{
+    char buffer[4096];
+    uint32_t buffer_size = 4096; // in bytes
+
+    auto sdo = slave.mailbox.createSDOInfoGetED(index, subindex, value_info, &buffer, &buffer_size, 100ms);
+    bus.waitForMessage(sdo);
+    if (sdo->status() != mailbox::request::MessageStatus::SUCCESS)
+    {
+        THROW_ERROR_CODE("Error while get Entry Description", sdo->status());
+    }
+
+    CoE::SDO::information::EntryDescription* description = reinterpret_cast<CoE::SDO::information::EntryDescription*>(buffer);
+    printf("Received entry desc: %s \n", toString(*description).c_str());
+
+    for (uint32_t i = sizeof(CoE::SDO::information::EntryDescription); i < buffer_size; ++i)
+    {
+        printf("%c", buffer[i]);
+    }
+    printf("\n");
+}
+
+
+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");
+        return 1;
+    }
+
+    std::shared_ptr<AbstractSocket> socket_redundancy;
+    std::string red_interface_name = "null";
+    std::string nom_interface_name = argv[1];
+
+    if (argc == 2)
+    {
+        printf("No redundancy mode selected \n");
+        socket_redundancy = std::make_shared<SocketNull>();
+    }
+    else
+    {
+        socket_redundancy = std::make_shared<Socket>();
+        red_interface_name = argv[2];
+    }
+
+    selectInterface(nom_interface_name, red_interface_name);
+
+    auto socket_nominal = std::make_shared<Socket>();
+    try
+    {
+        socket_nominal->open(nom_interface_name);
+        socket_redundancy->open(red_interface_name);
+    }
+    catch (std::exception const &e)
+    {
+        std::cerr << e.what() << std::endl;
+        return 1;
+    }
+
+    auto report_redundancy = []()
+    {
+        printf("Redundancy has been activated due to loss of a cable \n");
+    };
+
+    std::shared_ptr<Link> link = std::make_shared<Link>(socket_nominal, socket_redundancy, report_redundancy);
+    link->setTimeout(2ms);
+    link->checkRedundancyNeeded();
+
+    Bus bus(link);
+    try
+    {
+        bus.init();
+
+        for (auto& slave: bus.slaves())
+        {
+            printInfo(slave);
+            printESC(slave);
+        }
+    }
+    catch (ErrorCode const &e)
+    {
+        std::cerr << e.what() << ": " << ALStatus_to_string(e.code()) << std::endl;
+        return 1;
+    }
+    catch (std::exception const &e)
+    {
+        std::cerr << e.what() << std::endl;
+        return 1;
+    }
+
+    auto& ingenia = bus.slaves().at(0);
+    //printObjectDictionnaryList(bus, ingenia, CoE::SDO::information::ListType::ALL);
+    printObjectDescription(bus, ingenia, 0x2025);
+    printEntryDescription(bus, ingenia, 0x2025, 0,
+        CoE::SDO::information::ValueInfo::MAXIMUM);
+
+    printEntryDescription(bus, ingenia, 0x2025, 0,
+        CoE::SDO::information::ValueInfo::DEFAULT);
+
+    printEntryDescription(bus, ingenia, 0x2373, 0,
+        CoE::SDO::information::ValueInfo::DEFAULT | CoE::SDO::information::ValueInfo::MINIMUM | CoE::SDO::information::ValueInfo::MAXIMUM);
+
+    return 0;
+}

lib/include/kickcat/CoE/mailbox/request.h

@@ -27,6 +27,26 @@ namespace kickcat::mailbox::request
         uint32_t* client_data_size_;
     };
 
+    class SDOInformationMessage : public AbstractMessage
+    {
+    public:
+        SDOInformationMessage(uint16_t mailbox_size, uint8_t request, void* data, uint32_t* data_size, uint32_t request_payload_size, nanoseconds timeout);
+        virtual ~SDOInformationMessage() = default;
+
+        ProcessingResult process(uint8_t const* received) override;
+
+    protected:
+        ProcessingResult processResponse(mailbox::Header const* header, CoE::ServiceDataInfo const* sdo,
+                                         uint8_t const* payload, uint8_t expected_opcode);
+
+        CoE::Header* coe_;
+        CoE::ServiceDataInfo* sdo_;
+        uint8_t* payload_;
+        uint8_t* client_data_;
+        uint32_t* client_data_size_;
+        uint32_t already_received_size_{0};
+    };
+
     class EmergencyMessage : public AbstractMessage
     {
     public:

lib/include/kickcat/CoE/protocol.h

@@ -90,27 +90,29 @@ namespace kickcat::CoE
             // ETG1000.5 chapter 6.1.4.3.9 and ETG1000.6 chapter 5.6.3.3
             enum ListType : uint16_t
             {
-                NumberOfObjects = 0x00,
-                AllObjects      = 0x01,
-                RxPDO           = 0x02,
-                TxPDO           = 0x03,
-                Backup          = 0x04,
-                Settings        = 0x05
+                NUMBER      = 0x00,
+                ALL         = 0x01,
+                RxPDO       = 0x02,
+                TxPDO       = 0x03,
+                BACKUP      = 0x04,
+                SETTINGS    = 0x05
             };
 
-            enum ValueInfo : uint8_t
+            namespace ValueInfo
             {
-                UnitType = (1 << 3),
-                Default  = (1 << 4),
-                Minimum  = (1 << 5),
-                Maximum  = (1 << 6)
-            };
+                constexpr uint8_t UNIT_TYPE = (1 << 3);
+                constexpr uint8_t DEFAULT   = (1 << 4);
+                constexpr uint8_t MINIMUM   = (1 << 5);
+                constexpr uint8_t MAXIMUM   = (1 << 6);
+
+                std::string toString(uint8_t value_info);
+            }
 
             struct EntryDescriptionRequest
             {
-                uint16_t  index;
-                uint8_t   subindex;
-                ValueInfo value_info;
+                uint16_t index;
+                uint8_t  subindex;
+                uint8_t  value_info;
             } __attribute__((__packed__));
 
             struct ObjectDescription
@@ -120,16 +122,18 @@ namespace kickcat::CoE
                 uint8_t  max_subindex;
                 ObjectCode object_code;
             } __attribute__((__packed__));
+            std::string toString(ObjectDescription const& object_description);
 
             struct EntryDescription
             {
-                uint16_t    index;
-                uint8_t     subindex;
-                ValueInfo   value_info;
-                DataType    data_type;
-                uint16_t    bit_length;
-                uint16_t    access;
+                uint16_t  index;
+                uint8_t   subindex;
+                uint8_t   value_info;
+                DataType  data_type;
+                uint16_t  bit_length;
+                uint16_t  access;
             } __attribute__((__packed__));
+            std::string toString(EntryDescription const& entry_description);
         }
 
         namespace abort

lib/include/kickcat/Mailbox.h

@@ -116,6 +116,13 @@ namespace kickcat::mailbox::request
         std::shared_ptr<AbstractMessage> createSDO(uint16_t index, uint8_t subindex, bool CA, uint8_t request, void* data, uint32_t* data_size, nanoseconds timeout = 20ms);
         std::shared_ptr<GatewayMessage>  createGatewayMessage(uint8_t const* raw_message, int32_t raw_message_size, uint16_t gateway_index, nanoseconds timeout = 20ms);
 
+        std::shared_ptr<AbstractMessage> createSDOInfoGetODList(CoE::SDO::information::ListType type, void* data,
+                                                                uint32_t* data_size, nanoseconds timeout = 20ms);
+        std::shared_ptr<AbstractMessage> createSDOInfoGetOD(uint16_t index, void* data, uint32_t* data_size,
+                                                            nanoseconds timeout = 20ms);
+        std::shared_ptr<AbstractMessage> createSDOInfoGetED(uint16_t index, uint8_t subindex, uint8_t value_info,
+                                                            void* data, uint32_t* data_size, nanoseconds timeout = 20ms);
+
         // helper to get next message to send and transfer it to reception callbacks if required
         std::shared_ptr<AbstractMessage> send();
 

lib/master/include/kickcat/Bus.h

@@ -123,6 +123,9 @@ namespace kickcat
         void resetSlaves(nanoseconds watchdog);
         void fetchESC();
 
+        // mailbox helpers
+        void waitForMessage(std::shared_ptr<mailbox::request::AbstractMessage> message);
+
     protected: // for unit testing
         // helper with trivial bus management (write then read)
         void processFrames();
@@ -139,9 +142,6 @@ namespace kickcat
         void readMappedPDO(Slave& slave, uint16_t index);
         void configureFMMUs();
 
-        // mailbox helpers
-        void waitForMessage(std::shared_ptr<mailbox::request::AbstractMessage> message);
-
         std::shared_ptr<Link> link_;
         std::vector<Slave> slaves_;