Fix up FSK

app/fskmodem.c

@@ -31,20 +31,31 @@ typedef enum {
 RXState rxState = Ready;
 
 void FSKModem_TimeSlice500ms(void) {
-    if (SMSResponseCounter && SMSResponseCounter++ > 3) {
+    if (SMSResponseCounter && SMSResponseCounter++ > 6) {
         switch (gSMSResponseState) {
             case SMS_RESPONSE_ACK:
-                inBoundPacket.flags |= 0x64;
-                inBoundPacket.dest = inBoundPacket.src;
-                inBoundPacket.src = gEeprom.FSKSRCAddress;
-                SYSTEM_DelayMs(200);
                 MSG_FSKSendData(&inBoundPacket);
+                UART_String("Acking");
+                gSMSResponseState = SMS_RESPONSE_IDLE;
+                SMSResponseCounter = 0;
                 break;
             case SMS_RESPONSE_RETRANSMIT:
-                SYSTEM_DelayMs(200);
+                MSG_FSKSendData(&inBoundPacket);
-                BK4819_ToggleGpioOut(BK4819_GPIO5_PIN1_RED, true);
+                UART_String("ReTX");
-                MSG_FSKSendData(packet);
+                gSMSResponseState = SMS_RESPONSE_IDLE;
+                SMSResponseCounter = 0;
+                break;
+            case SMS_RESPONSE_RESEND:
+                MSG_FSKSendData(&dataPacket);
+                UART_String("ReSend");
+                gSMSResponseState = SMS_RESPONSE_IDLE;
+                SMSResponseCounter = 0;
+                break;
+            case SMS_RESPONSE_ACK_LIGHT:
                 BK4819_ToggleGpioOut(BK4819_GPIO5_PIN1_RED, false);
+                BK4819_ToggleGpioOut(BK4819_GPIO6_PIN2_GREEN, false);
+                gSMSResponseState = SMS_RESPONSE_IDLE;
+                SMSResponseCounter = 0;
                 break;
             default:
                 break;
@@ -130,7 +141,6 @@ void MSG_ConfigureFSK(bool rx) {
         fskParams |= FSK_SCRAMBLE_ENABLE;
     }
     BK4819_WriteRegister(BK4819_REG_59, fskParams);
-
     BK4819_WriteRegister(BK4819_REG_02, 0);
 }
 
@@ -157,11 +167,24 @@ void processReceivedPacket(DataPacket *packet) {
     const unsigned int vfo = (gEeprom.CROSS_BAND_RX_TX == CROSS_BAND_OFF) ? gEeprom.RX_VFO
                                                                           : gEeprom.TX_VFO;
 
+    UART_Send(packet, sizeof(DataPacket));
     if (packet->dest == gEeprom.FSKSRCAddress) {
-        if ((packet->flags & 0x128)) {
+        if ((packet->flags & 0x80)) {
-            if (packet->flags & 0x64) {
+            if (packet->flags & 0x40) {
-                if (memcmp(inBoundPacket.data, dataPacket.data, DataPacketDataSize) == 0) {
+                uint16_t crcSent = calculateCRC(dataPacket.data, 20);
+                uint16_t crcGot = inBoundPacket.data[0] | (inBoundPacket.data[1] << 8);
+                if (crcSent == crcGot) {
                     gGotACK = true;
+                    UART_String("GOT ACK");
+                    BK4819_ToggleGpioOut(BK4819_GPIO5_PIN1_RED, true);
+                    BK4819_ToggleGpioOut(BK4819_GPIO6_PIN2_GREEN, true);
+                    SMSResponseCounter = 1;
+                    gSMSResponseState = SMS_RESPONSE_ACK_LIGHT;
+                } else {
+                    if (--dataPacket.ttl) {
+                        gSMSResponseState = SMS_RESPONSE_RESEND;
+                        SMSResponseCounter = 1;
+                    }
                 }
             } else {
                 AUDIO_PlayBeep(BEEP_500HZ_60MS_DOUBLE_BEEP);
@@ -171,20 +194,18 @@ void processReceivedPacket(DataPacket *packet) {
                 strcat(String, numBuf);
                 MESSAGES_SAVE();
                 UI_DisplayPopup(String);
-                SMSResponseCounter = 1;
+                inBoundPacket.flags |= 0x40;
+                inBoundPacket.dest = inBoundPacket.src;
+                inBoundPacket.src = gEeprom.FSKSRCAddress;
+                uint16_t crcTest = calculateCRC(inBoundPacket.data, 20);
+                memset(inBoundPacket.data, 0, DataPacketDataSize);
+                inBoundPacket.data[0] = crcTest & 0xFF;
+                inBoundPacket.data[1] = (crcTest >> 8) & 0xFF;
                 gSMSResponseState = SMS_RESPONSE_ACK;
+                SMSResponseCounter = 1;
+                UI_PrintStringSmallNormal(String, 2, 0, 4);
             }
         }
-#ifdef ENABLE_FEAT_F4HWN
-        if (isMainOnly()) {
-            UI_PrintStringSmallNormal(String, 2, 0, 5);
-        } else {
-            UI_PrintStringSmallNormal(String, 2, 0, 3);
-        }
-#else
-        UI_PrintStringSmallNormal(String, 2, 0, 3);
-
-#endif
 
     } else if (VfoState[vfo] == VFO_STATE_NORMAL && !TX_freq_check(gCurrentVfo->freq_config_TX.Frequency)) {
         if (packet->ttl--) {
@@ -192,38 +213,6 @@ void processReceivedPacket(DataPacket *packet) {
             gSMSResponseState = SMS_RESPONSE_RETRANSMIT;
         }
     }
-
-    memset(packet, 0, sizeof(DataPacket)); // Clear data after processing (example action)
-}
-
-
-bool MSG_FSKReceiveData() {
-    if (!(BK4819_ReadRegister(BK4819_REG_0C) & (1U << 1))) {
-        return false; // No data available
-    }
-
-    // Read the received data from FIFO
-    uint16_t *ptr = (uint16_t * ) & inBoundPacket;
-    size_t wordCount = sizeof(inBoundPacket) / sizeof(uint16_t);
-
-    for (size_t i = 0; i < wordCount; i++) {
-        ptr[i] = BK4819_ReadRegister(BK4819_REG_5F);
-    }
-
-    // Clear the RX interrupt flag
-    BK4819_WriteRegister(BK4819_REG_02, 0);
-
-    // Validate checksum (assuming last 2 bytes are CRC-16)
-    size_t dataLength = sizeof(inBoundPacket.data) - 2;
-    uint16_t receivedCRC = (inBoundPacket.data[dataLength] << 8) | inBoundPacket.data[dataLength + 1];
-    uint16_t calculatedCRC = calculateCRC(inBoundPacket.data, dataLength);
-
-    if (receivedCRC == calculatedCRC) {
-        processReceivedPacket(&inBoundPacket);
-        return true;
-    }
-
-    return false; // CRC check failed
 }
 
 void MSG_EnableRX(const bool enable) {
@@ -240,121 +229,134 @@ void MSG_EnableRX(const bool enable) {
 }
 
 void MSG_FSKSendData(DataPacket *dataPacketIn) {
+    const unsigned int vfo = (gEeprom.CROSS_BAND_RX_TX == CROSS_BAND_OFF) ? gEeprom.RX_VFO
+                                                                          : gEeprom.TX_VFO;
+    if (VfoState[vfo] == VFO_STATE_NORMAL &&
+        !TX_freq_check(gCurrentVfo->freq_config_TX.Frequency)) {
 
-    bool isAudioOn = gEnableSpeaker;
+        RADIO_PrepareTX();
-    if (gEnableSpeaker) {
+        BK4819_SetScramble(gCurrentVfo->SCRAMBLING_TYPE);
-        AUDIO_AudioPathOff();
+        BK4819_ToggleGpioOut(BK4819_GPIO5_PIN1_RED, true);
-        BK4819_EnterTxMute();
-        gEnableSpeaker = false;
-    }
-    // turn off CTCSS/CDCSS during FFSK
-    const uint16_t css_val = BK4819_ReadRegister(BK4819_REG_51);
-    BK4819_WriteRegister(BK4819_REG_51, 0);
 
-    // set the FM deviation level
+        bool isAudioOn = gEnableSpeaker;
-    const uint16_t dev_val = BK4819_ReadRegister(BK4819_REG_40);
+        if (gEnableSpeaker) {
+            AUDIO_AudioPathOff();
+            BK4819_EnterTxMute();
+            gEnableSpeaker = false;
+        }
+        // turn off CTCSS/CDCSS during FFSK
+        const uint16_t css_val = BK4819_ReadRegister(BK4819_REG_51);
+        BK4819_WriteRegister(BK4819_REG_51, 0);
 
-    {
+        // set the FM deviation level
-        uint16_t deviation;
+        const uint16_t dev_val = BK4819_ReadRegister(BK4819_REG_40);
-        switch (gEeprom.VfoInfo[gEeprom.TX_VFO].CHANNEL_BANDWIDTH) {
+
-            case BK4819_FILTER_BW_WIDE:
+        {
-                deviation = 1300;
+            uint16_t deviation;
-                break; // 20k // measurements by kamilsss655
+            switch (gEeprom.VfoInfo[gEeprom.TX_VFO].CHANNEL_BANDWIDTH) {
-            case BK4819_FILTER_BW_NARROW:
+                case BK4819_FILTER_BW_WIDE:
-                deviation = 1200;
+                    deviation = 1300;
-                break; // 10k
+                    break; // 20k // measurements by kamilsss655
-                // case BK4819_FILTER_BW_NARROWAVIATION:  deviation =  850; break;  // 5k
+                case BK4819_FILTER_BW_NARROW:
-                // case BK4819_FILTER_BW_NARROWER:        deviation =  850; break;  // 5k
+                    deviation = 1200;
-                // case BK4819_FILTER_BW_NARROWEST:	      deviation =  850; break;  // 5k
+                    break; // 10k
-            default:
+                    // case BK4819_FILTER_BW_NARROWAVIATION:  deviation =  850; break;  // 5k
-                deviation = 850;
+                    // case BK4819_FILTER_BW_NARROWER:        deviation =  850; break;  // 5k
-                break;  // 5k
+                    // case BK4819_FILTER_BW_NARROWEST:	      deviation =  850; break;  // 5k
+                default:
+                    deviation = 850;
+                    break;  // 5k
+            }
+
+            //BK4819_WriteRegister(0x40, (3u << 12) | (deviation & 0xfff));
+            BK4819_WriteRegister(BK4819_REG_40, (dev_val & 0xf000) | (deviation & 0xfff));
         }
 
-        //BK4819_WriteRegister(0x40, (3u << 12) | (deviation & 0xfff));
+        // REG_2B   0
-        BK4819_WriteRegister(BK4819_REG_40, (dev_val & 0xf000) | (deviation & 0xfff));
+        //
-    }
+        // <15> 1 Enable CTCSS/CDCSS DC cancellation after FM Demodulation   1 = enable 0 = disable
+        // <14> 1 Enable AF DC cancellation after FM Demodulation            1 = enable 0 = disable
+        // <10> 0 AF RX HPF 300Hz filter     0 = enable 1 = disable
+        // <9>  0 AF RX LPF 3kHz filter      0 = enable 1 = disable
+        // <8>  0 AF RX de-emphasis filter   0 = enable 1 = disable
+        // <2>  0 AF TX HPF 300Hz filter     0 = enable 1 = disable
+        // <1>  0 AF TX LPF filter           0 = enable 1 = disable
+        // <0>  0 AF TX pre-emphasis filter  0 = enable 1 = disable
+        //
+        // disable the 300Hz HPF and FM pre-emphasis filter
+        //
+        const uint16_t filt_val = BK4819_ReadRegister(BK4819_REG_2B);
+        BK4819_WriteRegister(BK4819_REG_2B, (1u << 2) | (1u << 0));
 
-    // REG_2B   0
+        MSG_ConfigureFSK(false);
-    //
-    // <15> 1 Enable CTCSS/CDCSS DC cancellation after FM Demodulation   1 = enable 0 = disable
-    // <14> 1 Enable AF DC cancellation after FM Demodulation            1 = enable 0 = disable
-    // <10> 0 AF RX HPF 300Hz filter     0 = enable 1 = disable
-    // <9>  0 AF RX LPF 3kHz filter      0 = enable 1 = disable
-    // <8>  0 AF RX de-emphasis filter   0 = enable 1 = disable
-    // <2>  0 AF TX HPF 300Hz filter     0 = enable 1 = disable
-    // <1>  0 AF TX LPF filter           0 = enable 1 = disable
-    // <0>  0 AF TX pre-emphasis filter  0 = enable 1 = disable
-    //
-    // disable the 300Hz HPF and FM pre-emphasis filter
-    //
-    const uint16_t filt_val = BK4819_ReadRegister(BK4819_REG_2B);
-    BK4819_WriteRegister(BK4819_REG_2B, (1u << 2) | (1u << 0));
-
-    MSG_ConfigureFSK(false);
 
 
-    SYSTEM_DelayMs(100);
+        SYSTEM_DelayMs(100);
 
-    {    // load the entire packet data into the TX FIFO buffer
+        {    // load the entire packet data into the TX FIFO buffer
-        uint16_t *ptr = (uint16_t *) dataPacketIn;
+            uint16_t *ptr = (uint16_t *) dataPacketIn;
-        size_t wordCount = sizeof(*dataPacketIn) / sizeof(uint16_t);
+            size_t wordCount = sizeof(*dataPacketIn) / sizeof(uint16_t);
 
-        for (size_t i = 0; i < wordCount; i++) {
+            for (size_t i = 0; i < wordCount; i++) {
-            BK4819_WriteRegister(BK4819_REG_5F, ptr[i]);
+                BK4819_WriteRegister(BK4819_REG_5F, ptr[i]);
-        }
-    }
-
-    // enable FSK TX
-    BK4819_FskEnableTx();
-
-    {
-        // allow up to 310ms for the TX to complete
-        // if it takes any longer then somethings gone wrong, we shut the TX down
-        unsigned int timeout = 1000 / 5;
-
-        while (timeout-- > 0) {
-            SYSTEM_DelayMs(5);
-            if (BK4819_ReadRegister(BK4819_REG_0C) & (1u << 0)) {    // we have interrupt flags
-                BK4819_WriteRegister(BK4819_REG_02, 0);
-                if (BK4819_ReadRegister(BK4819_REG_02) & BK4819_REG_02_FSK_TX_FINISHED)
-                    timeout = 0;       // TX is complete
             }
         }
+
+        // enable FSK TX
+        BK4819_FskEnableTx();
+
+        {
+            // allow up to 310ms for the TX to complete
+            // if it takes any longer then somethings gone wrong, we shut the TX down
+            unsigned int timeout = 1000 / 5;
+
+            while (timeout-- > 0) {
+                SYSTEM_DelayMs(5);
+                if (BK4819_ReadRegister(BK4819_REG_0C) & (1u << 0)) {    // we have interrupt flags
+                    BK4819_WriteRegister(BK4819_REG_02, 0);
+                    if (BK4819_ReadRegister(BK4819_REG_02) & BK4819_REG_02_FSK_TX_FINISHED)
+                        timeout = 0;       // TX is complete
+                }
+            }
+        }
+        //BK4819_WriteRegister(BK4819_REG_02, 0);
+
+        SYSTEM_DelayMs(100);
+
+        // disable TX
+        MSG_ConfigureFSK(true);
+
+        // restore FM deviation level
+        BK4819_WriteRegister(BK4819_REG_40, dev_val);
+
+        // restore TX/RX filtering
+        BK4819_WriteRegister(BK4819_REG_2B, filt_val);
+
+        // restore the CTCSS/CDCSS setting
+        BK4819_WriteRegister(BK4819_REG_51, css_val);
+
+        SYSTEM_DelayMs(50);
+        APP_EndTransmission();
+        FUNCTION_Select(FUNCTION_FOREGROUND);
+        gUpdateStatus = true;
+        gUpdateDisplay = true;
+        gFlagEndTransmission = false;
+
+        if (isAudioOn) {
+            AUDIO_AudioPathOn();
+            gEnableSpeaker = true;
+            BK4819_ExitTxMute();
+        }
+
+        if (!(dataPacketIn->flags & 0x64)) {
+            gGotACK = false;
+        }
+        BK4819_ToggleGpioOut(BK4819_GPIO5_PIN1_RED, false);
+        MSG_EnableRX(true);
+        gVfoConfigureMode = VFO_CONFIGURE;
+        dataPTR = dataPacket.data;
+
     }
-    //BK4819_WriteRegister(BK4819_REG_02, 0);
-
-    SYSTEM_DelayMs(100);
-
-    // disable TX
-    MSG_ConfigureFSK(true);
-
-    // restore FM deviation level
-    BK4819_WriteRegister(BK4819_REG_40, dev_val);
-
-    // restore TX/RX filtering
-    BK4819_WriteRegister(BK4819_REG_2B, filt_val);
-
-    // restore the CTCSS/CDCSS setting
-    BK4819_WriteRegister(BK4819_REG_51, css_val);
-
-    SYSTEM_DelayMs(50);
-    APP_EndTransmission();
-    FUNCTION_Select(FUNCTION_FOREGROUND);
-    gUpdateStatus = true;
-    gUpdateDisplay = true;
-    gFlagEndTransmission = false;
-
-    if (isAudioOn) {
-        AUDIO_AudioPathOn();
-        gEnableSpeaker = true;
-        BK4819_ExitTxMute();
-    }
-
-    if (!(dataPacketIn->flags & 0x64)) {
-        gGotACK = false;
-    }
-
 }
 
 void prepareDataPacket() {
@@ -389,7 +391,7 @@ void FSK_HANDLE_IRQ(unsigned short irq) {
 
     if (rx_fifo_almost_full && rxState == Receiving) {
         const uint16_t count = BK4819_ReadRegister(BK4819_REG_5E) & (7u << 0);  // Almost full threshold
-        uint16_t *ptr = (uint16_t * ) & inBoundPacket;
+        uint16_t *ptr = (uint16_t *) &inBoundPacket;
         size_t wordCount = sizeof(inBoundPacket) / sizeof(uint16_t);
 
         for (uint16_t i = 0; i < count && gFSKWriteIndex < wordCount; i++) {

app/fskmodem.h

@@ -78,7 +78,7 @@
 #define FSK_CRC_ON (1U << 6)
 #define FSK_CRC_OFF (0U << 6)
 
-#define DataPacketDataSize (37)
+#define DataPacketDataSize (21)
 
 #define SEQParameterEEPROM 0x1BD0
 
@@ -98,15 +98,15 @@ void MSG_FSKSendData(DataPacket *dataPacketIn);
 void MSG_EnableRX(bool enable);
 
 void processReceivedPacket(DataPacket *packet);
+
 uint16_t calculateCRC(uint8_t *data, size_t length);
-bool MSG_FSKReceiveData();
 
 extern DataPacket dataPacket;
 
 extern DataPacket inBoundPacket;
 
 typedef enum {
-    SMS_NOT_ENTERING,
+    SMS_NOT_ENTERING = 0,
     SMS_ENTERING_DEST,
     SMS_ENTERING_MESSAGE
 } SMSEnteringState;
@@ -114,7 +114,9 @@ typedef enum {
 typedef enum {
     SMS_RESPONSE_IDLE,
     SMS_RESPONSE_ACK,
-    SMS_RESPONSE_RETRANSMIT
+    SMS_RESPONSE_ACK_LIGHT,
+    SMS_RESPONSE_RETRANSMIT,
+    SMS_RESPONSE_RESEND
 } SMSResponseState;
 
 extern SMSEnteringState gEnteringSMS;
@@ -127,6 +129,7 @@ extern bool gGotACK;
 extern uint8_t SMSResponseCounter;
 
 void FSK_HANDLE_IRQ(unsigned short irq);
+
 void FSKModem_TimeSlice500ms(void);
 
 #endif //UV_K5_FIRMWARE_CUSTOM_FSKMODEM_H

app/messages.c

@@ -38,20 +38,26 @@ void MESSAGES_SAVE() {
     Data[1]++;
     EEPROM_WriteBuffer(SEQParameterEEPROM, Data);
     EEPROM_WriteBuffer(MESSAGES_START + (msgIndex * sizeof(StoredPacket)) + 0,
-                       (uint8_t * ) & inBoundPacket + 4);   // metadata
+                       (uint8_t *) &inBoundPacket + 4);   // metadata
     EEPROM_WriteBuffer(MESSAGES_START + (msgIndex * sizeof(StoredPacket)) + 8,
-                       (uint8_t * ) & inBoundPacket + 12);   // message part 1
+                       (uint8_t *) &inBoundPacket + 12);   // message part 1
     EEPROM_WriteBuffer(MESSAGES_START + (msgIndex * sizeof(StoredPacket)) + 16,
-                       (uint8_t * ) & inBoundPacket + 20); // message part 2
+                       (uint8_t *) &inBoundPacket + 20); // message part 2
     EEPROM_WriteBuffer(MESSAGES_START + (msgIndex * sizeof(StoredPacket)) + 24,
-                       (uint8_t * ) & inBoundPacket + 28); // message part 3
+                       (uint8_t *) &inBoundPacket + 28); // message part 3
     EEPROM_WriteBuffer(MESSAGES_START + (msgIndex * sizeof(StoredPacket)) + 32,
-                       (uint8_t * ) & inBoundPacket + 36); // message part 4
+                       (uint8_t *) &inBoundPacket + 36); // message part 4
 
 }
 
 void MESSAGES_GET() {
-    EEPROM_ReadBuffer(MESSAGES_START + (gActiveMessage * sizeof(StoredPacket)), &loadedPacket, sizeof(StoredPacket));
+    if (gActiveMessage == MESSAGES_COUNT) {
+        memcpy(&loadedPacket, (uint8_t *) &dataPacket + (sizeof(DataPacket) - sizeof(StoredPacket)),
+               sizeof(StoredPacket));
+    } else {
+        EEPROM_ReadBuffer(MESSAGES_START + (gActiveMessage * sizeof(StoredPacket)), &loadedPacket,
+                          sizeof(StoredPacket));
+    }
 }
 
 void MESSAGES_DELETE() {
@@ -121,21 +127,7 @@ void MESSAGES_ProcessKeys(KEY_Code_t Key, bool bKeyPressed, bool bKeyHeld) {
             } else if (gEnteringSMS == SMS_ENTERING_MESSAGE) {
                 if (bKeyPressed) {
                     if (strlen((char *) dataPacket.data)) {
-                        const unsigned int vfo = (gEeprom.CROSS_BAND_RX_TX == CROSS_BAND_OFF) ? gEeprom.RX_VFO
+                        MSG_FSKSendData(&dataPacket);
-                                                                                              : gEeprom.TX_VFO;
-                        if (VfoState[vfo] == VFO_STATE_NORMAL &&
-                            !TX_freq_check(gCurrentVfo->freq_config_TX.Frequency)) {
-
-                            RADIO_PrepareTX();
-                            BK4819_SetScramble(gCurrentVfo->SCRAMBLING_TYPE);
-                            BK4819_ToggleGpioOut(BK4819_GPIO5_PIN1_RED, true);
-                            MSG_FSKSendData(&dataPacket);
-                            BK4819_ToggleGpioOut(BK4819_GPIO5_PIN1_RED, false);
-                            MSG_EnableRX(true);
-                            gVfoConfigureMode = VFO_CONFIGURE;
-                            dataPTR = dataPacket.data;
-                            memset(dataPacket.data, 0, DataPacketDataSize);
-                        }
                         gEnteringSMS = SMS_NOT_ENTERING;
                     }
                 }
@@ -145,7 +137,7 @@ void MESSAGES_ProcessKeys(KEY_Code_t Key, bool bKeyPressed, bool bKeyHeld) {
         case KEY_UP:
             if (bKeyPressed) {
                 gActiveMessage++;
-                if (gActiveMessage >= MESSAGES_COUNT) {
+                if (gActiveMessage > MESSAGES_COUNT) {
                     gActiveMessage = 0;
                 }
                 MESSAGES_GET();
@@ -154,7 +146,7 @@ void MESSAGES_ProcessKeys(KEY_Code_t Key, bool bKeyPressed, bool bKeyHeld) {
         case KEY_DOWN:
             if (bKeyPressed) {
                 gActiveMessage--;
-                if (gActiveMessage >= MESSAGES_COUNT) {
+                if (gActiveMessage > MESSAGES_COUNT) {
                     gActiveMessage = MESSAGES_COUNT - 1;
                 }
                 MESSAGES_GET();

ui/messages.c

@@ -34,39 +34,57 @@ void UI_DisplayMessages(void) {
         } else if (gEnteringSMS == SMS_ENTERING_MESSAGE) {
             UI_PrintString("SMS data", 0, 0, 0 /*, 8 */);
             UI_PrintStringSmallNormal((const char *) dataPacket.data, 1, 0, 1);
-            memset(gFrameBuffer[1] + 2 + (7 * strlen((const char *) dataPacket.data)), gKeyTimeout == 0 ? 0xFF : 0x80, 6);
+            memset(gFrameBuffer[1] + 2 + (7 * strlen((const char *) dataPacket.data)), gKeyTimeout == 0 ? 0xFF : 0x80,
+                   6);
         }
     } else {
 
 
         String[0] = 'M';
-        String[1] = 'E';
+        String[1] = 'S';
-        String[2] = 'S';
+        String[2] = 'G';
-        String[3] = 'S';
+        String[3] = ' ';
-        String[4] = 'A';
-        String[5] = 'G';
-        String[6] = 'E';
-        String[7] = 'S';
 
         for (size_t i = 8; i < 14; i++) {
             String[i] = ' ';
         }
 
+        String[4] = 'F';
+        String[5] = 'l';
+        String[6] = 'g';
+        String[7] = 's';
+        String[8] = ':';
+        String[9] = ' ';
+        u8_to_str(loadedPacket.flags, &String[10]);  // Write at offset 7
 
-        String[14] = '0' + gActiveMessage;
+        String[13] = ' ';
+        if (gActiveMessage == MESSAGES_COUNT) {
+            String[14] = 'T';
+        } else {
+            String[14] = '0' + gActiveMessage;
+        }
         String[15] = '/';
         String[16] = '0' + MESSAGES_COUNT;
         String[17] = 0;
 
         UI_PrintString(String, 2, 0, 0 /*, 8 */);
 
-        String[0] = 'F';
-        String[1] = 'r';
-        String[2] = 'o';
-        String[3] = 'm';
-        String[4] = ':';
         String[5] = ' ';
-        u32_to_str(loadedPacket.src, &String[6]);  // Write at offset 6
+        if (gActiveMessage == MESSAGES_COUNT) {
+            String[0] = 'T';
+            String[1] = 'o';
+            String[2] = ':';
+            String[3] = ' ';
+            String[4] = ' ';
+        } else {
+            String[0] = 'F';
+            String[1] = 'r';
+            String[2] = 'o';
+            String[3] = 'm';
+            String[4] = ':';
+        }
+        u32_to_str(gActiveMessage == MESSAGES_COUNT ? dataPacket.dest : loadedPacket.src,
+                   &String[6]);  // Write at offset 6
         UI_PrintString(String, 2, 0, 1 /*, 8 */);
 
         String[0] = 'S';
@@ -85,18 +103,19 @@ void UI_DisplayMessages(void) {
         u8_to_str(loadedPacket.ttl, &String[14]);  // Write at new offset
         UI_PrintString(String, 2, 0, 2 /*, 8 */);
 
-        String[0] = 'F';
-        String[1] = 'l';
-        String[2] = 'g';
-        String[3] = 's';
-        String[4] = ':';
-        String[5] = ' ';
-        String[6] = 0;
-        u8_to_str(loadedPacket.flags, &String[6]);  // Write at offset 7
-        UI_PrintString(String, 2, 0, 3 /*, 8 */);
 
-        UI_PrintString("Data:", 0, 0, 4 /*, 8 */);
+        char String2[13] = "Data:        ";
-        UI_PrintString((const char *) loadedPacket.data, 2, 0, 5 /*, 8 */);
+        if (gGotACK) {
+            String2[6] = 'G';
+            String2[7] = 'o';
+            String2[8] = 't';
+            String2[9] = 'A';
+            String2[10] = 'C';
+            String2[11] = 'K';
+        }
+        String2[12] = 0;
+        UI_PrintString(String2, 0, 0, 3 /*, 8 */);
+        UI_PrintString((const char *) loadedPacket.data, 2, 0, 4 /*, 8 */);
     }
     ST7565_BlitFullScreen();
 }