diff --git a/test_apps/test_common/mb_utest_lib/CMakeLists.txt b/test_apps/test_common/mb_utest_lib/CMakeLists.txt
index b396969..106414b 100644
--- a/test_apps/test_common/mb_utest_lib/CMakeLists.txt
+++ b/test_apps/test_common/mb_utest_lib/CMakeLists.txt
@@ -11,6 +11,7 @@ target_include_directories(mb_ut_lib PUBLIC
                             "${dir}/modbus/mb_controller/common/include"
                             "${dir}/modbus/mb_controller/serial"
                             "${dir}/modbus/mb_controller/tcp"
+                            "${dir}/modbus/mb_objects/common"
                             "${dir}/modbus/mb_objects/include"
                             "${dir}/modbus/mb_ports/common"
                             "${dir}/modbus/mb_ports/serial"
diff --git a/tools/robot/ModbusSupport.py b/tools/robot/ModbusSupport.py
index aada9d6..1cd0eca 100644
--- a/tools/robot/ModbusSupport.py
+++ b/tools/robot/ModbusSupport.py
@@ -36,8 +36,8 @@ class ModbusMBAP(Packet):
     name = "Modbus TCP"
     fields_desc = [ ShortField("transId", 0),
                     ShortField("protoId", 0),
-                    ShortField("len", 6),
-                    XByteField("UnitId", 247),
+                    ShortField("len", 0),
+                    XByteField("unitId", 0),
                     ]
 
 # Can be used to replace all Modbus read
@@ -252,10 +252,27 @@ class ModbusPDUXX_Custom_Request(Packet):
         FieldListField("customBytes", [0x00], XByteField("", 0x00))
     ]
 
+class ModbusPDUXX_Custom_Exception(Packet):
+    name = "Custom Command Exception"
+    fields_desc = [ 
+        XByteField("funcCode", 0x00),
+        ByteEnumField("exceptCode", 1, modbus_exceptions)
+    ]
+
+# Custom command respond
+class ModbusPDUXX_Custom_Answer(Packet):
+    name = "Custom Command Answer"
+    fields_desc = [
+        ConditionalField(XByteField("funcCode", 0x00), lambda pkt: (type(pkt.underlayer) is ModbusADU_Response)),
+        ConditionalField(FieldListField("customBytes", [0x00], XByteField("", 0x00), count_from = lambda pkt: pkt.underlayer.len if pkt.underlayer is not None else 0), lambda pkt: type(pkt.underlayer) is ModbusADU_Response)
+    ]
+
 # 0x11 - Report Slave Id
 class ModbusPDU11_Report_Slave_Id(Packet):
     name = "Report Slave Id"
-    fields_desc = [ XByteField("funcCode", 0x11) ]
+    fields_desc = [ 
+        XByteField("funcCode", 0x11)
+    ]
 
 class ModbusPDU11_Report_Slave_Id_Answer(Packet):
     name = "Report Slave Id Answer"
@@ -279,7 +296,8 @@ class ModbusADU_Request(ModbusMBAP):
             XShortField("transId", 0x0000), # needs to be unique
             XShortField("protoId", 0x0000), # needs to be zero (Modbus)
             XShortField("len", None),       # is calculated with payload
-            XByteField("unitId", 0x00)]     # 0xFF or 0x00 should be used for Modbus over TCP/IP
+            XByteField("unitId", 0x00)      # 0xFF or 0x00 should be used for Modbus over TCP/IP
+    ]
 
     def mb_get_last_exception(self):
         return _mb_exception
@@ -372,7 +390,8 @@ class ModbusADU_Request(ModbusMBAP):
 class ModbusADU_Response(ModbusMBAP):
     name = "ModbusADU Response"
     _mb_exception: modbus_exceptions = 0
