diff --git a/IOSTM.cpp b/IOSTM.cpp
index fdfb2ae..12265ad 100644
--- a/IOSTM.cpp
+++ b/IOSTM.cpp
@@ -23,44 +23,124 @@
 
 #if defined(STM32F4XX) || defined(STM32F4)
 
+#if defined(STM32F4_DISCOVERY)
 /*
-Pin definitions:
+Pin definitions for STM32F4 Discovery Board:
 
-PTT      PA6   output
+PTT      PB13   output
 COSLED   PA7   output
 LED      PD15  output
 COS      PA5   input
-ADC1     PA0   analog input
-ADC2     PA1   analog input
-DAC1     PA4   analog output
 
 DSTAR	 PD12   output
 DMR		 PD13   output
 YSF		 PD14   output
 P25		 PD11   output
+
+RX		 PA0   analog input
+RSSI     PA1   analog input
+TX       PA4   analog output
 */
 
-#define PIN_COS                GPIO_Pin_5
-#define PORT_COS               GPIOA
-#define PIN_PTT                GPIO_Pin_6
-#define PORT_PTT               GPIOA
-#define PIN_COSLED             GPIO_Pin_7
-#define PORT_COSLED            GPIOA
-#define PIN_LED                GPIO_Pin_15
-#define PORT_LED               GPIOD
-#define PIN_ADC                GPIO_Pin_0
-#define PORT_ADC               GPIOA
-#define PIN_DAC                GPIO_Pin_4
-#define PORT_DAC               GPIOA
+#define PIN_COS				GPIO_Pin_5
+#define PORT_COS			GPIOA
+#define RCC_Per_COS			RCC_AHB1Periph_GPIOA
+
+#define PIN_PTT				GPIO_Pin_13
+#define PORT_PTT			GPIOB
+#define RCC_Per_PTT			RCC_AHB1Periph_GPIOB
+
+#define PIN_COSLED			GPIO_Pin_7
+#define PORT_COSLED			GPIOA
+#define RCC_Per_COSLED		RCC_AHB1Periph_GPIOA
+
+#define PIN_LED				GPIO_Pin_15
+#define PORT_LED			GPIOD
+#define RCC_Per_LED			RCC_AHB1Periph_GPIOD
+
+#define PIN_P25				GPIO_Pin_11
+#define PORT_P25			GPIOD
+#define RCC_Per_P25			RCC_AHB1Periph_GPIOD
+
+#define PIN_DSTAR			GPIO_Pin_12
+#define PORT_DSTAR			GPIOD
+#define RCC_Per_DSTAR		RCC_AHB1Periph_GPIOD
+
+#define PIN_DMR				GPIO_Pin_13
+#define PORT_DMR			GPIOD
+#define RCC_Per_DMR			RCC_AHB1Periph_GPIOD
+
+#define PIN_YSF				GPIO_Pin_14
+#define PORT_YSF			GPIOD
+#define RCC_Per_YSF			RCC_AHB1Periph_GPIOD
+
+#define PIN_RX				GPIO_Pin_0
+#define PIN_RX_CH			ADC_Channel_0
+
+#define PIN_RSSI			GPIO_Pin_1
+#define PIN_RSSI_CH			ADC_Channel_1
+
+#elif defined(STM32F4_PI)
+/*
+Pin definitions for STM32F4 Pi Board:
+
+PTT      PB13   output
+COSLED   PB14   output
+LED      PB15  output
+COS      PC0   input
+
+DSTAR	 PC7   output
+DMR		 PC8   output
+YSF		 PA8   output
+P25		 PC9   output
+
+RX		 PA0   analog input
+RSSI     PA7   analog input
+TX       PA4   analog output
+*/
+
+#define PIN_COS				GPIO_Pin_0
+#define PORT_COS			GPIOC
+#define RCC_Per_COS			RCC_AHB1Periph_GPIOC
+
+#define PIN_PTT				GPIO_Pin_13
+#define PORT_PTT			GPIOB
+#define RCC_Per_PTT			RCC_AHB1Periph_GPIOB
+
+#define PIN_COSLED			GPIO_Pin_14
+#define PORT_COSLED			GPIOB
+#define RCC_Per_COSLED		RCC_AHB1Periph_GPIOB
+
+#define PIN_LED				GPIO_Pin_15
+#define PORT_LED			GPIOB
+#define RCC_Per_LED			RCC_AHB1Periph_GPIOB
+
+#define PIN_P25				GPIO_Pin_9
+#define PORT_P25			GPIOC
+#define RCC_Per_P25			RCC_AHB1Periph_GPIOC
+
+#define PIN_DSTAR			GPIO_Pin_7
+#define PORT_DSTAR			GPIOC
+#define RCC_Per_DSTAR		RCC_AHB1Periph_GPIOC
+
+#define PIN_DMR				GPIO_Pin_8
+#define PORT_DMR			GPIOC
+#define RCC_Per_DMR			RCC_AHB1Periph_GPIOC
+
+#define PIN_YSF				GPIO_Pin_8
+#define PORT_YSF			GPIOA
+#define RCC_Per_YSF			RCC_AHB1Periph_GPIOA
+
+#define PIN_RX				GPIO_Pin_0
+#define PIN_RX_CH			ADC_Channel_0
+
+#define PIN_RSSI			GPIO_Pin_7
+#define PIN_RSSI_CH			ADC_Channel_7
+
+#else
+#error "Either STM32F4_DISCOVERY or STM32F4_PI need to be defined"
+#endif
 
