diff --git a/flight/Bootloaders/OpenPilot/Makefile b/flight/Bootloaders/OpenPilot/Makefile
index 11d0c7f7e..27c91cdce 100644
--- a/flight/Bootloaders/OpenPilot/Makefile
+++ b/flight/Bootloaders/OpenPilot/Makefile
@@ -118,8 +118,8 @@ SRC += $(OPSYSTEM)/main.c
 SRC += $(OPSYSTEM)/pios_board.c
 SRC += $(OPSYSTEM)/op_dfu.c
 SRC += $(OPSYSTEM)/stopwatch.c
-
-
+SRC += $(OPSYSTEM)/ssp_timer.c
+SRC += $(OPSYSTEM)/ssp.c
 
 
 ## PIOS Hardware (STM32F10x)
@@ -329,7 +329,7 @@ endif
 
 CFLAGS += -O$(OPT)
 ifeq ($(DEBUG),NO)
-CFLAGS += -fdata-sections -ffunction-sections
+CFLAGS += -ffunction-sections
 endif
 
 CFLAGS += -mcpu=$(MCU) -mthumb
diff --git a/flight/Bootloaders/OpenPilot/inc/common.h b/flight/Bootloaders/OpenPilot/inc/common.h
index 3e8496d3f..e1194a01e 100644
--- a/flight/Bootloaders/OpenPilot/inc/common.h
+++ b/flight/Bootloaders/OpenPilot/inc/common.h
@@ -1,10 +1,30 @@
-/*
- * common.h
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotBL OpenPilot BootLoader
+ * @brief These files contain the code to the OpenPilot MB Bootloader.
  *
- *  Created on: 2010/08/18
- *      Author: Programacao
+ * @{
+ * @file       common.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      This file contains various common defines for the BootLoader
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
-
 #ifndef COMMON_H_
 #define COMMON_H_
 
diff --git a/flight/Bootloaders/OpenPilot/inc/op_dfu.h b/flight/Bootloaders/OpenPilot/inc/op_dfu.h
index b6e1bdbd9..e7e4faa06 100644
--- a/flight/Bootloaders/OpenPilot/inc/op_dfu.h
+++ b/flight/Bootloaders/OpenPilot/inc/op_dfu.h
@@ -1,17 +1,27 @@
-/******************** (C) COPYRIGHT 2010 STMicroelectronics ********************
-* File Name          : usb_desc.h
-* Author             : MCD Application Team
-* Version            : V3.2.1
-* Date               : 07/05/2010
-* Description        : Descriptor Header for Custom HID Demo
-********************************************************************************
-* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
-* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME.
-* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT,
-* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE
-* CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING
-* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
-*******************************************************************************/
+ /**
+ ******************************************************************************
+ *
+ * @file       op_dfu.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
 
 /* Define to prevent recursive inclusion -------------------------------------*/
 #ifndef __OP_DFU_H
diff --git a/flight/Bootloaders/OpenPilot/inc/pios_config.h b/flight/Bootloaders/OpenPilot/inc/pios_config.h
index 327a273f2..f0c46c8ed 100644
--- a/flight/Bootloaders/OpenPilot/inc/pios_config.h
+++ b/flight/Bootloaders/OpenPilot/inc/pios_config.h
@@ -1,10 +1,7 @@
 /**
  ******************************************************************************
- * @addtogroup OpenPilotSystem OpenPilot System
+ * @addtogroup OpenPilotBL OpenPilot BootLoader
  * @{
- * @addtogroup OpenPilotCore OpenPilot Core
- * @{
- *
  * @file       pios_config.h  
  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
  * @brief      PiOS configuration header. 
@@ -47,7 +44,7 @@
 #define PIOS_INCLUDE_COM
 #define PIOS_INCLUDE_GPIO
 #define PIOS_NO_GPS
-
+//#define DEBUG_SSP
 
 /* Defaults for Logging */
 #define LOG_FILENAME 			"PIOS.LOG"
