diff --git a/can/can_client.c b/can/can_client.c
index 14289dc..2e86f4a 100755
--- a/can/can_client.c
+++ b/can/can_client.c
@@ -1,4 +1,6 @@
 
+#include "main.h"
+
 #include <can/can_client.h>
 #include <io/io.h>
 #include <mqtt/mqtt_client.h>
@@ -6,7 +8,10 @@
 struct MOTOR_CONTROL_DATA motctrl[MOTOR_COUNT];
 struct CAN_INTERFACE_DATA intf_data[MOTOR_COUNT];
 
+
 int iBusTimeoutCounter = 0;
+/// @brief Increase counter for cycles without received telegram
+/// @param iMotorIndex 
 void IncBusTimeoutCounter(int iMotorIndex)
 {
     if (iBusTimeoutCounter < 2000)
@@ -14,11 +19,17 @@ void IncBusTimeoutCounter(int iMotorIndex)
         iBusTimeoutCounter++;
         if (iBusTimeoutCounter >= 2000)
         {
+            // long time no telegram received -> motor is not connected anymore
             motctrl[iMotorIndex].nSwitchState = 0;
+            motctrl[iMotorIndex].nDriveConnected = 0;
+            motctrl[iMotorIndex].nDriveReady = 0;
+            Can_SetMotorGear(iMotorIndex, 0);
             MqttClient_Publish_MotorSwitchState(iMotorIndex, motctrl[iMotorIndex].nSwitchState);
         }
     }
 }
+
+
 /// @brief Open socket of CAN interface for the given motor
 /// @param iMotorIndex 
 /// @param ifacename 
@@ -26,12 +37,16 @@ void IncBusTimeoutCounter(int iMotorIndex)
 int Can_OpenInterface(int iMotorIndex, const char * ifacename)
 {
     // Init control data
+    motctrl[iMotorIndex].nDriveConnected = 0;
+    motctrl[iMotorIndex].nDriveReady = 0;
     motctrl[iMotorIndex].iActualMotorPowerW = 0;
     motctrl[iMotorIndex].iMotorGear = MOTOR_GEAR_NEUTRAL;
     motctrl[iMotorIndex].iMotorPower = 0;
     motctrl[iMotorIndex].iMotorPowerPct = 0;
     motctrl[iMotorIndex].nSwitchState = 0;
 
+    mylog(LOG_INFO, "CAN: PWR_MIN_PCT=%d PWR_MAX_PCT=%d PWR_STEP=%d", MOTOR_PWR_MIN_PCT, MOTOR_PWR_MAX_PCT, MOTOR_PWR_STEP);
+    
     strcpy(intf_data[iMotorIndex].iface_name, ifacename);
     Can_SetMotorGear(iMotorIndex, MOTOR_GEAR_NEUTRAL);
     Can_SetMotorPower(iMotorIndex, 0);
@@ -39,7 +54,7 @@ int Can_OpenInterface(int iMotorIndex, const char * ifacename)
     // first we have to create a socket
     if ((intf_data[iMotorIndex].socket = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0)
     {
-        printf("Could not create socket for motor %d!\n", iMotorIndex);
+        mylog(LOG_ERR, "CAN: Could not create socket for motor %d!", iMotorIndex);
         return 1;
     }
 
@@ -47,7 +62,7 @@ int Can_OpenInterface(int iMotorIndex, const char * ifacename)
     strcpy(intf_data[iMotorIndex].ifr.ifr_name, intf_data[iMotorIndex].iface_name);
     if (ioctl(intf_data[iMotorIndex].socket, SIOCGIFINDEX, &intf_data[iMotorIndex].ifr) < 0)
     {
-        printf("Could not get interface index for motor %d!\n", iMotorIndex);
+        mylog(LOG_ERR, "CAN: Could not get interface index for motor %d!", iMotorIndex);
         return 2;
     }
 
@@ -57,25 +72,25 @@ int Can_OpenInterface(int iMotorIndex, const char * ifacename)
     intf_data[iMotorIndex].addr.can_ifindex = intf_data[iMotorIndex].ifr.ifr_ifindex;
     if (bind(intf_data[iMotorIndex].socket, (struct sockaddr *)&intf_data[iMotorIndex].addr, sizeof(intf_data[iMotorIndex].addr)) < 0)
     {
-        printf("Could not bind socket to inteface for motor %d!\n", iMotorIndex);
+        mylog(LOG_ERR, "CAN: Could not bind socket to inteface for motor %d!", iMotorIndex);
         return 3;
     }
 
-    // socket auf nicht-blockierend umstellen
+    // make socket to nonblocking
     int fcntl_flags = fcntl(intf_data[iMotorIndex].socket, F_GETFL, 0);
     if (fcntl_flags < 0)
     {
-        printf("Could not get file descriptor flags!\n");
+        mylog(LOG_ERR, "CAN: Could not get file descriptor flags!");
         return 4;
     }
     fcntl_flags |= O_NONBLOCK;
     if (fcntl(intf_data[iMotorIndex].socket, F_SETFL, fcntl_flags) < 0)
     {
-        printf("Could not set file descriptor flags (set socket none-blocking)!\n");
+        mylog(LOG_ERR, "CAN: Could not set file descriptor flags (set socket none-blocking)!");
         return 5;
     }
 
-    printf("Interface %s (motor %d) opened!\n", ifacename, iMotorIndex);
+    mylog(LOG_INFO, "CAN: Interface %s (motor %d) opened!", ifacename, iMotorIndex);
 
     return 0;
 }
@@ -87,11 +102,11 @@ void Can_CloseInterface(int iMotorIndex)
 {
     if (close(intf_data[iMotorIndex].socket) < 0)
     {
-        printf("Could not close socket of motor %d!\n", iMotorIndex);
+        mylog(LOG_ERR, "CAN: Could not close socket of motor %d!", iMotorIndex);
     }
     else
     {
-        printf("Interface %s (motor %d) closed.\n", intf_data[iMotorIndex].iface_name, iMotorIndex);
+        mylog(LOG_INFO, "CAN: Interface %s (motor %d) closed.", intf_data[iMotorIndex].iface_name, iMotorIndex);
     }
 }
 
@@ -101,25 +116,29 @@ void Can_CloseInterface(int iMotorIndex)
 /// @param iGear (-1=reverse, 0=neutral, 1=forward)
 void Can_SetMotorGear(int iMotorIndex, int iGear)
 {
-    if (iGear > 0)
+    if ((iGear > 0) && (motctrl[iMotorIndex].nDriveReady != 0))
     {
         if (motctrl[iMotorIndex].iMotorGear != MOTOR_GEAR_FORWARD)
         {
             MqttClient_Publish_MotorGear(iMotorIndex, iGear);
             motctrl[iMotorIndex].iMotorGear = MOTOR_GEAR_FORWARD;
+            // motor is switched to forward -> set min. power
+            Can_SetMotorPower(iMotorIndex, MOTOR_PWR_MIN_PCT);
         }
         WriteOutputPin(GPIO_LED_MOTRUN, HIGH);
-        printf("Motor[%d]: Set gear forward.\n", iMotorIndex);
+        mylog(LOG_INFO, "CAN: Motor[%d]: Set gear forward.", iMotorIndex);
     } 
-    else if (iGear < 0)
+    else if ((iGear < 0) && (motctrl[iMotorIndex].nDriveReady != 0))
     {
         if (motctrl[iMotorIndex].iMotorGear != MOTOR_GEAR_REVERSE)
         {
             MqttClient_Publish_MotorGear(iMotorIndex, iGear);
             motctrl[iMotorIndex].iMotorGear = MOTOR_GEAR_REVERSE;
+            // motor is switched to reverse -> set min. power
+            Can_SetMotorPower(iMotorIndex, MOTOR_PWR_MIN_PCT);
         }
         WriteOutputPin(GPIO_LED_MOTRUN, HIGH);
-        printf("Motor[%d]: Set gear reverse.\n", iMotorIndex);
+        mylog(LOG_INFO, "CAN: Motor[%d]: Set gear reverse.", iMotorIndex);
     }
     else 
     {
@@ -128,9 +147,10 @@ void Can_SetMotorGear(int iMotorIndex, int iGear)
             MqttClient_Publish_MotorGear(iMotorIndex, iGear);
             motctrl[iMotorIndex].iMotorGear = MOTOR_GEAR_NEUTRAL;
         }
-        Can_SetMotorPower(iMotorIndex, MOTOR_PWR_MIN_PCT);
+        // motor is switch to neutral -> set power to 0
+        Can_SetMotorPower(iMotorIndex, 0);
         WriteOutputPin(GPIO_LED_MOTRUN, LOW);
-        printf("Motor[%d]: Set gear neutral.\n", iMotorIndex);
+        mylog(LOG_INFO, "CAN: Motor[%d]: Set gear neutral.", iMotorIndex);
     } 
 }
 
