diff --git a/Source/controls.c b/Source/controls.c
index 04dab61..512d47a 100644
--- a/Source/controls.c
+++ b/Source/controls.c
@@ -420,6 +420,8 @@ void UpdateDisplay(void)
                 sprintf(line,"%s", IambicA);
             if(bConfig.KeyerMode == IAMBIC_B)
                 sprintf(line,"%s", IambicB);
+            if(bConfig.KeyerMode == ULTIMATIC)
+                sprintf(line,"%s", Ultimatic);
             lcd_puts(line);
             lcd_gotoxy(0,0);
             if(bConfig.TRX == 1)
diff --git a/Source/functions.c b/Source/functions.c
index 33e80fb..7a9d374 100644
--- a/Source/functions.c
+++ b/Source/functions.c
@@ -344,7 +344,8 @@ void CheckStraightKey(void)
 {
     if(PIND & (1<<STRAIGHT_KEY))
     {
-        TXKey(0);
+        if(bState.SendStatus == NOTHING)
+            TXKey(0);
         StateStraightKeyPressed = NO_KEY_PRESSED;
     } else {
         if(StateStraightKeyPressed == NO_KEY_PRESSED)
@@ -354,7 +355,8 @@ void CheckStraightKey(void)
         }
         if(StateStraightKeyPressed == KEY_PRESSED)
         {
-            TXKey(1);
+            if(bState.SendStatus == NOTHING)
+                TXKey(1);
         }
     }
 }
@@ -441,8 +443,7 @@ void CheckPaddles(void)
                 }
             break;
          }
-    } 
-
+    }
 }
 /** \brief CheckDitPaddle
  *
@@ -465,7 +466,8 @@ void CheckDitPaddle(void)
             TimerPaddleKeyPressed = 0;
         }
         if(StatePaddleKeyPressed == KEY_PRESSED) // Debounce ok
-        {
+        {   
+            SerialWriteString("470:DitBuffer == 1\r\n");
             DitBuffer = 1;
             StatePaddleKeyPressed = NO_KEY_PRESSED;
         }
@@ -492,8 +494,9 @@ void CheckDahPaddle(void)
         }
         if(StatePaddleKeyPressed == KEY_PRESSED) // Debounce ok
         {
+            SerialWriteString("496:DahBuffer == 1\r\n");
             DahBuffer = 1;
-            StatePaddleKeyPressed == NO_KEY_PRESSED;
+            StatePaddleKeyPressed = NO_KEY_PRESSED;
         }
     }
 }
@@ -517,17 +520,39 @@ void SymbolLoop(uint8_t Len)
     t_elementlength = 0;                // Reset Timer 0 variable
     while(t_elementlength < ticks)
     {
+        if(KeyerMode != ULTIMATIC)
+        {
+            if((KeyerMode == IAMBIC_A) && (!(PIND & (1<<LEFT_PADDLE))) && (!(PIND & (1<<RIGHT_PADDLE))))
+            {
+                IambicFlag = 1;
+                SerialWriteString("526:IambicFlag == 1\r\n");
+            }
 
-        if((KeyerMode == IAMBIC_A) && (!(PIND & (1<<LEFT_PADDLE))) && (!(PIND & (1<<RIGHT_PADDLE))))
-            IambicFlag = 1;
-
-        if(bState.SendStatus == SENDING_DIT)
-            CheckDahPaddle();
-        else if(bState.SendStatus == SENDING_DAH)
-            CheckDitPaddle();
-        else {
-            CheckDahPaddle();
-            CheckDitPaddle();
+            if(bState.SendStatus == SENDING_DIT)
+            {
+                CheckDahPaddle();
+            } else { 
+                if(bState.SendStatus == SENDING_DAH)
+                {
+                    CheckDitPaddle();
+                } else {
+                    CheckDahPaddle();
+                    CheckDitPaddle();
+                }
+            }
+        } else {
+            if(bState.SendStatus == SENDING_DIT)
+            {
+                CheckDahPaddle();
+            } else {
+                if(bState.SendStatus == SENDING_DAH)
+                {
+                    CheckDitPaddle();
+                } else {
+                    CheckDahPaddle();
+                    CheckDitPaddle();
+                }
+            }
         }
     }
     // Wenn Taste losgelassen, dann im KeyerMode IAMBIC_A das IambicFlag löschen
@@ -536,6 +561,7 @@ void SymbolLoop(uint8_t Len)
         IambicFlag = 0;
         DitBuffer = 0;
         DahBuffer = 0;
+        SerialWriteString("562:IambicFlag == 0\r\n");
     }
     if((bState.SendStatus == SENDING_DIT) || (bState.SendStatus == SENDING_DAH))
     {
@@ -581,7 +607,7 @@ void SendDah(uint8_t SendingType)
  */
 void DitDahBuffers(void)
 {
-    if((bConfig.KeyerMode == IAMBIC_A) || (bConfig.KeyerMode == IAMBIC_B) || (bConfig.KeyerMode == ULTIMATIC))
+    if((bConfig.KeyerMode == IAMBIC_A) || (bConfig.KeyerMode == IAMBIC_B))
     {
         // IAMBIC_A
         if((bConfig.KeyerMode == IAMBIC_A) && (IambicFlag) && (PIND & (1<<LEFT_PADDLE)) && (PIND & (1<<RIGHT_PADDLE))) 
@@ -589,6 +615,7 @@ void DitDahBuffers(void)
             IambicFlag = 0;
             DitBuffer = 0;
             DahBuffer = 0;
+            SerialWriteString("616:IambicFlag == 0\r\n");
         } else {
         // IAMBIC_B
             if(DitBuffer)
@@ -603,6 +630,11 @@ void DitDahBuffers(void)
             }
         }
     }
