diff --git a/DMRTX.cpp b/DMRTX.cpp
index af634ea..fbc23ff 100644
--- a/DMRTX.cpp
+++ b/DMRTX.cpp
@@ -84,7 +84,8 @@ m_frameCount(0U),
 m_abortCount(),
 m_abort(),
 m_controlChannel(false),
-m_trunking(false)
+m_trunking(false),
+m_colorCode(0U)
 {
   ::memset(m_modState, 0x00U, 16U * sizeof(q15_t));
 
@@ -227,6 +228,12 @@ uint8_t CDMRTX::writeAloha(const uint8_t* data, uint16_t length)
     return 4U;
 
   ::memcpy(m_aloha, data, length);
+
+  // Stamp the slot type with the current colour code so that the result
+  // doesn't depend on the ordering of the ALOHA and SET_CONFIG commands.
+  CDMRSlotType slotType;
+  slotType.encode(m_colorCode, DT_CSBK, m_aloha);
+
   m_controlChannel = true;
 
   return 0U;
@@ -386,7 +393,7 @@ void CDMRTX::createCACH(uint8_t txSlotIndex, uint8_t rxSlotIndex)
         ::memcpy(m_shortLC, EMPTY_SHORT_LC, 12U);
     } else {
       ::memcpy(m_shortLC, m_newShortLC, 12U);
-	}
+    }
   }
 
   ::memcpy(m_poBuffer, m_shortLC + m_cachPtr, 3U);
@@ -426,6 +433,8 @@ void CDMRTX::createCACH(uint8_t txSlotIndex, uint8_t rxSlotIndex)
 
 void CDMRTX::setColorCode(uint8_t colorCode)
 {
+  m_colorCode = colorCode;
+
   if (m_trunking)
     ::memcpy(m_idle, TERMINATOR_DATA, DMR_FRAME_LENGTH_BYTES);
   else
@@ -458,6 +467,9 @@ uint32_t CDMRTX::getFrameCount()
 void CDMRTX::setTrunking(bool trunking)
 {
   m_trunking = trunking;
+
+  if (!trunking)
+    m_controlChannel = false;
 }
 
 #endif
diff --git a/DMRTX.h b/DMRTX.h
index b58b87c..ba2ef65 100644
--- a/DMRTX.h
+++ b/DMRTX.h
@@ -83,6 +83,7 @@ private:
   bool                             m_abort[2U];
   bool                             m_controlChannel;
   bool                             m_trunking;
+  uint8_t                          m_colorCode;
 
   void createData(uint8_t slotIndex);
   void createCACH(uint8_t txSlotIndex, uint8_t rxSlotIndex);
diff --git a/FMNoiseSquelch.cpp b/FMNoiseSquelch.cpp
index bd01fe9..dd99cf5 100644
--- a/FMNoiseSquelch.cpp
+++ b/FMNoiseSquelch.cpp
@@ -36,7 +36,8 @@ m_count(0U),
 m_q0(0),
 m_q1(0),
 m_state(false),
-m_validCount(0U)
+m_validCount(0U),
+m_invalidCount(0U)
 {
 
 }
@@ -127,6 +128,8 @@ void CFMNoiseSquelch::reset()
   m_q1 = 0;
   m_state = false;
   m_count = 0U;
+  m_validCount = 0U;
+  m_invalidCount = 0U;
 }
 
 #endif
diff --git a/IODue.cpp b/IODue.cpp
index e77c388..8b5655f 100644
--- a/IODue.cpp
+++ b/IODue.cpp
@@ -110,7 +110,7 @@ void CIO::initInt()
 #if !defined(USE_ALTERNATE_POCSAG_LEDS)
   pinMode(PIN_POCSAG, OUTPUT);
 #endif
-#if !defined(USE_ALTERNATE_POCSAG_LEDS)
+#if !defined(USE_ALTERNATE_FM_LEDS)
   pinMode(PIN_FM,     OUTPUT);
 #endif
 #endif
diff --git a/STMUART.cpp b/STMUART.cpp
index c2e779d..8742dc5 100644
--- a/STMUART.cpp
+++ b/STMUART.cpp
@@ -61,7 +61,7 @@ void CSTMUART::handleIRQ()
   if (USART_GetITStatus(m_usart, USART_IT_RXNE)) {
     if(!m_rxFifo.isFull())
       m_rxFifo.put((uint8_t) USART_ReceiveData(m_usart));
-    USART_ClearITPendingBit(USART1, USART_IT_RXNE);
+    USART_ClearITPendingBit(m_usart, USART_IT_RXNE);
   }
 
   if (USART_GetITStatus(m_usart, USART_IT_TXE)) {
@@ -82,8 +82,12 @@ void CSTMUART::flush()
   if(m_usart == NULL)
     return;
 
-   // wait until the TXE shows the shift register is empty
-   while (USART_GetITStatus(m_usart, USART_FLAG_TXE))
+   // wait until the TX FIFO has drained
+   while (!m_txFifo.isEmpty())
+      ;
+
+   // wait until the TC flag shows the shift register is empty
+   while (USART_GetFlagStatus(m_usart, USART_FLAG_TC) == RESET)
       ;
 }
 
diff --git a/SerialPort.cpp b/SerialPort.cpp
index 7c6eb0c..220af49 100644
--- a/SerialPort.cpp
+++ b/SerialPort.cpp
@@ -745,7 +745,7 @@ void CSerialPort::setMode(MMDVM_STATE modemState)
       DEBUG1("Mode set to FM 20Khz Calibrate");
       break;
     case STATE_FMCAL25K:
-      DEBUG1("Mode set to FM 10Khz Calibrate");
+      DEBUG1("Mode set to FM 25Khz Calibrate");
       break;
     case STATE_FMCAL30K:
       DEBUG1("Mode set to FM 30Khz Calibrate");