diff --git a/src/keyer.cpp b/src/keyer.cpp
index bca845b..5fd8f19 100644
--- a/src/keyer.cpp
+++ b/src/keyer.cpp
@@ -50,10 +50,9 @@ void Keyer::setSpeed(uint8_t wpm)
 
 void Keyer::sendMessage(std::string msg)
 {
-    printf("Queue size: %d\n", m_messageQueue.size());
     std::string morse = messageToMorse(msg);
 
-    for (char c : morse) {
+    for (unsigned char c : morse) {
         m_messageQueue.push(c);
     }
 }
@@ -63,18 +62,17 @@ void Keyer::run()
     auto timestamp = get_absolute_time();
 
     // If there is some message to send …
-    //if (!m_messageQueue.empty() || m_messageKeyingState != MessageState::Wait) {
-    if (!m_messageQueue.empty()) {
+    if (!m_messageQueue.empty() || m_messageKeyingState != MessageState::Wait) {
         // Stop all paddle keying, if necessary
         if (m_paddleKeyingState != State::Wait) {
-            //gpio_put(LED_PIN, 0);
-            //gpio_put(CW_OUT_PIN, 0);
-            //m_buzzer.off();
-            //m_keyNextIambicB = false;
-            //m_currentlyPausing = false;
-            //m_currentlyKeying = false;
-            //m_previousState = State::Abort;
-            //m_paddleKeyingState = State::Wait;
+            gpio_put(LED_PIN, 0);
+            gpio_put(CW_OUT_PIN, 0);
+            m_buzzer.off();
+            m_keyNextIambicB = false;
+            m_currentlyPausing = false;
+            m_currentlyKeying = false;
+            m_previousState = State::Abort;
+            m_paddleKeyingState = State::Wait;
         }
 
         switch (m_messageKeyingState) {
diff --git a/src/morse.cpp b/src/morse.cpp
index 97ff2a2..8dac9f2 100644
--- a/src/morse.cpp
+++ b/src/morse.cpp
@@ -78,32 +78,8 @@ std::string messageToMorse(std::string &msg)
 
     // Append word space if last char was not a blank
     if (msg.back() != ' ') {
-        //morseString.push_back('w');
+        morseString.push_back('w');
     }
 
     return morseString;
-}
-
-std::string charToMorse(char ch) {
-    std::string morseString {};
-
-    if (ch == ' ') {
-            morseString.push_back('w');
-            return morseString;
-        }
-
-    for (unsigned int j = 0; j < morseCode[ch].length(); j++) {
-            char m = morseCode[ch][j];
-
-            morseString += m;
-
-            if (j < morseCode[ch].length() - 1) {
-                morseString.push_back('i');
-            }
-        }
-
-    //morseString.push_back('c');
-
-    return morseString;
-
 }
\ No newline at end of file
diff --git a/src/morse.h b/src/morse.h
index bdb10cd..4594f6b 100644
--- a/src/morse.h
+++ b/src/morse.h
@@ -2,5 +2,4 @@
 
 #include <string>
 
-std::string messageToMorse(std::string &msg);
-std::string charToMorse(char ch);
\ No newline at end of file
+std::string messageToMorse(std::string &msg);
\ No newline at end of file
diff --git a/src/raspi_keyer.cpp b/src/raspi_keyer.cpp
index f4559c5..d83869f 100644
--- a/src/raspi_keyer.cpp
+++ b/src/raspi_keyer.cpp
@@ -93,7 +93,6 @@ void core1_main()
             data.cmd = KeyerQueueCommand::Run;
             break;
         case KeyerQueueCommand::SendMessage:
-            printf("Sending: %s\n", data.message.c_str());
             keyer.sendMessage(data.message);
             data.cmd = KeyerQueueCommand::Run;
             break;
@@ -127,42 +126,25 @@ void cdc_task()
     if (tud_cdc_n_available(USB_IF)) {
         uint8_t buf[64];
 
-        //printf("AHA!!! %d\n", (int)tud_cdc_n_available(USB_IF));
+        // printf("AHA!!! %d\n", (int)tud_cdc_n_available(USB_IF));
         uint32_t count = tud_cdc_n_read(USB_IF, buf, sizeof(buf));
 
         if (count > 0) {
             switch (buf[0]) {
             case 'a' ... 'z':
-            [[fallthrough]];
             case 'A' ... 'Z':
-            [[fallthrough]];
             case '0' ... '9':
-            [[fallthrough]];
-            case ' ':
-            [[fallthrough]];
-            case '+':
-            [[fallthrough]];
-            case '=':
-            [[fallthrough]];
-            case '?':
-            [[fallthrough]];
-            case ',':
-            [[fallthrough]];
-            case '.':
-            {
-                printf("TODO: Send it: %c\n", buf[0]);
-                std::string msg(1, buf[0]);
-                KeyerQueueData keyerQueueData {KeyerQueueCommand::SendMessage, 0, Mode::IambicB, msg};
-                queue_add_blocking(&keyerQueue, &keyerQueueData);
+                printf("TODO: Send it!\n");
                 break;
-            }
             case 0x00: // ADMIN COMMAND
                 switch (buf[1]) {
-                case 0x02:              // HOST OPEN
+                case 0x02:              // Host open
                     usbSend(USB_IF, 9); // Send WK1 (v9) for now (no WinKeyer PTT control)
                     break;
-                case 0x04: // ECHO TEST
-                    usbSend(USB_IF, &buf[2], 1); // Send the received byte back
+                case 0x04: // Echo test
+                    printf("Echo test: %x\n", buf[2]);
+                    printf("buf address: %p\n", &buf[2]);
+                    usbSend(USB_IF, &buf[2], 1);
                     break;
                 default:
                     printf("Unknown admin command: %x\n", buf[1]);
@@ -227,6 +209,9 @@ int main()
     static bool used = false;
 
     while (true) {
+        tud_task();
+        cdc_task();
+
         currentWpm = calcWPM(potiRead(), settings.wpmPotiMin, settings.wpmPotiMax);
 
         // If WPM in settings is set to 0 -> take speed from poti
@@ -243,9 +228,6 @@ int main()
             // pse k"}; queue_add_blocking(&keyerQueue, &keyerQueueData);
             used = true;
         }
-
-        tud_task();
-        cdc_task();
     }
 
     return 0;