diff --git a/flight/OpenPilot/Makefile b/flight/OpenPilot/Makefile
index a0804cc5f..16c7392e4 100644
--- a/flight/OpenPilot/Makefile
+++ b/flight/OpenPilot/Makefile
@@ -108,7 +108,7 @@ DOXYGENDIR = ../Doc/Doxygen
 
 ## MODULES
 SRC = $(MODEXAMPLE)/examplemodevent.c $(MODEXAMPLE)/examplemodperiodic.c $(MODEXAMPLE)/examplemodthread.c
-SRC = $(MODSYSTEM)/systemmod.c
+SRC += $(MODSYSTEM)/systemmod.c
 SRC +=  $(MODTELEMETRY)/telemetry.c
 SRC += $(MODGPS)/GPS.c $(MODGPS)/buffer.c
 
diff --git a/flight/OpenPilot/Modules/Example/examplemodevent.c b/flight/OpenPilot/Modules/Example/examplemodevent.c
index f3d6ce3f5..6828e443e 100644
--- a/flight/OpenPilot/Modules/Example/examplemodevent.c
+++ b/flight/OpenPilot/Modules/Example/examplemodevent.c
@@ -62,7 +62,7 @@ static void ObjectUpdatedCb(UAVObjEvent* ev);
  * Initialise the module, called on startup.
  * \returns 0 on success or -1 if initialisation failed
  */