@@ -140,12 +160,19 @@ void Can_SetMotorGear(int iMotorIndex, int iGear)
 /// @param iPower (Range: 0..100)
 void Can_SetMotorPower(int iMotorIndex, int iPower)
 {
-    if (iPower <= MOTOR_PWR_MIN_PCT)
+    if ((motctrl[iMotorIndex].iMotorGear == MOTOR_GEAR_NEUTRAL) || (motctrl[iMotorIndex].nDriveReady == 0))
     {
+        // when motor is neutral or not ready set power to 0
+        motctrl[iMotorIndex].iMotorPowerPct = 0;
+    }
+    else if (iPower <= MOTOR_PWR_MIN_PCT)
+    {
+        // limit to min. power
         motctrl[iMotorIndex].iMotorPowerPct = MOTOR_PWR_MIN_PCT;
     }
     else if (iPower >= MOTOR_PWR_MAX_PCT)
     {
+        // limit to max. power
         motctrl[iMotorIndex].iMotorPowerPct = MOTOR_PWR_MAX_PCT;
     } 
     else 
@@ -154,9 +181,10 @@ void Can_SetMotorPower(int iMotorIndex, int iPower)
     } 
     MqttClient_Publish_MotorPower(iMotorIndex, motctrl[iMotorIndex].iMotorPowerPct);
 
+    // calc value for telegram
     motctrl[iMotorIndex].iMotorPower = 250 * motctrl[iMotorIndex].iMotorPowerPct / 100;
 
-    printf("Motor[%d]: Set power to %d%% -> %d\n",
+    mylog(LOG_INFO, "CAN: Motor[%d]: Set power to %d%% -> %d",
         iMotorIndex, motctrl[iMotorIndex].iMotorPowerPct, motctrl[iMotorIndex].iMotorPower);
 }
 
@@ -165,27 +193,24 @@ void Can_SetMotorPower(int iMotorIndex, int iPower)
 /// @param iMotorIndex 
 void Can_TransmitMotorGear(int iMotorIndex)
 {
-    //if (motctrl[iMotorIndex].nSwitchState > 0)
+    // Transmission rate: 100ms
+    struct can_frame frame;
+
+    frame.can_id = 0x18F005D0;
+    frame.can_id |= CAN_EFF_FLAG;
+    frame.can_dlc = 8;
+    frame.data[0] = motctrl[iMotorIndex].iMotorGear;
+    frame.data[1] = 0xFF;
+    frame.data[2] = 0xFF;
+    frame.data[3] = 0xFF;
+    frame.data[4] = 0xFF;
+    frame.data[5] = 0xFF;
+    frame.data[6] = 0xFF;
+    frame.data[7] = 0xFF;
+
+    if (write(intf_data[iMotorIndex].socket, &frame, sizeof(frame)) != sizeof(frame))
     {
-        // Transmission rate: 100ms
-        struct can_frame frame;
 
-        frame.can_id = 0x18F005D0;
-        frame.can_id |= CAN_EFF_FLAG;
-        frame.can_dlc = 8;
-        frame.data[0] = motctrl[iMotorIndex].iMotorGear;
-        frame.data[1] = 0xFF;
-        frame.data[2] = 0xFF;
-        frame.data[3] = 0xFF;
-        frame.data[4] = 0xFF;
-        frame.data[5] = 0xFF;
-        frame.data[6] = 0xFF;
-        frame.data[7] = 0xFF;
-
-        if (write(intf_data[iMotorIndex].socket, &frame, sizeof(frame)) != sizeof(frame))
-        {
-
-        }
     }
 }
 