-#define PIN_P25                GPIO_Pin_11
-#define PORT_P25               GPIOD
-#define PIN_DSTAR              GPIO_Pin_12
-#define PORT_DSTAR             GPIOD
-#define PIN_DMR                GPIO_Pin_13
-#define PORT_DMR               GPIOD
-#define PIN_YSF                GPIO_Pin_14
-#define PORT_YSF               GPIOD
 
 const uint16_t DC_OFFSET = 2048U;
 
@@ -76,86 +156,58 @@ extern "C" {
 void CIO::initInt()
 {  
   GPIO_InitTypeDef GPIO_InitStruct;
-
-  // PTT pin
-  RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
   GPIO_StructInit(&GPIO_InitStruct);
-  GPIO_InitStruct.GPIO_Pin   = PIN_PTT;
-  GPIO_InitStruct.GPIO_Mode  = GPIO_Mode_OUT;
   GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
   GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
   GPIO_InitStruct.GPIO_PuPd  = GPIO_PuPd_DOWN;
+  
+  // PTT pin
+  RCC_AHB1PeriphClockCmd(RCC_Per_PTT, ENABLE);
+  GPIO_InitStruct.GPIO_Pin   = PIN_PTT;
+  GPIO_InitStruct.GPIO_Mode  = GPIO_Mode_OUT;
   GPIO_Init(PORT_PTT, &GPIO_InitStruct);
 
   // COSLED pin
-  RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
-  GPIO_StructInit(&GPIO_InitStruct);
+  RCC_AHB1PeriphClockCmd(RCC_Per_COSLED, ENABLE);
   GPIO_InitStruct.GPIO_Pin   = PIN_COSLED;
   GPIO_InitStruct.GPIO_Mode  = GPIO_Mode_OUT;
-  GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
-  GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
-  GPIO_InitStruct.GPIO_PuPd  = GPIO_PuPd_DOWN;
   GPIO_Init(PORT_COSLED, &GPIO_InitStruct);
 
   // LED pin
-  RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);
-  GPIO_StructInit(&GPIO_InitStruct);
+  RCC_AHB1PeriphClockCmd(RCC_Per_LED, ENABLE);
   GPIO_InitStruct.GPIO_Pin   = PIN_LED;
   GPIO_InitStruct.GPIO_Mode  = GPIO_Mode_OUT;
-  GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
-  GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
-  GPIO_InitStruct.GPIO_PuPd  = GPIO_PuPd_DOWN;
   GPIO_Init(PORT_LED, &GPIO_InitStruct);
 
   // Init the input pins PIN_COS
-  RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
-  GPIO_StructInit(&GPIO_InitStruct);
+  RCC_AHB1PeriphClockCmd(RCC_Per_COS, ENABLE);
   GPIO_InitStruct.GPIO_Pin   = PIN_COS;
   GPIO_InitStruct.GPIO_Mode  = GPIO_Mode_IN;
-  GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
-  GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
-  GPIO_InitStruct.GPIO_PuPd  = GPIO_PuPd_NOPULL;
   GPIO_Init(PORT_COS, &GPIO_InitStruct);
 
 #if defined(ARDUINO_MODE_PINS)
   // DSTAR pin
-  RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);
-  GPIO_StructInit(&GPIO_InitStruct);
+  RCC_AHB1PeriphClockCmd(RCC_Per_DSTAR, ENABLE);
   GPIO_InitStruct.GPIO_Pin   = PIN_DSTAR;
   GPIO_InitStruct.GPIO_Mode  = GPIO_Mode_OUT;