diff --git a/flight/Bootloaders/OpenPilot/inc/ssp_timer.h b/flight/Bootloaders/OpenPilot/inc/ssp_timer.h
new file mode 100644
index 000000000..bf785d956
--- /dev/null
+++ b/flight/Bootloaders/OpenPilot/inc/ssp_timer.h
@@ -0,0 +1,55 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotBL OpenPilot BootLoader
+ * @{
+ *
+ * @file       ssp_timer.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Timer functions to be used with the SSP.
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#ifndef _SSP_TIMER_H
+#define _SSP_TIMER_H
+
+/////////////////////////////////////////////////////////////////////////////
+// Global definitions
+/////////////////////////////////////////////////////////////////////////////
+
+
+/////////////////////////////////////////////////////////////////////////////
+// Global Types
+/////////////////////////////////////////////////////////////////////////////
+
+
+/////////////////////////////////////////////////////////////////////////////
+// Prototypes
+/////////////////////////////////////////////////////////////////////////////
+
+extern s32 SSP_TIMER_Init(u32 resolution);
+extern s32 SSP_TIMER_Reset(void);
+extern u32 SSP_TIMER_ValueGet(void);
+
+
+/////////////////////////////////////////////////////////////////////////////
+// Export global variables
+/////////////////////////////////////////////////////////////////////////////
+
+
+#endif /* _SSP_TIMER_H */
diff --git a/flight/Bootloaders/OpenPilot/inc/stopwatch.h b/flight/Bootloaders/OpenPilot/inc/stopwatch.h
index 32e89f527..09fcbca12 100644
--- a/flight/Bootloaders/OpenPilot/inc/stopwatch.h
+++ b/flight/Bootloaders/OpenPilot/inc/stopwatch.h
@@ -1,3 +1,29 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotBL OpenPilot BootLoader
+ * @{
+ *
+ * @file       stopwatch.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Timer functions for the LED PWM.
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
 
 #ifndef _STOPWATCH_H
 #define _STOPWATCH_H
diff --git a/flight/Bootloaders/OpenPilot/main.c b/flight/Bootloaders/OpenPilot/main.c
index 067441183..6041fb77d 100644
--- a/flight/Bootloaders/OpenPilot/main.c
+++ b/flight/Bootloaders/OpenPilot/main.c
@@ -1,17 +1,12 @@
 /**
  ******************************************************************************
- * @addtogroup OpenPilotSystem OpenPilot System
- * @brief These files are the core system files of OpenPilot.
- * They are the ground layer just above PiOS. In practice, OpenPilot actually starts
- * in the main() function of openpilot.c
+ * @addtogroup OpenPilotBL OpenPilot BootLoader
+ * @brief These files contain the code to the OpenPilot MB Bootloader.
+ *
  * @{
- * @addtogroup OpenPilotCore OpenPilot Core
- * @brief This is where the OP firmware starts. Those files also define the compile-time
- * options of the firmware.
- * @{
- * @file       openpilot.c 
+ * @file       main.c
  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      Sets up and runs main OpenPilot tasks.
+ * @brief      This is the file with the main function of the OpenPilot BootLoader
  * @see        The GNU Public License (GPL) Version 3
  * 
  *****************************************************************************/
@@ -30,15 +25,16 @@
  * with this program; if not, write to the Free Software Foundation, Inc., 
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
-
-/* OpenPilot Includes */
-//#include "openpilot.h"
+/* Bootloader Includes */
 #include <pios.h>
 #include "pios_opahrs.h"
 #include "stopwatch.h"
+#include "ssp_timer.h"
 #include "op_dfu.h"
 #include "usb_lib.h"
 #include "pios_iap.h"
+#include "ssp.h"
+#include "fifo_buffer.h"
 /* Prototype of PIOS_Board_Init() function */
 extern void PIOS_Board_Init(void);
 extern void FLASH_Download();
@@ -58,9 +54,37 @@ uint32_t sweep_steps1 = 100; // * 5 mS -> 500 mS
 uint32_t period2 = 50; // *100 uS -> 5 mS
 uint32_t sweep_steps2 = 100; // * 5 mS -> 500 mS
 
+
+////////////////////////////////////////
+uint8_t tempcount=0;
+
+
+/// SSP SECTION
+/// SSP TIME SOURCE
+uint32_t ssp_time;
+#define MAX_PACKET_DATA_LEN	255
+#define MAX_PACKET_BUF_SIZE	(1+1+MAX_PACKET_DATA_LEN+2)
+#define UART_BUFFER_SIZE 1024
+uint8_t rx_buffer[UART_BUFFER_SIZE] __attribute__ ((aligned(4)));    // align to 32-bit to try and provide speed improvement;
+// master buffers...
+uint8_t SSP_TxBuf[MAX_PACKET_BUF_SIZE];
+uint8_t SSP_RxBuf[MAX_PACKET_BUF_SIZE];
+void SSP_CallBack(uint8_t *buf, uint16_t len);
+int16_t SSP_SerialRead(void);
+void SSP_SerialWrite( uint8_t);
+uint32_t SSP_GetTime(void);
+PortConfig_t SSP_PortConfig = { .rxBuf = SSP_RxBuf,
+		.rxBufSize = MAX_PACKET_DATA_LEN, .txBuf = SSP_TxBuf,
+		.txBufSize = MAX_PACKET_DATA_LEN, .max_retry = 10, .timeoutLen = 1000,
+		.pfCallBack = SSP_CallBack, .pfSerialRead = SSP_SerialRead,
+		.pfSerialWrite = SSP_SerialWrite, .pfGetTime = SSP_GetTime, };
+Port_t ssp_port;
+t_fifo_buffer ssp_buffer;
+
 /* Extern variables ----------------------------------------------------------*/
 DFUStates DeviceState;
 DFUPort ProgPort;
+int16_t status=0;
 uint8_t JumpToApp = FALSE;
 uint8_t GO_dfu = FALSE;
 uint8_t USB_connected = FALSE;
@@ -70,6 +94,7 @@ static uint8_t mReceive_Buffer[64];
 uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count);
 uint8_t processRX();
 void jump_to_app();
+uint32_t sspTimeSource();
 
 #define BLUE LED1
 #define RED	LED2
@@ -84,29 +109,47 @@ int main() {
 		USB_connected = TRUE;
 
 	PIOS_IAP_Init();
-	if (PIOS_IAP_CheckRequest() == TRUE) {
+	//if (PIOS_IAP_CheckRequest() == TRUE) {
+	{
 		User_DFU_request = TRUE;
 		PIOS_IAP_ClearRequest();
+
 	}
-	GO_dfu = (USB_connected==TRUE) || (User_DFU_request==TRUE);
+	GO_dfu = (USB_connected == TRUE) || (User_DFU_request == TRUE);
+
 	if (GO_dfu == TRUE) {
-		if(USB_connected)
-			ProgPort=Usb;
+		if (USB_connected)
+			ProgPort = Usb;
 		else
-			ProgPort=Serial;
+			ProgPort = Serial;
+		//ProgPort = Serial;
 		PIOS_Board_Init();
 		PIOS_OPAHRS_Init();
-		DeviceState = BLidle;
+		if(User_DFU_request == TRUE)
+			DeviceState = DFUidle;
+		else
+			DeviceState = BLidle;
 		STOPWATCH_Init(100);
+		if (ProgPort == Serial) {
+			fifoBuf_init(&ssp_buffer,rx_buffer,UART_BUFFER_SIZE);
+			SSP_TIMER_Init(100);
+			SSP_TIMER_Reset();
+			ssp_Init(&ssp_port, &SSP_PortConfig);
+		}
 		PIOS_SPI_RC_PinSet(PIOS_OPAHRS_SPI, 0);
-		//OPDfuIni(false);
-
 	} else
 		JumpToApp = TRUE;
 
 	STOPWATCH_Reset();
-
 	while (TRUE) {
+		if (ProgPort == Serial) {
+			ssp_ReceiveProcess(&ssp_port);
+			status=ssp_SendProcess(&ssp_port);
+			while((status!=SSP_TX_IDLE) && (status!=SSP_TX_ACKED)){
+				ssp_ReceiveProcess(&ssp_port);
+				status=ssp_SendProcess(&ssp_port);
+			}
+		}
 		if (JumpToApp == TRUE)
 			jump_to_app();
 		//pwm_period = 50; // *100 uS -> 5 mS
@@ -172,7 +215,6 @@ int main() {
 	}
 }
 
-
 void jump_to_app() {
 	if (((*(__IO uint32_t*) START_OF_USER_CODE) & 0x2FFE0000) == 0x20000000) { /* Jump to user application */
 		FLASH_Lock();
@@ -203,13 +245,48 @@ uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count) {
 }
 
 uint8_t processRX() {
-	while(PIOS_COM_ReceiveBufferUsed(PIOS_COM_TELEM_USB)>=63)
-	{
-		for (int32_t x = 0; x < 63; ++x) {
-			mReceive_Buffer[x] = PIOS_COM_ReceiveBuffer(PIOS_COM_TELEM_USB);
+	if (ProgPort == Usb) {
+		while (PIOS_COM_ReceiveBufferUsed(PIOS_COM_TELEM_USB) >= 63) {
+			for (int32_t x = 0; x < 63; ++x) {
+				mReceive_Buffer[x] = PIOS_COM_ReceiveBuffer(PIOS_COM_TELEM_USB);
+			}
+			//PIOS_IRQ_Enable();
+			processComand(mReceive_Buffer);
+		}
+	} else if (ProgPort == Serial) {
+
+
+		if (fifoBuf_getUsed(&ssp_buffer) >= 63) {
+			for (int32_t x = 0; x < 63; ++x) {
+				mReceive_Buffer[x] = fifoBuf_getByte(&ssp_buffer);
+			}
+			processComand(mReceive_Buffer);
 		}
-		//PIOS_IRQ_Enable();
-		processComand(mReceive_Buffer);
 	}
 	return TRUE;
 }
+
+uint32_t sspTimeSource() {
+	if (SSP_TIMER_ValueGet() > 5000) {
+		++ssp_time;
+		SSP_TIMER_Reset();
+	}
+	return ssp_time;
+}
+void SSP_CallBack(uint8_t *buf, uint16_t len) {
+	fifoBuf_putData(&ssp_buffer, buf, len);
+	}
+int16_t SSP_SerialRead(void) {
+	if(PIOS_COM_ReceiveBufferUsed(PIOS_COM_TELEM_RF)>0)
+	{
+		return PIOS_COM_ReceiveBuffer(PIOS_COM_TELEM_RF);
+	}
+	else
+		return -1;
+}
+void SSP_SerialWrite(uint8_t value) {
+	PIOS_COM_SendChar(PIOS_COM_TELEM_RF,value);
+}
+uint32_t SSP_GetTime(void) {
+	return sspTimeSource();
+}
diff --git a/flight/Bootloaders/OpenPilot/op_dfu.c b/flight/Bootloaders/OpenPilot/op_dfu.c
index 3265f8779..725f1f6af 100644
--- a/flight/Bootloaders/OpenPilot/op_dfu.c
+++ b/flight/Bootloaders/OpenPilot/op_dfu.c
@@ -1,37 +1,41 @@
-/******************** (C) COPYRIGHT 2010 STMicroelectronics ********************
- * File Name          : usb_endp.c
- * Author             : MCD Application Team
- * Version            : V3.2.1
- * Date               : 07/05/2010
- * Description        : Endpoint routines
- ********************************************************************************
- * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
- * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME.
- * AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT,
- * INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE
- * CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING
- * INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
- *******************************************************************************/
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotBL OpenPilot BootLoader
+ * @brief These files contain the code to the OpenPilot MB Bootloader.
+ *
+ * @{
+ * @file       op_dfu.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      This file contains the DFU commands handling code
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
 
 /* Includes ------------------------------------------------------------------*/
-//#include "platform_config.h"
 #include "pios.h"
-//#include "stm32f10x.h"
-//#include "usb_lib.h"
-//#include "usb_istr.h"
-//#include "stm32_eval.h"
-//#include "stm32f10x_flash.h"
-//#include "stm32f10x_crc.h"
-//#include "hw_config.h"
-//#include <string.h>
 #include "op_dfu.h"
 #include "pios_bl_helper.h"
 #include "pios_opahrs.h"
+#include "ssp.h"
 /* Private typedef -----------------------------------------------------------*/
 /* Private define ------------------------------------------------------------*/
 /* Private macro -------------------------------------------------------------*/
 /* Private variables ---------------------------------------------------------*/
-
 //programmable devices
 Device devicesTable[10];
 uint8_t numberOfDevices = 0;
@@ -71,9 +75,11 @@ DFUTransfer downType = 0;
 /* Extern variables ----------------------------------------------------------*/
 extern DFUStates DeviceState;
 extern uint8_t JumpToApp;
+extern Port_t ssp_port;
+extern DFUPort ProgPort;
 /* Private function prototypes -----------------------------------------------*/
 /* Private functions ---------------------------------------------------------*/
-
+void sendData(uint8_t * buf,uint16_t size);
 uint32_t CalcFirmCRC(void);
 
 void DataDownload(DownloadAction action) {
@@ -125,14 +131,17 @@ void DataDownload(DownloadAction action) {
 			DeviceState = Last_operation_Success;
 			Aditionals = (uint32_t) Download;
 		}
-		PIOS_COM_SendBuffer(PIOS_COM_TELEM_USB, SendBuffer + 1, 63);//FIX+1
-		//PIOS SetEPTxValid(ENDP1);
+		sendData(SendBuffer+1,63);
 	}
 }
-
 void processComand(uint8_t *xReceive_Buffer) {
 
 	Command = xReceive_Buffer[COMMAND];
+#ifdef DEBUG_SSP
+	char str[63]={0};
+	sprintf(str,"Received COMMAND:%d|",Command);
+	PIOS_COM_SendString(PIOS_COM_TELEM_USB,str);
+#endif
 	EchoReqFlag = (Command >> 7);
 	EchoAnsFlag = (Command >> 6) & 0x01;
 	StartFlag = (Command >> 5) & 0x01;
@@ -352,8 +361,8 @@ void processComand(uint8_t *xReceive_Buffer) {
 			Buffer[12] = devicesTable[Data0 - 1].FW_Crc >> 8;
 			Buffer[13] = devicesTable[Data0 - 1].FW_Crc;
 		}
-		//PIOS USB_SIL_Write(EP1_IN, (uint8_t*) Buffer, 64);
-		PIOS_COM_SendBuffer(PIOS_COM_TELEM_USB, Buffer + 1, 63);//FIX+1
+		sendData(Buffer + 1, 63);
+		//PIOS_COM_SendBuffer(PIOS_COM_TELEM_USB, Buffer + 1, 63);//FIX+1
 		break;
 	case JumpFW:
 		if (numberOfDevices > 1) {
@@ -397,7 +406,14 @@ void processComand(uint8_t *xReceive_Buffer) {
 
 		break;
 	case Download_Req:
+#ifdef DEBUG_SSP
+			sprintf(str,"COMMAND:DOWNLOAD_REQ 1 Status=%d|",DeviceState);
+			PIOS_COM_SendString(PIOS_COM_TELEM_USB,str);
+#endif
 		if (DeviceState == DFUidle) {
+#ifdef DEBUG_SSP
+			PIOS_COM_SendString(PIOS_COM_TELEM_USB,"COMMAND:DOWNLOAD_REQ 1|");
+#endif
 			downType = Data0;
 			downPacketTotal = Count;
 			downSizeOfLastPacket = Data1;
@@ -434,9 +450,7 @@ void processComand(uint8_t *xReceive_Buffer) {
 		Buffer[7] = 0;
 		Buffer[8] = 0;
 		Buffer[9] = 0;
-		PIOS_COM_SendBuffer(PIOS_COM_TELEM_USB, Buffer + 1, 63);//FIX+1
-		//PIOS USB_SIL_Write(EP1_IN, (uint8_t*) Buffer, 64);
-		//PIOS SetEPTxValid(ENDP1);
+		sendData(Buffer + 1, 63);
 		if (DeviceState == Last_operation_Success) {
 			DeviceState = DFUidle;
 		}
@@ -448,9 +462,7 @@ void processComand(uint8_t *xReceive_Buffer) {
 	}
 	if (EchoReqFlag == 1) {
 		echoBuffer[1] = echoBuffer[1] | EchoAnsFlag;
-		PIOS_COM_SendBuffer(PIOS_COM_TELEM_USB, echoBuffer, 63);
-		//PIOS USB_SIL_Write(EP1_IN, (uint8_t*) echoBuffer, 64);
-		//PIOS SetEPTxValid(ENDP1);
+		sendData(echoBuffer + 1, 63);
 	}
 	return;
 }
@@ -504,8 +516,7 @@ void OPDfuIni(uint8_t discover) {
 					devicesTable[1] = dev;
 				}
 			}
-		}
-		else
+		} else
 			PIOS_SPI_RC_PinSet(PIOS_OPAHRS_SPI, 0);
 	}
 	//TODO check other devices trough spi or whatever
@@ -559,3 +570,11 @@ uint32_t CalcFirmCRC() {
 	}
 
 }
+void sendData(uint8_t * buf,uint16_t size)
+{
+if (ProgPort == Usb) {
+			PIOS_COM_SendBuffer(PIOS_COM_TELEM_USB, buf, size);//FIX+1
+		} else if (ProgPort == Serial) {
+			ssp_SendData(&ssp_port, buf, size);
+		}
+}
diff --git a/flight/Bootloaders/OpenPilot/pios_board.c b/flight/Bootloaders/OpenPilot/pios_board.c
index d5dc23e7a..8272b8421 100644
--- a/flight/Bootloaders/OpenPilot/pios_board.c
+++ b/flight/Bootloaders/OpenPilot/pios_board.c
@@ -1,13 +1,11 @@
 /**
  ******************************************************************************
- * @addtogroup OpenPilotSystem OpenPilot System
- * @{
- * @addtogroup OpenPilotCore OpenPilot Core
+ * @addtogroup OpenPilotBL OpenPilot BootLoader
  * @{
  *
  * @file       pios_board.c
  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      Defines board specific static initializers for hardware for the OpenPilot board.
+ * @brief      Defines board specific static initialisers for hardware for the OpenPilot board.
  * @see        The GNU Public License (GPL) Version 3
  *
  *****************************************************************************/
@@ -64,7 +62,7 @@ void PIOS_Board_Init(void) {
 	PIOS_USB_HID_Init(0);
 #endif
 	//PIOS_I2C_Init();
-	RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE);
+	RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE);//TODO Tirar
 }
 
 /* MicroSD Interface
diff --git a/flight/Bootloaders/OpenPilot/ssp.c b/flight/Bootloaders/OpenPilot/ssp.c
index a72e8fae5..a1bebff94 100644
--- a/flight/Bootloaders/OpenPilot/ssp.c
+++ b/flight/Bootloaders/OpenPilot/ssp.c
@@ -72,9 +72,8 @@
 #include <stdint.h>
 #include <string.h>
 #include <stdio.h>
-
+#include <pios.h>
 #include "ssp.h"
-
 /** PRIVATE DEFINITIONS **/
 #define SYNC                        225                     // Sync character used in Serial Protocol
 #define ESC                         224                     // ESC character used in Serial Protocol
@@ -230,6 +229,11 @@ int16_t ssp_SendProcess( Port_t *thisport )
 				value = SSP_TX_WAITING;
 			} else {
 				// Give up, # of trys has exceded the limit
+#ifdef DEBUG_SSP
+				char str[63]={0};
+				sprintf(str,"Send Timeout|");
+				PIOS_COM_SendString(PIOS_COM_TELEM_USB,str);
+#endif
 				value = SSP_TX_TIMEOUT;
 				CLEARBIT( thisport->flags, ACK_RECEIVED);
                 thisport->SendState = SSP_IDLE;
@@ -381,9 +385,19 @@ int16_t ssp_SendData( Port_t *thisport, const uint8_t *data, const uint16_t leng
         sf_MakePacket( thisport->txBuf, data, length, thisport->txSeqNo );
 		sf_SendPacket( thisport );				// punch out the packet to the serial port
         sf_SetSendTimeout( thisport );	// do the timeout values
+#ifdef DEBUG_SSP
+        char str[63]={0};
+        sprintf(str,"Sent DATA PACKET:%d|",thisport->txSeqNo);
+        PIOS_COM_SendString(PIOS_COM_TELEM_USB,str);
+#endif
     } else {
         // error we are already sending a packet. Need to wait for the current packet to be acked or timeout.
-        value = SSP_TX_BUSY;
+#ifdef DEBUG_SSP
+    	char str[63]={0};
+    	sprintf(str,"Error sending TX was busy|");
+    	PIOS_COM_SendString(PIOS_COM_TELEM_USB,str);
+#endif
+    	value = SSP_TX_BUSY;
     }
     return value;
 }
@@ -513,6 +527,11 @@ void sf_MakePacket( uint8_t *txBuf, const uint8_t * pdata, uint16_t length, uint
 
 static void sf_SendAckPacket( Port_t *thisport, uint8_t seqNumber)
 {
+#ifdef DEBUG_SSP
+	char str[63]={0};
+	sprintf(str,"Sent ACK PACKET:%d|",seqNumber);
+	PIOS_COM_SendString(PIOS_COM_TELEM_USB,str);
+#endif
     uint8_t    AckSeqNumber = SETBIT( seqNumber, ACK_BIT );
 
     // create the packet, note we pass AckSequenceNumber directly
@@ -778,12 +797,20 @@ static int16_t sf_ReceivePacket(Port_t *thisport)
 			//  It matches the last packet sent by us
 			SETBIT( thisport->txSeqNo, ACK_BIT );
 			thisport->SendState = SSP_ACKED;
+#ifdef DEBUG_SSP
+			char str[63]={0};
+			sprintf(str,"Received ACK:%d|",(thisport->txSeqNo & 0x7F));
+			PIOS_COM_SendString(PIOS_COM_TELEM_USB,str);
+#endif
 			value = FALSE;
 		}
         // else ignore the ACK packet
 	} else {
 		//  Received a 'data' packet, figure out what type of packet we received...
 		if( thisport->rxBuf[SEQNUM] == 0 ) {
+#ifdef DEBUG_SSP
+			PIOS_COM_SendString(PIOS_COM_TELEM_USB,"Received SYNC Request|");
+#endif
 			// Synchronize sequence number with host
 #ifdef ACTIVE_SYNCH
             thisport->sendSynch = TRUE;
@@ -800,6 +827,11 @@ static int16_t sf_ReceivePacket(Port_t *thisport)
 			thisport->rxSeqNo = thisport->rxBuf[SEQNUM];
 			// Let the application do something with the data/packet.
 			if( thisport->pfCallBack != NULL ) {
+#ifdef DEBUG_SSP
+				char str[63]={0};
+				sprintf(str,"Received DATA PACKET:%d [0]=%d %d %d|",thisport->rxSeqNo,(uint8_t)thisport->rxBuf[2],(uint8_t)thisport->rxBuf[3],(uint8_t)thisport->rxBuf[4]);
+				PIOS_COM_SendString(PIOS_COM_TELEM_USB,str);
+#endif
 				// skip the first two bytes (length and seq. no.) in the buffer.
 				thisport->pfCallBack( &(thisport->rxBuf[2]), thisport->rxBufLen);
 			}
diff --git a/flight/Bootloaders/OpenPilot/ssp_timer.c b/flight/Bootloaders/OpenPilot/ssp_timer.c
new file mode 100644
index 000000000..b676f3c43
--- /dev/null
+++ b/flight/Bootloaders/OpenPilot/ssp_timer.c
@@ -0,0 +1,100 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotBL OpenPilot BootLoader
+ * @{
+ *
+ * @file       ssp_timer.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Timer functions to be used with the SSP.
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+
+/////////////////////////////////////////////////////////////////////////////
+// Include files
+/////////////////////////////////////////////////////////////////////////////
+
+#include "stm32f10x_tim.h"
+
+
+
+
+
+/////////////////////////////////////////////////////////////////////////////
+// Local definitions
+/////////////////////////////////////////////////////////////////////////////
+
+#define SSP_TIMER_TIMER_BASE                 TIM7
+#define SSP_TIMER_TIMER_RCC   RCC_APB1Periph_TIM7
+
+uint32_t SSP_TIMER_Init(u32 resolution)
+{
+  // enable timer clock
+  if( SSP_TIMER_TIMER_RCC == RCC_APB2Periph_TIM1 || SSP_TIMER_TIMER_RCC == RCC_APB2Periph_TIM8 )
+    RCC_APB2PeriphClockCmd(SSP_TIMER_TIMER_RCC, ENABLE);
+  else
+    RCC_APB1PeriphClockCmd(SSP_TIMER_TIMER_RCC, ENABLE);
+
+  // time base configuration
+  TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
+  TIM_TimeBaseStructure.TIM_Period = 0xffff; // max period
+  TIM_TimeBaseStructure.TIM_Prescaler = (72 * resolution)-1; // <resolution> uS accuracy @ 72 MHz
+  TIM_TimeBaseStructure.TIM_ClockDivision = 0;
+  TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
+  TIM_TimeBaseInit(SSP_TIMER_TIMER_BASE, &TIM_TimeBaseStructure);
+
+  // enable interrupt request
+  TIM_ITConfig(SSP_TIMER_TIMER_BASE, TIM_IT_Update, ENABLE);
+
+  // start counter
+  TIM_Cmd(SSP_TIMER_TIMER_BASE, ENABLE);
+
+  return 0; // no error
+}
+
+
+/////////////////////////////////////////////////////////////////////////////
+//! Resets the SSP_TIMER
+//! \return < 0 on errors
+/////////////////////////////////////////////////////////////////////////////
+uint32_t SSP_TIMER_Reset(void)
+{
+  // reset counter
+  SSP_TIMER_TIMER_BASE->CNT = 1; // set to 1 instead of 0 to avoid new IRQ request
+  TIM_ClearITPendingBit(SSP_TIMER_TIMER_BASE, TIM_IT_Update);
+
+  return 0; // no error
+}
+
+
+/////////////////////////////////////////////////////////////////////////////
+//! Returns current value of SSP_TIMER
+//! \return 1..65535: valid SSP_TIMER value
+//! \return 0xffffffff: counter overrun
+/////////////////////////////////////////////////////////////////////////////
+uint32_t SSP_TIMER_ValueGet(void)
+{
+	uint32_t value = SSP_TIMER_TIMER_BASE->CNT;
+
+  if( TIM_GetITStatus(SSP_TIMER_TIMER_BASE, TIM_IT_Update) != RESET )
+	  SSP_TIMER_Reset();
+
+  return value;
+}
+
diff --git a/flight/Bootloaders/OpenPilot/stopwatch.c b/flight/Bootloaders/OpenPilot/stopwatch.c
index 175205aed..47fc36c08 100644
--- a/flight/Bootloaders/OpenPilot/stopwatch.c
+++ b/flight/Bootloaders/OpenPilot/stopwatch.c
@@ -1,3 +1,29 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotBL OpenPilot BootLoader
+ * @{
+ *
+ * @file       stopwatch.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Timer functions for the LED PWM.
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
 
 
 /////////////////////////////////////////////////////////////////////////////