diff --git a/flight/OpenPilot.posix/Modules/Actuator/actuator.c b/flight/OpenPilot.posix/Modules/Actuator/actuator.c
index 1e6ebc59d..000af5ea6 100644
--- a/flight/OpenPilot.posix/Modules/Actuator/actuator.c
+++ b/flight/OpenPilot.posix/Modules/Actuator/actuator.c
@@ -147,7 +147,7 @@ static void actuatorTask(void* parameters)
 		// Update servo outputs
 		for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n)
 		{
-			PIOS_Servo_Set( n+1, cmd.Channel[n] );
+			PIOS_Servo_Set( n, cmd.Channel[n] );
 		}
 
 		// Update output object
@@ -269,7 +269,7 @@ static void setFailsafe()
 	// Update servo outputs
 	for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n)
 	{
-		PIOS_Servo_Set( n+1, cmd.Channel[n] );
+		PIOS_Servo_Set( n, cmd.Channel[n] );
 	}
 
 	// Update output object
diff --git a/flight/OpenPilot.posix/Modules/GPS/GPS.c b/flight/OpenPilot.posix/Modules/GPS/GPS.c
index beb7a9ba6..5d20de245 100644
--- a/flight/OpenPilot.posix/Modules/GPS/GPS.c
+++ b/flight/OpenPilot.posix/Modules/GPS/GPS.c
@@ -425,16 +425,25 @@ void nmeaProcessGPGGA(char* packet)
 	// get altitude (in meters mm.m)
 	tokens = strsep(&packet, delimiter);
 	//reuse variables for alt
-	deg=strtol (tokens,&pEnd,10); // always 0.1m resolution?
+	deg=strtol (tokens,&pEnd,10); // always 0.1m resolution? No
 	desim=strtol (pEnd+1,NULL,10);
-	if(1) // OPGPS 3 desimal
+	if(1) // OPGPS 3 decimal
 		GpsData.Altitude=deg+desim/1000.0;
 	else
 		GpsData.Altitude=deg+desim/10.0;
 
 	// next field: altitude units, always 'M'
-	// next field: geoid seperation
-	// next field: seperation units
+	tokens = strsep(&packet, delimiter);
+	// next field: geoid separation
+	tokens = strsep(&packet, delimiter);
+	//reuse variables for geoid separation
+	deg=strtol (tokens,&pEnd,10); // always 0.1m resolution? No
+	desim=strtol (pEnd+1,NULL,10);
+	if(1) // OPGPS 3 decimal
+		GpsData.GeoidSeparation=deg+desim/1000.0;
+	else
+		GpsData.GeoidSeparation=deg+desim/10.0;
+	// next field: separation units
 	// next field: DGPS age
 	// next field: DGPS station ID
 	// next field: checksum