+    if(bConfig.KeyerMode == ULTIMATIC)
+    {
+        SerialWriteString("Ultimatic\r\n");
+    }
+
 }
 
 /*
diff --git a/Source/globals.h b/Source/globals.h
index 3634748..e5fb76e 100644
--- a/Source/globals.h
+++ b/Source/globals.h
@@ -183,7 +183,6 @@ uint8_t LengthOfElement;
 // State Machines
 uint8_t PaddleMode;
 uint8_t KeyerMode;
-uint8_t state_sidetoneoff;
 // History
 uint8_t keyhistory;
 uint8_t KeyTX;
diff --git a/Source/main.c b/Source/main.c
index 527c098..812464a 100644
--- a/Source/main.c
+++ b/Source/main.c
@@ -306,34 +306,6 @@ ISR(USART_RX_vect)
     data = UDR0;
     SerialWriteChar(data);
 }
-/*
- ** DoMorse
- */
-void DoMorse(void)
-{
-    if((KeyerMode == IAMBIC_A) || (KeyerMode == IAMBIC_B) || KeyerMode == ULTIMATIC)
-    {
-        if((KeyerMode == IAMBIC_A) && (IambicFlag) && (ReadKeyPin(LEFT_PADDLE)))
-        {
-            IambicFlag = 0;
-            DitBuffer = 0;
-            DahBuffer = 0;
-        }
-        else
-        {
-            if(DitBuffer)
-            {
-                DitBuffer = 0;
-                SendDit(MAN);
-            }
-            if(DahBuffer)
-            {
-                DahBuffer = 0;
-                SendDah(MAN);
-            }
-        }
-    }
-}
 /** @brief  Initialisierung bei Reset und Neustart
  *  @param  none
  *  @return none
@@ -395,7 +367,6 @@ int main(void)
         CheckStraightKey();
         CheckPaddles();
         DitDahBuffers();
-        DoMorse();
     }
 }
 
diff --git a/Source/main.h b/Source/main.h
index 1771483..e1d5a2c 100644
--- a/Source/main.h
+++ b/Source/main.h
@@ -18,20 +18,11 @@ struct State bState;
 // Function prototypes
 void Init(void);
 void InitTimer(void);
+void ReStart(void);
 
-void CheckDahPaddle(void);
-void CheckDitPaddle(void);
-void CheckPaddle(void);
-void CheckStraightKey(void);
-void SendDit(uint8_t SendingType);
-void SendDah(uint8_t SendingType);
-void SideToneOff(void);
-void SideToneOn(void);
-void TXKey(uint8_t State);
-void SendChar(uint8_t);
-void ChangeSpeed(void);
-void DoMorse(void);
-
-void SetFrequency(uint16_t f);
-
+extern void CheckDahPaddle(void);
+extern void CheckDitPaddle(void);
+extern void CheckStraightKey(void);
+extern void SetFrequency(uint16_t f);
+extern void DitDahBuffers(void);
 #endif // BJ-KEYER_H_INCLUDED