-  GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
-  GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
-  GPIO_InitStruct.GPIO_PuPd  = GPIO_PuPd_DOWN;
   GPIO_Init(PORT_DSTAR, &GPIO_InitStruct);
 
   // DMR pin
-  RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);
-  GPIO_StructInit(&GPIO_InitStruct);
+  RCC_AHB1PeriphClockCmd(RCC_Per_DMR, ENABLE);
   GPIO_InitStruct.GPIO_Pin   = PIN_DMR;
   GPIO_InitStruct.GPIO_Mode  = GPIO_Mode_OUT;
-  GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
-  GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
-  GPIO_InitStruct.GPIO_PuPd  = GPIO_PuPd_DOWN;
   GPIO_Init(PORT_DMR, &GPIO_InitStruct);
 
   // YSF pin
-  RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);
-  GPIO_StructInit(&GPIO_InitStruct);
+  RCC_AHB1PeriphClockCmd(RCC_Per_YSF, ENABLE);
   GPIO_InitStruct.GPIO_Pin   = PIN_YSF;
   GPIO_InitStruct.GPIO_Mode  = GPIO_Mode_OUT;
-  GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
-  GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
-  GPIO_InitStruct.GPIO_PuPd  = GPIO_PuPd_DOWN;
   GPIO_Init(PORT_YSF, &GPIO_InitStruct);
 
   // P25 pin
-  RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);
-  GPIO_StructInit(&GPIO_InitStruct);
+  RCC_AHB1PeriphClockCmd(RCC_Per_P25, ENABLE);
   GPIO_InitStruct.GPIO_Pin   = PIN_P25;
   GPIO_InitStruct.GPIO_Mode  = GPIO_Mode_OUT;
-  GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
-  GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
-  GPIO_InitStruct.GPIO_PuPd  = GPIO_PuPd_DOWN;
   GPIO_Init(PORT_P25, &GPIO_InitStruct);
 #endif
 }
@@ -164,10 +216,6 @@ void CIO::startInt()
 {
   if ((ADC_GetFlagStatus(ADC1, ADC_FLAG_EOC) != RESET))
     io.interrupt(0U);
-
-  // ADC1   PA0   analog input
-  // ADC2   PA1   analog input
-  // DAC1   PA4   analog output
   
   // Init the ADC
   GPIO_InitTypeDef        GPIO_InitStruct;
@@ -186,11 +234,10 @@ void CIO::startInt()
   RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE);
 #endif
   
-  // For ADC1 on PA0, ADC2 on PA1
 #if defined(SEND_RSSI_DATA)
-  GPIO_InitStruct.GPIO_Pin  = GPIO_Pin_0 | GPIO_Pin_1;
+  GPIO_InitStruct.GPIO_Pin  = PIN_RX | PIN_RSSI;
 #else
-  GPIO_InitStruct.GPIO_Pin  = GPIO_Pin_0;
+  GPIO_InitStruct.GPIO_Pin  = PIN_RX;
 #endif
   GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AN;
   GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_NOPULL ;