-
+    _current_main_packet: Packet = None
+    _modbus_pdu: Packet = None
     fields_desc = [ 
             XShortField("transId", 0x0000), # needs to be unique
             XShortField("protoId", 0x0000), # needs to be zero (Modbus)
@@ -381,6 +400,15 @@ class ModbusADU_Response(ModbusMBAP):
 
     def mb_get_last_exception(self):
         return _mb_exception
+    
+    # def extract_padding(self, s):
+    #     print(f'Extract pedding: {self, s, self.len, self.underlayer}')
+    #     return self.guess_payload_class( s) #, s #self.extract_pedding(self, s)
+
+    def pre_dissect(self, s):
+        print(f'Pre desect: {self, s, self.len, self.underlayer}')
+        _current_main_packet = self
+        return s
 
     # Dissects packets
     def guess_payload_class(self, payload):
@@ -443,11 +471,14 @@ class ModbusADU_Response(ModbusMBAP):
             return ModbusPDU10_Write_Multiple_Registers_Exception
 
         elif funcCode == 0x11:
-            print(f'Packet answer: {payload}, func: {funcCode}')
             return ModbusPDU11_Report_Slave_Id_Answer
         elif funcCode == 0x91:
             self._mb_exception = int(payload[1])
             return ModbusPDU11_Report_Slave_Id_Exception
 
         else:
-            return Packet.guess_payload_class(self, payload)
\ No newline at end of file
+            if (funcCode & 0x80):
+                self._mb_exception = int(payload[1])
+                return ModbusPDUXX_Custom_Exception
+            return ModbusPDUXX_Custom_Answer
+            #return Packet.guess_payload_class(self, payload)
\ No newline at end of file
diff --git a/tools/robot/ModbusTestLib.py b/tools/robot/ModbusTestLib.py
index 1d2415e..438d352 100644
--- a/tools/robot/ModbusTestLib.py
+++ b/tools/robot/ModbusTestLib.py
@@ -42,6 +42,8 @@ MB_LOGGING_PATH = '.'
 
 # The constructed packets for self testing
 
+TEST_PACKET_REPORT_CUSTOM_0X41 = 'ModbusADU_Request(transId=MB_DEF_TRANS_ID, unitId=0x01, protoId=0)/\
+                            ModbusPDUXX_Custom_Request(customBytes=[0x41])'
 TEST_PACKET_REPORT_SLAVE_ID_CUSTOM = 'ModbusADU_Request(transId=MB_DEF_TRANS_ID, unitId=0x01, protoId=0)/\
                             ModbusPDUXX_Custom_Request(customBytes=[0x11])'
 TEST_PACKET_REPORT_SLAVE_ID = 'ModbusADU_Request(transId=MB_DEF_TRANS_ID, unitId=0x01, protoId=0)/\
@@ -431,6 +433,15 @@ class ModbusTestLib:
     def self_test(self) -> None:
        # type: () -> None
         self.connect(ip_addr=MB_DEF_SERVER_IP, port=MB_DEF_PORT)
+        packet = self.create_request(TEST_PACKET_REPORT_CUSTOM_0X41)
+        print(f"Test: 0x41 <Custom command> packet: {packet}")
+        response = self.send_packet_and_get_response(packet, timeout=1, verbose=0)
+        assert response and len(response) > 1, "No response from slave"
+        print(f"Test: received: {bytes(response)}")
+        pdu = self.translate_response(response)
+        if pdu is not None:
+           print(f"Received: {pdu}")
+           #print(f"PDU Exception: {self.check_response(pdu, packet.customBytes[0])}")
         packet = self.create_request(TEST_PACKET_REPORT_SLAVE_ID_CUSTOM)
         print(f"Test: 0x11 <Report Slave ID> packet: {packet}")
         response = self.send_packet_and_get_response(packet, timeout=1, verbose=0)
diff --git a/tools/robot/ModbusTestSuite.resource b/tools/robot/ModbusTestSuite.resource
index af91ec5..2849da6 100644
--- a/tools/robot/ModbusTestSuite.resource
+++ b/tools/robot/ModbusTestSuite.resource
@@ -18,6 +18,11 @@ Library    Collections
 Library    ModbusTestLib.py    WITH NAME    ModbusTestLib
 
 *** Keywords ***
+Create Custom Command Request
+    [Arguments]    ${uid}    ${customData}
+    ${packet} =    Create Request    ModbusADU_Request(unitId=${uid}, protoId=0)/ModbusPDUXX_Custom_Request(customBytes=${customData})
+    RETURN    ${packet}
+
 Create Report Slave Id Request
     [Arguments]    ${uid}    ${customData}
     #${packet} =    Create Request    ModbusADU_Request(unitId=${uid}, protoId=0)/ModbusPDU11_Report_Slave_Id(funcCode=${FUNC_REPORT_SLAVE_ID})
@@ -64,6 +69,48 @@ Create Discrete Read Request
     Log    Packet: ${packet}
     RETURN    ${packet}
 
+Lists Should Be Equal
+    [Arguments]    ${get_list}    ${exp_list}
+    Should Not Be Empty    ${get_list}
+    Should Not Be Empty    ${exp_list}
+    ${get_length} =    Get length    ${get_list}
+    ${exp_length} =    Get length    ${exp_list}
+    Should Be Equal As Integers    ${get_length}    ${exp_length}
+    FOR    ${i}    IN RANGE    ${exp_length}
+        ${get_item} =    Get From List    ${get_list}    ${i}
+        ${exp_item} =    Get From List    ${exp_list}    ${i}
+        Should Be Equal As Integers    ${get_item}    ${exp_item}
+    END
+
+Custom Command
+    [Arguments]    ${uid}    ${customData}    ${exception_expected}    ${expected_list}
+    ${classId} =    Get Class Id
+    Log    Library ClassId: ${classId}
+    Log    Get Slave Identificator UID:${uid}, Custom bytes: ${customData}
+    ${req} =    Create Custom Command Request    ${uid}    ${customData}
+    #Create Connection    ${server}    ${port}
+    ${response_frame} =    Send Packet And Get Response    ${req}
+    Should Not Be Empty    ${response_frame}
+    ${packet} =    Translate Response    ${response_frame}
+    Should Be Equal As Integers    ${req.transId}    ${packet.transId}
+    ${exception}    ${exp_message} =    Check Response    ${packet}    ${req.customBytes[0]}
+    Should Be Equal As Integers    ${exception}    ${exception_expected}
+    Log    exception: (${exception}: ${exp_message}), expected: ${exception_expected}
+    IF    ${exception} == ${0}
+        Log    SlaveUID:${uid}, Custom_data received:${packet.customBytes}
+        IF    ${expected_list} != ${None}
+            Log    ${expected_list}
+            Log    ${packet.customBytes}
+            ${get_list} =    Convert To List    ${packet.customBytes}
+            ${exp_list} =    Evaluate    ${expected_list}
+            Lists Should Be Equal    ${get_list}    ${exp_list}
+        ELSE
+            Log    "Skip comparison with expected list"
+        END
+    ELSE
+        Log    "Exception is evaluated correctly (${exception}: ${exp_message}) == ${exception_expected}"
+    END
+
 Report Slave Id
     [Arguments]    ${uid}    ${customData}   ${exception_expected}
     ${classId} =    Get Class Id
diff --git a/tools/robot/ModbusTestSuite.robot b/tools/robot/ModbusTestSuite.robot
index 6579f64..f890de7 100644
--- a/tools/robot/ModbusTestSuite.robot
+++ b/tools/robot/ModbusTestSuite.robot
@@ -11,6 +11,13 @@ Suite Teardown    Disconnect
 ${suiteConnection}    None
 
 *** Test Cases ***
+Test Cusom Command Request
+    [Documentation]    Test reading slave UID, running status, identificator structure (use custom frame template)
+    [Template]    Custom Command
+    0x01    [0x41]    0    ${None}                                                              # Try o send shortest request for custom command, do not check the buffer
+    0x01    [0x41, 0x11, 0x22, 0x33, 0x44]    0    ${None}                                      # Send custom data, do not compare the response buffer
+    0x01    [0x41, 0x11, 0x22, 0x33]    0    [17, 34, 51, 0x3A, 83, 108, 97, 118, 101]          # Send the custom command and compare expected response (can use hex or dec values)
+
 Test Report Slave Id
     [Documentation]    Test reading slave UID, running status, identificator structure (use custom frame template)
     [Template]    Report Slave Id