@@ -194,27 +219,24 @@ void Can_TransmitMotorGear(int iMotorIndex)
 /// @param iMotorIndex 
 void Can_TransmitMotorPower(int iMotorIndex)
 {
-    //if (motctrl[iMotorIndex].nSwitchState > 0)
+    // Transmission rate: 50ms
+    struct can_frame frame;
+
+    frame.can_id = 0x0CF003D0;
+    frame.can_id |= CAN_EFF_FLAG;
+    frame.can_dlc = 8;
+    frame.data[0] = 0xFF;
+    frame.data[1] = motctrl[iMotorIndex].iMotorPower;   // motor power 0 = 0%, 250 = 100%
+    frame.data[2] = 0xFF;
+    frame.data[3] = 0xFF;
+    frame.data[4] = 0xFF;
+    frame.data[5] = 0xFF;
+    frame.data[6] = 0xFF;
+    frame.data[7] = 0xFF;
+
+    if (write(intf_data[iMotorIndex].socket, &frame, sizeof(frame)) != sizeof(frame))
     {
-        // Transmission rate: 50ms
-        struct can_frame frame;
 
-        frame.can_id = 0x0CF003D0;
-        frame.can_id |= CAN_EFF_FLAG;
-        frame.can_dlc = 8;
-        frame.data[0] = 0xFF;
-        frame.data[1] = motctrl[iMotorIndex].iMotorPower;   // motor power 0 = 0%, 250 = 100%
-        frame.data[2] = 0xFF;
-        frame.data[3] = 0xFF;
-        frame.data[4] = 0xFF;
-        frame.data[5] = 0xFF;
-        frame.data[6] = 0xFF;
-        frame.data[7] = 0xFF;
-
-        if (write(intf_data[iMotorIndex].socket, &frame, sizeof(frame)) != sizeof(frame))
-        {
-
-        }
     }
 }
 
@@ -226,30 +248,34 @@ void Can_ReadData(int iMotorIndex)
     ssize_t nbytes = 0;
     struct can_frame frame;
 
+    // increment cycle counter
     IncBusTimeoutCounter(iMotorIndex);
 