@@ -219,7 +266,7 @@ void CIO::startInt()
   ADC_Init(ADC1, &ADC_InitStructure);
 
   ADC_EOCOnEachRegularChannelCmd(ADC1, ENABLE);
-  ADC_RegularChannelConfig(ADC1, ADC_Channel_0, 1, ADC_SampleTime_3Cycles);
+  ADC_RegularChannelConfig(ADC1, PIN_RX_CH, 1, ADC_SampleTime_3Cycles);
 
   // Enable ADC1
   ADC_Cmd(ADC1, ENABLE);
@@ -228,7 +275,7 @@ void CIO::startInt()
   ADC_Init(ADC2, &ADC_InitStructure);
 
   ADC_EOCOnEachRegularChannelCmd(ADC2, ENABLE);
-  ADC_RegularChannelConfig(ADC2, ADC_Channel_1, 1, ADC_SampleTime_3Cycles);
+  ADC_RegularChannelConfig(ADC2, PIN_RSSI_CH, 1, ADC_SampleTime_3Cycles);
 
   // Enable ADC2
   ADC_Cmd(ADC2, ENABLE);
@@ -240,13 +287,13 @@ void CIO::startInt()
   GPIO_StructInit(&GPIO_InitStruct);
   DAC_StructInit(&DAC_InitStructure);
 
-  // GPIOA & D clock enable
+  // GPIOA clock enable
   RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
 
   // DAC Periph clock enable
   RCC_APB1PeriphClockCmd(RCC_APB1Periph_DAC, ENABLE);
 
-  // GPIO CONFIGURATION of DAC Pins
+  // GPIO CONFIGURATION of DAC Pins (PA4)
   GPIO_InitStruct.GPIO_Pin   = GPIO_Pin_4;
   GPIO_InitStruct.GPIO_Mode  = GPIO_Mode_AN;
   GPIO_InitStruct.GPIO_PuPd  = GPIO_PuPd_NOPULL;
@@ -263,7 +310,10 @@ void CIO::startInt()
 
   TIM_TimeBaseInitTypeDef timerInitStructure;
   TIM_TimeBaseStructInit (&timerInitStructure);
-  timerInitStructure.TIM_Prescaler         = 1749;  // 24 kHz
+  
+  // TIM2 at 24 kHz 
+  timerInitStructure.TIM_Prescaler = (uint16_t) ((SystemCoreClock/(4*24000)) - 1);
+
   timerInitStructure.TIM_CounterMode       = TIM_CounterMode_Up;
   timerInitStructure.TIM_Period            = 1;
   timerInitStructure.TIM_ClockDivision     = TIM_CKD_DIV1;
diff --git a/Makefile b/Makefile
index 3a2d5fa..d23d1e8 100644
--- a/Makefile
+++ b/Makefile
@@ -85,9 +85,9 @@ MCFLAGS=-mcpu=cortex-m4 -mthumb -mlittle-endian \
 
 # COMPILE FLAGS
 # STM32F4 Discovery board:
-DEFS_DIS=-DUSE_STDPERIPH_DRIVER -DSTM32F4XX -DSTM32F40_41xxx -DHSE_VALUE=$(OSC)
+DEFS_DIS=-DUSE_STDPERIPH_DRIVER -DSTM32F4XX -DSTM32F40_41xxx -DSTM32F4_DISCOVERY -DHSE_VALUE=$(OSC)
 # Pi board:
-DEFS_PI=-DUSE_STDPERIPH_DRIVER -DSTM32F4XX -DSTM32F446xx -DHSE_VALUE=$(OSC)
+DEFS_PI=-DUSE_STDPERIPH_DRIVER -DSTM32F4XX -DSTM32F446xx -DSTM32F4_PI -DHSE_VALUE=$(OSC)
 CFLAGS=-c $(MCFLAGS) $(INCLUDES)
 CXXFLAGS=-c $(MCFLAGS) $(INCLUDES)
 
