Emergency messages (#13)

* Fix: wrong working counter check when setting addresses

* Add CoE emergency messages reception

Author: leducp

include/kickcat/Error.h

@@ -17,7 +17,7 @@ namespace kickcat
 #ifdef DEBUG
     #define DEBUG_PRINT(...) do { fprintf(stderr, "DEBUG: %s:%d: ", __FILE__, __LINE__); fprintf(stderr, ##__VA_ARGS__); } while(0);
 #else
-    #define DEBUG_PRINT(fmt, ...)
+    #define DEBUG_PRINT(...)
 #endif
 
     struct Error : public std::exception

include/kickcat/Mailbox.h

@@ -12,8 +12,9 @@ namespace kickcat
     enum class ProcessingResult
     {
         NOOP,
+        CONTINUE,
         FINALIZE,
-        CONTINUE
+        FINALIZE_AND_KEEP
     };
 
     namespace MessageStatus
@@ -57,7 +58,31 @@ namespace kickcat
         uint32_t status_;               // message current status
     };
 
-    class SDOMessage : public AbstractMessage
+    struct Mailbox
+    {
+        uint16_t recv_offset;
+        uint16_t recv_size;
+        uint16_t send_offset;
+        uint16_t send_size;
+
+        bool can_read;      // data available on the slave
+        bool can_write;     // free space for a new message on the slave
+        uint8_t counter{0}; // session handle, from 1 to 7
+        bool toggle;        // for SDO segmented transfer
+
+        std::shared_ptr<AbstractMessage> createSDO(uint16_t index, uint8_t subindex, bool CA, uint8_t request, void* data, uint32_t* data_size);
+
+        bool receive(uint8_t const* raw_message);
+        std::queue<std::shared_ptr<AbstractMessage>> to_send;     // message waiting to be sent
+        std::list <std::shared_ptr<AbstractMessage>> to_process;  // message already sent, waiting for an answer
+
+        uint8_t nextCounter();
+
+        std::vector<mailbox::Emergency> emergencies;
+    private:
+    };
+
+        class SDOMessage : public AbstractMessage
     {
     public:
         SDOMessage(uint16_t mailbox_size, uint16_t index, uint8_t subindex, bool CA, uint8_t request, void* data, uint32_t* data_size);
@@ -78,26 +103,17 @@ namespace kickcat
         uint32_t* client_data_size_;
     };
 
-    struct Mailbox
+    class EmergencyMessage : public AbstractMessage
     {
-        uint16_t recv_offset;
-        uint16_t recv_size;
-        uint16_t send_offset;
-        uint16_t send_size;
-
-        bool can_read;      // data available on the slave
-        bool can_write;     // free space for a new message on the slave
-        uint8_t counter{0}; // session handle, from 1 to 7
-        bool toggle;        // for SDO segmented transfer
-
-        std::shared_ptr<AbstractMessage> createSDO(uint16_t index, uint8_t subindex, bool CA, uint8_t request, void* data, uint32_t* data_size);
+    public:
+        EmergencyMessage(Mailbox& mailbox);
+        virtual ~EmergencyMessage() = default;
 
-        bool receive(uint8_t const* raw_message);
-        std::queue<std::shared_ptr<AbstractMessage>> to_send;     // message waiting to be sent
-        std::list <std::shared_ptr<AbstractMessage>> to_process;  // message already sent, waiting for an answer
+        bool needAcknowledge() const override { return false; }
+        ProcessingResult process(uint8_t const* received) override;
 
-        uint8_t nextCounter();
     private:
+        Mailbox& mailbox_;
     };
 }
 

include/kickcat/protocol.h

@@ -271,17 +271,26 @@ namespace kickcat
             uint8_t device_order_id;
             uint8_t device_name_id;
             uint8_t reserved_A;
-            uint8_t CoE_details;
+            uint8_t SDO_set : 1, // CoE details
+                    SDO_info : 1,
+                    PDO_assign : 1,
+                    PDO_configuration : 1,
+                    PDO_upload : 1,
+                    SDO_complete_access : 1,
+                    unused : 2;
             uint8_t FoE_details;
             uint8_t EoE_details;
             uint8_t SoE_channels;
             uint8_t DS402_channels;
             uint8_t SysmanClass;
             uint8_t flags;
-            int16_t current_on_ebus;
+            int16_t current_on_ebus; // mA, negative means feeding current
             uint8_t group_info_id_dup;
             uint8_t reserved_B;
