diff --git a/DMRDataHeader.cpp b/DMRDataHeader.cpp
index ef0922d..d1864a5 100644
--- a/DMRDataHeader.cpp
+++ b/DMRDataHeader.cpp
@@ -29,6 +29,8 @@
 #include <cassert>
 #include <cstring>
 
+const unsigned char UDTF_NMEA = 0x05U;
+
 CDMRDataHeader::CDMRDataHeader() :
 m_data(NULL),
 m_GI(false),
@@ -78,13 +80,13 @@ bool CDMRDataHeader::put(const unsigned char* bytes)
 
 	switch (dpf) {
 	case DPF_UNCONFIRMED_DATA:
-		CUtils::dump(1U, "Unconfirmed Data Header", m_data, 12U);
+		CUtils::dump(1U, "DMR, Unconfirmed Data Header", m_data, 12U);
 		m_F = (m_data[8U] & 0x80U) == 0x80U;
 		m_blocks = m_data[8U] & 0x7FU;
 		break;
 
 	case DPF_CONFIRMED_DATA:
-		CUtils::dump(1U, "Confirmed Data Header", m_data, 12U);
+		CUtils::dump(1U, "DMR, Confirmed Data Header", m_data, 12U);
 		m_F = (m_data[8U] & 0x80U) == 0x80U;
 		m_blocks = m_data[8U] & 0x7FU;
 		m_S = (m_data[9U] & 0x80U) == 0x80U;
@@ -92,38 +94,46 @@ bool CDMRDataHeader::put(const unsigned char* bytes)
 		break;
 
 	case DPF_RESPONSE:
-		CUtils::dump(1U, "Response Data Header", m_data, 12U);
+		CUtils::dump(1U, "DMR, Response Data Header", m_data, 12U);
 		m_blocks = m_data[8U] & 0x7FU;
 		break;
 
 	case DPF_PROPRIETARY:
-		CUtils::dump(1U, "Proprietary Data Header", m_data, 12U);
+		CUtils::dump(1U, "DMR, Proprietary Data Header", m_data, 12U);
 		break;
 
 	case DPF_DEFINED_RAW:
-		CUtils::dump(1U, "Raw or Status/Precoded Short Data Header", m_data, 12U);
+		CUtils::dump(1U, "DMR, Raw or Status/Precoded Short Data Header", m_data, 12U);
 		m_blocks = (m_data[0U] & 0x30U) + (m_data[1U] & 0x0FU);
 		m_F = (m_data[8U] & 0x01U) == 0x01U;
 		m_S = (m_data[8U] & 0x02U) == 0x02U;
 		break;
 
 	case DPF_DEFINED_SHORT:
-		CUtils::dump(1U, "Defined Short Data Header", m_data, 12U);
+		CUtils::dump(1U, "DMR, Defined Short Data Header", m_data, 12U);
 		m_blocks = (m_data[0U] & 0x30U) + (m_data[1U] & 0x0FU);
 		m_F = (m_data[8U] & 0x01U) == 0x01U;
 		m_S = (m_data[8U] & 0x02U) == 0x02U;
 		break;
 
 	case DPF_UDT:
-		CUtils::dump(1U, "Unified Data Transport Header", m_data, 12U);
+		CUtils::dump(1U, "DMR, Unified Data Transport Header", m_data, 12U);
 		m_blocks = m_data[8U] & 0x03U;
 		break;
 
 	default:
-		CUtils::dump("Unknown Data Header", m_data, 12U);
+		CUtils::dump("DMR, Unknown Data Header", m_data, 12U);
 		break;
 	}
 
+	if (dpf == DPF_UDT && m_blocks == 0U) {
+		unsigned char format = m_data[1U] & 0x0FU;
+		if (format == UDTF_NMEA) {
+			LogDebug("DMR, fixing broken Tytera MD-390 GPS data block count");
+			m_blocks = 3U;
+		}
+	}
+
 	return true;
 }
 
diff --git a/MMDVMHost.cpp b/MMDVMHost.cpp
index b2faa0d..6b495db 100644
--- a/MMDVMHost.cpp
+++ b/MMDVMHost.cpp
@@ -1144,6 +1144,12 @@ void CMMDVMHost::createDisplay()
 		m_display = new CNullDisplay;
 	}
 