diff --git a/SerialSTM.cpp b/SerialSTM.cpp
index 73cde0a..2955508 100644
--- a/SerialSTM.cpp
+++ b/SerialSTM.cpp
@@ -24,7 +24,7 @@
 
 #if defined(STM32F4XX) || defined(STM32F4)
 
-volatile uint32_t intcount1, intcount3;
+volatile uint32_t intcount1, intcount3, intcount5;
 
 #define TX_SERIAL_FIFO_SIZE 256U
 #define RX_SERIAL_FIFO_SIZE 256U
@@ -39,9 +39,15 @@ volatile uint8_t  RXSerialfifo3[RX_SERIAL_FIFO_SIZE];
 volatile uint16_t TXSerialfifohead3, TXSerialfifotail3;
 volatile uint16_t RXSerialfifohead3, RXSerialfifotail3;
 
+volatile uint8_t  TXSerialfifo5[TX_SERIAL_FIFO_SIZE];
+volatile uint8_t  RXSerialfifo5[RX_SERIAL_FIFO_SIZE];
+volatile uint16_t TXSerialfifohead5, TXSerialfifotail5;
+volatile uint16_t RXSerialfifohead5, RXSerialfifotail5;
+
 extern "C" {
   void USART1_IRQHandler();
   void USART3_IRQHandler();
+  void UART5_IRQHandler();
 }
 
 /* ************* USART1 ***************** */
@@ -330,15 +336,15 @@ void USART3_IRQHandler()
 
 void InitUSART3(int speed)
 {
-	// USART3 - TXD PB10 - RXD PB11 - pins when jumpered from FTDI board to F4Discovery board
+	// USART3 - TXD PC10 - RXD PC11
 	GPIO_InitTypeDef GPIO_InitStructure;
 	USART_InitTypeDef USART_InitStructure;
 	NVIC_InitTypeDef NVIC_InitStructure;
 
-	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
+	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE);
 	RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE);
-	GPIO_PinAFConfig(GPIOB, GPIO_PinSource10, GPIO_AF_USART3);
-	GPIO_PinAFConfig(GPIOB, GPIO_PinSource11, GPIO_AF_USART3);
+	GPIO_PinAFConfig(GPIOC, GPIO_PinSource10, GPIO_AF_USART3);
+	GPIO_PinAFConfig(GPIOC, GPIO_PinSource11, GPIO_AF_USART3);
 
 	// USART IRQ init
 	NVIC_InitStructure.NVIC_IRQChannel    = USART3_IRQn;
@@ -354,7 +360,7 @@ void InitUSART3(int speed)
 	GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_AF;
 	GPIO_InitStructure.GPIO_Pin   = GPIO_Pin_10 | GPIO_Pin_11; //  Tx | Rx
 	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
-	GPIO_Init(GPIOB, &GPIO_InitStructure);
+	GPIO_Init(GPIOC, &GPIO_InitStructure);
 
 	// Configure USART baud rate
 	USART_StructInit(&USART_InitStructure);
@@ -402,16 +408,203 @@ void WriteUSART3(const uint8_t* data, uint16_t length)
 	USART_ITConfig(USART3, USART_IT_TXE, ENABLE);
 }
 