-int32_t ExampleModEventInitialize(void)
+int32_t ExampleModEventInitialize()
 {
 	// Listen for ExampleObject1 updates, connect a callback function
 	ExampleObject1ConnectCallback(&ObjectUpdatedCb);
@@ -84,7 +84,7 @@ static void ObjectUpdatedCb(UAVObjEvent* ev)
 {
 	ExampleSettingsData settings;
 	ExampleObject1Data data1;
-	ExampleObject1Data data2;
+	ExampleObject2Data data2;
 	int32_t step;
 
 	// Make sure that the object update is for ExampleObject1
diff --git a/flight/OpenPilot/Modules/Example/examplemodperiodic.c b/flight/OpenPilot/Modules/Example/examplemodperiodic.c
index 07210eecb..439107b85 100644
--- a/flight/OpenPilot/Modules/Example/examplemodperiodic.c
+++ b/flight/OpenPilot/Modules/Example/examplemodperiodic.c
@@ -27,7 +27,7 @@
 
 /**
  * Input object: ExampleSettings
- * Output object: ExampleObject1
+ * Output object: ExampleObject2
  *
  * This module will periodically update the value of the ExampleObject object.
  * The module settings can configure how the ExampleObject is manipulated.
@@ -45,7 +45,7 @@
  */
 
 #include "examplemodperiodic.h"
-#include "exampleobject1.h" // object that will be updated by the module
+#include "exampleobject2.h" // object that will be updated by the module
 #include "examplesettings.h" // object holding module settings
 
 // Private constants
@@ -64,7 +64,7 @@ static void exampleTask(void* parameters);
  * Initialise the module, called on startup
  * \returns 0 on success or -1 if initialisation failed
  */
-int32_t ExampleModPeriodicInitialize(void)
+int32_t ExampleModPeriodicInitialize()
 {
 	// Start main task
 	xTaskCreate(exampleTask, (signed char*)"ExamplePeriodic", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle);
@@ -78,7 +78,7 @@ int32_t ExampleModPeriodicInitialize(void)
 static void exampleTask(void* parameters)
 {
 	ExampleSettingsData settings;
-	ExampleObject1Data data;
+	ExampleObject2Data data;
 	int32_t step;
 
 	// Main task loop
@@ -87,8 +87,13 @@ static void exampleTask(void* parameters)
 		// Update settings with latest value
 		ExampleSettingsGet(&settings);
 
+		// TODO: Remove, this is temporary for testing (force settings)
+		settings.StepDirection = EXAMPLESETTINGS_STEPDIRECTION_UP;
+		settings.StepSize = 1;
+		settings.UpdatePeriod = 10;
+
 		// Get the object data
-		ExampleObject1Get(&data);
+		ExampleObject2Get(&data);
 
 		// Determine how to update the data
 		if ( settings.StepDirection == EXAMPLESETTINGS_STEPDIRECTION_UP )
@@ -107,13 +112,12 @@ static void exampleTask(void* parameters)
 		data.Field4[0] += step;
 		data.Field4[1] += step;
 
-
 		// Update the ExampleObject, after this function is called
 		// notifications to any other modules listening to that object
 		// will be sent and the GCS object will be updated through the
 		// telemetry link. All operations will take place asynchronously
 		// and the following call will return immediately.
-		ExampleObject1Set(&data);
+		ExampleObject2Set(&data);
 
 		// Since this module executes at fixed time intervals, we need to
 		// block the task until it is time for the next update.
diff --git a/flight/OpenPilot/Modules/Example/examplemodthread.c b/flight/OpenPilot/Modules/Example/examplemodthread.c
index 650e6227e..8b795c60a 100644
--- a/flight/OpenPilot/Modules/Example/examplemodthread.c
+++ b/flight/OpenPilot/Modules/Example/examplemodthread.c
@@ -66,7 +66,7 @@ static void exampleTask(void* parameters);
  * Initialise the module, called on startup
  * \returns 0 on success or -1 if initialisation failed
  */
-int32_t ExampleModThreadInitialize(void)
+int32_t ExampleModThreadInitialize()
 {
 	// Create object queue
 	queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
diff --git a/flight/OpenPilot/Modules/Example/inc/examplemodevent.h b/flight/OpenPilot/Modules/Example/inc/examplemodevent.h
index 2ca43c90e..bcfdd052b 100644
--- a/flight/OpenPilot/Modules/Example/inc/examplemodevent.h
+++ b/flight/OpenPilot/Modules/Example/inc/examplemodevent.h
@@ -28,7 +28,7 @@
 
 #include "openpilot.h"
 
-int32_t ExampleModEventInitialize(void);
+int32_t ExampleModEventInitialize();
 
 #endif // EXAMPLEMODEVENT_H
 
diff --git a/flight/OpenPilot/Modules/Example/inc/examplemodperiodic.h b/flight/OpenPilot/Modules/Example/inc/examplemodperiodic.h
index 9ddc7bed6..36367d4bd 100644
--- a/flight/OpenPilot/Modules/Example/inc/examplemodperiodic.h
+++ b/flight/OpenPilot/Modules/Example/inc/examplemodperiodic.h
@@ -28,7 +28,7 @@
 
 #include "openpilot.h"
 
-int32_t ExampleModPeriodicInitialize(void);
+int32_t ExampleModPeriodicInitialize();
 
 #endif // EXAMPLEMODPERIODIC_H
 
diff --git a/flight/OpenPilot/Modules/Example/inc/examplemodthread.h b/flight/OpenPilot/Modules/Example/inc/examplemodthread.h
index 8cc697c93..3297ac940 100644
--- a/flight/OpenPilot/Modules/Example/inc/examplemodthread.h
+++ b/flight/OpenPilot/Modules/Example/inc/examplemodthread.h
@@ -28,7 +28,7 @@
 
 #include "openpilot.h"
 
-int32_t ExampleModThreadInitialize(void);
+int32_t ExampleModThreadInitialize();
 
 #endif // EXAMPLEMODTHREAD_H
 
diff --git a/flight/OpenPilot/Modules/GPS/GPS.c b/flight/OpenPilot/Modules/GPS/GPS.c
index b9b70d36d..0b9f4f731 100644
--- a/flight/OpenPilot/Modules/GPS/GPS.c
+++ b/flight/OpenPilot/Modules/GPS/GPS.c
@@ -25,6 +25,8 @@
 
 #include "openpilot.h"
 #include "buffer.h"
+#include "GPS.h"
+#include "gpsinfo.h"
 
 
 // constants/macros/typdefs
diff --git a/flight/OpenPilot/Modules/System/inc/systemmod.h b/flight/OpenPilot/Modules/System/inc/systemmod.h
index 8a0dc8dfb..1c6f7abd1 100644
--- a/flight/OpenPilot/Modules/System/inc/systemmod.h
+++ b/flight/OpenPilot/Modules/System/inc/systemmod.h
@@ -23,12 +23,12 @@
  * with this program; if not, write to the Free Software Foundation, Inc.,
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
-#ifndef SYSTEM_H
-#define SYSTEM_H
+#ifndef SYSTEMMOD_H
+#define SYSTEMMOD_H
 
 #include "openpilot.h"
 
 int32_t SystemModInitialize(void);
 
-#endif // SYSTEM_H
+#endif // SYSTEMMOD_H
 
diff --git a/flight/OpenPilot/Modules/Telemetry/telemetry.c b/flight/OpenPilot/Modules/Telemetry/telemetry.c
index 6c3c1ee76..8ff37b0ed 100644
--- a/flight/OpenPilot/Modules/Telemetry/telemetry.c
+++ b/flight/OpenPilot/Modules/Telemetry/telemetry.c
@@ -27,9 +27,9 @@
 
 // Private constants
 #define MAX_QUEUE_SIZE 20
-#define STACK_SIZE 100
+#define STACK_SIZE configMINIMAL_STACK_SIZE
 #define TASK_PRIORITY (tskIDLE_PRIORITY + 1)
-#define REQ_TIMEOUT_MS 500
+#define REQ_TIMEOUT_MS 250
 #define MAX_RETRIES 3
 
 // Private types
@@ -37,10 +37,12 @@
 // Private variables
 static COMPortTypeDef telemetryPort;
 static xQueueHandle queue;
-static xTaskHandle telemetryTaskHandle;
+static xTaskHandle telemetryTxTaskHandle;
+static xTaskHandle telemetryRxTaskHandle;
 
 // Private functions
-static void telemetryTask(void* parameters);
+static void telemetryTxTask(void* parameters);
+static void telemetryRxTask(void* parameters);
 static int32_t transmitData(uint8_t* data, int32_t length);
 static void registerObject(UAVObjHandle obj);
 static void updateObject(UAVObjHandle obj);
@@ -66,8 +68,9 @@ int32_t TelemetryInitialize(void)
 	// Process all registered objects and connect queue for updates
 	UAVObjIterate(&registerObject);
 
-	// Start telemetry task
-	xTaskCreate(telemetryTask, (signed char*)"Telemetry", STACK_SIZE, NULL, TASK_PRIORITY, &telemetryTaskHandle);
+	// Start telemetry tasks
+	xTaskCreate(telemetryTxTask, (signed char*)"TelemetryTx", STACK_SIZE, NULL, TASK_PRIORITY, &telemetryTxTaskHandle);
+	xTaskCreate(telemetryRxTask, (signed char*)"TelemetryRx", STACK_SIZE, NULL, TASK_PRIORITY, &telemetryRxTaskHandle);
 
 	return 0;
 }
@@ -145,30 +148,29 @@ static void updateObject(UAVObjHandle obj)
 }
 
 /**
- * Telemetry task. Processes queue events and periodic updates. It does not return.
+ * Telemetry transmit task. Processes queue events and periodic updates.
  */
-static void telemetryTask(void* parameters)
+static void telemetryTxTask(void* parameters)
 {
 	UAVObjEvent ev;
 	UAVObjMetadata metadata;
 	int32_t retries;
 	int32_t success;
-	PIOS_COM_SendFormattedStringNonBlocking(telemetryPort, "sendtest");
 
 	// Loop forever
 	while(1)
 	{
-//		PIOS_LED_Toggle(LED2);
 		// Wait for queue message
-		if(xQueueReceive(queue, &ev, 0) == pdTRUE)
+		if(xQueueReceive(queue, &ev, portMAX_DELAY) == pdTRUE)
 		{
 			// Get object metadata
 			UAVObjGetMetadata(ev.obj, &metadata);
 			// Act on event
+			retries = 0;
+			success = -1;
 			if(ev.event == EV_UPDATED || ev.event == EV_UPDATED_MANUAL)
 			{
 				// Send update to GCS (with retries)
-				retries = 0;
 				while(retries < MAX_RETRIES && success == -1)
 				{
 					success = UAVTalkSendObject(ev.obj, ev.instId, metadata.telemetryAcked, REQ_TIMEOUT_MS); // call blocks until ack is received or timeout
@@ -178,7 +180,6 @@ static void telemetryTask(void* parameters)
 			else if(ev.event == EV_UPDATE_REQ)
 			{
 				// Request object update from GCS (with retries)
-				retries = 0;
 				while(retries < MAX_RETRIES && success == -1)
 				{
 					success = UAVTalkSendObjectRequest(ev.obj, ev.instId, REQ_TIMEOUT_MS); // call blocks until update is received or timeout
@@ -191,28 +192,41 @@ static void telemetryTask(void* parameters)
 				updateObject(UAVObjGetLinkedObj(ev.obj)); // linked object will be the actual object the metadata are for
 			}
 		}
+	}
+}
 
-		/* This blocks the task until there is something on the buffer */
-		uint8_t bytes = PIOS_COM_ReceiveBufferUsed(telemetryPort);
-		if(bytes > 0)
+/**
+ * Telemetry transmit task. Processes queue events and periodic updates.
+ */
+static void telemetryRxTask(void* parameters)
+{
+	COMPortTypeDef inputPort;
+	int32_t len;
+
+	// Task loop
+	while (1)
+	{
+		// TODO: Disabled since the USB HID is not fully functional yet
+		inputPort = telemetryPort; // force input port, remove once USB HID is tested
+		// Determine input port (USB takes priority over telemetry port)
+		//if(!PIOS_USB_HID_CheckAvailable())
+		//{
+		//	inputPort = telemetryPort;
+		//}
+		//else
+		//{
+		//	inputPort = COM_USB_HID;
+		//}
+
+		// Block until data are available
+		// TODO: Update once the PIOS_COM is made blocking
+		//xSemaphoreTake(PIOS_USART1_Buffer, portMAX_DELAY);
+		len = PIOS_COM_ReceiveBufferUsed(inputPort);
+		for (int32_t n = 0; n < len; ++n)
 		{
-			PIOS_LED_Toggle(LED2);
-			uint8_t c=PIOS_COM_ReceiveBuffer(telemetryPort);
-			UAVTalkProcessInputStream(c);
-			//PIOS_COM_SendFormattedStringNonBlocking(COM_USART1, "%c", c);
+			//PIOS_LED_Toggle(LED1);
+			UAVTalkProcessInputStream(PIOS_COM_ReceiveBuffer(inputPort));
 		}
-		else if(PIOS_COM_ReceiveBufferUsed(COM_USB_HID) > 0)
-		{
-			PIOS_LED_Toggle(LED2);
-			//UAVTalkProcessInputStream(PIOS_COM_ReceiveBuffer(COM_USB_HID));
-			uint8_t c=PIOS_COM_ReceiveBuffer(COM_USB_HID);
-			if(c!=255)
-			{
-				PIOS_COM_SendCharNonBlocking(COM_USART1, c);
-				UAVTalkProcessInputStream(c);
-			}
-		}
-		//vTaskDelay(200 / portTICK_RATE_MS);
 	}
 }
 
@@ -224,19 +238,23 @@ static void telemetryTask(void* parameters)
  */
 static int32_t transmitData(uint8_t* data, int32_t length)
 {
-	COMPortTypeDef OutputPort;
+	COMPortTypeDef outputPort;
 
-	/* If USB HID transfer is possible */
-	if(!PIOS_USB_HID_CheckAvailable())
-	{
-		OutputPort = COM_USART1;
-	}
-	else
-	{
-		OutputPort = COM_USB_HID;
-	}
+	// TODO: Disabled since the USB HID is not fully functional yet
+	outputPort = telemetryPort; // force input port, remove once USB HID is tested
+	// Determine input port (USB takes priority over telemetry port)
+	//if(!PIOS_USB_HID_CheckAvailable())
+	//{
+	//	outputPort = telemetryPort;
+	//}
+	//else
+	//{
+	//	outputPort = COM_USB_HID;
+	//}
 
-	return PIOS_COM_SendBuffer(OutputPort, data, length);
+	// TODO: Update once the PIOS_COM is made blocking (it is implemented as a busy loop for now!)
+	//PIOS_LED_Toggle(LED2);
+	return PIOS_COM_SendBuffer(outputPort, data, length);
 }
 
 /**
diff --git a/flight/OpenPilot/System/inc/openpilot.h b/flight/OpenPilot/System/inc/openpilot.h
index 029c4c9ab..e7ecc0f7f 100644
--- a/flight/OpenPilot/System/inc/openpilot.h
+++ b/flight/OpenPilot/System/inc/openpilot.h
@@ -31,11 +31,6 @@
 /* PIOS Includes */
 #include <pios.h>
 
-/* OpenPilot Modules */
-#include "telemetry.h"
-#include "GPS.h"
-#include "gpsinfo.h"
-
 /* OpenPilot Includes */
 #include <op_config.h>
 #include <op_logging.h>
diff --git a/flight/OpenPilot/System/openpilot.c b/flight/OpenPilot/System/openpilot.c
index cf3d39c26..3c17c6b13 100644
--- a/flight/OpenPilot/System/openpilot.c
+++ b/flight/OpenPilot/System/openpilot.c
@@ -27,6 +27,12 @@
 /* OpenPilot Includes */
 #include "openpilot.h"
 #include "uavobjectsinit.h"
+#include "telemetry.h"
+#include "GPS.h"
+#include "systemmod.h"
+#include "examplemodevent.h"
+#include "examplemodperiodic.h"
+#include "examplemodthread.h"
 
 /* Task Priorities */
 #define PRIORITY_TASK_HOOKS             (tskIDLE_PRIORITY + 3)
@@ -38,7 +44,6 @@ static uint8_t sdcard_available;
 FILEINFO File;
 char Buffer[1024];
 uint32_t Cache;
-GpsInfoType GpsInfo;
 
 /* Function Prototypes */
 static void TaskTick(void *pvParameters);
@@ -114,8 +119,13 @@ int main()
 	//xTaskCreate(TaskTesting, (signed portCHAR *)"TaskTesting", configMINIMAL_STACK_SIZE , NULL, 4, NULL);
 	//xTaskCreate(TaskServos, (signed portCHAR *)"Servos", configMINIMAL_STACK_SIZE , NULL, 3, NULL);
 	//xTaskCreate(TaskSDCard, (signed portCHAR *)"SDCard", configMINIMAL_STACK_SIZE, NULL, (tskIDLE_PRIORITY + 2), NULL);
-	//GpsInitialize();
+
+	/* Initialize modules */
+	//SystemModInitialize();
 	TelemetryInitialize();
+	//ExampleModEventInitialize();
+	ExampleModPeriodicInitialize();
+	//ExampleModThreadInitialize();
 
 	/* Start the FreeRTOS scheduler */
 	vTaskStartScheduler();
@@ -139,7 +149,6 @@ static void TaskTick(void *pvParameters)
 
 	/* Setup the LEDs to Alternate */
 	PIOS_LED_On(LED1);
-	PIOS_LED_Off(LED2);
 
 	for(;;)
 	{
diff --git a/flight/OpenPilot/UAVObjects/eventdispatcher.c b/flight/OpenPilot/UAVObjects/eventdispatcher.c
index cff9e8bde..a91dd9ee9 100644
--- a/flight/OpenPilot/UAVObjects/eventdispatcher.c
+++ b/flight/OpenPilot/UAVObjects/eventdispatcher.c
@@ -176,7 +176,9 @@ static int32_t eventPeriodicCreate(UAVObjEvent* ev, UAVObjEventCallback cb, xQue
 	{
 		if (objEntry->evInfo.cb == cb &&
 			objEntry->evInfo.queue == queue &&
-			memcmp(&objEntry->evInfo.ev, ev, sizeof(UAVObjEvent)) == 0)
+			objEntry->evInfo.ev.obj == ev->obj &&
+			objEntry->evInfo.ev.instId == ev->instId &&
+			objEntry->evInfo.ev.event == ev->event)
 		{
 			// Already registered, do nothing
 			xSemaphoreGiveRecursive(mutex);
@@ -186,7 +188,9 @@ static int32_t eventPeriodicCreate(UAVObjEvent* ev, UAVObjEventCallback cb, xQue
     // Create handle
 	objEntry = (PeriodicObjectList*)pvPortMalloc(sizeof(PeriodicObjectList));
 	if (objEntry == NULL) return -1;
-	memcpy(&objEntry->evInfo.ev, ev, sizeof(UAVObjEvent));
+	objEntry->evInfo.ev.obj = ev->obj;
+	objEntry->evInfo.ev.instId = ev->instId;
+	objEntry->evInfo.ev.event = ev->event;
 	objEntry->evInfo.cb = cb;
 	objEntry->evInfo.queue = queue;
     objEntry->updatePeriodMs = periodMs;
@@ -216,7 +220,9 @@ static int32_t eventPeriodicUpdate(UAVObjEvent* ev, UAVObjEventCallback cb, xQue
 	{
 		if (objEntry->evInfo.cb == cb &&
 			objEntry->evInfo.queue == queue &&
-			memcmp(&objEntry->evInfo.ev, ev, sizeof(UAVObjEvent)) == 0)
+			objEntry->evInfo.ev.obj == ev->obj &&
+			objEntry->evInfo.ev.instId == ev->instId &&
+			objEntry->evInfo.ev.event == ev->event)
 		{
 			// Object found, update period
 			objEntry->updatePeriodMs = periodMs;
diff --git a/flight/OpenPilot/UAVTalk/uavtalk.c b/flight/OpenPilot/UAVTalk/uavtalk.c
index 8a4b61e6a..6e4d2687b 100644
--- a/flight/OpenPilot/UAVTalk/uavtalk.c
+++ b/flight/OpenPilot/UAVTalk/uavtalk.c
@@ -71,6 +71,7 @@ int32_t UAVTalkInitialize(UAVTalkOutputStream outputStream)
     outStream = outputStream;
     lock = xSemaphoreCreateRecursiveMutex();
     vSemaphoreCreateBinary(respSema);
+    xSemaphoreTake(respSema, 0); // reset to zero
     return 0;
 }
 
@@ -131,11 +132,11 @@ static int32_t objectTransaction(UAVObjHandle obj, uint16_t instId, uint8_t type
     // Send object depending on if a response is needed
     if (type == TYPE_OBJ_ACK || type == TYPE_OBJ_REQ)
     {
+    	xSemaphoreTake(respSema, 0); // non blocking call to make sure the value is zero (binary sema)
         sendObject(obj, instId, type);
         respObj = obj;
         respInstId = instId;
         xSemaphoreGiveRecursive(lock); // need to release lock since the next call will block until a response is received
-    	xSemaphoreTake(respSema, 0); // the semaphore needs to block on the next call, here we make sure the value is zero (binary sema)
     	respReceived = xSemaphoreTake(respSema, timeoutMs/portTICK_RATE_MS); // lock on object until a response is received (or timeout)
     	// Check if a response was received
     	if (respReceived == pdFALSE)
@@ -194,7 +195,6 @@ int32_t UAVTalkProcessInputStream(uint8_t rxbyte)
         if (rxCount == 4)
         {
             // Search for object, if not found reset state machine
-            //objId = (tmpBuffer[3] << 24) | (tmpBuffer[2] << 16) | (tmpBuffer[1] << 8) | (tmpBuffer[0]);
             objId = (tmpBuffer[0] << 24) | (tmpBuffer[1] << 16) | (tmpBuffer[2] << 8) | (tmpBuffer[3]);
             obj = UAVObjGetByID(objId);
             if (obj == 0)
@@ -249,7 +249,7 @@ int32_t UAVTalkProcessInputStream(uint8_t rxbyte)
         tmpBuffer[rxCount++] = rxbyte;
         if (rxCount == 2)
         {
-        	instId = (tmpBuffer[1] << 8) | (tmpBuffer[0]);
+        	instId = (tmpBuffer[0] << 8) | (tmpBuffer[1]);
         	cs = updateChecksum(cs, tmpBuffer, 2);
         	rxCount = 0;
         	// If there is a payload get it, otherwise receive checksum
@@ -289,6 +289,7 @@ int32_t UAVTalkProcessInputStream(uint8_t rxbyte)
     default:
         state = STATE_SYNC;
     }
+
     // Done
     return 0;
 }
@@ -455,10 +456,10 @@ static int32_t sendSingleObject(UAVObjHandle obj, uint16_t instId, uint8_t type)
     // Setup type and object id fields
     objId = UAVObjGetID(obj);
     txBuffer[0] = type;
-    txBuffer[1] = (uint8_t)(objId & 0xFF);
-    txBuffer[2] = (uint8_t)((objId >> 8) & 0xFF);
-    txBuffer[3] = (uint8_t)((objId >> 16) & 0xFF);
-    txBuffer[4] = (uint8_t)((objId >> 24) & 0xFF);
+    txBuffer[1] = (uint8_t)((objId >> 24) & 0xFF);
+    txBuffer[2] = (uint8_t)((objId >> 16) & 0xFF);
+    txBuffer[3] = (uint8_t)((objId >> 8) & 0xFF);
+    txBuffer[4] = (uint8_t)(objId & 0xFF);
 
     // Setup instance ID if one is required
     if (UAVObjIsSingleInstance(obj))
@@ -467,8 +468,8 @@ static int32_t sendSingleObject(UAVObjHandle obj, uint16_t instId, uint8_t type)
     }
     else
     {
-    	txBuffer[5] = (uint8_t)(instId & 0xFF);
-    	txBuffer[6] = (uint8_t)((instId >> 8) & 0xFF);
+    	txBuffer[5] = (uint8_t)((instId >> 8) & 0xFF);
+    	txBuffer[6] = (uint8_t)(instId & 0xFF);
     	dataOffset = 7;
     }
 
@@ -500,12 +501,11 @@ static int32_t sendSingleObject(UAVObjHandle obj, uint16_t instId, uint8_t type)
     // Calculate checksum
     cs = 0;
     cs = updateChecksum(cs, txBuffer, dataOffset+length);
-    txBuffer[dataOffset+length] = (uint8_t)(cs & 0xFF);
-    txBuffer[dataOffset+length+1] = (uint8_t)((cs >> 8) & 0xFF);
+    txBuffer[dataOffset+length] = (uint8_t)((cs >> 8) & 0xFF);
+    txBuffer[dataOffset+length+1] = (uint8_t)(cs & 0xFF);
 
     // Send buffer
-    if (outStream!=NULL)
-    	(*outStream)(txBuffer, dataOffset+length+CHECKSUM_LENGTH);
+    if (outStream!=NULL) (*outStream)(txBuffer, dataOffset+length+CHECKSUM_LENGTH);
 
     // Done
     return 0;
diff --git a/flight/PiOS/STM32F10x/pios_usart.c b/flight/PiOS/STM32F10x/pios_usart.c
index 204a8e6bc..98ff0481b 100644
--- a/flight/PiOS/STM32F10x/pios_usart.c
+++ b/flight/PiOS/STM32F10x/pios_usart.c
@@ -88,6 +88,7 @@ void PIOS_USART_Init(void)
 
 	/* Configure and Init USART Tx as alternate function open-drain */
 	GPIO_InitStructure.GPIO_Pin = PIOS_USART1_TX_PIN;
+	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
 	GPIO_Init(PIOS_USART1_GPIO_PORT, &GPIO_InitStructure);
 	
 	/* Configure and Init USART Rx input with internal pull-ups */
@@ -120,7 +121,7 @@ void PIOS_USART_Init(void)
 
 	/* Configure and Init USART Tx as alternate function open-drain */
 	GPIO_InitStructure.GPIO_Pin = PIOS_USART2_TX_PIN;
-	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
+	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
 	GPIO_Init(PIOS_USART2_GPIO_PORT, &GPIO_InitStructure);
 
 	/* Configure and Init USART Rx input with internal pull-ups */
@@ -153,7 +154,7 @@ void PIOS_USART_Init(void)
 
 	/* Configure and Init USART Tx as alternate function open-drain */
 	GPIO_InitStructure.GPIO_Pin = PIOS_USART3_TX_PIN;
-	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
+	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
 	GPIO_Init(PIOS_USART3_GPIO_PORT, &GPIO_InitStructure);
 
 	/* Configure and Init USART Rx input with internal pull-ups */