+	if (m_display == NULL) {
+		LogWarning("No valid display found, disabling");
+		m_display = new CNullDisplay;
+		return;
+	}
+
 	bool ret = m_display->open();
 	if (!ret) {
 		delete m_display;
diff --git a/UMP/UMP.ino b/UMP/UMP.ino
index 55f5c86..607d653 100644
--- a/UMP/UMP.ino
+++ b/UMP/UMP.ino
@@ -25,15 +25,17 @@
 #endif
 
 #if defined(__MK20DX256__)
-#define PIN_DSTAR   2
-#define PIN_DMR     3
-#define PIN_YSF     4
-#define PIN_P25     5
+#define PIN_DSTAR   3
+#define PIN_DMR     4
+#define PIN_YSF     5
+#define PIN_P25     6
 
 #define PIN_TX      10
 #define PIN_CD      11
 
 #define PIN_LOCKOUT 12
+
+#define FLASH_DELAY 200000U
 #else
 #define PIN_DSTAR   2
 #define PIN_DMR     3
@@ -44,6 +46,8 @@
 #define PIN_CD      7
 
 #define PIN_LOCKOUT 8
+
+#define FLASH_DELAY 3200U
 #endif
 
 #if !defined(__AVR_ATmega1280__) && !defined(__AVR_ATmega2560__) && !defined(__AVR_ATmega32U4__) && !defined(__SAM3X8E__) && !defined(__MK20DX256__)
@@ -187,13 +191,13 @@ void loop()
 
   m_count++;
   if (m_started) {
-    if (m_count > 3200U) {
+    if (m_count > FLASH_DELAY) {
       digitalWrite(PIN_LED, m_led ? LOW : HIGH);
       m_led = !m_led;
       m_count = 0U;
     }
   } else {
-    if (m_count > 32000U) {
+    if (m_count > (FLASH_DELAY * 3U)) {
       digitalWrite(PIN_LED, m_led ? LOW : HIGH);
       m_led = !m_led;
       m_count = 0U;
diff --git a/YSFPayload.cpp b/YSFPayload.cpp
index c74d7d6..e6f4238 100644
--- a/YSFPayload.cpp
+++ b/YSFPayload.cpp
@@ -417,7 +417,7 @@ unsigned int CYSFPayload::processVDMode2AudioBlock(unsigned char* data)
 	// Deinterleave
 	for (unsigned int i = 0U; i < 104U; i++) {
 		unsigned int n = INTERLEAVE_TABLE_26_4[i];
-		bool s = READ_BIT1(data, n);
+		bool s = READ_BIT1(data, n) != 0x00U;
 		WRITE_BIT1(vch, i, s);
 	}
 
@@ -426,29 +426,35 @@ unsigned int CYSFPayload::processVDMode2AudioBlock(unsigned char* data)
 		vch[i] ^= WHITENING_DATA[i];
 
 	for (unsigned int i = 0U; i < 81U; i += 3U) {
-		unsigned int n = i;
-		bool bit1 = READ_BIT1(vch, n);
-		n++;
-		bool bit2 = READ_BIT1(vch, n);
-		n++;
-		bool bit3 = READ_BIT1(vch, n);
+		unsigned int n1 = i + 0U;
+		unsigned int n2 = i + 1U;
+		unsigned int n3 = i + 2U;
 
-		if ((bit1 && bit2 && !bit3) || (bit1 && !bit2 && bit3) || (!bit1 && bit2 && bit3)) {
-			unsigned int n = i;
-			WRITE_BIT1(vch, n, true);
-			n++;
-			WRITE_BIT1(vch, n, true);
-			n++;
-			WRITE_BIT1(vch, n, true);
+		unsigned char code = READ_BIT1(vch, n1) ? 1U : 0U +
+							 READ_BIT1(vch, n2) ? 2U : 0U +
+							 READ_BIT1(vch, n3) ? 4U : 0U;
+
+		switch (code) {
+		case 3U:
+		case 5U:
+		case 6U:
+			WRITE_BIT1(vch, n1, true);
+			WRITE_BIT1(vch, n2, true);
+			WRITE_BIT1(vch, n3, true);
 			errors++;
-		} else if ((!bit1 && !bit2 && bit3) || (!bit1 && bit2 && !bit3) || (bit1 && !bit2 && !bit3)) {
-			unsigned int n = i;
-			WRITE_BIT1(vch, n, false);
-			n++;
-			WRITE_BIT1(vch, n, false);
-			n++;
-			WRITE_BIT1(vch, n, false);
+			break;
+		case 1U:
+		case 2U:
+		case 4U:
+			WRITE_BIT1(vch, n1, false);
+			WRITE_BIT1(vch, n2, false);
+			WRITE_BIT1(vch, n3, false);
 			errors++;
+			break;
+		// case 0U:
+		// case 7U:
+		default:
+			break;
 		}
 	}
 
@@ -459,7 +465,7 @@ unsigned int CYSFPayload::processVDMode2AudioBlock(unsigned char* data)
 	// Interleave
 	for (unsigned int i = 0U; i < 104U; i++) {
 		unsigned int n = INTERLEAVE_TABLE_26_4[i];
-		bool s = READ_BIT1(vch, i);
+		bool s = READ_BIT1(vch, i) != 0x00U;
 		WRITE_BIT1(data, n, s);
 	}