diff --git a/flight/OpenPilot.posix/Modules/Telemetry/telemetry.c b/flight/OpenPilot.posix/Modules/Telemetry/telemetry.c
index 36d60bf96..0cbc05daa 100644
--- a/flight/OpenPilot.posix/Modules/Telemetry/telemetry.c
+++ b/flight/OpenPilot.posix/Modules/Telemetry/telemetry.c
@@ -308,7 +308,7 @@ static void telemetryRxTask(void* parameters)
 	{
 #if ALLOW_HID_TELEMETRY
 		// Determine input port (USB takes priority over telemetry port)
-		if(PIOS_USB_HID_CheckAvailable())
+		if(PIOS_USB_HID_CheckAvailable(0))
 		{
 			inputPort = PIOS_COM_TELEM_USB;
 		}
@@ -343,9 +343,9 @@ static int32_t transmitData(uint8_t* data, int32_t length)
 
 	// Determine input port (USB takes priority over telemetry port)
 #if ALLOW_HID_TELEMETRY
-	if(PIOS_USB_HID_CheckAvailable())
+	if(PIOS_USB_HID_CheckAvailable(0))
 	{
-		outputPort = COM_USB_HID;
+		outputPort = PIOS_COM_TELEM_USB;
 	}
 	else
 #endif /* ALLOW_HID_TELEMETRY */
diff --git a/flight/OpenPilot.posix/System/openpilot.c b/flight/OpenPilot.posix/System/openpilot.c
index e414da92d..3745acb98 100644
--- a/flight/OpenPilot.posix/System/openpilot.c
+++ b/flight/OpenPilot.posix/System/openpilot.c
@@ -221,6 +221,8 @@ static void TaskServos(void *pvParameters)
 	/* Used to test servos, cycles all servos from one side to the other */
 	for(;;) {
 		/*xDelay = 250 / portTICK_RATE_MS;
+		PIOS_Servo_Set(0, 2000);
+		vTaskDelay(xDelay);
 		PIOS_Servo_Set(1, 2000);
 		vTaskDelay(xDelay);
 		PIOS_Servo_Set(2, 2000);
@@ -235,11 +237,7 @@ static void TaskServos(void *pvParameters)
 		vTaskDelay(xDelay);
 		PIOS_Servo_Set(7, 2000);
 		vTaskDelay(xDelay);
-		PIOS_Servo_Set(8, 2000);
-		vTaskDelay(xDelay);
 
-		PIOS_Servo_Set(8, 1000);
-		vTaskDelay(xDelay);
 		PIOS_Servo_Set(7, 1000);
 		vTaskDelay(xDelay);
 		PIOS_Servo_Set(6, 1000);
@@ -253,10 +251,13 @@ static void TaskServos(void *pvParameters)
 		PIOS_Servo_Set(2, 1000);
 		vTaskDelay(xDelay);
 		PIOS_Servo_Set(1, 1000);
+		vTaskDelay(xDelay);
+		PIOS_Servo_Set(0, 1000);
 		vTaskDelay(xDelay);*/
 
 		xDelay = 1 / portTICK_RATE_MS;
 		for(int i = 1000; i < 2000; i++) {
+			PIOS_Servo_Set(0, i);
 			PIOS_Servo_Set(1, i);
 			PIOS_Servo_Set(2, i);
 			PIOS_Servo_Set(3, i);
@@ -264,10 +265,10 @@ static void TaskServos(void *pvParameters)
 			PIOS_Servo_Set(5, i);
 			PIOS_Servo_Set(6, i);
 			PIOS_Servo_Set(7, i);
-			PIOS_Servo_Set(8, i);
 			vTaskDelay(xDelay);
 		}
 		for(int i = 2000; i > 1000; i--) {
+			PIOS_Servo_Set(0, i);
 			PIOS_Servo_Set(1, i);
 			PIOS_Servo_Set(2, i);
 			PIOS_Servo_Set(3, i);
@@ -275,7 +276,6 @@ static void TaskServos(void *pvParameters)
 			PIOS_Servo_Set(5, i);
 			PIOS_Servo_Set(6, i);
 			PIOS_Servo_Set(7, i);
-			PIOS_Servo_Set(8, i);
 			vTaskDelay(xDelay);
 		}
 	}
diff --git a/flight/OpenPilot.posix/UAVObjects/inc/positionactual.h b/flight/OpenPilot.posix/UAVObjects/inc/positionactual.h
index 6b01caef3..cd619d348 100644
--- a/flight/OpenPilot.posix/UAVObjects/inc/positionactual.h
+++ b/flight/OpenPilot.posix/UAVObjects/inc/positionactual.h
@@ -33,7 +33,7 @@
 #define POSITIONACTUAL_H
 
 // Object constants
-#define POSITIONACTUAL_OBJID 981132812U
+#define POSITIONACTUAL_OBJID 1265479538U
 #define POSITIONACTUAL_NAME "PositionActual"
 #define POSITIONACTUAL_METANAME "PositionActualMeta"
 #define POSITIONACTUAL_ISSINGLEINST 1
@@ -61,6 +61,7 @@ typedef struct {
     float Latitude;
     float Longitude;
     float Altitude;
+    float GeoidSeparation;
     float Heading;
     float Groundspeed;
     int8_t Satellites;
@@ -77,6 +78,7 @@ typedef enum { POSITIONACTUAL_STATUS_NOGPS=0, POSITIONACTUAL_STATUS_NOFIX=1, POS
 // Field Latitude information
 // Field Longitude information
 // Field Altitude information
+// Field GeoidSeparation information
 // Field Heading information
 // Field Groundspeed information
 // Field Satellites information