-    // wir wollen immer alle verfügbaren Frames lesen
-    while ((nbytes = read(intf_data[iMotorIndex].socket, &frame, sizeof(frame))) > 0)
+    // read one frame
+    if ((nbytes = read(intf_data[iMotorIndex].socket, &frame, sizeof(frame))) > 0)
     {
         canid_t pgn = frame.can_id & 0x00FFFF00;
 
         switch(pgn)
         {
+            case 0x00EF6400:    // "repeat data"
+                break;
+
             case 0x00F00300:    // PGN 61443 "Electronic Engine Controller 2"
-                // haben wir selber gesendet -> ignorieren
+                // we have sent this -> ignore
                 break;
 
             case 0x00F00500:    // PGN 61445 "Electronic Transmission Controller 2"
-                // haben wir selber gesendet -> ignorieren
+                // we have sent this -> ignore
                 break;
 
             case 0x00FF1300:    // PGN 65299 "Manufacturer PGN"
-                // hier finden wir die Zustände der Schalter
+                // here we find the states of the switches
                 Can_Read_Manu_PGN(iMotorIndex, &frame);
                 break;
 
             case 0x00FF1400:    // PGN 65300 "Manufacturer PGN 2"
-                // hier bekommen wir die Leistung des Motors angezeigt
+                // here we find the actual power of the motor
                 Can_Read_Manu_PGN2(iMotorIndex, &frame);
                 break;
         }
@@ -260,9 +286,16 @@ void Can_ReadData(int iMotorIndex)
 /// @param frame 
 void Can_Read_Manu_PGN(int iMotorIndex, struct can_frame *frame)
 {
+    // we received a valid telegram -> set timeout counter to zero
     iBusTimeoutCounter = 0;
+
+    // get switch states
     motctrl[iMotorIndex].nSwitchState = frame->data[4];
     MqttClient_Publish_MotorSwitchState(iMotorIndex, motctrl[iMotorIndex].nSwitchState);
+
+    // set flags
+    motctrl[iMotorIndex].nDriveConnected = 1;       // we received a PGN -> so we are connected
+    motctrl[iMotorIndex].nDriveReady = ((motctrl[iMotorIndex].nSwitchState & 0x80) != 0) ? 1 : 0;   // this bit shows if the drive is ready to run
 }
 
 /// @brief Read PGN 65300
diff --git a/can/can_client.h b/can/can_client.h
index c324312..7a36a7c 100755
--- a/can/can_client.h
+++ b/can/can_client.h
@@ -5,6 +5,7 @@
 #include <stdlib.h>
 #include <string.h>
 #include <unistd.h>
+#include <syslog.h>
 
 #include <fcntl.h>
 #include <net/if.h>
@@ -23,10 +24,12 @@
 
 #define MOTOR_PWR_MIN_PCT 15
 #define MOTOR_PWR_MAX_PCT 100
-#define MOTOR_PWR_STEP 12
+#define MOTOR_PWR_STEP ((MOTOR_PWR_MAX_PCT - MOTOR_PWR_MIN_PCT) / 7)
 
 struct MOTOR_CONTROL_DATA
 {
+    char nDriveConnected;
+    char nDriveReady;
     int iMotorGear;
     int iMotorPower;
     int iMotorPowerPct;
diff --git a/io/io.c b/io/io.c
index 2ea7569..875548d 100755
--- a/io/io.c
+++ b/io/io.c
@@ -1,4 +1,5 @@
 
+#include "main.h"
 #include "io.h"
 #include <can/can_client.h>
 
@@ -6,29 +7,35 @@ struct GPIO_KEY_DATA gpioKeyStop;
 struct GPIO_KEY_DATA gpioKeyPwrUp;
 struct GPIO_KEY_DATA gpioKeyPwrDown;
 
+char nInitialized = 0;
+
 int IO_Init()
 {
     if (wiringPiSetupPinType(WPI_PIN_BCM))
     {
-        printf("IO: Set up wiringPi failed!\n");
+        mylog(LOG_ERR, "IO: Set up wiringPi failed!");
         return 1;
     }
+    nInitialized = 1;
 
-    // IO-Pins für Tasten konfigurieren
+    // config IO-pins for the keys
     SetupKeyPin(&gpioKeyStop, GPIO_KEY_STOP);
     SetupKeyPin(&gpioKeyPwrUp, GPIO_KEY_PWRUP);
     SetupKeyPin(&gpioKeyPwrDown, GPIO_KEY_PWRDOWN);
 
-    // IO-Pins für Ausgänge konfigurieren
+    // config IO-pins for outputs
     SetupOutputPin(GPIO_LED_MOTRUN);
     SetupOutputPin(GPIO_OUT_PWRON);
 
-    printf("IO initialized successfull!\n");
-
+    mylog(LOG_INFO, "IO: Initialized successfull!");
+    
     return 0;
 }
 
 
+/// @brief Setup a pin for a key input
+/// @param pdata 
+/// @param iKeyPin 
 void SetupKeyPin(struct GPIO_KEY_DATA *pdata, int iKeyPin)
 {
     pdata->iKeyPin = iKeyPin;
@@ -40,24 +47,26 @@ void SetupKeyPin(struct GPIO_KEY_DATA *pdata, int iKeyPin)
     pdata->iKeyPressedCycleCounter = 0;
     pdata->iKeyRepeatCycleCounter = 0;
 
-    // Wenn Eingang verwendet wird
-    if (pdata->iKeyPin > 0)
+    if ((pdata->iKeyPin > 0) && nInitialized)
     {
+        //mylog(LOG_INFO, "IO: Config Pin %d as input", iOutPin);
         pinMode(pdata->iKeyPin, INPUT);
         pullUpDnControl(pdata->iKeyPin, PUD_UP);
     }
 }
 
 
+/// @brief Read a key input
+/// @param pdata 
 void ReadKey(struct GPIO_KEY_DATA *pdata)
 {
     if (pdata->iKeyPin > 0)
     {
         int newval = pdata->iKeyValue;
 
-        if (digitalRead(pdata->iKeyPin) == LOW)     // invertierte Logik weil wir PullUp-Widerstand bei Betätigung auf low ziehen
+        if (digitalRead(pdata->iKeyPin) == LOW)     // we use pull-up resistors so we have inverted logic
         {
-            // Signal liegt an
+            // key is pressed
             if (pdata->nLowCycleCounter > 0)
             {
                 pdata->nLowCycleCounter--;
@@ -68,14 +77,14 @@ void ReadKey(struct GPIO_KEY_DATA *pdata)
                 pdata->nHighCycleCounter++;
                 if (pdata->nHighCycleCounter >= KEY_RISING_FILTERCYCLES)
                 {
-                    // gewünschte Anzahl Zyklen stabil
+                    // key is stable pressed
                     newval = 1;
                 }
             }
         }
         else
         {
-            // Signal liegt nicht an
+            // key is not pressed
             if (pdata->nHighCycleCounter > 0)
             {
                 pdata->nHighCycleCounter--;
@@ -86,7 +95,7 @@ void ReadKey(struct GPIO_KEY_DATA *pdata)
                 pdata->nLowCycleCounter++;
                 if (pdata->nLowCycleCounter >= KEY_FALLING_FILTERCYCLES)
                 {
-                    // gewünschte Anzahl Zyklen stabil
+                    // key is stable not pressed
                     newval = 0;
                 }
             }
@@ -94,7 +103,7 @@ void ReadKey(struct GPIO_KEY_DATA *pdata)
 
         if (newval && !pdata->iKeyValue)
         {
-            // Taster wurde betätigt
+            // key was pressed -> rising edge
             pdata->iKeyRisingEdge = 1;
             pdata->iKeyValue = newval;
             pdata->iKeyPressedCycleCounter = 0;
@@ -102,32 +111,32 @@ void ReadKey(struct GPIO_KEY_DATA *pdata)
         }
         else if (pdata->iKeyValue && !newval)
         {
-            // Taster wurde losgelassen
+            // key was released -> falling edge
             pdata->iKeyFallingEdge = 1;
             pdata->iKeyValue = newval;
         }
         else
         {
-            // Keine Änderung
+            // no change
             pdata->iKeyRisingEdge = 0;
             pdata->iKeyFallingEdge = 0;
 
             if (pdata->iKeyValue)
             {
-                // Wenn Taste gedrückt ist
+                // when key is pressed
                 if (pdata->iKeyPressedCycleCounter < KEY_START_REPEAT_CYCLECOUNT)
                 {
-                    // Zyklen zählen
+                    // count cycles
                     pdata->iKeyPressedCycleCounter++;
                 }
 
                 if (pdata->iKeyPressedCycleCounter >= KEY_START_REPEAT_CYCLECOUNT)
                 {
-                    // Wenn Taste länger als KEY_START_REPEAT_CYCLECOUNT gedrückt ist
+                    // when key is pressed for more then KEY_START_REPEAT_CYCLECOUNT cycles
                     pdata->iKeyRepeatCycleCounter++;
                     if (pdata->iKeyRepeatCycleCounter >= KEY_REPEAT_CYCLECOUNT)
                     {
-                        // alle KEY_REPEAT_CYCLECOUNT Zyklen Tastendruck signalisieren
+                        // signal key press every KEY_REPEAT_CYCLECOUNT cycles
                         pdata->iKeyRisingEdge = 1;
                         pdata->iKeyRepeatCycleCounter = 0;
                     }
@@ -137,24 +146,31 @@ void ReadKey(struct GPIO_KEY_DATA *pdata)
     }
 }
 
-
+/// @brief Config pin for output
+/// @param iOutPin 
 void SetupOutputPin(int iOutPin)
 {
-    if (iOutPin > 0)
+    if ((iOutPin > 0) && nInitialized)
     {
+        //mylog(LOG_INFO, "IO: Config Pin %d as output", iOutPin);
         pinMode(iOutPin, OUTPUT);
         digitalWrite(iOutPin, LOW);
     }
 }
 
+/// @brief Write an output pin to HIGH or LOW
+/// @param iOutPin 
+/// @param iValue 
 void WriteOutputPin(int iOutPin, int iValue)
 {
-    if (iOutPin > 0)
+    if ((iOutPin > 0) && nInitialized)
     {
         digitalWrite(iOutPin, iValue);
     }
 }
 
+
+/// @brief look cyclic for the keys
 void IO_DoCyclic()
 {
     ReadKey(&gpioKeyStop);
@@ -163,37 +179,42 @@ void IO_DoCyclic()
 
     if (gpioKeyStop.iKeyValue)
     {
-        // Stop-Taste betätigt -> hat Vorrang vor den anderen Tasten
+        // stop key is pressed
         if (gpioKeyStop.iKeyRisingEdge)
         {
+            mylog(LOG_INFO, "IO: KEY-Stop: Stop motor.");
             Can_SetMotorGear(0, 0);
         }
     }
-    else
+    else if (motctrl[0].nDriveReady)    // plus and minus keys only when drive is ready
     {
         if (gpioKeyPwrUp.iKeyRisingEdge)
         {
-            // Leistung erhöhen
+            // plus key is pressed -> increase power
             if (motctrl[0].iMotorGear == MOTOR_GEAR_NEUTRAL)
             {
+                mylog(LOG_INFO, "KEY-Plus: Start motor.");
                 Can_SetMotorGear(0, 1);
                 Can_SetMotorPower(0, MOTOR_PWR_MIN_PCT);
             }
             else
             {
+                mylog(LOG_INFO, "KEY-Plus: Increase power.");
                 Can_SetMotorPower(0, motctrl[0].iMotorPowerPct + MOTOR_PWR_STEP);
             }
         }
 
         if (gpioKeyPwrDown.iKeyRisingEdge)
         {
-            // Leistung verringern
+            // minus key is pressed -> decrease power
             if (motctrl[0].iMotorPowerPct > MOTOR_PWR_MIN_PCT)
             {
+                mylog(LOG_INFO, "KEY-Minus: Decrease power.");
                 Can_SetMotorPower(0, motctrl[0].iMotorPowerPct - MOTOR_PWR_STEP);
             }
             else
             {
+                mylog(LOG_INFO, "KEY-Minus: Stop motor.");
                 Can_SetMotorGear(0, 0);
             }
         }
diff --git a/io/io.h b/io/io.h
index c420ab6..9893013 100755
--- a/io/io.h
+++ b/io/io.h
@@ -3,20 +3,21 @@
 
 #include <wiringPi.h>
 #include <stdio.h>
+#include <syslog.h>
 
-#define GPIO_LED_MOTRUN 17      // GPIO Pin fuer LED Motor läuft
-#define GPIO_OUT_PWRON 22       // GPIO Pin für Relais "Zündschlüssel"
+#define GPIO_LED_MOTRUN 17      // GPIO Pin for LED motor is running
+#define GPIO_OUT_PWRON 22       // GPIO Pin for output "Ignition Key"
 
-#define GPIO_KEY_STOP 26        // GPIO Pin fuer Taster Stop
-#define GPIO_KEY_PWRUP 5        // GPIO Pin fuer Taster Leistung-Erhöhen
-#define GPIO_KEY_PWRDOWN 6      // GPIO Pin fuer Taster Leistung-Verringern 
+#define GPIO_KEY_STOP 26        // GPIO Pin for Key "Stop"
+#define GPIO_KEY_PWRUP 5        // GPIO Pin for Key "Increase Power"
+#define GPIO_KEY_PWRDOWN 6      // GPIO Pin for Key "Decrease Power"
 
-#define KEY_RISING_FILTERCYCLES 5           // Filterwert für Eingänge steigende Flanke
-#define KEY_FALLING_FILTERCYCLES 15         // Filterwert für Eingänge (Zyklen-Zähler)
-#define KEY_START_REPEAT_CYCLECOUNT 50      // Anzahl Zyklen, nach denen Wiederholungen beginnen
-#define KEY_REPEAT_CYCLECOUNT 50            // Anzahl Zyklen, nach den wiederholt wird
+#define KEY_RISING_FILTERCYCLES 5           // filter value for input rising edge
+#define KEY_FALLING_FILTERCYCLES 15         // filter value for input falling edge
+#define KEY_START_REPEAT_CYCLECOUNT 50      // number of cycles when to start repeating key presses
+#define KEY_REPEAT_CYCLECOUNT 50            // number of cycles how often to repeat key presses
 
-// Datenstruktur für einen Taster
+// data structure for a key
 struct GPIO_KEY_DATA
 {
     int iKeyPin;
diff --git a/main.c b/main.c
index 032be45..a4d6800 100755
--- a/main.c
+++ b/main.c
@@ -7,6 +7,38 @@
 // Period info of the realtime task
 struct period_info pinfo;
 int iThreadControl = 0;         // 0: thread is running, <0: thread shall exit, >0 thread has exited
+int iLogToConsole = 1;
+
+
+/// @brief send a log message
+/// @param prio 
+/// @param format 
+/// @param  
+void mylog(int prio, const char *format, ...)
+{
+va_list args;
+
+    // 1. Initialisiere die Argumentenliste mit dem letzten festen Parameter
+    va_start(args, format);
+
+    // 2. Übergabe an vsyslog (statt syslog)
+    // vsyslog nimmt eine va_list entgegen
+    vsyslog(prio, format, args);
+
+    // 3. Optional: Zusätzlich auf die Konsole ausgeben
+    // Wir müssen die Liste neu initialisieren, da va_list "verbraucht" wird
+    if (iLogToConsole)
+    {
+        va_end(args);
+        va_start(args, format);
+        vfprintf(stderr, format, args);
+        fprintf(stderr, "\n");
+    }
+
+    // 4. Aufräumen
+    va_end(args);
+}
+
 
 /// @brief Initialize period_info with period_ms for cyclic task
 /// @param period_ms 
@@ -77,7 +109,6 @@ static void do_cyclic_1ms(struct period_info *pinfo)
                 // called every 100ms
                 nCalled |= 0x0010;
                 Can_TransmitMotorGear(0);
-                //printf("%.3f: 100ms-Cycle %ld...\n", clock_gettime_s() - pinfo->fStartTime, pinfo->cyclecounter);
         }
 
         if (((pinfo->cyclecounter + 20) % 100) == 0)
@@ -89,14 +120,9 @@ static void do_cyclic_1ms(struct period_info *pinfo)
 
         if (((pinfo->cyclecounter + 30) % 500) == 0)
         {
-                // called every 250ms
+                // called every 500ms
                 MqttClient_Publisher();
         }
-
-        if (nCalled > 0)
-        {
-                //printf("%.3f: Called 0x%.4X at cycle %ld...\n", clock_gettime_s() - pinfo->fStartTime, nCalled, pinfo->cyclecounter);
-        }
 }
  
 
@@ -105,31 +131,31 @@ static void do_cyclic_1ms(struct period_info *pinfo)
 /// @return 
 void *thread_func(void *data)
 {
+        // Initialize IO Ports
+        if (IO_Init())
+        {
+                mylog(LOG_ERR, "IO_Init() failed!");
+                return NULL;
+        }
+
         // Open CAN interface first motor
         if (Can_OpenInterface(0, "can0"))
         {
-                printf("Can_OpenInterface() failed!\n");
+                mylog(LOG_ERR, "Can_OpenInterface() failed!");
                 return NULL;
         }
 
         // Connect to mqtt broker
         while (MqttClient_Connect() && (iThreadControl == 0))
         {
-                printf("MqttClient_Connect() failed!\n");
+                mylog(LOG_ERR, "MqttClient_Connect() failed!");
                 sleep(10);
         }
 
-        // Initialize IO Ports
-        if (IO_Init())
-        {
-                printf("IO_Init() failed!\n");
-                return NULL;
-        }
-
         // initialize cyclic task
         periodic_task_init(1, &pinfo);
  
-        // "Zündung" ein
+        // Ignition on
         WriteOutputPin(GPIO_OUT_PWRON, HIGH);
 
         // cyclic call of do_cyclic_1ms()
@@ -145,7 +171,7 @@ void *thread_func(void *data)
                 wait_rest_of_period(&pinfo);
         }
 
-        // "Zündung" aus
+        // Ignition off
         WriteOutputPin(GPIO_OUT_PWRON, LOW);
 
         // Disconnect from mqtt broker
@@ -154,6 +180,7 @@ void *thread_func(void *data)
         // Close CAN interface
         Can_CloseInterface(0);
         
+        // signal thread has finnished
         iThreadControl = 1;
         return NULL;
 }
@@ -165,13 +192,13 @@ void sig_handler(int signo)
 {
         if ((signo == SIGINT) || (signo == SIGTERM))
         {
-                printf("Received signal %d\n", signo);
+                mylog(LOG_INFO, "Received signal %d", signo);
                 iThreadControl = -1;    // signal realtime thread to exit
         }
 }
 
 
-/// @brief Hauptfunktion Echtzeit-Task erstellen und starten
+/// @brief Main function, create and start realtime task
 /// @param argc 
 /// @param argv 
 /// @return 
@@ -182,22 +209,25 @@ int main(int argc, char* argv[])
         pthread_t thread;
         int ret;
 
+        openlog("CanRtDriver", LOG_PID | LOG_CONS, LOG_DAEMON);
+        mylog(LOG_INFO, "Service started. PID: %d", getpid());
+
         // catch signals
         if (signal(SIGTERM, sig_handler) == SIG_ERR)
         {
-                printf("Can't catch SIGTERM\n");
+                mylog(LOG_ERR, "Can't catch SIGTERM");
                 exit(-1);
         }
         if (signal(SIGINT, sig_handler) == SIG_ERR)
         {
-                printf("Can't catch SIGINT\n");
+                mylog(LOG_ERR, "Can't catch SIGINT");
                 exit(-2);
         }
 
         /* Lock memory */
         if(mlockall(MCL_CURRENT | MCL_FUTURE) == -1) 
         {
-                printf("mlockall failed: %m\n");
+                mylog(LOG_ERR, "mlockall failed: %m");
                 exit(-3);
         }
 
@@ -205,7 +235,7 @@ int main(int argc, char* argv[])
         ret = pthread_attr_init(&attr);
         if (ret) 
         {
-                printf("init pthread attributes failed\n");
+                mylog(LOG_ERR, "init pthread attributes failed");
                 goto out;
         }
 
@@ -213,7 +243,7 @@ int main(int argc, char* argv[])
         ret = pthread_attr_setstacksize(&attr, PTHREAD_STACK_MIN);
         if (ret) 
         {
-            printf("pthread setstacksize failed\n");
+            mylog(LOG_ERR, "pthread setstacksize failed");
             goto out;
         }
 
@@ -221,21 +251,21 @@ int main(int argc, char* argv[])
         ret = pthread_attr_setschedpolicy(&attr, SCHED_FIFO);
         if (ret) 
         {
-                printf("pthread setschedpolicy failed\n");
+                mylog(LOG_ERR, "pthread setschedpolicy failed");
                 goto out;
         }
         param.sched_priority = 99;  // Priority between 1 (low) and 99() high)
         ret = pthread_attr_setschedparam(&attr, &param);
         if (ret) 
         {
-                printf("pthread setschedparam failed\n");
+                mylog(LOG_ERR, "pthread setschedparam failed");
                 goto out;
         }
         /* Use scheduling parameters of attr */
         ret = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
         if (ret) 
         {
-                printf("pthread setinheritsched failed\n");
+                mylog(LOG_ERR, "pthread setinheritsched failed");
                 goto out;
         }
 
@@ -245,7 +275,7 @@ int main(int argc, char* argv[])
         ret = pthread_create(&thread, &attr, thread_func, NULL);
         if (ret) 
         {
-                printf("create pthread failed\n");
+                mylog(LOG_ERR, "create pthread failed");
                 goto out;
         }
 
@@ -253,10 +283,13 @@ int main(int argc, char* argv[])
         ret = pthread_join(thread, NULL);
         if (ret)
         {
-                printf("faild to join thread!\n");
+                mylog(LOG_ERR, "faild to join thread!");
         }
 
 out:
+        mylog(LOG_INFO, "Service quit.");
+        closelog();
+
         return ret;
 }
 
diff --git a/main.h b/main.h
index 8907a5a..0aead00 100755
--- a/main.h
+++ b/main.h
@@ -11,6 +11,8 @@
 #include <time.h>
 #include <errno.h>
 #include <signal.h>
+#include <syslog.h>
+#include <stdarg.h>
 
 
 struct period_info 
@@ -22,5 +24,6 @@ struct period_info
 };
 
 extern struct period_info pinfo;
+extern void mylog(int prio, const char *format, ...);
 
 #endif
\ No newline at end of file
diff --git a/mqtt/mqtt_client.c b/mqtt/mqtt_client.c
index eae3893..5cd3e1b 100755
--- a/mqtt/mqtt_client.c
+++ b/mqtt/mqtt_client.c
@@ -5,7 +5,7 @@
     https://mosquitto.org/api/files/mosquitto-h.html
 */
 
-#include <main.h>
+#include "main.h"
 #include <mqtt/mqtt_client.h>
 #include <can/can_client.h>
 #include <string.h>
@@ -39,6 +39,11 @@ struct mosquitto *mosq;     /**< Libmosquito MQTT client instance. */
 
 int iHadConnectError = 0;
 
+
+/// @brief callback function for incoming mqtt messages
+/// @param mosq 
+/// @param userdata 
+/// @param message 
 void my_message_callback(struct mosquitto *mosq, void *userdata, const struct mosquitto_message *message)
 {
     char* topic_value = (char *)malloc(message->payloadlen + 1);
@@ -50,13 +55,13 @@ void my_message_callback(struct mosquitto *mosq, void *userdata, const struct mo
         int val = 9999;
         if (sscanf(topic_value, "%d", &val))
         {
-            printf("%ld: Received value for mqtt_topic_motor1_gear: %d\n", pinfo.cyclecounter, val);
+            mylog(LOG_INFO, "MQTT: Received value for mqtt_topic_motor1_gear: %d", val);
             iMqttMotor1Gear = val;
             Can_SetMotorGear(0, val);
         }
         else
         {
-            printf("%ld: Received mqtt_topic_motor1_gear: %s\n", pinfo.cyclecounter, topic_value);
+            mylog(LOG_WARNING, "MQTT: Received mqtt_topic_motor1_gear: %s", topic_value);
         }
     }
     else if (strcmp(message->topic, mqtt_topic_motor1_power) == 0)
@@ -64,13 +69,13 @@ void my_message_callback(struct mosquitto *mosq, void *userdata, const struct mo
         int val = 9999;
         if (sscanf(topic_value, "%d", &val))
         {
-            printf("%ld: Received value for mqtt_topic_motor1_power: %d\n", pinfo.cyclecounter, val);
+            mylog(LOG_INFO, "MQTT: Received value for mqtt_topic_motor1_power: %d", val);
             iMqttMotor1Power = val;
             Can_SetMotorPower(0, val);
         }
         else
         {
-            printf("%ld: Received mqtt_topic_motor1_power: %s\n", pinfo.cyclecounter, topic_value);
+            mylog(LOG_WARNING, "MQTT: Received mqtt_topic_motor1_power: %s", topic_value);
         }
     }
     else if (strcmp(message->topic, mqtt_topic_motor2_gear) == 0)
@@ -78,13 +83,13 @@ void my_message_callback(struct mosquitto *mosq, void *userdata, const struct mo
         int val = 9999;
         if (sscanf(topic_value, "%d", &val))
         {
-            printf("%ld: Received value for mqtt_topic_motor2_gear: %d\n", pinfo.cyclecounter, val);
+            mylog(LOG_INFO, "MQTT: Received value for mqtt_topic_motor2_gear: %d", val);
             iMqttMotor2Gear = val;
             Can_SetMotorGear(1, val);
         }
         else
         {
-            printf("%ld: Received mqtt_topic_motor2_gear: %s\n", pinfo.cyclecounter, topic_value);
+            mylog(LOG_WARNING, "MQTT: Received mqtt_topic_motor2_gear: %s", topic_value);
         }
     }
     else if (strcmp(message->topic, mqtt_topic_motor2_power) == 0)
@@ -92,30 +97,32 @@ void my_message_callback(struct mosquitto *mosq, void *userdata, const struct mo
         int val = 9999;
         if (sscanf(topic_value, "%d", &val))
         {
-            printf("%ld: Received value for mqtt_topic_motor2_power: %d\n", pinfo.cyclecounter, val);
+            mylog(LOG_INFO, "MQTT: Received value for mqtt_topic_motor2_power: %d", val);
             iMqttMotor2Power = val;
             Can_SetMotorPower(1, val);
         }
         else
         {
-            printf("%ld: Received mqtt_topic_motor2_power: %s\n", pinfo.cyclecounter, topic_value);
+            mylog(LOG_WARNING, "MQTT: Received mqtt_topic_motor2_power: %s", topic_value);
         }
     }
     else
     {
-        printf("%ld: Received publish('%s'): %s\n", pinfo.cyclecounter, message->topic, topic_value);
+        mylog(LOG_WARNING, "MQTT: Received publish('%s'): %s", message->topic, topic_value);
     }
 
     free(topic_value);
 }
 
 
+/// @brief connect to mqtt broker
+/// @return 
 int MqttClient_Connect()
 {
     int major, minor, revision;
 
     mosquitto_lib_version(&major, &minor, &revision);
-    printf("Libmosquitto version: %d.%d.%d\n", major, minor, revision);
+    mylog(LOG_INFO, "MQTT: Libmosquitto version: %d.%d.%d", major, minor, revision);
 
     // libmosquitto initialization
     mosquitto_lib_init();
@@ -124,7 +131,7 @@ int MqttClient_Connect()
     mosq = mosquitto_new(NULL, true, NULL);
     if (mosq == NULL)
     {
-        printf("Failed to create mosquitto client!/n");
+        mylog(LOG_ERR, "MQTT: Failed to create mosquitto client!/n");
         iHadConnectError++;
         return 1;
     }
@@ -135,7 +142,7 @@ int MqttClient_Connect()
     // Connect to MQTT broker
     if (mosquitto_connect(mosq, mqtt_broker_addr, mqtt_broker_port, 60) != MOSQ_ERR_SUCCESS)
     {
-	    printf("Error: connecting to MQTT broker failed\n");
+	    mylog(LOG_ERR, "MQTT: Connecting to MQTT broker failed");
         MqttClient_Close();
         iHadConnectError++;
         return 2;
@@ -153,28 +160,29 @@ int MqttClient_Connect()
     snprintf(message, sizeof(message), "0");
     if (mosquitto_publish(mosq, NULL, mqtt_topic_motor1_gear, strlen(message), &message, 0, false) != MOSQ_ERR_SUCCESS)
     {
-        printf("mosquitto_publish(mqtt_topic_motor1_gear) fehlgeschlagen!\n");
+        mylog(LOG_ERR, "MQTT: mosquitto_publish(mqtt_topic_motor1_gear) failed!");
         MqttClient_Close();
         iHadConnectError++;
         return 10;
     }
     if (mosquitto_publish(mosq, NULL, mqtt_topic_motor1_power, strlen(message), &message, 0, false) != MOSQ_ERR_SUCCESS)
     {
-        printf("mosquitto_publish(mqtt_topic_motor1_power) fehlgeschlagen!\n");
+        mylog(LOG_ERR, "MQTT: mosquitto_publish(mqtt_topic_motor1_power) failed!");
         MqttClient_Close();
         iHadConnectError++;
         return 11;
     }
     if (mosquitto_publish(mosq, NULL, mqtt_topic_motor2_gear, strlen(message), &message, 0, false) != MOSQ_ERR_SUCCESS)
     {
-        printf("mosquitto_publish(mqtt_topic_motor2_gear) fehlgeschlagen!\n");
+        mylog(LOG_ERR, "MQTT: mosquitto_publish(mqtt_topic_motor2_gear) failed!");
         MqttClient_Close();
         iHadConnectError++;
         return 12;
+        MqttClient_Close();
     }
     if (mosquitto_publish(mosq, NULL, mqtt_topic_motor2_power, strlen(message), &message, 0, false) != MOSQ_ERR_SUCCESS)
     {
-        printf("mosquitto_publish(mqtt_topic_motor2_power) fehlgeschlagen!\n");
+        mylog(LOG_ERR, "MQTT: mosquitto_publish(mqtt_topic_motor2_power) failed!");
         MqttClient_Close();
         iHadConnectError++;
         return 13;
@@ -183,54 +191,65 @@ int MqttClient_Connect()
     // subscribe all needed topics
     if (mosquitto_subscribe(mosq, NULL, mqtt_topic_motor1_gear, 0) != MOSQ_ERR_SUCCESS)
     {
-        printf("mosquitto_subscribe(mqtt_topic_motor1_gear) fehlgeschlagen!\n");
-        MqttClient_Close();
+        mylog(LOG_ERR, "MQTT: mosquitto_subscribe(mqtt_topic_motor1_gear) failed!");
         iHadConnectError++;
         return 20;
     }
     if (mosquitto_subscribe(mosq, NULL, mqtt_topic_motor1_power, 0) != MOSQ_ERR_SUCCESS)
     {
-        printf("mosquitto_subscribe(mqtt_topic_motor1_power) fehlgeschlagen!\n");
+        mylog(LOG_ERR, "MQTT: mosquitto_subscribe(mqtt_topic_motor1_power) failed!");
         MqttClient_Close();
         iHadConnectError++;
         return 21;
     }
     if (mosquitto_subscribe(mosq, NULL, mqtt_topic_motor2_gear, 0) != MOSQ_ERR_SUCCESS)
     {
-        printf("mosquitto_subscribe(mqtt_topic_motor2_gear) fehlgeschlagen!\n");
+        mylog(LOG_ERR, "MQTT: mosquitto_subscribe(mqtt_topic_motor2_gear) failed!");
         MqttClient_Close();
         iHadConnectError++;
         return 22;
     }
     if (mosquitto_subscribe(mosq, NULL, mqtt_topic_motor2_power, 0) != MOSQ_ERR_SUCCESS)
     {
-        printf("mosquitto_subscribe(mqtt_topic_motor2_power) fehlgeschlagen!\n");
+        mylog(LOG_ERR, "MQTT: mosquitto_subscribe(mqtt_topic_motor2_power) failed!");
         MqttClient_Close();
         iHadConnectError++;
         return 23;
     }
 
-    printf("MQTT: Connected successfull!\n");
+    nMqttMotor1SwitchState = 255;
+    MqttClient_Publish_MotorSwitchState(0, 0);
+    nMqttMotor2SwitchState = 255;
+    MqttClient_Publish_MotorSwitchState(1, 0);
+    iMqttMotor1ActualPowerW = -1;
+    MqttClient_Publish_MotorActualPowerW(0, 0);
+    iMqttMotor2ActualPowerW = -1;
+    MqttClient_Publish_MotorActualPowerW(1, 0);
+
+    mylog(LOG_INFO, "MQTT: Connected successfull!");
     return 0;
 }
 
 
+/// @brief Disconnect from mqtt broker
 void MqttClient_Close()
 {
     //Clean up/destroy objects created by libmosquitto
     mosquitto_destroy(mosq);
     mosquitto_lib_cleanup();
 
-    printf("MQTT: Disconnected!\n");
+    mylog(LOG_INFO, "MQTT: Disconnected!");
 }
 
 
+/// @brief Has to be called cyclic, does the work
 void MqttClient_Refresher()
 {
     mosquitto_loop(mosq, 0, 1);
 }
 
 
+/// @brief publish the cycle counter as a sign of life
 void MqttClient_Publisher()
 {
     char message[100];
@@ -240,6 +259,9 @@ void MqttClient_Publisher()
 }
 
 
+/// @brief Publish the actual motor gear
+/// @param iMotorIndex 
+/// @param iGear 
 void MqttClient_Publish_MotorGear(int iMotorIndex, int iGear)
 {
     if (iMotorIndex == 0)
@@ -265,6 +287,9 @@ void MqttClient_Publish_MotorGear(int iMotorIndex, int iGear)
 }
 
 
+/// @brief Publish the requested motor power
+/// @param iMotorIndex 
+/// @param iPower 
 void MqttClient_Publish_MotorPower(int iMotorIndex, int iPower)
 {
     if (iMotorIndex == 0)
@@ -289,6 +314,10 @@ void MqttClient_Publish_MotorPower(int iMotorIndex, int iPower)
     }
 }
 
+
+/// @brief Publish the actual switches states
+/// @param iMotorIndex 
+/// @param nSwitchState 
 void MqttClient_Publish_MotorSwitchState(int iMotorIndex, unsigned char nSwitchState)
 {
     if (iMotorIndex == 0)
@@ -314,6 +343,9 @@ void MqttClient_Publish_MotorSwitchState(int iMotorIndex, unsigned char nSwitchS
 }
 
 
+/// @brief Publish the actual real motor power
+/// @param iMotorIndex 
+/// @param iMotorPowerW 
 void MqttClient_Publish_MotorActualPowerW(int iMotorIndex, int iMotorPowerW)
 {
     if (iMotorIndex == 0)