+/* ************* UART5 ***************** */
+
+// Init queues
+void TXSerialfifoinit5()
+{
+	TXSerialfifohead5 = 0U;
+	TXSerialfifotail5 = 0U;
+}
+
+void RXSerialfifoinit5()
+{
+	RXSerialfifohead5 = 0U;
+	RXSerialfifotail5 = 0U;
+}
+
+// How full is queue
+// TODO decide if how full or how empty is preferred info to return
+uint16_t TXSerialfifolevel5()
+{
+  uint32_t tail = TXSerialfifotail5;
+  uint32_t head = TXSerialfifohead5;
+
+  if (tail > head)
+    return TX_SERIAL_FIFO_SIZE + head - tail;
+  else
+    return head - tail;
+}
+
+uint16_t RXSerialfifolevel5()
+{
+  uint32_t tail = RXSerialfifotail5;
+  uint32_t head = RXSerialfifohead5;
+
+  if (tail > head)
+    return RX_SERIAL_FIFO_SIZE + head - tail;
+  else
+    return head - tail;
+}
+
+// Flushes the transmit shift register
+// warning: this call is blocking
+void TXSerialFlush5()
+{
+  // wait until the TXE shows the shift register is empty
+  while (USART_GetITStatus(UART5, USART_FLAG_TXE))
+    ;
+}
+
+uint8_t TXSerialfifoput5(uint8_t next)
+{
+  if (TXSerialfifolevel5() < TX_SERIAL_FIFO_SIZE) {
+    TXSerialfifo5[TXSerialfifohead5] = next;
+
+    TXSerialfifohead5++;
+    if (TXSerialfifohead5 >= TX_SERIAL_FIFO_SIZE)
+      TXSerialfifohead5 = 0U;
+
+    // make sure transmit interrupts are enabled as long as there is data to send
+    USART_ITConfig(UART5, USART_IT_TXE, ENABLE);
+    return 1U;
+  } else {
+    return 0U; // signal an overflow occurred by returning a zero count
+  }
+}
+
+void UART5_IRQHandler()
+{
+  uint8_t c;
+
+  if (USART_GetITStatus(UART5, USART_IT_RXNE)) {
+    c = (uint8_t) USART_ReceiveData(UART5);
+
+    if (RXSerialfifolevel5() < RX_SERIAL_FIFO_SIZE) {
+      RXSerialfifo5[RXSerialfifohead5] = c;
+
+      RXSerialfifohead5++;
+      if (RXSerialfifohead5 >= RX_SERIAL_FIFO_SIZE)
+        RXSerialfifohead5 = 0U;
+    } else {
+      // TODO - do something if rx fifo is full?
+    }
+
+    USART_ClearITPendingBit(UART5, USART_IT_RXNE);
+    intcount5++;
+  }
+
+  if (USART_GetITStatus(UART5, USART_IT_TXE)) {
+    c = 0U;
+
+    if (TXSerialfifohead5 != TXSerialfifotail5) { // if the fifo is not empty
+      c = TXSerialfifo5[TXSerialfifotail5];
+
+      TXSerialfifotail5++;
+      if (TXSerialfifotail5 >= TX_SERIAL_FIFO_SIZE)
+        TXSerialfifotail5 = 0U;
+
+      USART_SendData(UART5, c);
+    } else { // if there's no more data to transmit then turn off TX interrupts
+      USART_ITConfig(UART5, USART_IT_TXE, DISABLE);
+    }
+
+    USART_ClearITPendingBit(UART5, USART_IT_TXE);
+  }
+}
+
+void InitUART5(int speed)
+{
+	// USART5 - TXD PC12 - RXD PD2
+	GPIO_InitTypeDef GPIO_InitStructure;
+	USART_InitTypeDef USART_InitStructure;
+	NVIC_InitTypeDef NVIC_InitStructure;
+
+	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE);
+	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);
+	RCC_APB1PeriphClockCmd(RCC_APB1Periph_UART5, ENABLE);
+	GPIO_PinAFConfig(GPIOC, GPIO_PinSource12, GPIO_AF_UART5);
+	GPIO_PinAFConfig(GPIOD, GPIO_PinSource2, GPIO_AF_UART5);
+
+	// USART IRQ init
+	NVIC_InitStructure.NVIC_IRQChannel    = UART5_IRQn;
+	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
+	NVIC_InitStructure.NVIC_IRQChannelSubPriority        = 1;
+	NVIC_Init(&NVIC_InitStructure);
+
+	// Configure USART as alternate function
+	GPIO_StructInit(&GPIO_InitStructure);
+	GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+	GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_UP;
+	GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_AF;
+	GPIO_InitStructure.GPIO_Pin   = GPIO_Pin_12; //  Tx
+	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+	GPIO_Init(GPIOC, &GPIO_InitStructure);
+
+	GPIO_InitStructure.GPIO_Pin   = GPIO_Pin_2; //  Rx
+	GPIO_Init(GPIOD, &GPIO_InitStructure);
+
+	// Configure USART baud rate
+	USART_StructInit(&USART_InitStructure);
+	USART_InitStructure.USART_BaudRate   = speed;
+	USART_InitStructure.USART_WordLength = USART_WordLength_8b;
+	USART_InitStructure.USART_StopBits   = USART_StopBits_1;
+	USART_InitStructure.USART_Parity     = USART_Parity_No;
+	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
+	USART_InitStructure.USART_Mode       = USART_Mode_Rx | USART_Mode_Tx;
+	USART_Init(UART5, &USART_InitStructure);
+
+	USART_Cmd(UART5, ENABLE);
+
+	USART_ITConfig(UART5, USART_IT_RXNE, ENABLE);
+
+	// initialize the fifos
+	TXSerialfifoinit5();
+	RXSerialfifoinit5();
+}
+
+uint8_t AvailUART5(void)
+{
+	if (RXSerialfifolevel5() > 0U)
+		return 1U;
+	else
+		return 0U;
+}
+
+uint8_t ReadUART5(void)
+{
+	uint8_t data_c = RXSerialfifo5[RXSerialfifotail5];
+
+  RXSerialfifotail5++;
+  if (RXSerialfifotail5 >= RX_SERIAL_FIFO_SIZE)
+    RXSerialfifotail5 = 0U;
+
+	return data_c;
+}
+
+void WriteUART5(const uint8_t* data, uint16_t length)
+{
+	for (uint16_t i = 0U; i < length; i++)
+		TXSerialfifoput5(data[i]);
+		
+	USART_ITConfig(UART5, USART_IT_TXE, ENABLE);
+}
+
 /////////////////////////////////////////////////////////////////
 
 void CSerialPort::beginInt(uint8_t n, int speed)
 {
   switch (n) {
     case 1U:
-		  InitUSART3(speed);
+		#if defined(STM32F4_DISCOVERY)
+			InitUSART3(speed);
+		#elif defined(STM32F4_PI)
+			InitUSART1(speed);
+		#endif
       break;
     case 3U:
-		  InitUSART1(speed);
+		  InitUART5(speed);
       break;
     default:
       break;
@@ -421,10 +614,14 @@ void CSerialPort::beginInt(uint8_t n, int speed)
 int CSerialPort::availableInt(uint8_t n)
 { 
     switch (n) {
-      case 1U: 
-    	  return AvailUSART3();
+      case 1U:
+		#if defined(STM32F4_DISCOVERY)
+    		return AvailUSART3();
+		#elif defined(STM32F4_PI)
+    		return AvailUSART1();
+		#endif
       case 3U: 
-        return AvailUSART1();
+        return AvailUART5();
       default:
         return false;
   	}
@@ -434,9 +631,13 @@ uint8_t CSerialPort::readInt(uint8_t n)
 {   
 	switch (n) {
   	case 1U:
-	  	return ReadUSART3();
+		#if defined(STM32F4_DISCOVERY)
+	  		return ReadUSART3();
+		#elif defined(STM32F4_PI)
+	  		return ReadUSART1();
+		#endif	  	
 	  case 3U:
-		  return ReadUSART1();
+		  return ReadUART5();
 	  default:
       return 0U;
 	}
@@ -446,14 +647,20 @@ void CSerialPort::writeInt(uint8_t n, const uint8_t* data, uint16_t length, bool
 {
   switch (n) {
     case 1U:
-    	WriteUSART3(data, length);
-      if (flush)
-        TXSerialFlush3();
-      break;
+		#if defined(STM32F4_DISCOVERY)
+			WriteUSART3(data, length);
+		  if (flush)
+			TXSerialFlush3();
+		#elif defined(STM32F4_PI)
+			WriteUSART1(data, length);
+		  if (flush)
+			TXSerialFlush1();
+		#endif		
+		break;
     case 3U:
-      WriteUSART1(data, length);
+      WriteUART5(data, length);
       if (flush)
-        TXSerialFlush1();
+        TXSerialFlush5();
        break;
     default:
        break;