diff --git a/Periphery.cpp b/Periphery.cpp
index 73da635..fe7199c 100644
--- a/Periphery.cpp
+++ b/Periphery.cpp
@@ -34,7 +34,11 @@ void Periphery::config(){
     _canb.configRxMBox(canSpace::DIGITAL_OUTPUT_MBOX, canSpace::MsgID(0x31), canSpace::MsgCtrlReg(0x2));   // Send DO
 
     // Remote frame MBOX
-    _canb.configTxMBox(canSpace::COMM_VERSION_MBOX, canSpace::MsgID(0x1, false, true), canSpace::MsgCtrlReg(0));
+    // _canb.configRxMBox(canSpace::MODBUS_SETTINGS_MBOX, canSpace::MsgID(0x0), canSpace::MsgCtrlReg(0x10));
+    _canb.configTxMBox(canSpace::COMM_VERSION_MBOX, canSpace::MsgID(0x1, false, true), canSpace::MsgCtrlReg(0x4));
+
+    _canb.enableTimeOutControl(canSpace::MODBUS_SETTINGS_MBOX);
+    _canb.setTimeOutValue(canSpace::MODBUS_SETTINGS_MBOX, 1000);
 
     // Interrupts
     _canb.configSystemIsr(canSpace::I0EN_ENABLE |
@@ -84,23 +88,7 @@ void Periphery::updateVersionFPGA(){
 }
 
 
-void Periphery::processDigitalInput(){
-    Uint16 data = 0;
-    _digitalIO.readDigitalIO(data);
-    _canb.transmitMsg(canSpace::DIGITAL_INPUT_MBOX, (data));
-}
-
-
-void Periphery::processDigitalOutput(){
-    if(_canb.isNewMessage(canSpace::DIGITAL_OUTPUT_MBOX)){
-        _canb.receiveMsg(canSpace::DIGITAL_OUTPUT_MBOX, _message);
-        Uint16 data = _message.mdl.word.LOW_WORD;
-        _digitalIO.writeDigitalIO(data);
-    }
-}
-
-
-void Periphery::init_modbus_table(){
+void Periphery::initModbusTable(){
     modbusRTU.setInputRegsAddr(400);
     modbusRTU.setOutputRegsAddr(400);
 
@@ -118,13 +106,8 @@ void Periphery::init_modbus_table(){
 }
 
 
-bool Periphery::isModbusInit() const{
-    return _modbusInitFlag;
-}
-
-
 void Periphery::receiveModbusParameters(){
-    if (!_modbusInitFlag && (_canb.isNewMessage(canSpace::MODBUS_SETTINGS_MBOX))) { // TODO check for init is incorect here maybe
+    if (_canb.isNewMessage(canSpace::MODBUS_SETTINGS_MBOX)) {
         _canb.receiveMsg(canSpace::MODBUS_SETTINGS_MBOX, _message);
 
         MODBUSRTU::modbusConfiguration.node_id = _message.mdl.byte.BYTE0;
@@ -134,13 +117,65 @@ void Periphery::receiveModbusParameters(){
         MODBUSRTU::modbusConfiguration.config.stopbits = static_cast<DSP28335::SCIStopBits>(_message.mdh.byte.BYTE5);
 
         modbusRTU.configure(MODBUSRTU::modbusConfiguration);
-
-        _modbusInitFlag = true; // TODO modbus init function must be here
         _modbusRegCounter = 0;
     }
 }
 
 
+void Periphery::getModbusConfiguration(){
+    _canb.resetTimeStampCounter();
+    _canb.clearTimeOutFlag(canSpace::MODBUS_SETTINGS_MBOX);
+    bool requestIsSent = false;
+
+    while(!_canb.isNewMessage(canSpace::MODBUS_SETTINGS_MBOX)){
+        if(_canb.isTimeOut(canSpace::MODBUS_SETTINGS_MBOX)){
+            if (!requestIsSent) {
+                _canb.resetTimeStampCounter();
+                _canb.clearTimeOutFlag(canSpace::MODBUS_SETTINGS_MBOX);
+
+                _message.mdl.word.LOW_WORD = _softVersion.DSP;
+                _message.mdl.word.HI_WORD = _softVersion.CPLD;
+                _canb.transmitMsg(canSpace::COMM_VERSION_MBOX, _message);
+
+                requestIsSent = true;
+            }
+            else{
+                _canb.clearTimeOutFlag(canSpace::MODBUS_SETTINGS_MBOX);
+                _canb.disableTimeOutControl(canSpace::MODBUS_SETTINGS_MBOX);
+                _error = true;
+                return;
+            }
+        }
+    }
+
+    _canb.receiveMsg(canSpace::MODBUS_SETTINGS_MBOX, _message);
+    MODBUSRTU::modbusConfiguration.node_id = _message.mdl.byte.BYTE0;
+    MODBUSRTU::modbusConfiguration.config.lenght = static_cast<DSP28335::SCICharLenght>(_message.mdl.byte.BYTE1);
+    MODBUSRTU::modbusConfiguration.config.baudrate = static_cast<DSP28335::SCIBaudRate>(_message.mdl.word.LOW_WORD);
+    MODBUSRTU::modbusConfiguration.config.parity = static_cast<DSP28335::SCIParity>(_message.mdh.byte.BYTE4);
+    MODBUSRTU::modbusConfiguration.config.stopbits = static_cast<DSP28335::SCIStopBits>(_message.mdh.byte.BYTE5);
+
+    modbusRTU.configure(MODBUSRTU::modbusConfiguration);
+    _modbusRegCounter = 0;
+}
+
+
+void Periphery::processDigitalInput(){
+    Uint16 data = 0;
+    _digitalIO.readDigitalIO(data);
+    _canb.transmitMsg(canSpace::DIGITAL_INPUT_MBOX, (data));
+}
+
+
+void Periphery::processDigitalOutput(){
+    if(_canb.isNewMessage(canSpace::DIGITAL_OUTPUT_MBOX)){
+        _canb.receiveMsg(canSpace::DIGITAL_OUTPUT_MBOX, _message);
+        Uint16 data = _message.mdl.word.LOW_WORD;
+        _digitalIO.writeDigitalIO(data);
+    }
+}
+
+
 void Periphery::sendModbusDataToCPU(){
     _message.mdl.all = 0;   // TODO delete maybe?
     _message.mdh.all = 0;
diff --git a/Periphery.h b/Periphery.h
index 33e0c34..afea853 100644
--- a/Periphery.h
+++ b/Periphery.h
@@ -1,6 +1,5 @@
 #pragma once
 
-// #include <cstdint>
 #include <stdint.h>
 
 #include "DSP28x_Project.h"
@@ -28,12 +27,13 @@ public:
     Uint16 getVersionFPGA();
     void updateVersionFPGA();
 
+    void receiveModbusParameters();
+    void getModbusConfiguration();
+    void initModbusTable(); // TODO Must be outside periphery!
+
     void processDigitalInput();
     void processDigitalOutput();
 
-    void init_modbus_table();
-    bool isModbusInit() const;
-    void receiveModbusParameters();
     void sendModbusDataToCPU();
     void receiveCpuModbusData();
 
@@ -47,6 +47,7 @@ private:
     SoftwareVersion _softVersion;
     canSpace::CANMessage _message;
     bool _modbusInitFlag;
+    bool _error;
     uint16_t _modbusRegCounter;
 
     float test_hmi_float_reg_400_test;
diff --git a/Protocol/CAN.cpp b/Protocol/CAN.cpp
index a3c3b18..7c68c06 100644
--- a/Protocol/CAN.cpp
+++ b/Protocol/CAN.cpp
@@ -253,7 +253,7 @@ bool CAN::isNewMessage(Uint16 boxNumber){
 
 void CAN::resetTimeStampCounter(){
     EALLOW;
-    ECanbRegs.CANTSC = 0;
+    p_CanRegs_->CANTSC = 0;
     EDIS;
 }
 
diff --git a/main.cpp b/main.cpp
index a622e58..55bd288 100644
--- a/main.cpp
+++ b/main.cpp
@@ -77,13 +77,14 @@ void main()
     PieCtrlRegs.PIEIER9.bit.INTx8 = 1;
 
     periphery.config();
-    periphery.init_modbus_table();
+    periphery.updateVersionFPGA();
+    periphery.getModbusConfiguration();
+    periphery.initModbusTable();
 
     // Enable global Interrupts and higher priority real-time debug events:
     EINT;   // Enable Global interrupt INTM
     ERTM;   // Enable Global realtime interrupt DBGM
 
-    periphery.updateVersionFPGA();
 
     CpuTimer0.RegsAddr->TCR.bit.TSS = 0;
     CpuTimer1.RegsAddr->TCR.bit.TSS = 0;
@@ -100,17 +101,12 @@ void idle_loop()
     {
         infCounter++;
 
-        if (!periphery.isModbusInit()){
-            periphery.receiveModbusParameters();
-            // modbus_port.configure(modbus_port_configuration);
-        }
+        periphery.receiveModbusParameters();
 
         //
         // MODBUS RTU HMI Service
         //
-        if (periphery.isModbusInit()){
-            periphery.modbusRTU.execute();
-        }
+        periphery.modbusRTU.execute();
 
         periphery.receiveCpuModbusData();