-            uint16_t physical_port;
+            uint16_t port_0 : 4,
+                     port_1 : 4,
+                     port_2 : 4,
+                     port_3 : 4;
             uint16_t physical_memory_address;
             uint8_t reserved_C[12];
         } __attribute__((__packed__));
@@ -344,12 +353,24 @@ namespace kickcat
             uint16_t index;
             uint8_t subindex;
         } __attribute__((__packed__));
+
+        /// ETG 1000.6
+        struct Emergency
+        {
+            uint16_t number : 9,
+                     reserved : 3,
+                     service : 4; // i.e. request, response
+
+            uint16_t error_code;
+            uint8_t  error_register;
+            uint8_t  data[5];
+        } __attribute__((__packed__));
     }
 
     namespace CoE
     {
-        constexpr uint16_t SM_COM_TYPE = 0x1C00; // each sub-entry described SM[x] com type (mailbox in/out, PDO in/out, not used)
-        constexpr uint16_t SM_CHANNEL  = 0x1C10; // each entry is associated with the mapped PDOs (if in used)
+        constexpr uint16_t SM_COM_TYPE       = 0x1C00; // each sub-entry described SM[x] com type (mailbox in/out, PDO in/out, not used)
+        constexpr uint16_t SM_CHANNEL        = 0x1C10; // each entry is associated with the mapped PDOs (if in used)
 
         enum Service
         {

src/Bus.cc

@@ -73,6 +73,16 @@ namespace kickcat
         auto error_callback = [&](){ THROW_ERROR("init error while cleaning slaves mailboxes"); };
         checkMailboxes(error_callback);
         processMessages(error_callback);
+
+        // create CoE emergency reception callback
+        for (auto& slave : slaves_)
+        {
+            if (slave.supported_mailbox & eeprom::MailboxProtocol::CoE)
+            {
+                auto emg = std::make_shared<EmergencyMessage>(slave.mailbox);
+                slave.mailbox.to_process.push_back(emg);
+            }
+        }
     }
 
 
@@ -204,9 +214,9 @@ namespace kickcat
         size_t detected_slaves = slaves_.size();
         for (size_t i = 0; i < slaves_.size(); ++i)
         {
-            auto process = [detected_slaves](DatagramHeader const*, uint8_t const*, uint16_t wkc)
+            auto process = [](DatagramHeader const*, uint8_t const*, uint16_t wkc)
             {
-                if (wkc != detected_slaves)
+                if (wkc != 1)
                 {
                     return true;
                 }

src/Mailbox.cc

@@ -53,6 +53,10 @@ namespace kickcat
                     it = to_process.erase(it);
                     return true;
                 }
+                case ProcessingResult::FINALIZE_AND_KEEP:
+                {
+                    return true;
+                }
                 default: { }
             }
         }
@@ -299,4 +303,32 @@ namespace kickcat
 
         return ProcessingResult::FINALIZE;
     }
+
+
+    EmergencyMessage::EmergencyMessage(Mailbox& mailbox)
+        : AbstractMessage(mailbox.recv_size)
+        , mailbox_{mailbox}
+    {
+
+    }
+
+    ProcessingResult EmergencyMessage::process(uint8_t const* received)
+    {
+        mailbox::Header const* header = reinterpret_cast<mailbox::Header const*>(received);
+        mailbox::Emergency const* emg = reinterpret_cast<mailbox::Emergency const*>(received + sizeof(mailbox::Header));
+
+        // check if the received message is an emergency one
+        if (header->type != mailbox::Type::CoE)
+        {
+            return ProcessingResult::NOOP;
+        }
+
+        if (emg->service != CoE::Service::EMERGENCY)
+        {
+            return ProcessingResult::NOOP;
+        }
+
+        mailbox_.emergencies.push_back(*emg);
+        return ProcessingResult::FINALIZE_AND_KEEP;
+    }
 }

src/Slave.cc

@@ -1,4 +1,5 @@
 #include "Slave.h"
+#include "Error.h"
 
 namespace kickcat
 {
@@ -90,7 +91,7 @@ namespace kickcat
                 }
                 case eeprom::Category::DataTypes:
                 {
-                    printf("DataTypes!\n");
+                    DEBUG_PRINT("DataTypes!\n");
                     break;
                 }
                 case eeprom::Category::General:
@@ -120,7 +121,7 @@ namespace kickcat
                 }
                 case eeprom::Category::DC:
                 {
-                    printf("DC!\n");
+                    DEBUG_PRINT("DC!\n");
                     break;
                 }
                 case eeprom::Category::End:

src/protocol.cc

@@ -30,6 +30,7 @@ namespace kickcat
             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";                                    }