Merge remote-tracking branch 'op-public/next' into revo-next

Conflicts:
	flight/Libraries/inc/NMEA.h
	flight/Modules/GPS/GPS.c
	flight/Modules/GPS/NMEA.c
	flight/Modules/GPS/UBX.c
	flight/Modules/GPS/inc/NMEA.h
	flight/Modules/GPS/inc/UBX.h
	flight/Modules/PathPlanner/inc/pathplanner.h
	flight/PiOS/STM32F10x/link_STM32103CB_PIPXTREME_sections.ld
	flight/Revolution/System/inc/pios_config.h
	flight/Revolution/UAVObjects.inc
	flight/SimPosix/UAVObjects.inc
	ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/qdebughandler.h
	ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
	shared/uavobjectdefinition/gpsvelocity.xml

.gitignore

@@ -20,6 +20,9 @@
 /flight/OpenPilot/Build
 /flight/OpenPilot/Build.win32
 
+#flight/Project/OpenPilotOSX
+flight/Project/OpenPilotOSX/build
+
 # /flight/PipBee/
 /flight/PipBee/Build
 

HISTORY.txt

@@ -1,5 +1,21 @@
 Short summary of changes.  For a complete list see the git log.
 
+2012-07-20
+AeroSimRC simulator plugin is now included into the Windows distribution
+(will be installed into .../OpenPilot/misc/AeroSIM-RC directory).  Still
+being an experimental development tool, it could be used to play with
+HITL version 2.  Other platforms include udp_test utility which can be
+used to check the connectivity with AeroSimRC plugin running on Windows
+machine.
+
+2012-07-10
+On Windows the installation mode was changed from per-user to per-machine
+(for all users) installation.  It is recommended to completely uninstall
+previous version before installing new one to remove per-user installed
+files.  Per-machine installation requires elevated (administrator) previleges
+during install.  But since the same rights are now required to install
+optional CDC driver (virtual communication port), it was deemed acceptable.
+
 2012-06-04
 AeroSimRC support merged into next
 

flight/CopterControl/Makefile

@@ -199,6 +199,8 @@ SRC += $(OPUAVSYNTHDIR)/attitudesettings.c
 SRC += $(OPUAVSYNTHDIR)/camerastabsettings.c
 SRC += $(OPUAVSYNTHDIR)/cameradesired.c
 SRC += $(OPUAVSYNTHDIR)/gpsposition.c
+SRC += $(OPUAVSYNTHDIR)/gpsvelocity.c
+SRC += $(OPUAVSYNTHDIR)/gpssettings.c
 SRC += $(OPUAVSYNTHDIR)/hwsettings.c
 SRC += $(OPUAVSYNTHDIR)/gcsreceiver.c
 SRC += $(OPUAVSYNTHDIR)/receiveractivity.c

flight/CopterControl/System/inc/pios_config.h

@@ -58,7 +58,8 @@
 #define PIOS_INCLUDE_TELEMETRY_RF
 #define PIOS_INCLUDE_GPS
 #define PIOS_GPS_MINIMAL
-
+#define PIOS_INCLUDE_GPS_NMEA_PARSER /* Include the NMEA protocol parser */
+#define PIOS_INCLUDE_GPS_UBX_PARSER  /* Include the UBX protocol parser */
 #define PIOS_INCLUDE_SERVO
 #define PIOS_INCLUDE_SPI
 #define PIOS_INCLUDE_SYS

flight/Libraries/inc/packet_handler.h

@@ -106,7 +106,7 @@ void PHRegisterStatusHandler(PHInstHandle h, PHStatusHandler f);
 void PHRegisterPPMHandler(PHInstHandle h, PHPPMHandler f);
 uint32_t PHConnect(PHInstHandle h, uint32_t dest_id);
 PHPacketHandle PHGetRXPacket(PHInstHandle h);
-void PHReleaseTXPacket(PHInstHandle h, PHPacketHandle p);
+void PHReleaseRXPacket(PHInstHandle h, PHPacketHandle p);
 PHPacketHandle PHGetTXPacket(PHInstHandle h);
 void PHReleaseTXPacket(PHInstHandle h, PHPacketHandle p);
 uint8_t PHTransmitPacket(PHInstHandle h, PHPacketHandle p);

flight/Libraries/packet_handler.c

@@ -169,16 +169,15 @@ PHPacketHandle PHGetTXPacket(PHInstHandle h)
 
 	// Lock
 	xSemaphoreTakeRecursive(data->lock, portMAX_DELAY);
-	PHPacketHandle p = data->tx_packets + data->tx_win_end;
 
-	// Is the window full?
+	// Find a free packet.
-	uint8_t next_end = (data->tx_win_end + 1) % data->cfg.winSize;
+	PHPacketHandle p = NULL;
-	if(next_end == data->tx_win_start)
+	for (uint8_t i = 0; i < data->cfg.winSize; ++i)
-	{
+		if (data->tx_packets[i].header.type == PACKET_TYPE_NONE)
-		xSemaphoreGiveRecursive(data->lock);
+		{
-		return NULL;
+			p = data->tx_packets + i;
-	}
+			break;
-	data->tx_win_end = next_end;
+		}
 
 	// Release lock
 	xSemaphoreGiveRecursive(data->lock);
@@ -224,17 +223,15 @@ PHPacketHandle PHGetRXPacket(PHInstHandle h)
 
 	// Lock
 	xSemaphoreTakeRecursive(data->lock, portMAX_DELAY);
-	PHPacketHandle p = data->rx_packets + data->rx_win_end;
 
-	// Is the window full?
+	// Find a free packet.
-	uint8_t next_end = (data->rx_win_end + 1) % data->cfg.winSize;
+	PHPacketHandle p = NULL;
-	if(next_end == data->rx_win_start)
+	for (uint8_t i = 0; i < data->cfg.winSize; ++i)
-	{
+		if (data->rx_packets[i].header.type == PACKET_TYPE_NONE)
-		// Release lock
+		{
-		xSemaphoreGiveRecursive(data->lock);
+			p = data->rx_packets + i;
-		return NULL;
+			break;
-	}
+		}
-	data->rx_win_end = next_end;
 
 	// Release lock
 	xSemaphoreGiveRecursive(data->lock);

flight/Modules/Attitude/attitude.c

@@ -375,6 +375,10 @@ static int32_t updateSensorsCC3D(AccelsData * accelsData, GyrosData * gyrosData)
 	if(xQueueReceive(queue, (void *) &mpu6000_data, SENSOR_PERIOD) == errQUEUE_EMPTY)
 		return -1;	// Error, no data
 
+	// Do not read raw sensor data in simulation mode
+	if (GyrosReadOnly() || AccelsReadOnly())
+		return 0;
+
 	gyros[0] = -mpu6000_data.gyro_y * PIOS_MPU6000_GetScale();
 	gyros[1] = -mpu6000_data.gyro_x * PIOS_MPU6000_GetScale();
 	gyros[2] = -mpu6000_data.gyro_z * PIOS_MPU6000_GetScale();

flight/Modules/FlightPlan/lib/uavobject.py

@@ -109,7 +109,10 @@ class UAVObject:
 
 	def addField(self, field):
 		append(self.fields, field)
-
+	'''
+	#
+	# Support for getName was removed from embedded UAVO database to save RAM + Flash
+	#
 	def getName(self):
 		"""__NATIVE__
 		UAVObjHandle objHandle;
@@ -143,6 +146,7 @@ class UAVObject:
 		return PM_RET_OK;
 		"""
 		pass
+	'''
 
 	def read(self):
 		"""__NATIVE__

flight/Modules/GPS/GPS.c

@@ -33,13 +33,12 @@
 #include "openpilot.h"
 #include "GPS.h"
 
-#include <stdbool.h>
-
 #include "gpsposition.h"
 #include "homelocation.h"
 #include "gpstime.h"
 #include "gpssatellites.h"
 #include "gpsvelocity.h"
+#include "gpssettings.h"
 #include "WorldMagModel.h"
 #include "CoordinateConversions.h"
 #include "hwsettings.h"
@@ -63,16 +62,18 @@ static float GravityAccel(float latitude, float longitude, float altitude);
 // Private constants
 
 #define GPS_TIMEOUT_MS                  500
-#define NMEA_MAX_PACKET_LENGTH          96 // 82 max NMEA msg size plus 12 margin (because some vendors add custom crap) plus CR plus Linefeed
-// same as in COM buffer
 
 
 #ifdef PIOS_GPS_SETS_HOMELOCATION
 // Unfortunately need a good size stack for the WMM calculation
-	#define STACK_SIZE_BYTES            800
+	#define STACK_SIZE_BYTES            750
+#else
+#if defined(PIOS_GPS_MINIMAL)
+	#define STACK_SIZE_BYTES            500
 #else
 	#define STACK_SIZE_BYTES            650
-#endif
+#endif // PIOS_GPS_MINIMAL
+#endif // PIOS_GPS_SETS_HOMELOCATION
 
 #define TASK_PRIORITY                   (tskIDLE_PRIORITY + 1)
 
@@ -88,9 +89,8 @@ static char* gps_rx_buffer;
 
 static uint32_t timeOfLastCommandMs;
 static uint32_t timeOfLastUpdateMs;
-static uint32_t numUpdates;
+
-static uint32_t numChecksumErrors;
+static struct GPS_RX_STATS gpsRxStats;
-static uint32_t numParsingErrors;
 
 // ****************
 /**
@@ -122,6 +122,7 @@ int32_t GPSStart(void)
 int32_t GPSInitialize(void)
 {
 	gpsPort = PIOS_COM_GPS;
+	uint8_t	gpsProtocol;
 
 #ifdef MODULE_GPS_BUILTIN
 	gpsEnabled = true;
@@ -137,19 +138,9 @@ int32_t GPSInitialize(void)
 		gpsEnabled = false;
 #endif
 
-#if defined(REVOLUTION)
-	// Revolution expects these objects to always be defined.  Not doing so will fail some
-	// queue connections in navigation
-	GPSPositionInitialize();
-	GPSVelocityInitialize();
-	GPSTimeInitialize();
-	GPSSatellitesInitialize();
-	HomeLocationInitialize();
-	updateSettings();
-
-#else
 	if (gpsPort && gpsEnabled) {
 		GPSPositionInitialize();
+		GPSVelocityInitialize();
 #if !defined(PIOS_GPS_MINIMAL)
 		GPSTimeInitialize();
 		GPSSatellitesInitialize();
@@ -159,10 +150,20 @@ int32_t GPSInitialize(void)
 #endif
 		updateSettings();
 	}
-#endif
 
 	if (gpsPort && gpsEnabled) {
-		gps_rx_buffer = pvPortMalloc(NMEA_MAX_PACKET_LENGTH);
+		GPSSettingsInitialize();
+		GPSSettingsDataProtocolGet(&gpsProtocol);
+		switch (gpsProtocol) {
+			case GPSSETTINGS_DATAPROTOCOL_NMEA:
+				gps_rx_buffer = pvPortMalloc(NMEA_MAX_PACKET_LENGTH);
+				break;
+			case GPSSETTINGS_DATAPROTOCOL_UBX:
+				gps_rx_buffer = pvPortMalloc(sizeof(struct UBXPacket));
+				break;
+			default:
+				gps_rx_buffer = NULL;
+		}
 		PIOS_Assert(gps_rx_buffer);
 
 		return 0;
@@ -181,216 +182,75 @@ MODULE_INITCALL(GPSInitialize, GPSStart)
 static void gpsTask(void *parameters)
 {
 	portTickType xDelay = 100 / portTICK_RATE_MS;
-	uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;;
+	uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
-	GPSPositionData GpsData;
-	UBXPacket *ubx = (UBXPacket *)gps_rx_buffer;
 
-	uint8_t rx_count = 0;
+	GPSPositionData gpsposition;
-//	bool start_flag = false;
+	uint8_t	gpsProtocol;
-	bool found_cr = false;
-	enum proto_states {START,NMEA,UBX_SY2,UBX_CLASS,UBX_ID,UBX_LEN1,
-		UBX_LEN2,UBX_PAYLOAD,UBX_CHK1,UBX_CHK2};
-	enum proto_states proto_state = START;
-	int32_t gpsRxOverflow = 0;
 
-	numUpdates = 0;
+	GPSSettingsDataProtocolGet(&gpsProtocol);
-	numChecksumErrors = 0;
-	numParsingErrors = 0;
 
 	timeOfLastUpdateMs = timeNowMs;
 	timeOfLastCommandMs = timeNowMs;
 
-
+	GPSPositionGet(&gpsposition);
-	GPSPositionGet(&GpsData);
 	// Loop forever
 	while (1)
 	{
 		uint8_t c;
 
-		// NMEA or SINGLE-SENTENCE GPS mode
-
 		// This blocks the task until there is something on the buffer
 		while (PIOS_COM_ReceiveBuffer(gpsPort, &c, 1, xDelay) > 0)
 		{
-		
+			int res;
-			// detect start while acquiring stream
+			switch (gpsProtocol) {
-			switch (proto_state)
+#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER)
-			{
+				case GPSSETTINGS_DATAPROTOCOL_NMEA:
-				case START: // detect protocol
+					res = parse_nmea_stream (c,gps_rx_buffer, &gpsposition, &gpsRxStats);
-					switch (c)
-					{
-						case UBX_SYNC1: // first UBX sync char found
-							proto_state = UBX_SY2;
-							continue;
-						case '$': // NMEA identifier found
-							proto_state = NMEA;
-							found_cr = false;
-							rx_count = 0;
-							break;
-						default:
-							continue;
-					}
 					break;
-				case UBX_SY2:
+#endif
-					if (c == UBX_SYNC2) // second UBX sync char found
+#if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
-					{
+				case GPSSETTINGS_DATAPROTOCOL_UBX:
-						proto_state = UBX_CLASS;
+					res = parse_ubx_stream (c,gps_rx_buffer, &gpsposition, &gpsRxStats);
-						found_cr = false;
+					break;
-						rx_count = 0;
+#endif
-					}
+				default:
-					else
+					res = NO_PARSER; // this should not happen
-					{
-						proto_state = START; // reset state
-					}
-					continue;
-				case UBX_CLASS:
-					ubx->header.class = c;
-					proto_state = UBX_ID;
-					continue;
-				case UBX_ID:
-					ubx->header.id = c;
-					proto_state = UBX_LEN1;
-					continue;
-				case UBX_LEN1:
-					ubx->header.len = c;
-					proto_state = UBX_LEN2;
-					continue;
-				case UBX_LEN2:
-					ubx->header.len += (c << 8);
-					if ((sizeof (UBXHeader)) + ubx->header.len > NMEA_MAX_PACKET_LENGTH)
-					{
-						gpsRxOverflow++;
-						proto_state = START;
-						found_cr = false;
-						rx_count = 0;
-					}
-					else
-					{
-						proto_state = UBX_PAYLOAD;
-					}
-					continue;
-				case UBX_PAYLOAD:
-					if (rx_count < ubx->header.len)
-					{
-						ubx->payload.payload[rx_count] = c;
-						if (++rx_count == ubx->header.len)
-							proto_state = UBX_CHK1;
-					}
-					else
-						proto_state = START;
-					continue;
-				case UBX_CHK1:
-					ubx->header.ck_a = c;
-					proto_state = UBX_CHK2;
-					continue;
-				case UBX_CHK2:
-					ubx->header.ck_b = c;
-					if (checksum_ubx_message(ubx))
-					{
-						parse_ubx_message(ubx);
-					}
-					proto_state = START;
-					continue;
-				case NMEA:
 					break;
 			}
 
-
+			if (res == PARSER_COMPLETE) {
-			if (rx_count >= NMEA_MAX_PACKET_LENGTH)
+				timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
-			{
+				timeOfLastUpdateMs = timeNowMs;
-				// The buffer is already full and we haven't found a valid NMEA sentence.
+				timeOfLastCommandMs = timeNowMs;
-				// Flush the buffer and note the overflow event.
-				gpsRxOverflow++;
-				proto_state = START;
-				found_cr = false;
-				rx_count = 0;
-			}
-			else
-			{
-				gps_rx_buffer[rx_count] = c;
-				rx_count++;
-			}
-		
-			// look for ending '\r\n' sequence
-			if (!found_cr && (c == '\r') )
-				found_cr = true;
-			else
-			if (found_cr && (c != '\n') )
-				found_cr = false;  // false end flag
-			else
-			if (found_cr && (c == '\n') )
-			{
-				// The NMEA functions require a zero-terminated string
-				// As we detected \r\n, the string as for sure 2 bytes long, we will also strip the \r\n
-				gps_rx_buffer[rx_count-2] = 0;
-
-				// prepare to parse next sentence
-				proto_state = START;
-				found_cr = false;
-				rx_count = 0;
-				// Our rxBuffer must look like this now:
-				//   [0]           = '$'
-				//   ...           = zero or more bytes of sentence payload
-				//   [end_pos - 1] = '\r'
-				//   [end_pos]     = '\n'
-				//
-				// Prepare to consume the sentence from the buffer
-			
-				// Validate the checksum over the sentence
-				if (!NMEA_checksum(&gps_rx_buffer[1]))
-				{	// Invalid checksum.  May indicate dropped characters on Rx.
-					//PIOS_DEBUG_PinHigh(2);
-					++numChecksumErrors;
-					//PIOS_DEBUG_PinLow(2);
-				}
-				else
-				{	// Valid checksum, use this packet to update the GPS position
-					if (!NMEA_update_position(&gps_rx_buffer[1],&GpsData)) {
-						//PIOS_DEBUG_PinHigh(2);
-						++numParsingErrors;
-						//PIOS_DEBUG_PinLow(2);
-					}
-					else
-						++numUpdates;
-
-					timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
-					timeOfLastUpdateMs = timeNowMs;
-					timeOfLastCommandMs = timeNowMs;
-				}
 			}
 		}
 
 		// Check for GPS timeout
 		timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
-		if ((timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS)
+		if ((timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS) {
-		{	// we have not received any valid GPS sentences for a while.
+			// we have not received any valid GPS sentences for a while.
 			// either the GPS is not plugged in or a hardware problem or the GPS has locked up.
-
+			uint8_t status = GPSPOSITION_STATUS_NOGPS;
-			GpsData.Status = GPSPOSITION_STATUS_NOGPS;
+			GPSPositionStatusSet(&status);
-			GPSPositionSet(&GpsData);
 			AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR);
-
+		} else {
-		}
+			// we appear to be receiving GPS sentences OK, we've had an update
-		else
-		{	// we appear to be receiving GPS sentences OK, we've had an update
-
-#ifdef PIOS_GPS_SETS_HOMELOCATION
-			HomeLocationData home;
-			HomeLocationGet(&home);
-
-			if ((GpsData.Status == GPSPOSITION_STATUS_FIX3D) && (home.Set == HOMELOCATION_SET_FALSE))
-				setHomeLocation(&GpsData);
-#endif
-
 			//criteria for GPS-OK taken from this post...
 			//http://forums.openpilot.org/topic/1523-professors-insgps-in-svn/page__view__findpost__p__5220
-			if ((GpsData.PDOP < 3.5) && (GpsData.Satellites >= 7))
+			if ((gpsposition.PDOP < 3.5) && (gpsposition.Satellites >= 7) &&
+					(gpsposition.Status == GPSPOSITION_STATUS_FIX3D)) {
 				AlarmsClear(SYSTEMALARMS_ALARM_GPS);
-			else
+#ifdef PIOS_GPS_SETS_HOMELOCATION
-			if (GpsData.Status == GPSPOSITION_STATUS_FIX3D)
+				HomeLocationData home;
-				AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_WARNING);
+				HomeLocationGet(&home);
-			else
+
-				AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL);
+				if (home.Set == HOMELOCATION_SET_FALSE)
+					setHomeLocation(&gpsposition);
+#endif
+			} else if (gpsposition.Status == GPSPOSITION_STATUS_FIX3D)
+						AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_WARNING);
+					else
+						AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL);
 		}
 
 	}

flight/Modules/GPS/NMEA.c

@@ -30,10 +30,14 @@
 
 #include "openpilot.h"
 #include "pios.h"
+
+#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER)
+
 #include "gpsposition.h"
 #include "NMEA.h"
 #include "gpstime.h"
 #include "gpssatellites.h"
+#include "GPS.h"
 
 //#define ENABLE_DEBUG_MSG						///< define to enable debug-messages
 #define DEBUG_PORT		PIOS_COM_TELEM_RF		///< defines which serial port is ued for debug-messages
@@ -43,7 +47,7 @@
 // Debugging
 #ifdef ENABLE_DEBUG_MSG
 //#define DEBUG_MSG_IN			///< define to display the incoming NMEA messages
-//#define DEBUG_PARS			///< define to display the incoming NMEA messages split into its parameters
+//#define DEBUG_PARAMS			///< define to display the incoming NMEA messages split into its parameters
 //#define DEBUG_MGSID_IN		///< define to display the the names of the incoming NMEA messages
 //#define NMEA_DEBUG_PKT		///< define to enable debug of all NMEA messages
 //#define NMEA_DEBUG_GGA		///< define to enable debug of GGA messages
@@ -63,7 +67,6 @@
 struct nmea_parser {
 	const char *prefix;
 	bool(*handler) (GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam);
-	uint32_t cnt;
 };
 
 static bool nmeaProcessGPGGA(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam);
@@ -75,50 +78,125 @@ static bool nmeaProcessGPGSA(GPSPositionData * GpsData, bool* gpsDataUpdated, ch
 	static bool nmeaProcessGPGSV(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam);
 #endif //PIOS_GPS_MINIMAL
 
-
+const static struct nmea_parser nmea_parsers[] = {
-static struct nmea_parser nmea_parsers[] = {
 	{
 		.prefix = "GPGGA",
 		.handler = nmeaProcessGPGGA,
-		.cnt = 0,
 	},
 	{
 		.prefix = "GPVTG",
 		.handler = nmeaProcessGPVTG,
-		.cnt = 0,
 	},
 	{
 		.prefix = "GPGSA",
 		.handler = nmeaProcessGPGSA,
-		.cnt = 0,
 	},
 	{
 		.prefix = "GPRMC",
 		.handler = nmeaProcessGPRMC,
-		.cnt = 0,
 	},
 #if !defined(PIOS_GPS_MINIMAL)
 	{
 		.prefix = "GPZDA",
 		.handler = nmeaProcessGPZDA,
-		.cnt = 0,
 	},
 	{
 		.prefix = "GPGSV",
 		.handler = nmeaProcessGPGSV,
-		.cnt = 0,
 	},
 #endif //PIOS_GPS_MINIMAL
 };
 
-static struct nmea_parser *NMEA_find_parser_by_prefix(const char *prefix)
+int parse_nmea_stream (uint8_t c, char *gps_rx_buffer, GPSPositionData *GpsData, struct GPS_RX_STATS *gpsRxStats)
+{
+	static uint8_t rx_count = 0;
+	static bool start_flag = false;
+	static bool found_cr = false;
+
+	// detect start while acquiring stream
+	if (!start_flag && (c == '$')) // NMEA identifier found
+	{
+		start_flag = true;
+		found_cr = false;
+		rx_count = 0;
+	}
+	else
+	if (!start_flag)
+		return PARSER_ERROR;
+
+	if (rx_count >= NMEA_MAX_PACKET_LENGTH)
+	{
+		// The buffer is already full and we haven't found a valid NMEA sentence.
+		// Flush the buffer and note the overflow event.
+		gpsRxStats->gpsRxOverflow++;
+		start_flag = false;
+		found_cr = false;
+		rx_count = 0;
+		return PARSER_OVERRUN;
+	}
+	else
+	{
+		gps_rx_buffer[rx_count] = c;
+		rx_count++;
+	}
+
+	// look for ending '\r\n' sequence
+	if (!found_cr && (c == '\r') )
+		found_cr = true;
+	else
+	if (found_cr && (c != '\n') )
+		found_cr = false;  // false end flag
+	else
+	if (found_cr && (c == '\n') )
+	{
+		// The NMEA functions require a zero-terminated string
+		// As we detected \r\n, the string as for sure 2 bytes long, we will also strip the \r\n
+		gps_rx_buffer[rx_count-2] = 0;
+
+		// prepare to parse next sentence
+		start_flag = false;
+		found_cr = false;
+		rx_count = 0;
+		// Our rxBuffer must look like this now:
+		//   [0]           = '$'
+		//   ...           = zero or more bytes of sentence payload
+		//   [end_pos - 1] = '\r'
+		//   [end_pos]     = '\n'
+		//
+		// Prepare to consume the sentence from the buffer
+
+		// Validate the checksum over the sentence
+		if (!NMEA_checksum(&gps_rx_buffer[1]))
+		{	// Invalid checksum.  May indicate dropped characters on Rx.
+			//PIOS_DEBUG_PinHigh(2);
+			gpsRxStats->gpsRxChkSumError++;
+			//PIOS_DEBUG_PinLow(2);
+			return PARSER_ERROR;
+		}
+		else
+		{	// Valid checksum, use this packet to update the GPS position
+			if (!NMEA_update_position(&gps_rx_buffer[1], GpsData)) {
+				//PIOS_DEBUG_PinHigh(2);
+				gpsRxStats->gpsRxParserError++;
+				//PIOS_DEBUG_PinLow(2);
+			}
+			else
+				gpsRxStats->gpsRxReceived++;;
+
+			return PARSER_COMPLETE;
+		}
+	}
+	return PARSER_INCOMPLETE;
+}
+
+const static struct nmea_parser *NMEA_find_parser_by_prefix(const char *prefix)
 {
 	if (!prefix) {
 		return (NULL);
 	}
 
 	for (uint8_t i = 0; i < NELEMENTS(nmea_parsers); i++) {
-		struct nmea_parser *parser = &nmea_parsers[i];
+		const struct nmea_parser *parser = &nmea_parsers[i];
 
 		/* Use strcmp to check for exact equality over the entire prefix */
 		if (!strcmp(prefix, parser->prefix)) {
@@ -331,7 +409,7 @@ bool NMEA_update_position(char *nmea_sentence, GPSPositionData *GpsData)
 #endif
 
 	// The first parameter is the message name, lets see if we find a parser for it
-	struct nmea_parser *parser;
+	const struct nmea_parser *parser;
 	parser = NMEA_find_parser_by_prefix(params[0]);
 	if (!parser) {
 		// No parser found
@@ -339,16 +417,22 @@ bool NMEA_update_position(char *nmea_sentence, GPSPositionData *GpsData)
 		return false;
 	}
 
-	parser->cnt++;
 	#ifdef DEBUG_MGSID_IN
-		DEBUG_MSG("%s %d ", params[0], parser->cnt);
+		DEBUG_MSG("%s %d ", params[0]);
 	#endif
 	// Send the message to the parser and get it update the GpsData
+	// Information from various different NMEA messages are temporarily
+	// cumulated in the GpsData structure. An actual GPSPosition update
+	// is triggered by GGA messages only. This message type sets the
+	// gpsDataUpdated flag to request this.
 	bool gpsDataUpdated = false;
 
 	if (!parser->handler(GpsData, &gpsDataUpdated, params, nbParams)) {
 		// Parse failed
 		DEBUG_MSG("PARSE FAILED (\"%s\")\n", params[0]);
+		if (gpsDataUpdated && (GpsData->Status == GPSPOSITION_STATUS_NOFIX)) {
+			GPSPositionSet(GpsData);
+		}
 		return false;
 	}
 
@@ -392,6 +476,13 @@ static bool nmeaProcessGPGGA(GPSPositionData * GpsData, bool* gpsDataUpdated, ch
 
 	*gpsDataUpdated = true;
 
+	// check for invalid GPS fix
+	// do this first to make sure we get this information, even if later checks exit
+	// this function early
+	if (param[6][0] == '0') {
+		GpsData->Status = GPSPOSITION_STATUS_NOFIX; // treat invalid fix as NOFIX
+	}
+
 	// get latitude [DDMM.mmmmm] [N|S]
 	if (!NMEA_latlon_to_fixed_point(&GpsData->Latitude, param[2], param[3][0] == 'S')) {
 		return false;
@@ -681,3 +772,4 @@ static bool nmeaProcessGPGSA(GPSPositionData * GpsData, bool* gpsDataUpdated, ch
 	return true;
 }
 
+#endif // PIOS_INCLUDE_GPS_NMEA_PARSER

flight/Modules/GPS/UBX.c

@@ -30,10 +30,137 @@
 
 #include "openpilot.h"
 #include "pios.h"
-#include "UBX.h"
-#include "gpsvelocity.h"
 
-bool checksum_ubx_message (UBXPacket *ubx)
+#if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
+
+#include "UBX.h"
+#include "GPS.h"
+
+// parse incoming character stream for messages in UBX binary format
+
+int parse_ubx_stream (uint8_t c, char *gps_rx_buffer, GPSPositionData *GpsData, struct GPS_RX_STATS *gpsRxStats)
+{
+	enum proto_states {
+		START,
+		UBX_SY2,
+		UBX_CLASS,
+		UBX_ID,
+		UBX_LEN1,
+		UBX_LEN2,
+		UBX_PAYLOAD,
+		UBX_CHK1,
+		UBX_CHK2,
+		FINISHED
+	};
+
+	static enum proto_states proto_state = START;
+	static uint8_t rx_count = 0;
+	struct UBXPacket *ubx = (struct UBXPacket *)gps_rx_buffer;
+
+	switch (proto_state) {
+		case START: // detect protocol
+			if (c ==  UBX_SYNC1) // first UBX sync char found
+				proto_state = UBX_SY2;
+			break;
+		case UBX_SY2:
+			if (c == UBX_SYNC2) // second UBX sync char found
+				proto_state = UBX_CLASS;
+			else
+				proto_state = START; // reset state
+			break;
+		case UBX_CLASS:
+			ubx->header.class = c;
+			proto_state = UBX_ID;
+			break;
+		case UBX_ID:
+			ubx->header.id = c;
+			proto_state = UBX_LEN1;
+			break;
+		case UBX_LEN1:
+			ubx->header.len = c;
+			proto_state = UBX_LEN2;
+			break;
+		case UBX_LEN2:
+			ubx->header.len += (c << 8);
+			if (ubx->header.len > sizeof(UBXPayload)) {
+				gpsRxStats->gpsRxOverflow++;
+				proto_state = START;
+			} else {
+				rx_count = 0;
+				proto_state = UBX_PAYLOAD;
+			}
+			break;
+		case UBX_PAYLOAD:
+			if (rx_count < ubx->header.len) {
+				ubx->payload.payload[rx_count] = c;
+				if (++rx_count == ubx->header.len)
+					proto_state = UBX_CHK1;
+			} else {
+				gpsRxStats->gpsRxOverflow++;
+				proto_state = START;
+			}
+			break;
+		case UBX_CHK1:
+			ubx->header.ck_a = c;
+			proto_state = UBX_CHK2;
+			break;
+		case UBX_CHK2:
+			ubx->header.ck_b = c;
+			if (checksum_ubx_message(ubx)) { // message complete and valid
+				parse_ubx_message(ubx, GpsData);
+				proto_state = FINISHED;
+			} else {
+				gpsRxStats->gpsRxChkSumError++;
+				proto_state = START;
+			}
+			break;
+		default: break;
+	}
+
+	if (proto_state == START)
+		return PARSER_ERROR;	// parser couldn't use this byte
+	else if (proto_state == FINISHED) {
+		gpsRxStats->gpsRxReceived++;
+		proto_state = START;
+		return PARSER_COMPLETE;	// message complete & processed
+	}
+
+	return PARSER_INCOMPLETE; // message not (yet) complete
+}
+
+
+// Keep track of various GPS messages needed to make up a single UAVO update
+// time-of-week timestamp is used to correlate matching messages
+
+#define POSLLH_RECEIVED	(1 << 0)
+#define STATUS_RECEIVED	(1 << 1)
+#define DOP_RECEIVED	(1 << 2)
+#define VELNED_RECEIVED	(1 << 3)
+#define SOL_RECEIVED	(1 << 4)
+#define	ALL_RECEIVED	(SOL_RECEIVED | VELNED_RECEIVED | DOP_RECEIVED | POSLLH_RECEIVED)
+#define NONE_RECEIVED	0
+
+static struct msgtracker{
+		uint32_t	currentTOW;		// TOW of the message set currently in progress
+		uint8_t		msg_received;	// keep track of received message types
+	} msgtracker;
+
+// Check if a message belongs to the current data set and register it as 'received'
+bool check_msgtracker (uint32_t tow, uint8_t msg_flag)
+{
+
+	if (tow > msgtracker.currentTOW ? true                  // start of a new message set
+		: (msgtracker.currentTOW - tow > 6*24*3600*1000)) { // 6 days, TOW wrap around occured
+		msgtracker.currentTOW = tow;
+		msgtracker.msg_received = NONE_RECEIVED;
+	} else if (tow < msgtracker.currentTOW)	// message outdated (don't process)
+				return false;
+
+	msgtracker.msg_received |= msg_flag; // register reception of this msg type
+	return true;
+}
+
+bool checksum_ubx_message (struct UBXPacket *ubx)
 {
 	int i;
 	uint8_t ck_a, ck_b;
@@ -50,50 +177,165 @@ bool checksum_ubx_message (UBXPacket *ubx)
 	ck_a += ubx->header.len >> 8;
 	ck_b += ck_a;
 
-
+	for (i = 0; i < ubx->header.len; i++) {
-
-	for (i = 0; i < ubx->header.len; i++)
-	{
 		ck_a += ubx->payload.payload[i];
 		ck_b += ck_a;
 	}
 
 	if (ubx->header.ck_a == ck_a &&
 			ubx->header.ck_b == ck_b)
-	{
 		return true;
-	}
 	else
 		return false;
 
 }
 
-void parse_ubx_nav_velned (UBXPayload payload)
+void parse_ubx_nav_posllh (struct UBX_NAV_POSLLH *posllh, GPSPositionData *GpsPosition)
 {
-#if defined(REVOLUTION)
+	if (check_msgtracker(posllh->iTOW, POSLLH_RECEIVED)) {
-	GPSVelocityData GpsVelocity;
+		if (GpsPosition->Status != GPSPOSITION_STATUS_NOFIX) {
-	GPSVelocityGet(&GpsVelocity);
+			GpsPosition->Altitude = (float)posllh->hMSL*0.001f;
-
+			GpsPosition->GeoidSeparation = (float)(posllh->height - posllh->hMSL)*0.001f;
-	GpsVelocity.North	= (float)payload.nav_velned.velN/100.0f;
+			GpsPosition->Latitude = posllh->lat;
-	GpsVelocity.East	= (float)payload.nav_velned.velE/100.0f;
+			GpsPosition->Longitude = posllh->lon;
-	GpsVelocity.Down	= (float)payload.nav_velned.velD/100.0f;
+		}
-
-	GPSVelocitySet(&GpsVelocity);
-#endif
-}
-
-void parse_ubx_message (UBXPacket *ubx)
-{
-	switch (ubx->header.class)
-	{
-		case UBX_CLASS_NAV:
-			switch (ubx->header.id)
-			{
-				case UBX_ID_VELNED:
-					parse_ubx_nav_velned (ubx->payload);
-			}
-			break;
 	}
 }
 
+void parse_ubx_nav_sol (struct UBX_NAV_SOL *sol, GPSPositionData *GpsPosition)
+{
+	if (check_msgtracker(sol->iTOW, SOL_RECEIVED)) {
+		GpsPosition->Satellites = sol->numSV;
+
+		if (sol->flags & STATUS_FLAGS_GPSFIX_OK) {
+			switch (sol->gpsFix) {
+				case STATUS_GPSFIX_2DFIX:
+					GpsPosition->Status = GPSPOSITION_STATUS_FIX2D;
+					break;
+				case STATUS_GPSFIX_3DFIX:
+					GpsPosition->Status = GPSPOSITION_STATUS_FIX3D;
+					break;
+				default: GpsPosition->Status = GPSPOSITION_STATUS_NOFIX;
+			}
+		}
+		else // fix is not valid so we make sure to treat is as NOFIX
+			GpsPosition->Status = GPSPOSITION_STATUS_NOFIX;
+	}
+}
+
+void parse_ubx_nav_dop (struct UBX_NAV_DOP *dop, GPSPositionData *GpsPosition)
+{
+	if (check_msgtracker(dop->iTOW, DOP_RECEIVED)) {
+		GpsPosition->HDOP = (float)dop->hDOP * 0.01f;
+		GpsPosition->VDOP = (float)dop->vDOP * 0.01f;
+		GpsPosition->PDOP = (float)dop->pDOP * 0.01f;
+	}
+}
+
+void parse_ubx_nav_velned (struct UBX_NAV_VELNED *velned, GPSPositionData *GpsPosition)
+{
+	GPSVelocityData GpsVelocity;
+
+	if (check_msgtracker(velned->iTOW, VELNED_RECEIVED)) {
+		if (GpsPosition->Status != GPSPOSITION_STATUS_NOFIX) {
+			GpsVelocity.North	= (float)velned->velN/100.0f;
+			GpsVelocity.East	= (float)velned->velE/100.0f;
+			GpsVelocity.Down	= (float)velned->velD/100.0f;
+			GPSVelocitySet(&GpsVelocity);
+			GpsPosition->Groundspeed = (float)velned->gSpeed * 0.01f;
+			GpsPosition->Heading = (float)velned->heading * 1.0e-5f;
+		}
+	}
+}
+
+#if !defined(PIOS_GPS_MINIMAL)
+void parse_ubx_nav_timeutc (struct UBX_NAV_TIMEUTC *timeutc)
+{
+	if (!(timeutc->valid & TIMEUTC_VALIDUTC))
+		return;
+
+	GPSTimeData GpsTime;
+
+	GpsTime.Year = timeutc->year;
+	GpsTime.Month = timeutc->month;
+	GpsTime.Day = timeutc->day;
+	GpsTime.Hour = timeutc->hour;
+	GpsTime.Minute = timeutc->min;
+	GpsTime.Second = timeutc->sec;
+
+	GPSTimeSet(&GpsTime);
+}
+#endif
+
+#if !defined(PIOS_GPS_MINIMAL)
+void parse_ubx_nav_svinfo (struct UBX_NAV_SVINFO *svinfo)
+{
+	uint8_t chan;
+	GPSSatellitesData svdata;
+
+	svdata.SatsInView = 0;
+	for (chan = 0; chan < svinfo->numCh;	chan++) {
+		if (svdata.SatsInView < GPSSATELLITES_PRN_NUMELEM) {
+			svdata.Azimuth[svdata.SatsInView] = (float)svinfo->sv[chan].azim;
+			svdata.Elevation[svdata.SatsInView] = (float)svinfo->sv[chan].elev;
+			svdata.PRN[svdata.SatsInView] = svinfo->sv[chan].svid;
+			svdata.SNR[svdata.SatsInView] = svinfo->sv[chan].cno;
+			svdata.SatsInView++;
+		}
+
+	}
+	// fill remaining slots (if any)
+	for (chan = svdata.SatsInView; chan < GPSSATELLITES_PRN_NUMELEM; chan++) {
+		svdata.Azimuth[chan] = (float)0.0f;
+		svdata.Elevation[chan] = (float)0.0f;
+		svdata.PRN[chan] = 0;
+		svdata.SNR[chan] = 0;
+	}
+
+	GPSSatellitesSet(&svdata);
+}
+#endif
+
+// UBX message parser
+// returns UAVObjectID if a UAVObject structure is ready for further processing
+
+uint32_t parse_ubx_message (struct UBXPacket *ubx, GPSPositionData *GpsPosition)
+{
+	uint32_t id = 0;
+
+	switch (ubx->header.class) {
+		case UBX_CLASS_NAV:
+			switch (ubx->header.id) {
+				case UBX_ID_POSLLH:
+					parse_ubx_nav_posllh (&ubx->payload.nav_posllh, GpsPosition);
+					break;
+				case UBX_ID_DOP:
+					parse_ubx_nav_dop (&ubx->payload.nav_dop, GpsPosition);
+					break;
+				case UBX_ID_SOL:
+					parse_ubx_nav_sol (&ubx->payload.nav_sol, GpsPosition);
+					break;
+				case UBX_ID_VELNED:
+					parse_ubx_nav_velned (&ubx->payload.nav_velned, GpsPosition);
+					break;
+#if !defined(PIOS_GPS_MINIMAL)
+				case UBX_ID_TIMEUTC:
+					parse_ubx_nav_timeutc (&ubx->payload.nav_timeutc);
+					break;
+				case UBX_ID_SVINFO:
+					parse_ubx_nav_svinfo (&ubx->payload.nav_svinfo);
+					break;
+#endif
+			}
+			break;
+	}
+	if (msgtracker.msg_received == ALL_RECEIVED) {
+		GPSPositionSet(GpsPosition);
+		msgtracker.msg_received = NONE_RECEIVED;
+		id = GPSPOSITION_OBJID;
+	}
+	return id;
+}
+
+#endif // PIOS_INCLUDE_GPS_UBX_PARSER
 

flight/Modules/GPS/inc/GPS.h

@@ -34,6 +34,24 @@
 #ifndef GPS_H
 #define GPS_H
 
+#include "gpsvelocity.h"
+#include "gpssatellites.h"
+#include "gpsposition.h"
+#include "gpstime.h"
+
+#define	NO_PARSER	-3 // no parser available
+#define	PARSER_OVERRUN	-2 // message buffer overrun before completing the message
+#define	PARSER_ERROR	-1 // message unparsable by this parser
+#define PARSER_INCOMPLETE	0 // parser needs more data to complete the message
+#define PARSER_COMPLETE	1 // parser has received a complete message and finished processing
+
+struct GPS_RX_STATS {
+	uint16_t gpsRxReceived;
+	uint16_t gpsRxChkSumError;
+	uint16_t gpsRxOverflow;
+	uint16_t gpsRxParserError;
+};
+
 int32_t GPSInitialize(void);
 
 #endif // GPS_H

flight/Modules/GPS/inc/NMEA.h

@@ -33,8 +33,12 @@
 
 #include <stdbool.h>
 #include <stdint.h>
+#include "GPS.h"
+
+#define NMEA_MAX_PACKET_LENGTH          96 // 82 max NMEA msg size plus 12 margin (because some vendors add custom crap) plus CR plus Linefeed
 
 extern bool NMEA_update_position(char *nmea_sentence, GPSPositionData *GpsData);
 extern bool NMEA_checksum(char *nmea_sentence);
+extern int parse_nmea_stream(uint8_t, char *, GPSPositionData *, struct GPS_RX_STATS *);
 
 #endif /* NMEA_H */

flight/Modules/GPS/inc/UBX.h

@@ -31,6 +31,9 @@
 #ifndef UBX_H
 #define UBX_H
 #include "openpilot.h"
+#include "gpsposition.h"
+#include "GPS.h"
+
 
 #define UBX_SYNC1						0xb5 // UBX protocol synchronization characters
 #define UBX_SYNC2						0x62
@@ -41,40 +44,181 @@
 #define UBX_CLASS_NAV	0x01
 
 // Message IDs
+#define UBX_ID_POSLLH	0x02
+#define UBX_ID_STATUS	0x03
+#define UBX_ID_DOP		0x04
+#define UBX_ID_SOL		0x06
 #define	UBX_ID_VELNED	0x12
+#define UBX_ID_TIMEUTC	0x21
+#define UBX_ID_SVINFO	0x30
 
 // private structures
 
-typedef struct  {
+// Geodetic Position Solution
-	uint32_t	iTOW;	// ms GPS Millisecond Time of Week
+struct UBX_NAV_POSLLH {
-	int32_t		velN;	// cm/s NED north velocity
+	uint32_t	iTOW;   // GPS Millisecond Time of Week (ms)
-	int32_t		velE;	// cm/s NED east velocity
+	int32_t		lon;    // Longitude (deg*1e-7)
-	int32_t		velD;	// cm/s NED down velocity
+	int32_t		lat;    // Latitude (deg*1e-7)
-	uint32_t	speed;	// cm/s Speed (3-D)
+	int32_t		height; // Height above Ellipsoid (mm)
-	uint32_t	gSpeed;	// cm/s Ground Speed (2-D)
+	int32_t		hMSL;   // Height above mean sea level (mm)
-	int32_t		heading;	// 1e-5 *deg Heading of motion 2-D
+	uint32_t	hAcc;   // Horizontal Accuracy Estimate (mm)
-	uint32_t	sAcc;	// cm/s Speed Accuracy Estimate
+	uint32_t	vAcc;   // Vertical Accuracy Estimate (mm)
-	uint32_t	cAcc;	// 1e-5 *deg Course / Heading Accuracy Estimate
+};
-} UBX_NAV_VELNED;
 
-typedef union {		// add more message types later
+// Receiver Navigation Status
+
+#define STATUS_GPSFIX_NOFIX		0x00
+#define STATUS_GPSFIX_DRONLY	0x01
+#define STATUS_GPSFIX_2DFIX		0x02
+#define STATUS_GPSFIX_3DFIX		0x03
+#define STATUS_GPSFIX_GPSDR		0x04
+#define STATUS_GPSFIX_TIMEONLY	0x05
+
+#define STATUS_FLAGS_GPSFIX_OK	(1 << 0)
+#define STATUS_FLAGS_DIFFSOLN	(1 << 1)
+#define STATUS_FLAGS_WKNSET		(1 << 2)
+#define STATUS_FLAGS_TOWSET		(1 << 3)
+
+struct UBX_NAV_STATUS {
+	uint32_t	iTOW;    // GPS Millisecond Time of Week (ms)
+	uint8_t		gpsFix;  // GPS fix type
+	uint8_t		flags;   // Navigation Status Flags
+	uint8_t		fixStat; // Fix Status Information
+	uint8_t		flags2;  // Additional navigation output information
+	uint32_t	ttff;    // Time to first fix (ms)
+	uint32_t	msss;    // Milliseconds since startup/reset (ms)
+};
+
+// Dilution of precision
+struct UBX_NAV_DOP {
+	uint32_t	iTOW;  // GPS Millisecond Time of Week (ms)
+	uint16_t	gDOP;  // Geometric DOP
+	uint16_t	pDOP;  // Position DOP
+	uint16_t	tDOP;  // Time DOP
+	uint16_t	vDOP;  // Vertical DOP
+	uint16_t	hDOP;  // Horizontal DOP
+	uint16_t	nDOP;  // Northing DOP
+	uint16_t	eDOP;  // Easting DOP
+};
+
+// Navigation solution
+
+struct UBX_NAV_SOL {
+	uint32_t	iTOW;       // GPS Millisecond Time of Week (ms)
+	int32_t		fTOW;       // fractional nanoseconds (ns)
+	int16_t		week;       // GPS week
+	uint8_t		gpsFix;     // GPS fix type
+	uint8_t		flags;      // Fix status flags
+	int32_t		ecefX;      // ECEF X coordinate (cm)
+	int32_t		ecefY;      // ECEF Y coordinate (cm)
+	int32_t		ecefZ;      // ECEF Z coordinate (cm)
+	uint32_t	pAcc;       // 3D Position Accuracy Estimate (cm)
+	int32_t		ecefVX;     // ECEF X coordinate (cm/s)
+	int32_t		ecefVY;     // ECEF Y coordinate (cm/s)
+	int32_t		ecefVZ;     // ECEF Z coordinate (cm/s)
+	uint32_t	sAcc;       // Speed Accuracy Estimate
+	uint16_t	pDOP;       // Position DOP
+	uint8_t		reserved1;  // Reserved
+	uint8_t		numSV;      // Number of SVs used in Nav Solution
+	uint32_t	reserved2;  // Reserved
+};
+
+// North/East/Down velocity
+
+struct UBX_NAV_VELNED {
+	uint32_t	iTOW;     // ms GPS Millisecond Time of Week
+	int32_t		velN;     // cm/s NED north velocity
+	int32_t		velE;     // cm/s NED east velocity
+	int32_t		velD;     // cm/s NED down velocity
+	uint32_t	speed;    // cm/s Speed (3-D)
+	uint32_t	gSpeed;   // cm/s Ground Speed (2-D)
+	int32_t		heading;  // 1e-5 *deg Heading of motion 2-D
+	uint32_t	sAcc;     // cm/s Speed Accuracy Estimate
+	uint32_t	cAcc;     // 1e-5 *deg Course / Heading Accuracy Estimate
+};
+
+// UTC Time Solution
+
+#define TIMEUTC_VALIDTOW	(1 << 0)
+#define TIMEUTC_VALIDWKN	(1 << 1)
+#define TIMEUTC_VALIDUTC	(1 << 2)
+
+struct UBX_NAV_TIMEUTC {
+	uint32_t	iTOW;   // GPS Millisecond Time of Week (ms)
+	uint32_t	tAcc;   // Time Accuracy Estimate (ns)
+	int32_t		nano;   // Nanoseconds of second
+	uint16_t	year;
+	uint8_t		month;
+	uint8_t		day;
+	uint8_t		hour;
+	uint8_t		min;
+	uint8_t		sec;
+	uint8_t		valid;  // Validity Flags
+};
+
+// Space Vehicle (SV) Information
+
+// Single SV information block
+
+#define SVUSED		(1 << 0) // This SV is used for navigation
+#define DIFFCORR 	(1 << 1) // Differential correction available
+#define ORBITAVAIL	(1 << 2) // Orbit information available
+#define ORBITEPH	(1 << 3) // Orbit information is Ephemeris
+#define UNHEALTHY	(1 << 4) // SV is unhealthy
+#define ORBITALM	(1 << 5) // Orbit information is Almanac Plus
+#define ORBITAOP	(1 << 6) // Orbit information is AssistNow Autonomous
+#define	SMOOTHED	(1 << 7) // Carrier smoothed pseudoranges used
+
+struct UBX_NAV_SVINFO_SV {
+	uint8_t		chn;      // Channel number
+	uint8_t		svid;     // Satellite ID
+	uint8_t		flags;    // Misc SV information
+	uint8_t		quality;  // Misc quality indicators
+	uint8_t		cno;      // Carrier to Noise Ratio (dbHz)
+	int8_t		elev;     // Elevation (integer degrees)
+	int16_t		azim;     // Azimuth	(integer degrees)
+	int32_t		prRes;    // Pseudo range residual (cm)
+};
+
+// SV information message
+#define MAX_SVS	16
+
+struct UBX_NAV_SVINFO {
+	uint32_t	iTOW;         // GPS Millisecond Time of Week (ms)
+	uint8_t		numCh;        // Number of channels
+	uint8_t		globalFlags;  //
+	uint16_t	reserved2;    // Reserved
+	struct UBX_NAV_SVINFO_SV	sv[MAX_SVS]; // Repeated 'numCh' times
+};
+
+typedef union {
 	uint8_t		payload[0];
-	UBX_NAV_VELNED	nav_velned;
+	struct UBX_NAV_POSLLH	nav_posllh;
+	struct UBX_NAV_STATUS	nav_status;
+	struct UBX_NAV_DOP		nav_dop;
+	struct UBX_NAV_SOL		nav_sol;
+	struct UBX_NAV_VELNED	nav_velned;
+#if !defined(PIOS_GPS_MINIMAL)
+	struct UBX_NAV_TIMEUTC	nav_timeutc;
+	struct UBX_NAV_SVINFO	nav_svinfo;
+#endif
 } UBXPayload;
 
-typedef struct {
+struct UBXHeader {
 	uint8_t 	class;
 	uint8_t 	id;
 	uint16_t	len;
 	uint8_t 	ck_a;
 	uint8_t 	ck_b;
-} UBXHeader;
+};
 
-typedef struct {
+struct UBXPacket {
-	UBXHeader	header;
+	struct UBXHeader	header;
 	UBXPayload	payload;
-} UBXPacket;
+};
+
+bool checksum_ubx_message(struct UBXPacket *);
+uint32_t parse_ubx_message(struct UBXPacket *, GPSPositionData *);
+int  parse_ubx_stream(uint8_t, char *, GPSPositionData *, struct GPS_RX_STATS *);
 
-bool checksum_ubx_message(UBXPacket *);
-void parse_ubx_message(UBXPacket *);
 #endif /* UBX_H */

flight/Modules/RadioComBridge/RadioComBridge.c

@@ -61,8 +61,9 @@
 #define PACKET_QUEUE_SIZE 10
 #define MAX_PORT_DELAY 200
 #define EV_PACKET_RECEIVED 0x20
+#define EV_TRANSMIT_PACKET 0x30
 #define EV_SEND_ACK 0x40
-#define EV_SEND_NACK 0x80
+#define EV_SEND_NACK 0x50
 
 // ****************
 // Private types
@@ -79,22 +80,36 @@ typedef struct {
 } PairStats;
 
 typedef struct {
+	uint32_t comPort;
+	UAVTalkConnection UAVTalkCon;
+	xQueueHandle sendQueue;
+	xQueueHandle recvQueue;
+	xQueueHandle gcsQueue;
+	uint16_t wdg;
+	bool checkHID;
+} UAVTalkComTaskParams;
+
+typedef struct {
+
 	// The task handles.
-	xTaskHandle comUAVTalkTaskHandle;
+	xTaskHandle GCSUAVTalkRecvTaskHandle;
+	xTaskHandle UAVTalkRecvTaskHandle;
 	xTaskHandle radioReceiveTaskHandle;
 	xTaskHandle sendPacketTaskHandle;
-	xTaskHandle sendDataTaskHandle;
+	xTaskHandle UAVTalkSendTaskHandle;
 	xTaskHandle radioStatusTaskHandle;
 	xTaskHandle transparentCommTaskHandle;
 	xTaskHandle ppmInputTaskHandle;
 
 	// The UAVTalk connection on the com side.
-	UAVTalkConnection inUAVTalkCon;
+	UAVTalkConnection UAVTalkCon;
-	UAVTalkConnection outUAVTalkCon;
+	UAVTalkConnection GCSUAVTalkCon;
 
 	// Queue handles.
-	xQueueHandle sendPacketQueue;
+	xQueueHandle radioPacketQueue;
-	xQueueHandle objEventQueue;
+	xQueueHandle gcsEventQueue;
+	xQueueHandle uavtalkEventQueue;
+	xQueueHandle ppmOutQueue;
 
 	// Error statistics.
 	uint32_t comTxErrors;
@@ -122,6 +137,10 @@ typedef struct {
 	// The RSSI of the last packet received.
 	int8_t RSSI;
 
+	// Thread parameters.
+	UAVTalkComTaskParams uavtalk_params;
+	UAVTalkComTaskParams gcs_uavtalk_params;
+
 } RadioComBridgeData;
 
 typedef struct {
@@ -135,21 +154,24 @@ typedef struct {
 // ****************
 // Private functions
 
-static void comUAVTalkTask(void *parameters);
+static void UAVTalkRecvTask(void *parameters);
 static void radioReceiveTask(void *parameters);
 static void sendPacketTask(void *parameters);
-static void sendDataTask(void *parameters);
+static void UAVTalkSendTask(void *parameters);
 static void transparentCommTask(void * parameters);
 static void radioStatusTask(void *parameters);
 static void ppmInputTask(void *parameters);
-static int32_t transmitData(uint8_t * data, int32_t length);
+static int32_t UAVTalkSendHandler(uint8_t * data, int32_t length);
+static int32_t GCSUAVTalkSendHandler(uint8_t * data, int32_t length);
 static int32_t transmitPacket(PHPacketHandle packet);
 static void receiveData(uint8_t *buf, uint8_t len, int8_t rssi, int8_t afc);
+static void transmitData(uint32_t outputPort, uint8_t *buf, uint8_t len, bool checkHid);
 static void StatusHandler(PHStatusPacketHandle p, int8_t rssi, int8_t afc);
 static void PPMHandler(uint16_t *channels);
 static BufferedReadHandle BufferedReadInit(uint32_t com_port, uint16_t buffer_length);
 static bool BufferedRead(BufferedReadHandle h, uint8_t *value, uint32_t timeout_ms);
 static void BufferedReadSetCom(BufferedReadHandle h, uint32_t com_port);
+static void queueEvent(xQueueHandle queue, void *obj, uint16_t instId, UAVObjEventType type);
 static void updateSettings();
 
 // ****************
@@ -165,23 +187,34 @@ static RadioComBridgeData *data;
 static int32_t RadioComBridgeStart(void)
 {
 	if(data) {
+		// Start the primary tasks for receiving/sending UAVTalk packets from the GCS.
+		xTaskCreate(UAVTalkRecvTask, (signed char *)"GCSUAVTalkRecvTask", STACK_SIZE_BYTES, (void*)&(data->gcs_uavtalk_params), TASK_PRIORITY + 2, &(data->GCSUAVTalkRecvTaskHandle));
+		xTaskCreate(UAVTalkSendTask, (signed char *)"GCSUAVTalkSendTask", STACK_SIZE_BYTES, (void*)&(data->gcs_uavtalk_params), TASK_PRIORITY+ 2, &(data->UAVTalkSendTaskHandle));
+
+		// If a UAVTalk (non-GCS) com port is set it implies that the com port is connected on the flight side.
+		// In this case we want to start another com thread on the HID port to talk to the GCS when connected.
+		if (PIOS_COM_UAVTALK)
+		{
+			xTaskCreate(UAVTalkRecvTask, (signed char *)"UAVTalkRecvTask", STACK_SIZE_BYTES, (void*)&(data->uavtalk_params), TASK_PRIORITY + 2, &(data->UAVTalkRecvTaskHandle));
+			xTaskCreate(UAVTalkSendTask, (signed char *)"UAVTalkSendTask", STACK_SIZE_BYTES, (void*)&(data->uavtalk_params), TASK_PRIORITY+ 2, &(data->UAVTalkSendTaskHandle));
+		}
+
 		// Start the tasks
-		xTaskCreate(comUAVTalkTask, (signed char *)"ComUAVTalk", STACK_SIZE_BYTES, NULL, TASK_PRIORITY + 2, &(data->comUAVTalkTaskHandle));
 		if(PIOS_COM_TRANS_COM)
 			xTaskCreate(transparentCommTask, (signed char *)"transparentComm", STACK_SIZE_BYTES, NULL, TASK_PRIORITY + 2, &(data->transparentCommTaskHandle));
 		xTaskCreate(radioReceiveTask, (signed char *)"RadioReceive", STACK_SIZE_BYTES, NULL, TASK_PRIORITY+ 2, &(data->radioReceiveTaskHandle));
 		xTaskCreate(sendPacketTask, (signed char *)"SendPacketTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY + 2, &(data->sendPacketTaskHandle));
-		xTaskCreate(sendDataTask, (signed char *)"SendDataTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY+ 2, &(data->sendDataTaskHandle));
 		xTaskCreate(radioStatusTask, (signed char *)"RadioStatus", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->radioStatusTaskHandle));
 		if(PIOS_PPM_RECEIVER)
 			xTaskCreate(ppmInputTask, (signed char *)"PPMInputTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY + 2, &(data->ppmInputTaskHandle));
 #ifdef PIOS_INCLUDE_WDG
-		PIOS_WDG_RegisterFlag(PIOS_WDG_COMUAVTALK);
+		PIOS_WDG_RegisterFlag(PIOS_WDG_COMGCS);
+		if(PIOS_COM_UAVTALK)
+			PIOS_WDG_RegisterFlag(PIOS_WDG_COMUAVTALK);
 		if(PIOS_COM_TRANS_COM)
 			PIOS_WDG_RegisterFlag(PIOS_WDG_TRANSCOMM);
 		PIOS_WDG_RegisterFlag(PIOS_WDG_RADIORECEIVE);
 		//PIOS_WDG_RegisterFlag(PIOS_WDG_SENDPACKET);
-		//PIOS_WDG_RegisterFlag(PIOS_WDG_SENDDATA);
 		if(PIOS_PPM_RECEIVER)
 			PIOS_WDG_RegisterFlag(PIOS_WDG_PPMINPUT);
 #endif
@@ -211,12 +244,21 @@ static int32_t RadioComBridgeInitialize(void)
 	updateSettings();
 
 	// Initialise UAVTalk
-	data->inUAVTalkCon = UAVTalkInitialize(0);
+	data->GCSUAVTalkCon = UAVTalkInitialize(&GCSUAVTalkSendHandler);
-	data->outUAVTalkCon = UAVTalkInitialize(&transmitData);
+	if (PIOS_COM_UAVTALK)
+		data->UAVTalkCon = UAVTalkInitialize(&UAVTalkSendHandler);
 
 	// Initialize the queues.
-	data->sendPacketQueue = xQueueCreate(PACKET_QUEUE_SIZE, sizeof(PHPacketHandle));
+	data->ppmOutQueue = 0;
-	data->objEventQueue = xQueueCreate(PACKET_QUEUE_SIZE, sizeof(UAVObjEvent));
+	data->radioPacketQueue = xQueueCreate(PACKET_QUEUE_SIZE, sizeof(UAVObjEvent));
+	data->gcsEventQueue = xQueueCreate(PACKET_QUEUE_SIZE, sizeof(UAVObjEvent));
+	if (PIOS_COM_UAVTALK)
+		data->uavtalkEventQueue = xQueueCreate(PACKET_QUEUE_SIZE, sizeof(UAVObjEvent));
+	else
+	{
+		data->uavtalkEventQueue = 0;
+		data->ppmOutQueue = data->radioPacketQueue;
+	}
 
 	// Initialize the statistics.
 	data->radioTxErrors = 0;
@@ -232,8 +274,8 @@ static int32_t RadioComBridgeInitialize(void)
 	// Register the callbacks with the packet handler
 	PHRegisterOutputStream(pios_packet_handler, transmitPacket);
 	PHRegisterDataHandler(pios_packet_handler, receiveData);
-	PHRegisterStatusHandler(pios_packet_handler, StatusHandler);
 	PHRegisterPPMHandler(pios_packet_handler, PPMHandler);
+	PHRegisterStatusHandler(pios_packet_handler, StatusHandler);
 
 	// Initialize the packet send timeout
 	data->send_timeout = 25; // ms
@@ -255,9 +297,28 @@ static int32_t RadioComBridgeInitialize(void)
 	PipXSettingsPairIDGet(&(data->pairStats[0].pairID));
 
 	// Configure our UAVObjects for updates.
-	UAVObjConnectQueue(UAVObjGetByName("PipXStatus"), data->objEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ);
+	UAVObjConnectQueue(UAVObjGetByID(PIPXSTATUS_OBJID), data->gcsEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ);
-	UAVObjConnectQueue(UAVObjGetByName("GCSReceiver"), data->objEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ);
+	UAVObjConnectQueue(UAVObjGetByID(GCSRECEIVER_OBJID), data->uavtalkEventQueue ? data->uavtalkEventQueue : data->gcsEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ);
-	UAVObjConnectQueue(UAVObjGetByName("ObjectPersistence"), data->objEventQueue, EV_UPDATED | EV_UPDATED_MANUAL);
+	UAVObjConnectQueue(UAVObjGetByID(OBJECTPERSISTENCE_OBJID), data->gcsEventQueue, EV_UPDATED | EV_UPDATED_MANUAL);
+
+	// Initialize the UAVTalk comm parameters.
+	data->gcs_uavtalk_params.UAVTalkCon = data->GCSUAVTalkCon;
+	data->gcs_uavtalk_params.sendQueue = data->radioPacketQueue;
+	data->gcs_uavtalk_params.recvQueue = data->gcsEventQueue;
+	data->gcs_uavtalk_params.wdg = PIOS_WDG_COMGCS;
+	data->gcs_uavtalk_params.checkHID = true;
+	data->gcs_uavtalk_params.comPort = PIOS_COM_GCS;
+	if (PIOS_COM_UAVTALK)
+	{
+		data->gcs_uavtalk_params.sendQueue = data->uavtalkEventQueue;
+		data->uavtalk_params.UAVTalkCon = data->UAVTalkCon;
+		data->uavtalk_params.sendQueue = data->radioPacketQueue;
+		data->uavtalk_params.recvQueue = data->uavtalkEventQueue;
+		data->uavtalk_params.gcsQueue = data->gcsEventQueue;
+		data->uavtalk_params.wdg = PIOS_WDG_COMUAVTALK;
+		data->uavtalk_params.checkHID = false;
+		data->uavtalk_params.comPort = PIOS_COM_UAVTALK;
+	}
 
 	return 0;
 }
@@ -266,30 +327,32 @@ MODULE_INITCALL(RadioComBridgeInitialize, RadioComBridgeStart)
 /**
  * Reads UAVTalk messages froma com port and creates packets out of them.
  */
-static void comUAVTalkTask(void *parameters)
+static void UAVTalkRecvTask(void *parameters)
 {
+	UAVTalkComTaskParams *params = (UAVTalkComTaskParams *)parameters;
 	PHPacketHandle p = NULL;
 
 	// Create the buffered reader.
-	BufferedReadHandle f = BufferedReadInit(PIOS_COM_UAVTALK, TEMP_BUFFER_SIZE);
+	BufferedReadHandle f = BufferedReadInit(params->comPort, TEMP_BUFFER_SIZE);
 
 	while (1) {
 
 #ifdef PIOS_INCLUDE_WDG
 		// Update the watchdog timer.
-		PIOS_WDG_UpdateFlag(PIOS_WDG_COMUAVTALK);
+		if (params->wdg)
+			PIOS_WDG_UpdateFlag(params->wdg);
 #endif /* PIOS_INCLUDE_WDG */
 
 		// Receive from USB HID if available, otherwise UAVTalk com if it's available.
 #if defined(PIOS_INCLUDE_USB)
 		// Determine input port (USB takes priority over telemetry port)
-		if (PIOS_USB_CheckAvailable(0))
+		if (params->checkHID && PIOS_USB_CheckAvailable(0))
 			BufferedReadSetCom(f, PIOS_COM_USB_HID);
 		else
 #endif /* PIOS_INCLUDE_USB */
 		{
-			if (PIOS_COM_UAVTALK)
+			if (params->comPort)
-				BufferedReadSetCom(f, PIOS_COM_UAVTALK);
+				BufferedReadSetCom(f, params->comPort);
 			else
 			{
 				vTaskDelay(5);
@@ -307,7 +370,7 @@ static void comUAVTalkTask(void *parameters)
 		{
 
 			// Wait until we receive a sync.
-			UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(data->inUAVTalkCon, rx_byte);
+			UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(params->UAVTalkCon, rx_byte);
 			if (state != UAVTALK_STATE_TYPE)
 				continue;
 
@@ -324,7 +387,6 @@ static void comUAVTalkTask(void *parameters)
 			// Initialize the packet.
 			p->header.destination_id = data->destination_id;
 			p->header.source_id = PIOS_RFM22B_DeviceID(pios_rfm22b_id);
-			//p->header.type = PACKET_TYPE_ACKED_DATA;
 			p->header.type = PACKET_TYPE_DATA;
 			p->data[0] = rx_byte;
 			p->header.data_size = 1;
@@ -335,12 +397,19 @@ static void comUAVTalkTask(void *parameters)
 		p->data[p->header.data_size++] = rx_byte;
 
 		// Keep reading until we receive a completed packet.
-		UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(data->inUAVTalkCon, rx_byte);
+		UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(params->UAVTalkCon, rx_byte);
-		UAVTalkConnectionData *connection = (UAVTalkConnectionData*)(data->inUAVTalkCon);
+		UAVTalkConnectionData *connection = (UAVTalkConnectionData*)(params->UAVTalkCon);
 		UAVTalkInputProcessor *iproc = &(connection->iproc);
 
 		if (state == UAVTALK_STATE_COMPLETE)
 		{
+			xQueueHandle sendQueue = params->sendQueue;
+#if defined(PIOS_INCLUDE_USB)
+			if (params->gcsQueue)
+				if (PIOS_USB_CheckAvailable(0) && PIOS_COM_USB_HID)
+					sendQueue = params->gcsQueue;
+#endif /* PIOS_INCLUDE_USB */
+
 			// Is this a local UAVObject?
 			// We only generate GcsReceiver ojects, we don't consume them.
 			if ((iproc->obj != NULL) && (iproc->objId != GCSRECEIVER_OBJID))
@@ -359,11 +428,7 @@ static void comUAVTalkTask(void *parameters)
 					if (obj_per.ObjectID == PIPXSETTINGS_OBJID)
 					{
 						// Queue up the ACK.
-						UAVObjEvent ev;
+						queueEvent(params->recvQueue, (void*)iproc->obj, iproc->instId, EV_SEND_ACK);
-						ev.obj = iproc->obj;
-						ev.instId = iproc->instId;
-						ev.event = EV_SEND_ACK;
-						xQueueSend(data->objEventQueue, &ev, MAX_PORT_DELAY);
 
 						// Is this a save, load, or delete?
 						bool success = true;
@@ -390,6 +455,19 @@ static void comUAVTalkTask(void *parameters)
 							int32_t ret = PIOS_EEPROM_Save((uint8_t*)&pipxSettings, sizeof(PipXSettingsData));
 							if (ret != 0)
 								success = false;
+#endif
+							break;
+						}
+						case OBJECTPERSISTENCE_OPERATION_DELETE:
+						{
+#if defined(PIOS_INCLUDE_FLASH_EEPROM)
+							// Erase the settings.
+							PipXSettingsData pipxSettings;
+							uint8_t *ptr = (uint8_t*)&pipxSettings;
+							memset(ptr, 0, sizeof(PipXSettingsData));
+							int32_t ret = PIOS_EEPROM_Save(ptr, sizeof(PipXSettingsData));
+							if (ret != 0)
+								success = false;
 #endif
 							break;
 						}
@@ -408,14 +486,11 @@ static void comUAVTalkTask(void *parameters)
 					else
 					{
 						// Otherwise, queue the packet for transmission.
-						xQueueSend(data->sendPacketQueue, &p, MAX_PORT_DELAY);
+						queueEvent(sendQueue, (void*)p, 0, EV_TRANSMIT_PACKET);
 					}
 				}
 				else
 				{
-					UAVObjEvent ev;
-					ev.obj = iproc->obj;
-					ev.instId = 0;
 					switch (iproc->type)
 					{
 					case UAVTALK_TYPE_OBJ:
@@ -424,16 +499,12 @@ static void comUAVTalkTask(void *parameters)
 						break;
 					case UAVTALK_TYPE_OBJ_REQ:
 						// Queue up an object send request.
-						ev.event = EV_UPDATE_REQ;
+						queueEvent(params->recvQueue, (void*)iproc->obj, iproc->instId, EV_UPDATE_REQ);
-						xQueueSend(data->objEventQueue, &ev, MAX_PORT_DELAY);
 						break;
 					case UAVTALK_TYPE_OBJ_ACK:
 						if (UAVObjUnpack(iproc->obj, iproc->instId, connection->rxBuffer) == 0)
-						{
 							// Queue up an ACK
-							ev.event = EV_SEND_ACK;
+							queueEvent(params->recvQueue, (void*)iproc->obj, iproc->instId, EV_SEND_ACK);
-							xQueueSend(data->objEventQueue, &ev, MAX_PORT_DELAY);
-						}
 						break;
 					}
 
@@ -444,22 +515,24 @@ static void comUAVTalkTask(void *parameters)
 			else
 			{
 				// Queue the packet for transmission.
-				xQueueSend(data->sendPacketQueue, &p, MAX_PORT_DELAY);
+				queueEvent(sendQueue, (void*)p, 0, EV_TRANSMIT_PACKET);
 			}
 			p = NULL;
 
 		} else if(state == UAVTALK_STATE_ERROR) {
-			DEBUG_PRINTF(1, "UAVTalk FAILED!\n\r");
+			xQueueHandle sendQueue = params->sendQueue;
+#if defined(PIOS_INCLUDE_USB)
+			if (params->gcsQueue)
+				if (PIOS_USB_CheckAvailable(0) && PIOS_COM_USB_HID)
+					sendQueue = params->gcsQueue;
+#endif /* PIOS_INCLUDE_USB */
 			data->UAVTalkErrors++;
 
 			// Send a NACK if required.
 			if((iproc->obj) && (iproc->type == UAVTALK_TYPE_OBJ_ACK))
 			{
 				// Queue up a NACK
-				UAVObjEvent ev;
+				queueEvent(params->recvQueue, iproc->obj, iproc->instId, EV_SEND_NACK);
-				ev.obj = iproc->obj;
-				ev.event = EV_SEND_NACK;
-				xQueueSend(data->objEventQueue, &ev, MAX_PORT_DELAY);
 
 				// Release the packet and start over again.
 				PHReleaseTXPacket(pios_packet_handler, p);
@@ -467,7 +540,7 @@ static void comUAVTalkTask(void *parameters)
 			else
 			{
 				// Transmit the packet anyway...
-				xQueueSend(data->sendPacketQueue, &p, MAX_PORT_DELAY);
+				queueEvent(sendQueue, (void*)p, 0, EV_TRANSMIT_PACKET);
 			}
 			p = NULL;
 		}
@@ -512,7 +585,7 @@ static void radioReceiveTask(void *parameters)
 			UAVObjEvent ev;
 			ev.obj = (UAVObjHandle)p;
 			ev.event = EV_PACKET_RECEIVED;
-			xQueueSend(data->objEventQueue, &ev, portMAX_DELAY);
+			xQueueSend(data->gcsEventQueue, &ev, portMAX_DELAY);
 		} else
 		{
 			data->packetErrors++;
@@ -527,7 +600,7 @@ static void radioReceiveTask(void *parameters)
  */
 static void sendPacketTask(void *parameters)
 {
-	PHPacketHandle p;
+	UAVObjEvent ev;
 
 	// Loop forever
 	while (1) {
@@ -536,19 +609,21 @@ static void sendPacketTask(void *parameters)
 		//PIOS_WDG_UpdateFlag(PIOS_WDG_SENDPACKET);
 #endif /* PIOS_INCLUDE_WDG */
 		// Wait for a packet on the queue.
-		if (xQueueReceive(data->sendPacketQueue, &p, MAX_PORT_DELAY) == pdTRUE) {
+		if (xQueueReceive(data->radioPacketQueue, &ev, MAX_PORT_DELAY) == pdTRUE) {
+			PHPacketHandle p = (PHPacketHandle)ev.obj;
 			// Send the packet.
 			if(!PHTransmitPacket(pios_packet_handler, p))
-				PHReleaseTXPacket(pios_packet_handler, p);
+				PHReleaseRXPacket(pios_packet_handler, p);
 		}
 	}
 }
 
 /**
- * Send packets to the radio.
+ * Send packets to the com port.
  */
-static void sendDataTask(void *parameters)
+static void UAVTalkSendTask(void *parameters)
 {
+	UAVTalkComTaskParams *params = (UAVTalkComTaskParams *)parameters;
 	UAVObjEvent ev;
 
 	// Loop forever
@@ -560,15 +635,16 @@ static void sendDataTask(void *parameters)
 		//PIOS_WDG_UpdateFlag(PIOS_WDG_SENDDATA);
 #endif /* PIOS_INCLUDE_WDG */
 		// Wait for a packet on the queue.
-		if (xQueueReceive(data->objEventQueue, &ev, MAX_PORT_DELAY) == pdTRUE) {
+		if (xQueueReceive(params->recvQueue, &ev, MAX_PORT_DELAY) == pdTRUE) {
 			if ((ev.event == EV_UPDATED) || (ev.event == EV_UPDATE_REQ))
 			{
 				// Send update (with retries)
 				uint32_t retries = 0;
 				int32_t success = -1;
 				while (retries < MAX_RETRIES && success == -1) {
-					success = UAVTalkSendObject(data->outUAVTalkCon, ev.obj, 0, 0, RETRY_TIMEOUT_MS);
+					success = UAVTalkSendObject(params->UAVTalkCon, ev.obj, 0, 0, RETRY_TIMEOUT_MS) == 0;
-					++retries;
+					if (!success)
+						++retries;
 				}
 				data->comTxRetries += retries;
 			}
@@ -578,8 +654,9 @@ static void sendDataTask(void *parameters)
 				uint32_t retries = 0;
 				int32_t success = -1;
 				while (retries < MAX_RETRIES && success == -1) {
-					success = UAVTalkSendAck(data->outUAVTalkCon, ev.obj, ev.instId);
+					success = UAVTalkSendAck(params->UAVTalkCon, ev.obj, ev.instId) == 0;
-					++retries;
+					if (!success)
+						++retries;
 				}
 				data->comTxRetries += retries;
 			}
@@ -589,8 +666,9 @@ static void sendDataTask(void *parameters)
 				uint32_t retries = 0;
 				int32_t success = -1;
 				while (retries < MAX_RETRIES && success == -1) {
-					success = UAVTalkSendNack(data->outUAVTalkCon, UAVObjGetID(ev.obj));
+					success = UAVTalkSendNack(params->UAVTalkCon, UAVObjGetID(ev.obj)) == 0;
-					++retries;
+					if (!success)
+						++retries;
 				}
 				data->comTxRetries += retries;
 			}
@@ -599,6 +677,13 @@ static void sendDataTask(void *parameters)
 				// Receive the packet.
 				PHReceivePacket(pios_packet_handler, (PHPacketHandle)ev.obj, false);
 			}
+			else if(ev.event == EV_TRANSMIT_PACKET)
+			{
+				// Transmit the packet.
+				PHPacketHandle p = (PHPacketHandle)ev.obj;
+				transmitData(params->comPort, p->data, p->header.data_size, params->checkHID);
+				PHReleaseTXPacket(pios_packet_handler, p);
+			}
 		}
 	}
 }
@@ -643,7 +728,7 @@ static void transparentCommTask(void * parameters)
 		}
 
 		// Receive data from the com port
-		uint32_t cur_rx_bytes = PIOS_COM_ReceiveBuffer(PIOS_COM_TRANS_COM, p->data + p->header.data_size,
+		uint32_t cur_rx_bytes =	PIOS_COM_ReceiveBuffer(PIOS_COM_TRANS_COM, p->data + p->header.data_size,
 							       PH_MAX_DATA - p->header.data_size, timeout);
 
 		// Do we have an data to send?
@@ -675,7 +760,7 @@ static void transparentCommTask(void * parameters)
 			if (send_packet)
 			{
 				// Queue the packet for transmission.
-				xQueueSend(data->sendPacketQueue, &p, MAX_PORT_DELAY);
+				queueEvent(data->radioPacketQueue, (void*)p, 0, EV_TRANSMIT_PACKET);
 
 				// Reset the timeout
 				timeout = MAX_PORT_DELAY;
@@ -767,7 +852,7 @@ static void radioStatusTask(void *parameters)
 				status_packet.dropped = data->droppedPackets;
 				status_packet.resets = PIOS_RFM22B_Resets(pios_rfm22b_id);
 				PHPacketHandle sph = (PHPacketHandle)&status_packet;
-				xQueueSend(data->sendPacketQueue, &sph, MAX_PORT_DELAY);
+				queueEvent(data->radioPacketQueue, (void*)sph, 0, EV_TRANSMIT_PACKET);
 				cntr = 0;
 			}
 		}
@@ -792,22 +877,52 @@ static void ppmInputTask(void *parameters)
 		PIOS_WDG_UpdateFlag(PIOS_WDG_PPMINPUT);
 #endif /* PIOS_INCLUDE_WDG */
 
-		// Send the PPM packet
+		// Read the receiver.
 		for (uint8_t i = 1; i <= PIOS_PPM_NUM_INPUTS; ++i)
 			ppm_packet.channels[i - 1] = PIOS_RCVR_Read(PIOS_PPM_RECEIVER, i);
 
-		// Send the packet.
+		// Send the PPM packet
-		ppm_packet.header.destination_id = data->destination_id;
+		if (data->ppmOutQueue)
-		ppm_packet.header.source_id = PIOS_RFM22B_DeviceID(pios_rfm22b_id);
+		{
-		ppm_packet.header.type = PACKET_TYPE_PPM;
+			ppm_packet.header.destination_id = data->destination_id;
-		ppm_packet.header.data_size = PH_PPM_DATA_SIZE(&ppm_packet);
+			ppm_packet.header.source_id = PIOS_RFM22B_DeviceID(pios_rfm22b_id);
-		xQueueSend(data->sendPacketQueue, &pph, MAX_PORT_DELAY);
+			ppm_packet.header.type = PACKET_TYPE_PPM;
+			ppm_packet.header.data_size = PH_PPM_DATA_SIZE(&ppm_packet);
+			queueEvent(data->ppmOutQueue, (void*)pph, 0, EV_TRANSMIT_PACKET);
+		}
+		else
+			PPMHandler(ppm_packet.channels);
 
 		// Delay until the next update period.
 		vTaskDelay(PIOS_PPM_PACKET_UPDATE_PERIOD_MS / portTICK_RATE_MS);
 	}
 }
 
+/**
+ * Transmit data buffer to the com port.
+ * \param[in] params The comm parameters.
+ * \param[in] buf Data buffer to send
+ * \param[in] length Length of buffer
+ * \return -1 on failure
+ * \return number of bytes transmitted on success
+ */
+static int32_t UAVTalkSend(UAVTalkComTaskParams *params, uint8_t *buf, int32_t length)
+{
+	uint32_t outputPort = params->comPort;
+#if defined(PIOS_INCLUDE_USB)
+	if (params->checkHID)
+	{
+		// Determine output port (USB takes priority over telemetry port)
+		if (PIOS_USB_CheckAvailable(0) && PIOS_COM_USB_HID)
+			outputPort = PIOS_COM_USB_HID;
+	}
+#endif /* PIOS_INCLUDE_USB */
+	if(outputPort)
+		return PIOS_COM_SendBuffer(outputPort, buf, length);
+	else
+		return -1;
+}
+
 /**
  * Transmit data buffer to the com port.
  * \param[in] buf Data buffer to send
@@ -815,18 +930,21 @@ static void ppmInputTask(void *parameters)
  * \return -1 on failure
  * \return number of bytes transmitted on success
  */
-static int32_t transmitData(uint8_t *buf, int32_t length)
+static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length)
 {
-	uint32_t outputPort = PIOS_COM_UAVTALK;
+	return UAVTalkSend(&(data->uavtalk_params), buf, length);
-#if defined(PIOS_INCLUDE_USB)
+}
-	// Determine output port (USB takes priority over telemetry port)
+
-	if (PIOS_USB_CheckAvailable(0) && PIOS_COM_USB_HID)
+/**
-		outputPort = PIOS_COM_USB_HID;
+ * Transmit data buffer to the com port.
-#endif /* PIOS_INCLUDE_USB */
+ * \param[in] buf Data buffer to send
-	if(outputPort)
+ * \param[in] length Length of buffer
-		return PIOS_COM_SendBuffer(outputPort, buf, length);
+ * \return -1 on failure
-	else
+ * \return number of bytes transmitted on success
-		return -1;
+ */
+static int32_t GCSUAVTalkSendHandler(uint8_t *buf, int32_t length)
+{
+	return UAVTalkSend(&(data->gcs_uavtalk_params), buf, length);
 }
 
 /**
@@ -847,22 +965,15 @@ static int32_t transmitPacket(PHPacketHandle p)
  * \param[in] buf The received data buffer
  * \param[in] length Length of buffer
  */
-static void receiveData(uint8_t *buf, uint8_t len, int8_t rssi, int8_t afc)
+static void transmitData(uint32_t outputPort, uint8_t *buf, uint8_t len, bool checkHid)
 {
-	data->RSSI = rssi;
-
-	// Packet data should go to transparent com if it's configured,
-	// USB HID if it's connected, otherwise, UAVTalk com if it's configured.
-	uint32_t outputPort = PIOS_COM_TRANS_COM;
-	if (!outputPort)
-	{
-		outputPort = PIOS_COM_UAVTALK;
 #if defined(PIOS_INCLUDE_USB)
+	// See if USB is connected if requested.
+	if(checkHid)
 		// Determine output port (USB takes priority over telemetry port)
 		if (PIOS_USB_CheckAvailable(0) && PIOS_COM_USB_HID)
 			outputPort = PIOS_COM_USB_HID;
 #endif /* PIOS_INCLUDE_USB */
-	}
 	if (!outputPort)
 		return;
 
@@ -872,6 +983,22 @@ static void receiveData(uint8_t *buf, uint8_t len, int8_t rssi, int8_t afc)
 		data->comTxErrors++;
 }
 
+/**
+ * Receive a packet
+ * \param[in] buf The received data buffer
+ * \param[in] length Length of buffer
+ */
+static void receiveData(uint8_t *buf, uint8_t len, int8_t rssi, int8_t afc)
+{
+	data->RSSI = rssi;
+
+	// Packet data should go to transparent com if it's configured,
+	// USB HID if it's connected, otherwise, UAVTalk com if it's configured.
+	uint32_t outputPort = PIOS_COM_TRANS_COM ? PIOS_COM_TRANS_COM : PIOS_COM_UAVTALK;
+	bool checkHid = (PIOS_COM_TRANS_COM == 0);
+	transmitData(outputPort, buf, len, checkHid);
+}
+
 /**
  * Receive a status packet
  * \param[in] status The status structure
@@ -987,6 +1114,21 @@ static void BufferedReadSetCom(BufferedReadHandle h, uint32_t com_port)
 	h->com_port = com_port;
 }
 
+/**
+ * Queue and event into an event queue.
+ * \param[in] queue  The event queue
+ * \param[in] obj  The data pointer
+ * \param[in] type The event type
+ */
+static void queueEvent(xQueueHandle queue, void *obj, uint16_t instId, UAVObjEventType type)
+{
+	UAVObjEvent ev;
+	ev.obj = (UAVObjHandle)obj;
+	ev.instId = instId;
+	ev.event = type;
+	xQueueSend(queue, &ev, portMAX_DELAY);
+}
+
 /**
  * Update the telemetry settings, called on startup.
  * FIXME: This should be in the TelemetrySettings object. But objects
@@ -1003,7 +1145,6 @@ static void updateSettings()
 
 	// Initialize the destination ID
 	data->destination_id = pipxSettings.PairID ? pipxSettings.PairID : 0xffffffff;
-	DEBUG_PRINTF(2, "PairID: %x\n\r", data->destination_id);
 	
 	if (PIOS_COM_TELEMETRY) {
 		switch (pipxSettings.TelemetrySpeed) {

flight/OSD/Makefile

@@ -153,6 +153,8 @@ SRC += $(OPUAVSYNTHDIR)/mixerstatus.c
 SRC += $(OPUAVSYNTHDIR)/homelocation.c
 SRC += $(OPUAVSYNTHDIR)/gpsposition.c
 SRC += $(OPUAVSYNTHDIR)/gpssatellites.c
+SRC += $(OPUAVSYNTHDIR)/gpssettings.c
+SRC += $(OPUAVSYNTHDIR)/gpsvelocity.c
 SRC += $(OPUAVSYNTHDIR)/gpstime.c
 SRC += $(OPUAVSYNTHDIR)/osdsettings.c
 

flight/OSD/System/inc/pios_config.h

@@ -75,6 +75,8 @@
 #define PIOS_TELEM_PRIORITY_QUEUE       /* Enable a priority queue in telemetry */
 #define PIOS_QUATERNION_STABILIZATION   /* Stabilization options */
 #define PIOS_GPS_SETS_HOMELOCATION      /* GPS options */
+#define PIOS_INCLUDE_GPS_NMEA_PARSER /* Include the NMEA protocol parser */
+#define PIOS_INCLUDE_GPS_UBX_PARSER  /* Include the UBX protocol parser */
 
 /* Alarm Thresholds */
 #define HEAP_LIMIT_WARNING		4000

flight/PiOS.posix/inc/pios_struct_helper.h

@@ -0,0 +1,12 @@
+/* Taken from include/linux/kernel.h from the Linux kernel tree */
+
+/**
+ * container_of - cast a member of a structure out to the containing structure
+ * @ptr:	the pointer to the member.
+ * @type:	the type of the container struct this is embedded in.
+ * @member:	the name of the member within the struct.
+ *
+ */
+#define container_of(ptr, type, member) ({			\
+	const typeof( ((type *)0)->member ) *__mptr = (ptr);	\
+	(type *)( (char *)__mptr - offsetof(type,member) );})

flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h

@@ -77,6 +77,7 @@ TIM4  |  RC In 1  |  Servo 3  |  Servo 2  |  Servo 1
 #define PIOS_WDG_SENDDATA        0x0008
 #define PIOS_WDG_TRANSCOMM       0x0010
 #define PIOS_WDG_PPMINPUT        0x0020
+#define PIOS_WDG_COMGCS          0x0040
 
 //------------------------
 // TELEMETRY
@@ -157,6 +158,7 @@ extern uint32_t pios_com_telemetry_id;
 extern uint32_t pios_com_flexi_id;
 extern uint32_t pios_com_vcp_id;
 extern uint32_t pios_com_uavtalk_com_id;
+extern uint32_t pios_com_gcs_com_id;
 extern uint32_t pios_com_trans_com_id;
 extern uint32_t pios_com_debug_id;
 extern uint32_t pios_com_rfm22b_id;
@@ -166,6 +168,7 @@ extern uint32_t pios_ppm_rcvr_id;
 #define PIOS_COM_FLEXI             (pios_com_flexi_id)
 #define PIOS_COM_VCP               (pios_com_vcp_id)
 #define PIOS_COM_UAVTALK           (pios_com_uavtalk_com_id)
+#define PIOS_COM_GCS               (pios_com_gcs_com_id)
 #define PIOS_COM_TRANS_COM         (pios_com_trans_com_id)
 #define PIOS_COM_DEBUG             (pios_com_debug_id)
 #define PIOS_COM_RADIO             (pios_com_rfm22b_id)

flight/PiOS/STM32F10x/Libraries/CMSIS/Core/CM3/core_cm3.c

@@ -733,7 +733,7 @@ uint32_t __STREXB(uint8_t value, uint8_t *addr)
 {
    uint32_t result=0;
   
-   __ASM volatile ("strexb %0, %2, [%1]" : "=r" (result) : "r" (addr), "r" (value) );
+   __ASM volatile ("strexb %0, %2, [%1]" : "=&r" (result) : "r" (addr), "r" (value) );
    return(result);
 }
 
@@ -750,7 +750,7 @@ uint32_t __STREXH(uint16_t value, uint16_t *addr)
 {
    uint32_t result=0;
   
-   __ASM volatile ("strexh %0, %2, [%1]" : "=r" (result) : "r" (addr), "r" (value) );
+   __ASM volatile ("strexh %0, %2, [%1]" : "=&r" (result) : "r" (addr), "r" (value) );
    return(result);
 }
 
@@ -767,7 +767,7 @@ uint32_t __STREXW(uint32_t value, uint32_t *addr)
 {
    uint32_t result=0;
   
-   __ASM volatile ("strex %0, %2, [%1]" : "=r" (result) : "r" (addr), "r" (value) );
+   __ASM volatile ("strex %0, %2, [%1]" : "=&r" (result) : "r" (addr), "r" (value) );
    return(result);
 }
 

flight/PiOS/STM32F10x/link_STM32103CB_PIPXTREME_sections.ld

@@ -99,7 +99,7 @@ SECTIONS
     } > SRAM
 
     _eram = ORIGIN(SRAM) + LENGTH(SRAM) ;
-    _ebss = _eram;
+    _ebss = _eram ;
     
     /* keep the heap section at the end of the SRAM
      * this will allow to claim the remaining bytes not used

flight/PiOS/inc/pios_struct_helper.h

@@ -0,0 +1,12 @@
+/* Taken from include/linux/kernel.h from the Linux kernel tree */
+
+/**
+ * container_of - cast a member of a structure out to the containing structure
+ * @ptr:	the pointer to the member.
+ * @type:	the type of the container struct this is embedded in.
+ * @member:	the name of the member within the struct.
+ *
+ */
+#define container_of(ptr, type, member) ({			\
+	const typeof( ((type *)0)->member ) *__mptr = (ptr);	\
+	(type *)( (char *)__mptr - offsetof(type,member) );})

flight/PipXtreme/System/pios_board.c

@@ -33,26 +33,27 @@
 #include <pipxsettings.h>
 #include <board_hw_defs.c>
 
-#define PIOS_COM_SERIAL_RX_BUF_LEN 256
+#define PIOS_COM_SERIAL_RX_BUF_LEN 128
-#define PIOS_COM_SERIAL_TX_BUF_LEN 256
+#define PIOS_COM_SERIAL_TX_BUF_LEN 128
 
-#define PIOS_COM_FLEXI_RX_BUF_LEN 256
+#define PIOS_COM_FLEXI_RX_BUF_LEN 128
-#define PIOS_COM_FLEXI_TX_BUF_LEN 256
+#define PIOS_COM_FLEXI_TX_BUF_LEN 128
 
-#define PIOS_COM_TELEM_USB_RX_BUF_LEN 256
+#define PIOS_COM_TELEM_USB_RX_BUF_LEN 128
-#define PIOS_COM_TELEM_USB_TX_BUF_LEN 256
+#define PIOS_COM_TELEM_USB_TX_BUF_LEN 128
 
-#define PIOS_COM_VCP_USB_RX_BUF_LEN 256
+#define PIOS_COM_VCP_USB_RX_BUF_LEN 128
-#define PIOS_COM_VCP_USB_TX_BUF_LEN 256
+#define PIOS_COM_VCP_USB_TX_BUF_LEN 128
 
-#define PIOS_COM_RFM22B_RF_RX_BUF_LEN 256
+#define PIOS_COM_RFM22B_RF_RX_BUF_LEN 128
-#define PIOS_COM_RFM22B_RF_TX_BUF_LEN 256
+#define PIOS_COM_RFM22B_RF_TX_BUF_LEN 128
 
 uint32_t pios_com_telem_usb_id = 0;
 uint32_t pios_com_telemetry_id;
 uint32_t pios_com_flexi_id;
 uint32_t pios_com_vcp_id;
 uint32_t pios_com_uavtalk_com_id = 0;
+uint32_t pios_com_gcs_com_id = 0;
 uint32_t pios_com_trans_com_id = 0;
 uint32_t pios_com_debug_id = 0;
 uint32_t pios_com_rfm22b_id = 0;
@@ -96,7 +97,7 @@ void PIOS_Board_Init(void) {
 
 	PipXSettingsData pipxSettings;
 #if defined(PIOS_INCLUDE_FLASH_EEPROM)
-	PIOS_EEPROM_Init(&pios_eeprom_cfg);
+ 	PIOS_EEPROM_Init(&pios_eeprom_cfg);
 
 	/* Read the settings from flash. */
 	/* NOTE: We probably need to save/restore the objID here incase the object changed but the size doesn't */
@@ -129,20 +130,17 @@ void PIOS_Board_Init(void) {
 
 
 	/* Flags to determine if various USB interfaces are advertised */
-	bool usb_hid_present = false;
 	bool usb_cdc_present = false;
 
 #if defined(PIOS_INCLUDE_USB_CDC)
 	if (PIOS_USB_DESC_HID_CDC_Init()) {
 		PIOS_Assert(0);
 	}
-	usb_hid_present = true;
 	usb_cdc_present = true;
 #else
 	if (PIOS_USB_DESC_HID_ONLY_Init()) {
 		PIOS_Assert(0);
 	}
-	usb_hid_present = true;
 #endif
 
 	uint32_t pios_usb_id;
@@ -172,7 +170,7 @@ void PIOS_Board_Init(void) {
 											tx_buffer, PIOS_COM_VCP_USB_TX_BUF_LEN)) {
 			PIOS_Assert(0);
 		}
-		switch (pipxSettings.TelemetryConfig)
+		switch (pipxSettings.VCPConfig)
 		{
 		case PIPXSETTINGS_VCPCONFIG_SERIAL:
 			pios_com_trans_com_id = pios_com_vcp_id;
@@ -190,10 +188,6 @@ void PIOS_Board_Init(void) {
 
 #if defined(PIOS_INCLUDE_USB_HID)
 
-	if (!usb_hid_present) {
-		PIOS_Assert(0);
-	}
-
 	/* Configure the usb HID port */
 #if defined(PIOS_INCLUDE_COM)
 	{
@@ -222,6 +216,7 @@ void PIOS_Board_Init(void) {
 	{
 	case PIPXSETTINGS_TELEMETRYCONFIG_SERIAL:
 	case PIPXSETTINGS_TELEMETRYCONFIG_UAVTALK:
+	case PIPXSETTINGS_TELEMETRYCONFIG_GCS:
 	case PIPXSETTINGS_TELEMETRYCONFIG_DEBUG:
 	{
 		uint32_t pios_usart1_id;
@@ -245,6 +240,9 @@ void PIOS_Board_Init(void) {
 		case PIPXSETTINGS_TELEMETRYCONFIG_UAVTALK:
 			pios_com_uavtalk_com_id = pios_com_telemetry_id;
 			break;
+		case PIPXSETTINGS_TELEMETRYCONFIG_GCS:
+			pios_com_gcs_com_id = pios_com_telemetry_id;
+			break;
 		case PIPXSETTINGS_TELEMETRYCONFIG_DEBUG:
 			pios_com_debug_id = pios_com_telemetry_id;
 			break;
@@ -260,6 +258,7 @@ void PIOS_Board_Init(void) {
 	{
 	case PIPXSETTINGS_FLEXICONFIG_SERIAL:
 	case PIPXSETTINGS_FLEXICONFIG_UAVTALK:
+	case PIPXSETTINGS_FLEXICONFIG_GCS:
 	case PIPXSETTINGS_FLEXICONFIG_DEBUG:
 	{
 		uint32_t pios_usart3_id;
@@ -283,6 +282,9 @@ void PIOS_Board_Init(void) {
 		case PIPXSETTINGS_FLEXICONFIG_UAVTALK:
 			pios_com_uavtalk_com_id = pios_com_flexi_id;
 			break;
+		case PIPXSETTINGS_FLEXICONFIG_GCS:
+			pios_com_gcs_com_id = pios_com_flexi_id;
+			break;
 		case PIPXSETTINGS_FLEXICONFIG_DEBUG:
 			pios_com_debug_id = pios_com_flexi_id;
 			break;

flight/Project/gdb/simposix

@@ -0,0 +1 @@
+handle SIGUSR1 noprint nostop

flight/Revolution/System/inc/pios_config.h

@@ -99,6 +99,8 @@
 #define PIOS_TELEM_PRIORITY_QUEUE       /* Enable a priority queue in telemetry */
 //#define PIOS_QUATERNION_STABILIZATION   /* Stabilization options */
 #define PIOS_GPS_SETS_HOMELOCATION      /* GPS options */
+#define PIOS_INCLUDE_GPS_NMEA_PARSER /* Include the NMEA protocol parser */
+#define PIOS_INCLUDE_GPS_UBX_PARSER  /* Include the UBX protocol parser */
 
 /* Alarm Thresholds */
 #define HEAP_LIMIT_WARNING       1000

flight/Revolution/UAVObjects.inc

@@ -48,6 +48,7 @@ UAVOBJSRCFILENAMES += flighttelemetrystats
 UAVOBJSRCFILENAMES += gcstelemetrystats
 UAVOBJSRCFILENAMES += gpsposition
 UAVOBJSRCFILENAMES += gpssatellites
+UAVOBJSRCFILENAMES += gpssettings
 UAVOBJSRCFILENAMES += gpstime
 UAVOBJSRCFILENAMES += gpsvelocity
 UAVOBJSRCFILENAMES += fixedwingpathfollowersettings

flight/SimPosix/Makefile

@@ -38,7 +38,7 @@ OUTDIR := $(TOP)/build/$(TARGET)
 
 # Set developer code and compile options
 # Set to YES for debugging
-DEBUG ?= NO
+DEBUG ?= YES
 
 # Set to YES when using Code Sourcery toolchain
 CODE_SOURCERY ?= NO

flight/SimPosix/UAVObjects.inc

@@ -48,6 +48,7 @@ UAVOBJSRCFILENAMES += gcstelemetrystats
 UAVOBJSRCFILENAMES += gpsposition
 UAVOBJSRCFILENAMES += gpssatellites
 UAVOBJSRCFILENAMES += gpstime
+UAVOBJSRCFILENAMES += gpssettings
 UAVOBJSRCFILENAMES += gpsvelocity
 UAVOBJSRCFILENAMES += vtolpathfollowersettings
 UAVOBJSRCFILENAMES += fixedwingpathfollowersettings

flight/UAVObjects/inc/uavobjectmanager.h

@@ -133,7 +133,7 @@ typedef void (*UAVObjEventCallback)(UAVObjEvent* ev);
 /**
  * Callback used to initialize the object fields to their default values.
  */
-typedef void (*UAVObjInitializeCallback)(UAVObjHandle obj, uint16_t instId);
+typedef void (*UAVObjInitializeCallback)(UAVObjHandle obj_handle, uint16_t instId);
 
 /**
  * Event manager statistics
@@ -148,25 +148,23 @@ typedef struct {
 int32_t UAVObjInitialize();
 void UAVObjGetStats(UAVObjStats* statsOut);
 void UAVObjClearStats();
-UAVObjHandle UAVObjRegister(uint32_t id, const char* name, const char* metaName, int32_t isMetaobject,
+UAVObjHandle UAVObjRegister(uint32_t id,
 		int32_t isSingleInstance, int32_t isSettings, uint32_t numBytes, UAVObjInitializeCallback initCb);
 UAVObjHandle UAVObjGetByID(uint32_t id);
-UAVObjHandle UAVObjGetByName(char* name);
 uint32_t UAVObjGetID(UAVObjHandle obj);
-const char* UAVObjGetName(UAVObjHandle obj);
 uint32_t UAVObjGetNumBytes(UAVObjHandle obj);
 uint16_t UAVObjGetNumInstances(UAVObjHandle obj);
 UAVObjHandle UAVObjGetLinkedObj(UAVObjHandle obj);
-uint16_t UAVObjCreateInstance(UAVObjHandle obj, UAVObjInitializeCallback initCb);
+uint16_t UAVObjCreateInstance(UAVObjHandle obj_handle, UAVObjInitializeCallback initCb);
-int32_t UAVObjIsSingleInstance(UAVObjHandle obj);
+bool UAVObjIsSingleInstance(UAVObjHandle obj);
-int32_t UAVObjIsMetaobject(UAVObjHandle obj);
+bool UAVObjIsMetaobject(UAVObjHandle obj);
-int32_t UAVObjIsSettings(UAVObjHandle obj);
+bool UAVObjIsSettings(UAVObjHandle obj);
-int32_t UAVObjUnpack(UAVObjHandle obj, uint16_t instId, const uint8_t* dataIn);
+int32_t UAVObjUnpack(UAVObjHandle obj_handle, uint16_t instId, const uint8_t* dataIn);
-int32_t UAVObjPack(UAVObjHandle obj, uint16_t instId, uint8_t* dataOut);
+int32_t UAVObjPack(UAVObjHandle obj_handle, uint16_t instId, uint8_t* dataOut);
-int32_t UAVObjSave(UAVObjHandle obj, uint16_t instId);
+int32_t UAVObjSave(UAVObjHandle obj_handle, uint16_t instId);
-int32_t UAVObjLoad(UAVObjHandle obj, uint16_t instId);
+int32_t UAVObjLoad(UAVObjHandle obj_handle, uint16_t instId);
-int32_t UAVObjDelete(UAVObjHandle obj, uint16_t instId);
+int32_t UAVObjDelete(UAVObjHandle obj_handle, uint16_t instId);
-int32_t UAVObjSaveToFile(UAVObjHandle obj, uint16_t instId, FILEINFO* file);
+int32_t UAVObjSaveToFile(UAVObjHandle obj_handle, uint16_t instId, FILEINFO* file);
 UAVObjHandle UAVObjLoadFromFile(FILEINFO* file);
 int32_t UAVObjSaveSettings();
 int32_t UAVObjLoadSettings();
@@ -174,18 +172,17 @@ int32_t UAVObjDeleteSettings();
 int32_t UAVObjSaveMetaobjects();
 int32_t UAVObjLoadMetaobjects();
 int32_t UAVObjDeleteMetaobjects();
-int32_t UAVObjSetData(UAVObjHandle obj, const void* dataIn);
+int32_t UAVObjSetData(UAVObjHandle obj_handle, const void* dataIn);
-int32_t UAVObjSetDataField(UAVObjHandle obj, const void* dataIn, uint32_t offset, uint32_t size);
+int32_t UAVObjSetDataField(UAVObjHandle obj_handle, const void* dataIn, uint32_t offset, uint32_t size);
-int32_t UAVObjGetData(UAVObjHandle obj, void* dataOut);
+int32_t UAVObjGetData(UAVObjHandle obj_handle, void* dataOut);
-int32_t UAVObjGetDataField(UAVObjHandle obj, void* dataOut, uint32_t offset, uint32_t size);
+int32_t UAVObjGetDataField(UAVObjHandle obj_handle, void* dataOut, uint32_t offset, uint32_t size);
-int32_t UAVObjSetInstanceData(UAVObjHandle obj, uint16_t instId, const void* dataIn);
+int32_t UAVObjSetInstanceData(UAVObjHandle obj_handle, uint16_t instId, const void* dataIn);
-int32_t UAVObjSetInstanceDataField(UAVObjHandle obj, uint16_t instId, const void* dataIn, uint32_t offset, uint32_t size);
+int32_t UAVObjSetInstanceDataField(UAVObjHandle obj_handle, uint16_t instId, const void* dataIn, uint32_t offset, uint32_t size);
-int32_t UAVObjGetInstanceData(UAVObjHandle obj, uint16_t instId, void* dataOut);
+int32_t UAVObjGetInstanceData(UAVObjHandle obj_handle, uint16_t instId, void* dataOut);
-int32_t UAVObjGetInstanceDataField(UAVObjHandle obj, uint16_t instId, void* dataOut, uint32_t offset, uint32_t size);
+int32_t UAVObjGetInstanceDataField(UAVObjHandle obj_handle, uint16_t instId, void* dataOut, uint32_t offset, uint32_t size);
-int32_t UAVObjSetMetadata(UAVObjHandle obj, const UAVObjMetadata* dataIn);
+int32_t UAVObjSetMetadata(UAVObjHandle obj_handle, const UAVObjMetadata* dataIn);
-int32_t UAVObjGetMetadata(UAVObjHandle obj, UAVObjMetadata* dataOut);
+int32_t UAVObjGetMetadata(UAVObjHandle obj_handle, UAVObjMetadata* dataOut);
 uint8_t UAVObjGetMetadataAccess(const UAVObjMetadata* dataOut);
-void UAVObjMetadataInitialize(UAVObjMetadata* dataOut);
 UAVObjAccessType UAVObjGetAccess(const UAVObjMetadata* dataOut);
 void UAVObjSetAccess(UAVObjMetadata* dataOut, UAVObjAccessType mode);
 UAVObjAccessType UAVObjGetGcsAccess(const UAVObjMetadata* dataOut);
@@ -199,14 +196,14 @@ void UAVObjSetTelemetryUpdateMode(UAVObjMetadata* dataOut, UAVObjUpdateMode val)
 UAVObjUpdateMode UAVObjGetGcsTelemetryUpdateMode(const UAVObjMetadata* dataOut);
 void UAVObjSetTelemetryGcsUpdateMode(UAVObjMetadata* dataOut, UAVObjUpdateMode val);
 int8_t UAVObjReadOnly(UAVObjHandle obj);
-int32_t UAVObjConnectQueue(UAVObjHandle obj, xQueueHandle queue, uint8_t eventMask);
+int32_t UAVObjConnectQueue(UAVObjHandle obj_handle, xQueueHandle queue, uint8_t eventMask);
-int32_t UAVObjDisconnectQueue(UAVObjHandle obj, xQueueHandle queue);
+int32_t UAVObjDisconnectQueue(UAVObjHandle obj_handle, xQueueHandle queue);
-int32_t UAVObjConnectCallback(UAVObjHandle obj, UAVObjEventCallback cb, uint8_t eventMask);
+int32_t UAVObjConnectCallback(UAVObjHandle obj_handle, UAVObjEventCallback cb, uint8_t eventMask);
-int32_t UAVObjDisconnectCallback(UAVObjHandle obj, UAVObjEventCallback cb);
+int32_t UAVObjDisconnectCallback(UAVObjHandle obj_handle, UAVObjEventCallback cb);
 void UAVObjRequestUpdate(UAVObjHandle obj);
-void UAVObjRequestInstanceUpdate(UAVObjHandle obj, uint16_t instId);
+void UAVObjRequestInstanceUpdate(UAVObjHandle obj_handle, uint16_t instId);
 void UAVObjUpdated(UAVObjHandle obj);
-void UAVObjInstanceUpdated(UAVObjHandle obj, uint16_t instId);
+void UAVObjInstanceUpdated(UAVObjHandle obj_handle, uint16_t instId);
 void UAVObjIterate(void (*iterator)(UAVObjHandle obj));
 
 #endif // UAVOBJECTMANAGER_H

flight/UAVObjects/inc/uavobjecttemplate.h

@@ -42,8 +42,6 @@
 
 // Object constants
 #define $(NAMEUC)_OBJID $(OBJIDHEX)
-#define $(NAMEUC)_NAME "$(NAME)"
-#define $(NAMEUC)_METANAME "$(NAME)Meta"
 #define $(NAMEUC)_ISSINGLEINST $(ISSINGLEINST)
 #define $(NAMEUC)_ISSETTINGS $(ISSETTINGS)
 #define $(NAMEUC)_NUMBYTES sizeof($(NAME)Data)

flight/UAVObjects/uavobjectmanager.c

@@ -1,20 +1,20 @@
 /**
- ******************************************************************************
+******************************************************************************
- * @addtogroup UAVObjects OpenPilot UAVObjects
+* @addtogroup UAVObjects OpenPilot UAVObjects
- * @{ 
+* @{ 
- * @addtogroup UAV Object Manager 
+* @addtogroup UAV Object Manager 
- * @brief The core UAV Objects functions, most of which are wrappered by
+* @brief The core UAV Objects functions, most of which are wrappered by
- * autogenerated defines
+* autogenerated defines
- * @{ 
+* @{ 
- *
+*
- *
+*
- * @file       uavobjectmanager.h
+* @file       uavobjectmanager.h
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+* @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      Object manager library. This library holds a collection of all objects.
+* @brief      Object manager library. This library holds a collection of all objects.
- *             It can be used by all modules/libraries to find an object reference.
+*             It can be used by all modules/libraries to find an object reference.
- * @see        The GNU Public License (GPL) Version 3
+* @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
@@ -32,6 +32,7 @@
  */
 
 #include "openpilot.h"
+#include "pios_struct_helper.h"
 
 // Constants
 
@@ -39,84 +40,144 @@
 
 // Macros
 #define SET_BITS(var, shift, value, mask) var = (var & ~(mask << shift)) | (value << shift);
-#define OLGetIsMetaobject(olp)  ((olp)->flags & OL_IS_METAOBJECT)
-#define OLSetIsMetaobject(olp, val)  ((olp)->flags = (((val) == 0) ? ((olp)->flags & ~OL_IS_METAOBJECT) : ((olp)->flags | OL_IS_METAOBJECT)))
-#define OLGetIsSingleInstance(olp)  ((olp)->flags & OL_IS_SINGLE_INSTANCE)
-#define OLSetIsSingleInstance(olp, val)  ((olp)->flags = (((val) == 0) ? ((olp)->flags & ~OL_IS_SINGLE_INSTANCE) : ((olp)->flags | OL_IS_SINGLE_INSTANCE)))
-#define OLGetIsSettings(olp)  ((olp)->flags & OL_IS_SETTINGS)
-#define OLSetIsSettings(olp, val)  ((olp)->flags = (((val) == 0) ? ((olp)->flags & ~OL_IS_SETTINGS) : ((olp)->flags | OL_IS_SETTINGS)))
 
 /**
  * List of event queues and the eventmask associated with the queue.
  */
-struct ObjectEventListStruct {
-	  xQueueHandle queue;
-	  UAVObjEventCallback cb;
-	  uint8_t eventMask;
-	  struct ObjectEventListStruct *next;
-};
-typedef struct ObjectEventListStruct ObjectEventList;
 
-/**
+/** opaque type for instances **/
- * List of object instances, holds the actual data structure and instance ID
+typedef void* InstanceHandle; 
+
+struct ObjectEventEntry {
+	xQueueHandle              queue;
+	UAVObjEventCallback       cb;
+	uint8_t                   eventMask;
+	struct ObjectEventEntry * next;
+};
+
+/*
+  MetaInstance   == [UAVOBase [UAVObjMetadata]]
+  SingleInstance == [UAVOBase [UAVOData [InstanceData]]]
+  MultiInstance  == [UAVOBase [UAVOData [NumInstances [InstanceData0 [next]]]]
+                                                  ____________________/
+                                                  \-->[InstanceData1 [next]]
+                                                  _________...________/
+                                                  \-->[InstanceDataN [next]]
  */
-struct ObjectInstListStruct {
-	  void *data;
-	  uint16_t instId;
-	  struct ObjectInstListStruct *next;
-};
-typedef struct ObjectInstListStruct ObjectInstList;
 
-typedef enum {
+/*
-	OL_IS_METAOBJECT = 0x01, /** Set if this is a metaobject */
+ * UAVO Base Type
-	OL_IS_SINGLE_INSTANCE = 0x02, /** Set if this object has a single instance */
+ *   - All Types of UAVObjects are of this base type
-	OL_IS_SETTINGS = 0x04 /** Set if this object is a settings object */
+ *   - The flags determine what type(s) this object
-} ObjectListFlags;
-	
-/**
- * List of objects registered in the object manager
  */
-struct ObjectListStruct {
+struct UAVOBase {
-	  uint32_t id;
+	/* Let these objects be added to an event queue */
-		     /** The object ID */
+	struct ObjectEventEntry * next_event;
-	  const char *name;
+
-			  /** The object name */
+	/* Describe the type of object that follows this header */
-	  ObjectListFlags flags;
+	struct UAVOInfo {
-                               /** The object list mode flags */
+		bool isMeta        : 1;
-	  uint16_t numBytes;
+		bool isSingle      : 1;
-			   /** Number of data bytes contained in the object (for a single instance) */
+		bool isSettings    : 1;
-	  uint16_t numInstances;
+	} flags;
-			       /** Number of instances */
+
-	  struct ObjectListStruct *linkedObj;
+} __attribute__((packed));
-					    /** Linked object, for regular objects this is the metaobject and for metaobjects it is the parent object */
+
-	  ObjectInstList instances;
+/* Augmented type for Meta UAVO */
-				  /** List of object instances, instance 0 always exists */
+struct UAVOMeta {
-	  ObjectEventList *events;
+	struct UAVOBase   base;
-				 /** Event queues registered on the object */
+	UAVObjMetadata    instance0;
-	  struct ObjectListStruct *next;
+} __attribute__((packed));
-				       /** Needed by linked list library (utlist.h) */
+
-};
+/* Shared data structure for all data-carrying UAVObjects (UAVOSingle and UAVOMulti) */
-typedef struct ObjectListStruct ObjectList;
+struct UAVOData {
+	struct UAVOBase   base;
+	uint32_t          id;
+	/*
+	 * Embed the Meta object as another complete UAVO
+	 * inside the payload for this UAVO.
+	 */
+	struct UAVOMeta   metaObj;
+	struct UAVOData * next;
+	uint16_t          instance_size;
+} __attribute__((packed));
+
+/* Augmented type for Single Instance Data UAVO */
+struct UAVOSingle {
+	struct UAVOData   uavo;
+
+	uint8_t           instance0[];
+	/* 
+	 * Additional space will be malloc'd here to hold the
+	 * the data for this instance.
+	 */
+} __attribute__((packed));
+
+/* Part of a linked list of instances chained off of a multi instance UAVO. */
+struct UAVOMultiInst {
+	struct UAVOMultiInst * next;
+	uint8_t                instance[];
+	/* 
+	 * Additional space will be malloc'd here to hold the
+	 * the data for this instance.
+	 */
+} __attribute__((packed));
+
+/* Augmented type for Multi Instance Data UAVO */
+struct UAVOMulti {
+	struct UAVOData        uavo;
+
+	uint16_t               num_instances;
+	struct UAVOMultiInst   instance0;
+	/*
+	 * Additional space will be malloc'd here to hold the
+	 * the data for instance 0.
+	 */
+} __attribute__((packed));
+
+/** all information about a metaobject are hardcoded constants **/
+#define MetaNumBytes sizeof(UAVObjMetadata)
+#define MetaBaseObjectPtr(obj) ((struct UAVOData *)((obj)-offsetof(struct UAVOData, metaObj)))
+#define MetaObjectPtr(obj) ((struct UAVODataMeta*) &((obj)->metaObj))
+#define MetaDataPtr(obj) ((UAVObjMetadata*)&((obj)->instance0))
+#define LinkedMetaDataPtr(obj) ((UAVObjMetadata*)&((obj)->metaObj.instance0))
+#define MetaObjectId(id) ((id)+1)
+
+/** all information about instances are dependant on object type **/
+#define ObjSingleInstanceDataOffset(obj) ((void*)(&(( (struct UAVOSingle*)obj )->instance0)))
+#define InstanceDataOffset(inst) ((void*)&(( (struct UAVOMultiInst*)inst )->instance))
+#define InstanceData(instance) (void*)instance
 
 // Private functions
-static int32_t sendEvent(ObjectList * obj, uint16_t instId,
+static int32_t sendEvent(struct UAVOBase * obj, uint16_t instId,
-			 UAVObjEventType event);
+			UAVObjEventType event);
-static ObjectInstList *createInstance(ObjectList * obj, uint16_t instId);
+static InstanceHandle createInstance(struct UAVOData * obj, uint16_t instId);
-static ObjectInstList *getInstance(ObjectList * obj, uint16_t instId);
+static InstanceHandle getInstance(struct UAVOData * obj, uint16_t instId);
-static int32_t connectObj(UAVObjHandle obj, xQueueHandle queue,
+static int32_t connectObj(UAVObjHandle obj_handle, xQueueHandle queue,
-			  UAVObjEventCallback cb, uint8_t eventMask);
+			UAVObjEventCallback cb, uint8_t eventMask);
-static int32_t disconnectObj(UAVObjHandle obj, xQueueHandle queue,
+static int32_t disconnectObj(UAVObjHandle obj_handle, xQueueHandle queue,
-			     UAVObjEventCallback cb);
+			UAVObjEventCallback cb);
 
 #if defined(PIOS_INCLUDE_SDCARD)
-static void objectFilename(ObjectList * obj, uint8_t * filename);
+static void objectFilename(UAVObjHandle obj_handle, uint8_t * filename);
 static void customSPrintf(uint8_t * buffer, uint8_t * format, ...);
 #endif
 
 // Private variables
-static ObjectList *objList;
+static struct UAVOData * uavo_list;
 static xSemaphoreHandle mutex;
-static UAVObjMetadata defMetadata;
+static const UAVObjMetadata defMetadata = {
+	.flags = (ACCESS_READWRITE << UAVOBJ_ACCESS_SHIFT |
+		ACCESS_READWRITE << UAVOBJ_GCS_ACCESS_SHIFT |
+		1 << UAVOBJ_TELEMETRY_ACKED_SHIFT |
+		1 << UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT |
+		UPDATEMODE_ONCHANGE << UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT |
+		UPDATEMODE_ONCHANGE << UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT),
+	.telemetryUpdatePeriod    = 0,
+	.gcsTelemetryUpdatePeriod = 0,
+	.loggingUpdatePeriod      = 0,
+};
+
 static UAVObjStats stats;
 
 /**
@@ -126,31 +187,32 @@ static UAVObjStats stats;
  */
 int32_t UAVObjInitialize()
 {
-	  // Initialize variables
+	// Initialize variables
-	  objList = NULL;
+	uavo_list = NULL;
-	  memset(&stats, 0, sizeof(UAVObjStats));
+	memset(&stats, 0, sizeof(UAVObjStats));
 
-	  // Create mutex
+	// Create mutex
-	  mutex = xSemaphoreCreateRecursiveMutex();
+	mutex = xSemaphoreCreateRecursiveMutex();
-	  if (mutex == NULL)
+	if (mutex == NULL)
-		    return -1;
+		return -1;
 
-	  // Initialize default metadata structure (metadata of metaobjects)
+	// Done
-	  UAVObjMetadataInitialize(&defMetadata);
+	return 0;
-
-	  // Done
-	  return 0;
 }
 
+/*****************
+ * Statistics
+ ****************/
+
 /**
  * Get the statistics counters
  * @param[out] statsOut The statistics counters will be copied there
  */
 void UAVObjGetStats(UAVObjStats * statsOut)
 {
-	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
+	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
-	  memcpy(statsOut, &stats, sizeof(UAVObjStats));
+	memcpy(statsOut, &stats, sizeof(UAVObjStats));
-	  xSemaphoreGiveRecursive(mutex);
+	xSemaphoreGiveRecursive(mutex);
 }
 
 /**
@@ -158,17 +220,84 @@ void UAVObjGetStats(UAVObjStats * statsOut)
  */
 void UAVObjClearStats()
 {
-	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
+	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
-	  memset(&stats, 0, sizeof(UAVObjStats));
+	memset(&stats, 0, sizeof(UAVObjStats));
-	  xSemaphoreGiveRecursive(mutex);
+	xSemaphoreGiveRecursive(mutex);
 }
 
+/************************
+ * Object Initialization
+ ***********************/
+
+static void UAVObjInitMetaData (struct UAVOMeta * obj_meta)
+{
+	/* Fill in the common part of the UAVO */
+	struct UAVOBase * uavo_base = &(obj_meta->base);
+	memset(uavo_base, 0, sizeof(*uavo_base));
+	uavo_base->flags.isMeta   = true;
+	uavo_base->flags.isSingle = true;
+	uavo_base->next_event     = NULL;
+
+	/* Clear the instance data carried in the UAVO */
+	memset(&(obj_meta->instance0), 0, sizeof(obj_meta->instance0));
+}
+
+static struct UAVOData * UAVObjAllocSingle(uint32_t num_bytes)
+{
+	/* Compute the complete size of the object, including the data for a single embedded instance */
+	uint32_t object_size = sizeof(struct UAVOSingle) + num_bytes;
+
+	/* Allocate the object from the heap */
+	struct UAVOSingle * uavo_single = (struct UAVOSingle *) pvPortMalloc(object_size);
+	if (!uavo_single)
+		return (NULL);
+
+	/* Fill in the common part of the UAVO */
+	struct UAVOBase * uavo_base = &(uavo_single->uavo.base);
+	memset(uavo_base, 0, sizeof(*uavo_base));
+	uavo_base->flags.isSingle = true;
+	uavo_base->next_event     = NULL;
+
+	/* Clear the instance data carried in the UAVO */
+	memset(&(uavo_single->instance0), 0, num_bytes);
+
+	/* Give back the generic UAVO part */
+	return (&(uavo_single->uavo));
+}
+
+static struct UAVOData * UAVObjAllocMulti(uint32_t num_bytes)
+{
+	/* Compute the complete size of the object, including the data for a single embedded instance */
+	uint32_t object_size = sizeof(struct UAVOMulti) + num_bytes;
+
+	/* Allocate the object from the heap */
+	struct UAVOMulti * uavo_multi = (struct UAVOMulti *) pvPortMalloc(object_size);
+	if (!uavo_multi)
+		return (NULL);
+
+	/* Fill in the common part of the UAVO */
+	struct UAVOBase * uavo_base = &(uavo_multi->uavo.base);
+	memset(uavo_base, 0, sizeof(*uavo_base));
+	uavo_base->flags.isSingle = false;
+	uavo_base->next_event     = NULL;
+
+	/* Set up the type-specific part of the UAVO */
+	uavo_multi->num_instances = 1;
+
+	/* Clear the instance data carried in the UAVO */
+	memset (&(uavo_multi->instance0), 0, num_bytes);
+
+	/* Give back the generic UAVO part */
+	return (&(uavo_multi->uavo));
+}
+
+/**************************
+ * UAVObject Database APIs
+ *************************/
+
 /**
  * Register and new object in the object manager.
  * \param[in] id Unique object ID
- * \param[in] name Object name
- * \param[in] nameName Metaobject name
- * \param[in] isMetaobject Is this a metaobject (1:true, 0:false)
  * \param[in] isSingleInstance Is this a single instance or multi-instance object
  * \param[in] isSettings Is this a settings object
  * \param[in] numBytes Number of bytes of object data (for one instance)
@@ -176,85 +305,60 @@ void UAVObjClearStats()
  * \return Object handle, or NULL if failure.
  * \return
  */
-UAVObjHandle UAVObjRegister(uint32_t id, const char *name,
+UAVObjHandle UAVObjRegister(uint32_t id, 
-			    const char *metaName, int32_t isMetaobject,
+			int32_t isSingleInstance, int32_t isSettings,
-			    int32_t isSingleInstance, int32_t isSettings,
+			uint32_t num_bytes,
-			    uint32_t numBytes,
+			UAVObjInitializeCallback initCb)
-			    UAVObjInitializeCallback initCb)
 {
-	  ObjectList *objEntry;
+	struct UAVOData * uavo_data = NULL;
-	  ObjectInstList *instEntry;
-	  ObjectList *metaObj;
 
-	  // Get lock
+	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
-	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
 
-	  // Check that the object is not already registered
+	/* Don't allow duplicate registrations */
-	  LL_FOREACH(objList, objEntry) {
+	if (UAVObjGetByID(id))
-		    if (objEntry->id == id) {
+		goto unlock_exit;
-			      // Already registered, ignore
-			      xSemaphoreGiveRecursive(mutex);
-			      return NULL;
-		    }
-	  }
 
-	  // Create and append entry
+	/* Map the various flags to one of the UAVO types we understand */
-	  objEntry = (ObjectList *) pvPortMalloc(sizeof(ObjectList));
+	if (isSingleInstance) {
-	  if (objEntry == NULL) {
+		uavo_data = UAVObjAllocSingle (num_bytes);
-		    xSemaphoreGiveRecursive(mutex);
+	} else {
-		    return NULL;
+		uavo_data = UAVObjAllocMulti (num_bytes);
-	  }
+	}
-	  objEntry->id = id;
-	  objEntry->name = name;
-	  OLSetIsMetaobject(objEntry, isMetaobject);
-	  OLSetIsSingleInstance(objEntry, isSingleInstance);
-	  OLSetIsSettings(objEntry, isSettings);
-	  objEntry->numBytes = numBytes;
-	  objEntry->events = NULL;
-	  objEntry->numInstances = 0;
-	  objEntry->instances.data = NULL;
-	  objEntry->instances.instId = 0xFFFF;
-	  objEntry->instances.next = NULL;
-	  objEntry->linkedObj = NULL;	// will be set later
-	  LL_APPEND(objList, objEntry);
 
-	  // Create instance zero
+	if (!uavo_data)
-	  instEntry = createInstance(objEntry, 0);
+		goto unlock_exit;
-	  if (instEntry == NULL) {
-		    xSemaphoreGiveRecursive(mutex);
-		    return NULL;
-	  }
-	  // Create metaobject and update linkedObj
-	  if (isMetaobject) {
-		    objEntry->linkedObj = NULL;	// will be set later
-	  } else {
-		    // Create metaobject
-		    metaObj =
-			(ObjectList *) UAVObjRegister(id + 1, metaName,
-						      NULL, 1, 1, 0,
-						      sizeof
-						      (UAVObjMetadata),
-						      NULL);
-		    // Link two objects
-		    objEntry->linkedObj = metaObj;
-		    metaObj->linkedObj = objEntry;
-	  }
 
-	  // Initialize object fields and metadata to default values
+	/* Fill in the details about this UAVO */
-	  if (initCb != NULL) {
+	uavo_data->id            = id;
-		    initCb((UAVObjHandle) objEntry, 0);
+	uavo_data->instance_size = num_bytes;
-	  }
+	if (isSettings) {
-	  // Attempt to load object's metadata from the SD card (not done directly on the metaobject, but through the object)
+		uavo_data->base.flags.isSettings = true;
-	  if (!OLGetIsMetaobject(objEntry)) {
+	}
-		    UAVObjLoad((UAVObjHandle) objEntry->linkedObj, 0);
+
-	  }
+	/* Initialize the embedded meta UAVO */
-	  // If this is a settings object, attempt to load from SD card
+	UAVObjInitMetaData (&uavo_data->metaObj);
-	  if (OLGetIsSettings(objEntry)) {
+
-		    UAVObjLoad((UAVObjHandle) objEntry, 0);
+	/* Add the newly created object to the global list of objects */
-	  }
+	LL_APPEND(uavo_list, uavo_data);
-	  // Release lock
+
-	  xSemaphoreGiveRecursive(mutex);
+	/* Initialize object fields and metadata to default values */
-	  return (UAVObjHandle) objEntry;
+	if (initCb)
+		initCb((UAVObjHandle) uavo_data, 0);
+
+	/* Always try to load the meta object from flash */
+	UAVObjLoad((UAVObjHandle) &(uavo_data->metaObj), 0);
+
+	/* Attempt to load settings object from flash */
+	if (uavo_data->base.flags.isSettings)
+		UAVObjLoad((UAVObjHandle) uavo_data, 0);
+
+	// fire events for outer object and its embedded meta object
+	UAVObjInstanceUpdated((UAVObjHandle) uavo_data, 0);
+	UAVObjInstanceUpdated((UAVObjHandle) &(uavo_data->metaObj), 0);
+
+unlock_exit:
+	xSemaphoreGiveRecursive(mutex);
+	return (UAVObjHandle) uavo_data;
 }
 
 /**
@@ -264,52 +368,27 @@ UAVObjHandle UAVObjRegister(uint32_t id, const char *name,
  */
 UAVObjHandle UAVObjGetByID(uint32_t id)
 {
-	  ObjectList *objEntry;
+	UAVObjHandle * found_obj = (UAVObjHandle *) NULL;
 
-	  // Get lock
+	// Get lock
-	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
+	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
 
-	  // Look for object
+	// Look for object
-	  LL_FOREACH(objList, objEntry) {
+	struct UAVOData * tmp_obj;
-		    if (objEntry->id == id) {
+	LL_FOREACH(uavo_list, tmp_obj) {
-			      // Release lock
+		if (tmp_obj->id == id) {
-			      xSemaphoreGiveRecursive(mutex);
+			found_obj = (UAVObjHandle *)tmp_obj;
-			      // Done, object found
+			goto unlock_exit;
-			      return (UAVObjHandle) objEntry;
+		}
-		    }
+		if (MetaObjectId(tmp_obj->id) == id) {
-	  }
+			found_obj = (UAVObjHandle *)&(tmp_obj->metaObj);
+			goto unlock_exit;
+		}
+	}
 
-	  // Object not found, release lock and return error
+unlock_exit:
-	  xSemaphoreGiveRecursive(mutex);
+	xSemaphoreGiveRecursive(mutex);
-	  return NULL;
+	return found_obj;
-}
-
-/**
- * Retrieve an object from the list given its name
- * \param[in] name The name of the object
- * \return The object or NULL if not found.
- */
-UAVObjHandle UAVObjGetByName(char *name)
-{
-	  ObjectList *objEntry;
-
-	  // Get lock
-	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
-
-	  // Look for object
-	  LL_FOREACH(objList, objEntry) {
-		    if (objEntry->name != NULL
-			&& strcmp(objEntry->name, name) == 0) {
-			      // Release lock
-			      xSemaphoreGiveRecursive(mutex);
-			      // Done, object found
-			      return (UAVObjHandle) objEntry;
-		    }
-	  }
-
-	  // Object not found, release lock and return error
-	  xSemaphoreGiveRecursive(mutex);
-	  return NULL;
 }
 
 /**
@@ -317,19 +396,24 @@ UAVObjHandle UAVObjGetByName(char *name)
  * \param[in] obj The object handle
  * \return The object ID
  */
-uint32_t UAVObjGetID(UAVObjHandle obj)
+uint32_t UAVObjGetID(UAVObjHandle obj_handle)
 {
-	  return ((ObjectList *) obj)->id;
+	PIOS_Assert(obj_handle);
-}
 
-/**
+	/* Recover the common object header */
- * Get the object's name
+	struct UAVOBase * uavo_base = (struct UAVOBase *) obj_handle;
- * \param[in] obj The object handle
+
- * \return The object's name
+	if (UAVObjIsMetaobject(obj_handle)) {
- */
+		/* We have a meta object, find our containing UAVO */
-const char *UAVObjGetName(UAVObjHandle obj)
+		struct UAVOData * uavo_data = container_of ((struct UAVOMeta *)uavo_base, struct UAVOData, metaObj);
-{
+
-	  return ((ObjectList *) obj)->name;
+		return MetaObjectId (uavo_data->id);
+	} else {
+		/* We have a data object, augment our pointer */
+		struct UAVOData * uavo_data = (struct UAVOData *) uavo_base;
+
+		return (uavo_data->id);
+	}
 }
 
 /**
@@ -339,7 +423,23 @@ const char *UAVObjGetName(UAVObjHandle obj)
  */
 uint32_t UAVObjGetNumBytes(UAVObjHandle obj)
 {
-	  return ((ObjectList *) obj)->numBytes;
+	PIOS_Assert(obj);
+
+	uint32_t instance_size;
+
+	/* Recover the common object header */
+	struct UAVOBase * uavo_base = (struct UAVOBase *) obj;
+
+	if (uavo_base->flags.isMeta) {
+		instance_size = MetaNumBytes;
+	} else {
+		/* We have a data object, augment our pointer */
+		struct UAVOData * uavo = (struct UAVOData *) uavo_base;
+
+		instance_size = uavo->instance_size;
+	}
+
+	return (instance_size);
 }
 
 /**
@@ -349,9 +449,24 @@ uint32_t UAVObjGetNumBytes(UAVObjHandle obj)
  * \param[in] obj The object handle
  * \return The object linked object handle
  */
-UAVObjHandle UAVObjGetLinkedObj(UAVObjHandle obj)
+UAVObjHandle UAVObjGetLinkedObj(UAVObjHandle obj_handle)
 {
-	  return (UAVObjHandle) (((ObjectList *) obj)->linkedObj);
+	PIOS_Assert(obj_handle);
+
+	/* Recover the common object header */
+	struct UAVOBase * uavo_base = (struct UAVOBase *) obj_handle;
+
+	if (UAVObjIsMetaobject(obj_handle)) {
+		/* We have a meta object, find our containing UAVO. */
+		struct UAVOData * uavo_data = container_of ((struct UAVOMeta *)uavo_base, struct UAVOData, metaObj);
+
+		return (UAVObjHandle) uavo_data;
+	} else {
+		/* We have a data object, augment our pointer */
+		struct UAVOData * uavo_data = (struct UAVOData *) uavo_base;
+
+		return (UAVObjHandle) &(uavo_data->metaObj);
+	}
 }
 
 /**
@@ -359,13 +474,19 @@ UAVObjHandle UAVObjGetLinkedObj(UAVObjHandle obj)
  * \param[in] obj The object handle
  * \return The number of instances
  */
-uint16_t UAVObjGetNumInstances(UAVObjHandle obj)
+uint16_t UAVObjGetNumInstances(UAVObjHandle obj_handle)
 {
-	  uint32_t numInstances;
+	PIOS_Assert(obj_handle);
-	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
+
-	  numInstances = ((ObjectList *) obj)->numInstances;
+	if (UAVObjIsSingleInstance(obj_handle)) {
-	  xSemaphoreGiveRecursive(mutex);
+		/* Only one instance is allowed */
-	  return numInstances;
+		return 1;
+	} else {
+		/* Multi-instance object.  Inspect the object */
+		/* Augment our pointer to reflect the proper type */
+		struct UAVOMulti * uavo_multi = (struct UAVOMulti *) obj_handle;
+		return uavo_multi->num_instances;
+	}
 }
 
 /**
@@ -373,29 +494,37 @@ uint16_t UAVObjGetNumInstances(UAVObjHandle obj)
  * \param[in] obj The object handle
  * \return The instance ID or 0 if an error
  */
-uint16_t UAVObjCreateInstance(UAVObjHandle obj,
+uint16_t UAVObjCreateInstance(UAVObjHandle obj_handle,
-			      UAVObjInitializeCallback initCb)
+			UAVObjInitializeCallback initCb)
 {
-	  ObjectList *objEntry;
+	PIOS_Assert(obj_handle);
-	  ObjectInstList *instEntry;
 
-	  // Lock
+	if (UAVObjIsMetaobject(obj_handle)) {
-	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
+		return 0;
+	}
 
-	  // Create new instance
+	// Lock
-	  objEntry = (ObjectList *) obj;
+	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
-	  instEntry = createInstance(objEntry, objEntry->numInstances);
+
-	  if (instEntry == NULL) {
+	InstanceHandle instEntry;
-		    xSemaphoreGiveRecursive(mutex);
+	uint16_t instId = 0;
-		    return -1;
+
-	  }
+	// Create new instance
-	  // Initialize instance data
+	instId = UAVObjGetNumInstances(obj_handle);
-	  if (initCb != NULL) {
+	instEntry = createInstance( (struct UAVOData *)obj_handle, instId);
-		    initCb(obj, instEntry->instId);
+	if (instEntry == NULL) {
-	  }
+		goto unlock_exit;
-	  // Unlock
+	}
-	  xSemaphoreGiveRecursive(mutex);
+
-	  return instEntry->instId;
+	// Initialize instance data
+	if (initCb) {
+		initCb(obj_handle, instId);
+	}
+
+unlock_exit:
+	xSemaphoreGiveRecursive(mutex);
+
+	return instId;
 }
 
 /**
@@ -403,9 +532,14 @@ uint16_t UAVObjCreateInstance(UAVObjHandle obj,
  * \param[in] obj The object handle
  * \return True (1) if this is a single instance object
  */
-int32_t UAVObjIsSingleInstance(UAVObjHandle obj)
+bool UAVObjIsSingleInstance(UAVObjHandle obj_handle)
 {
-	  return OLGetIsSingleInstance((ObjectList *) obj);
+	PIOS_Assert(obj_handle);
+
+	/* Recover the common object header */
+	struct UAVOBase * uavo_base = (struct UAVOBase *) obj_handle;
+
+	return uavo_base->flags.isSingle;
 }
 
 /**
@@ -413,9 +547,14 @@ int32_t UAVObjIsSingleInstance(UAVObjHandle obj)
  * \param[in] obj The object handle
  * \return True (1) if this is metaobject
  */
-int32_t UAVObjIsMetaobject(UAVObjHandle obj)
+bool UAVObjIsMetaobject(UAVObjHandle obj_handle)
 {
-	  return OLGetIsMetaobject((ObjectList *) obj);
+	PIOS_Assert(obj_handle);
+
+	/* Recover the common object header */
+	struct UAVOBase * uavo_base = (struct UAVOBase *) obj_handle;
+
+	return uavo_base->flags.isMeta;
 }
 
 /**
@@ -423,9 +562,14 @@ int32_t UAVObjIsMetaobject(UAVObjHandle obj)
  * \param[in] obj The object handle
  * \return True (1) if this is a settings object
  */
-int32_t UAVObjIsSettings(UAVObjHandle obj)
+bool UAVObjIsSettings(UAVObjHandle obj_handle)
 {
-	  return OLGetIsSettings((ObjectList *) obj);
+	PIOS_Assert(obj_handle);
+
+	/* Recover the common object header */
+	struct UAVOBase * uavo_base = (struct UAVOBase *) obj_handle;
+
+	return uavo_base->flags.isSettings;
 }
 
 /**
@@ -435,39 +579,49 @@ int32_t UAVObjIsSettings(UAVObjHandle obj)
  * \param[in] dataIn The byte array
  * \return 0 if success or -1 if failure
  */
-int32_t UAVObjUnpack(UAVObjHandle obj, uint16_t instId,
+int32_t UAVObjUnpack(UAVObjHandle obj_handle, uint16_t instId,
-		     const uint8_t * dataIn)
+		const uint8_t * dataIn)
 {
-	  ObjectList *objEntry;
+	PIOS_Assert(obj_handle);
-	  ObjectInstList *instEntry;
 
-	  // Lock
+	// Lock
-	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
+	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
 
-	  // Cast handle to object
+	int32_t rc = -1;
-	  objEntry = (ObjectList *) obj;
 
-	  // Get the instance
+	if (UAVObjIsMetaobject(obj_handle)) {
-	  instEntry = getInstance(objEntry, instId);
+		if (instId != 0) {
+			goto unlock_exit;
+		}
+		memcpy(MetaDataPtr((struct UAVOMeta *)obj_handle), dataIn, MetaNumBytes);
+	} else {
+		struct UAVOData *obj;
+		InstanceHandle instEntry;
 
-	  // If the instance does not exist create it and any other instances before it
+		// Cast handle to object
-	  if (instEntry == NULL) {
+		obj = (struct UAVOData *) obj_handle;
-		    instEntry = createInstance(objEntry, instId);
-		    if (instEntry == NULL) {
-			      // Error, unlock and return
-			      xSemaphoreGiveRecursive(mutex);
-			      return -1;
-		    }
-	  }
-	  // Set the data
-	  memcpy(instEntry->data, dataIn, objEntry->numBytes);
 
-	  // Fire event
+		// Get the instance
-	  sendEvent(objEntry, instId, EV_UNPACKED);
+		instEntry = getInstance(obj, instId);
 
-	  // Unlock
+		// If the instance does not exist create it and any other instances before it
-	  xSemaphoreGiveRecursive(mutex);
+		if (instEntry == NULL) {
-	  return 0;
+			instEntry = createInstance(obj, instId);
+			if (instEntry == NULL) {
+				goto unlock_exit;
+			}
+		}
+		// Set the data
+		memcpy(InstanceData(instEntry), dataIn, obj->instance_size);
+	}
+
+	// Fire event
+	sendEvent((struct UAVOBase*)obj_handle, instId, EV_UNPACKED);
+	rc = 0;
+
+unlock_exit:
+	xSemaphoreGiveRecursive(mutex);
+	return rc;
 }
 
 /**
@@ -477,30 +631,41 @@ int32_t UAVObjUnpack(UAVObjHandle obj, uint16_t instId,
  * \param[out] dataOut The byte array
  * \return 0 if success or -1 if failure
  */
-int32_t UAVObjPack(UAVObjHandle obj, uint16_t instId, uint8_t * dataOut)
+int32_t UAVObjPack(UAVObjHandle obj_handle, uint16_t instId, uint8_t * dataOut)
 {
-	  ObjectList *objEntry;
+	PIOS_Assert(obj_handle);
-	  ObjectInstList *instEntry;
 
-	  // Lock
+	// Lock
-	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
+	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
 
-	  // Cast handle to object
+	int32_t rc = -1;
-	  objEntry = (ObjectList *) obj;
 
-	  // Get the instance
+	if (UAVObjIsMetaobject(obj_handle)) {
-	  instEntry = getInstance(objEntry, instId);
+		if (instId != 0) {
-	  if (instEntry == NULL) {
+			goto unlock_exit;
-		    // Error, unlock and return
+		}
-		    xSemaphoreGiveRecursive(mutex);
+		memcpy(dataOut, MetaDataPtr((struct UAVOMeta *)obj_handle), MetaNumBytes);
-		    return -1;
+	} else {
-	  }
+		struct UAVOData *obj;
-	  // Pack data
+		InstanceHandle instEntry;
-	  memcpy(dataOut, instEntry->data, objEntry->numBytes);
 
-	  // Unlock
+		// Cast handle to object
-	  xSemaphoreGiveRecursive(mutex);
+		obj = (struct UAVOData *) obj_handle;
-	  return 0;
+
+		// Get the instance
+		instEntry = getInstance(obj, instId);
+		if (instEntry == NULL) {
+			goto unlock_exit;
+		}
+		// Pack data
+		memcpy(dataOut, InstanceData(instEntry), obj->instance_size);
+	}
+
+	rc = 0;
+
+unlock_exit:
+	xSemaphoreGiveRecursive(mutex);
+	return rc;
 }
 
 /**
@@ -512,50 +677,72 @@ int32_t UAVObjPack(UAVObjHandle obj, uint16_t instId, uint8_t * dataOut)
  * @param[in] file File to append to
  * @return 0 if success or -1 if failure
  */
-int32_t UAVObjSaveToFile(UAVObjHandle obj, uint16_t instId,
+int32_t UAVObjSaveToFile(UAVObjHandle obj_handle, uint16_t instId,
-			 FILEINFO * file)
+			FILEINFO * file)
 {
+	PIOS_Assert(obj_handle);
+
 #if defined(PIOS_INCLUDE_SDCARD)
-	  uint32_t bytesWritten;
+	uint32_t bytesWritten;
-	  ObjectList *objEntry;
+	// Check for file system availability
-	  ObjectInstList *instEntry;
+	if (PIOS_SDCARD_IsMounted() == 0) {
+		return -1;
+	}
+	// Lock
+	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
 
-	  // Check for file system availability
+	if (UAVObjIsMetaobject(obj_handle)) {
-	  if (PIOS_SDCARD_IsMounted() == 0) {
+		// Get the instance information
-		    return -1;
+		if (instId != 0) {
-	  }
+			xSemaphoreGiveRecursive(mutex);
-	  // Lock
+			return -1;
-	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
+		}
+		// Write the object ID
+		uint32_t objId = UAVObjGetID(obj_handle);
+		PIOS_FWRITE(file, &objId, sizeof(objId),
+			&bytesWritten);
 
-	  // Cast to object
+		// Write the data and check that the write was successful
-	  objEntry = (ObjectList *) obj;
+		PIOS_FWRITE(file, MetaDataPtr((struct UAVOMeta *)obj_handle), MetaNumBytes,
+			&bytesWritten);
+		if (bytesWritten != MetaNumBytes) {
+			xSemaphoreGiveRecursive(mutex);
+			return -1;
+		}
+	} else {
+		struct UAVOData * uavo;
+		InstanceHandle instEntry;
 
-	  // Get the instance information
+		// Cast to object
-	  instEntry = getInstance(objEntry, instId);
+		uavo = (struct UAVOData *) obj_handle;
-	  if (instEntry == NULL) {
-		    xSemaphoreGiveRecursive(mutex);
-		    return -1;
-	  }
-	  // Write the object ID
-	  PIOS_FWRITE(file, &objEntry->id, sizeof(objEntry->id),
-		      &bytesWritten);
 
-	  // Write the instance ID
+		// Get the instance information
-	  if (!OLGetIsSingleInstance(objEntry)) {
+		instEntry = getInstance(uavo, instId);
-		    PIOS_FWRITE(file, &instEntry->instId,
+		if (instEntry == NULL) {
-				sizeof(instEntry->instId), &bytesWritten);
+			xSemaphoreGiveRecursive(mutex);
-	  }
+			return -1;
-	  // Write the data and check that the write was successful
+		}
-	  PIOS_FWRITE(file, instEntry->data, objEntry->numBytes,
+		// Write the object ID
-		      &bytesWritten);
+		PIOS_FWRITE(file, &uavo->id, sizeof(uavo->id),
-	  if (bytesWritten != objEntry->numBytes) {
+			&bytesWritten);
-		    xSemaphoreGiveRecursive(mutex);
+
-		    return -1;
+		// Write the instance ID
-	  }
+		if (!UAVObjIsSingleInstance(obj_handle)) {
-	  // Done
+			PIOS_FWRITE(file, &instId,
-	  xSemaphoreGiveRecursive(mutex);
+				sizeof(instId), &bytesWritten);
+		}
+		// Write the data and check that the write was successful
+		PIOS_FWRITE(file, InstanceData(instEntry), uavo->instance_size,
+			&bytesWritten);
+		if (bytesWritten != uavo->instance_size) {
+			xSemaphoreGiveRecursive(mutex);
+			return -1;
+		}
+	}
+	// Done
+	xSemaphoreGiveRecursive(mutex);
 #endif /* PIOS_INCLUDE_SDCARD */
-	  return 0;
+	return 0;
 }
 
 /**
@@ -568,59 +755,60 @@ int32_t UAVObjSaveToFile(UAVObjHandle obj, uint16_t instId,
  * @param[in] file File to append to
  * @return 0 if success or -1 if failure
  */
-int32_t UAVObjSave(UAVObjHandle obj, uint16_t instId)
+int32_t UAVObjSave(UAVObjHandle obj_handle, uint16_t instId)
 {
+	PIOS_Assert(obj_handle);
+
 #if defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS)
-	  ObjectList *objEntry = (ObjectList *) obj;
+	if (UAVObjIsMetaobject(obj_handle)) {
+		if (instId != 0)
+			return -1;
 
-	  if (objEntry == NULL)
+		if (PIOS_FLASHFS_ObjSave(obj_handle, instId, (uint8_t*) MetaDataPtr((struct UAVOMeta *)obj_handle)) != 0)
-		    return -1;
+			return -1;
+	} else {
+		InstanceHandle instEntry = getInstance( (struct UAVOData *)obj_handle, instId);
 
-	  ObjectInstList *instEntry = getInstance(objEntry, instId);
+		if (instEntry == NULL)
+			return -1;
 
-	  if (instEntry == NULL)
+		if (InstanceData(instEntry) == NULL)
-		    return -1;
+			return -1;
 
-	  if (instEntry->data == NULL)
+		if (PIOS_FLASHFS_ObjSave(obj_handle, instId, InstanceData(instEntry)) != 0)
-		    return -1;
+			return -1;
-
+	}
-	  if (PIOS_FLASHFS_ObjSave(obj, instId, instEntry->data) != 0)
-		    return -1;
 #endif
 #if defined(PIOS_INCLUDE_SDCARD)
-	  FILEINFO file;
+	FILEINFO file;
-	  ObjectList *objEntry;
+	uint8_t filename[14];
-	  uint8_t filename[14];
 
-	  // Check for file system availability
+	// Check for file system availability
-	  if (PIOS_SDCARD_IsMounted() == 0) {
+	if (PIOS_SDCARD_IsMounted() == 0) {
-		    return -1;
+		return -1;
-	  }
+	}
-	  // Lock
+	// Lock
-	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
+	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
 
-	  // Cast to object
+	// Get filename
-	  objEntry = (ObjectList *) obj;
+	objectFilename(obj_handle, filename);
 
-	  // Get filename
+	// Open file
-	  objectFilename(objEntry, filename);
+	if (PIOS_FOPEN_WRITE(filename, file)) {
-
+		xSemaphoreGiveRecursive(mutex);
-	  // Open file
+		return -1;
-	  if (PIOS_FOPEN_WRITE(filename, file)) {
+	}
-		    xSemaphoreGiveRecursive(mutex);
+	// Append object
-		    return -1;
+	if (UAVObjSaveToFile(obj_handle, instId, &file) == -1) {
-	  }
+		PIOS_FCLOSE(file);
-	  // Append object
+		xSemaphoreGiveRecursive(mutex);
-	  if (UAVObjSaveToFile(obj, instId, &file) == -1) {
+		return -1;
-		    PIOS_FCLOSE(file);
+	}
-		    xSemaphoreGiveRecursive(mutex);
+	// Done, close file and unlock
-		    return -1;
+	PIOS_FCLOSE(file);
-	  }
+	xSemaphoreGiveRecursive(mutex);
-	  // Done, close file and unlock
-	  PIOS_FCLOSE(file);
-	  xSemaphoreGiveRecursive(mutex);
 #endif /* PIOS_INCLUDE_SDCARD */
-	  return 0;
+	return 0;
 }
 
 /**
@@ -631,68 +819,87 @@ int32_t UAVObjSave(UAVObjHandle obj, uint16_t instId)
 UAVObjHandle UAVObjLoadFromFile(FILEINFO * file)
 {
 #if defined(PIOS_INCLUDE_SDCARD)
-	  uint32_t bytesRead;
+	uint32_t bytesRead;
-	  ObjectList *objEntry;
+	struct UAVOBase *objEntry;
-	  ObjectInstList *instEntry;
+	InstanceHandle instEntry;
-	  uint32_t objId;
+	uint32_t objId;
-	  uint16_t instId;
+	uint16_t instId;
-	  UAVObjHandle obj;
+	UAVObjHandle obj_handle;
 
-	  // Check for file system availability
+	// Check for file system availability
-	  if (PIOS_SDCARD_IsMounted() == 0) {
+	if (PIOS_SDCARD_IsMounted() == 0) {
-		    return NULL;
+		return NULL;
-	  }
+	}
-	  // Lock
+	// Lock
-	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
+	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
 
-	  // Read the object ID
+	// Read the object ID
-	  if (PIOS_FREAD(file, &objId, sizeof(objId), &bytesRead)) {
+	if (PIOS_FREAD(file, &objId, sizeof(objId), &bytesRead)) {
-		    xSemaphoreGiveRecursive(mutex);
+		xSemaphoreGiveRecursive(mutex);
-		    return NULL;
+		return NULL;
-	  }
+	}
-	  // Get the object
+	// Get the object
-	  obj = UAVObjGetByID(objId);
+	obj_handle = UAVObjGetByID(objId);
-	  if (obj == 0) {
+	if (obj_handle == 0) {
-		    xSemaphoreGiveRecursive(mutex);
+		xSemaphoreGiveRecursive(mutex);
-		    return NULL;
+		return NULL;
-	  }
+	}
-	  objEntry = (ObjectList *) obj;
+	objEntry = (struct UAVOBase *) obj_handle;
 
-	  // Get the instance ID
+	// Get the instance ID
-	  instId = 0;
+	instId = 0;
-	  if (!OLGetIsSingleInstance(objEntry)) {
+	if (!UAVObjIsSingleInstance(obj_handle)) {
-		    if (PIOS_FREAD
+		if (PIOS_FREAD
 			(file, &instId, sizeof(instId), &bytesRead)) {
-			      xSemaphoreGiveRecursive(mutex);
+			xSemaphoreGiveRecursive(mutex);
-			      return NULL;
+			return NULL;
-		    }
+		}
-	  }
+	}
-	  // Get the instance information
-	  instEntry = getInstance(objEntry, instId);
 
-	  // If the instance does not exist create it and any other instances before it
+	if (UAVObjIsMetaobject(obj_handle)) {
-	  if (instEntry == NULL) {
+		// If the instance does not exist create it and any other instances before it
-		    instEntry = createInstance(objEntry, instId);
+		if (instId != 0) {
-		    if (instEntry == NULL) {
+			// Error, unlock and return
-			      // Error, unlock and return
+			xSemaphoreGiveRecursive(mutex);
-			      xSemaphoreGiveRecursive(mutex);
+			return NULL;
-			      return NULL;
+		}
-		    }
+		// Read the instance data
-	  }
+		if (PIOS_FREAD
-	  // Read the instance data
+			(file, MetaDataPtr((struct UAVOMeta *)obj_handle), MetaNumBytes, &bytesRead)) {
-	  if (PIOS_FREAD
+			xSemaphoreGiveRecursive(mutex);
-	      (file, instEntry->data, objEntry->numBytes, &bytesRead)) {
+			return NULL;
-		    xSemaphoreGiveRecursive(mutex);
+		}
-		    return NULL;
+	} else {
-	  }
-	  // Fire event
-	  sendEvent(objEntry, instId, EV_UNPACKED);
 
-	  // Unlock
+		// Get the instance information
-	  xSemaphoreGiveRecursive(mutex);
+		instEntry = getInstance((struct UAVOData *)objEntry, instId);
-	  return obj;
+
+		// If the instance does not exist create it and any other instances before it
+		if (instEntry == NULL) {
+			instEntry = createInstance((struct UAVOData *)objEntry, instId);
+			if (instEntry == NULL) {
+				// Error, unlock and return
+				xSemaphoreGiveRecursive(mutex);
+				return NULL;
+			}
+		}
+		// Read the instance data
+		if (PIOS_FREAD
+			(file, InstanceData(instEntry), ((struct UAVOData *)objEntry)->instance_size, &bytesRead)) {
+			xSemaphoreGiveRecursive(mutex);
+			return NULL;
+		}
+
+	}
+
+	// Fire event
+	sendEvent(objEntry, instId, EV_UNPACKED);
+
+	// Unlock
+	xSemaphoreGiveRecursive(mutex);
+	return obj_handle;
 #else /* PIOS_INCLUDE_SDCARD */
-	  return NULL;
+	return NULL;
 #endif
 }
 
@@ -704,74 +911,74 @@ UAVObjHandle UAVObjLoadFromFile(FILEINFO * file)
  * @param[in] instId The object instance
  * @return 0 if success or -1 if failure
  */
-int32_t UAVObjLoad(UAVObjHandle obj, uint16_t instId)
+int32_t UAVObjLoad(UAVObjHandle obj_handle, uint16_t instId)
 {
+	PIOS_Assert(obj_handle);
+
 #if defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS)
-	ObjectList *objEntry = (ObjectList *) obj;
+	if (UAVObjIsMetaobject(obj_handle)) {
+		if (instId != 0)
+			return -1;
 
-	if (objEntry == NULL)
+		// Fire event on success
-		return -1;
+		if (PIOS_FLASHFS_ObjLoad(obj_handle, instId, (uint8_t*) MetaDataPtr((struct UAVOMeta *)obj_handle)) == 0)
+			sendEvent((struct UAVOBase*)obj_handle, instId, EV_UNPACKED);
+		else
+			return -1;
+	} else {
 
-	ObjectInstList *instEntry = getInstance(objEntry, instId);
+		InstanceHandle instEntry = getInstance( (struct UAVOData *)obj_handle, instId);
 
-	if (instEntry == NULL)
+		if (instEntry == NULL)
-		return -1;
+			return -1;
 
-	if (instEntry->data == NULL)
+		// Fire event on success
-		return -1;
+		if (PIOS_FLASHFS_ObjLoad(obj_handle, instId, InstanceData(instEntry)) == 0)
+			sendEvent((struct UAVOBase*)obj_handle, instId, EV_UNPACKED);
+		else
+			return -1;
+	}
 
-	// Fire event on success
-	int32_t retval;
-	if ((retval = PIOS_FLASHFS_ObjLoad(obj, instId, instEntry->data)) == 0)
-		sendEvent(objEntry, instId, EV_UNPACKED);
-	else
-		return retval;
 #endif
 
 #if defined(PIOS_INCLUDE_SDCARD)
-	  FILEINFO file;
+	FILEINFO file;
-	  ObjectList *objEntry;
+	UAVObjHandle loadedObj;
-	  UAVObjHandle loadedObj;
+	uint8_t filename[14];
-	  ObjectList *loadedObjEntry;
-	  uint8_t filename[14];
 
-	  // Check for file system availability
+	// Check for file system availability
-	  if (PIOS_SDCARD_IsMounted() == 0) {
+	if (PIOS_SDCARD_IsMounted() == 0) {
-		    return -1;
+		return -1;
-	  }
+	}
-	  // Lock
+	// Lock
-	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
+	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
 
-	  // Cast to object
+	// Get filename
-	  objEntry = (ObjectList *) obj;
+	objectFilename(obj_handle, filename);
 
-	  // Get filename
+	// Open file
-	  objectFilename(objEntry, filename);
+	if (PIOS_FOPEN_READ(filename, file)) {
-
+		xSemaphoreGiveRecursive(mutex);
-	  // Open file
+		return -1;
-	  if (PIOS_FOPEN_READ(filename, file)) {
+	}
-		    xSemaphoreGiveRecursive(mutex);
+	// Load object
-		    return -1;
+	loadedObj = UAVObjLoadFromFile(&file);
-	  }
+	if (loadedObj == 0) {
-	  // Load object
+		PIOS_FCLOSE(file);
-	  loadedObj = UAVObjLoadFromFile(&file);
+		xSemaphoreGiveRecursive(mutex);
-	  if (loadedObj == 0) {
+		return -1;
-		    PIOS_FCLOSE(file);
+	}
-		    xSemaphoreGiveRecursive(mutex);
+	// Check that the IDs match
-		    return -1;
+	if (UAVObjGetID(loadedObj) != UAVObjGetID(obj_handle)) {
-	  }
+		PIOS_FCLOSE(file);
-	  // Check that the IDs match
+		xSemaphoreGiveRecursive(mutex);
-	  loadedObjEntry = (ObjectList *) loadedObj;
+		return -1;
-	  if (loadedObjEntry->id != objEntry->id) {
+	}
-		    PIOS_FCLOSE(file);
+	// Done, close file and unlock
-		    xSemaphoreGiveRecursive(mutex);
+	PIOS_FCLOSE(file);
-		    return -1;
+	xSemaphoreGiveRecursive(mutex);
-	  }
-	  // Done, close file and unlock
-	  PIOS_FCLOSE(file);
-	  xSemaphoreGiveRecursive(mutex);
 #endif /* PIOS_INCLUDE_SDCARD */
-	  return 0;
+	return 0;
 }
 
 /**
@@ -780,35 +987,32 @@ int32_t UAVObjLoad(UAVObjHandle obj, uint16_t instId)
  * @param[in] instId The object instance
  * @return 0 if success or -1 if failure
  */
-int32_t UAVObjDelete(UAVObjHandle obj, uint16_t instId)
+int32_t UAVObjDelete(UAVObjHandle obj_handle, uint16_t instId)
 {
+	PIOS_Assert(obj_handle);
 #if defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS)
-	  PIOS_FLASHFS_ObjDelete(obj, instId);
+	PIOS_FLASHFS_ObjDelete(obj_handle, instId);
 #endif
 #if defined(PIOS_INCLUDE_SDCARD)
-	  ObjectList *objEntry;
+	uint8_t filename[14];
-	  uint8_t filename[14];
 
-	  // Check for file system availability
+	// Check for file system availability
-	  if (PIOS_SDCARD_IsMounted() == 0) {
+	if (PIOS_SDCARD_IsMounted() == 0) {
-		    return -1;
+		return -1;
-	  }
+	}
-	  // Lock
+	// Lock
-	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
+	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
 
-	  // Cast to object
+	// Get filename
-	  objEntry = (ObjectList *) obj;
+	objectFilename(obj_handle, filename);
 
-	  // Get filename
+	// Delete file
-	  objectFilename(objEntry, filename);
+	PIOS_FUNLINK(filename);
 
-	  // Delete file
+	// Done
-	  PIOS_FUNLINK(filename);
+	xSemaphoreGiveRecursive(mutex);
-
-	  // Done
-	  xSemaphoreGiveRecursive(mutex);
 #endif /* PIOS_INCLUDE_SDCARD */
-	  return 0;
+	return 0;
 }
 
 /**
@@ -817,27 +1021,30 @@ int32_t UAVObjDelete(UAVObjHandle obj, uint16_t instId)
  */
 int32_t UAVObjSaveSettings()
 {
-	  ObjectList *objEntry;
+	struct UAVOData *obj;
 
-	  // Get lock
+	// Get lock
-	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
+	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
 
-	  // Save all settings objects
+	int32_t rc = -1;
-	  LL_FOREACH(objList, objEntry) {
-		    // Check if this is a settings object
-		    if (OLGetIsSettings(objEntry)) {
-			      // Save object
-			      if (UAVObjSave((UAVObjHandle) objEntry, 0) ==
-				  -1) {
-					xSemaphoreGiveRecursive(mutex);
-					return -1;
-			      }
-		    }
-	  }
 
-	  // Done
+	// Save all settings objects
-	  xSemaphoreGiveRecursive(mutex);
+	LL_FOREACH(uavo_list, obj) {
-	  return 0;
+		// Check if this is a settings object
+		if (UAVObjIsSettings(obj)) {
+			// Save object
+			if (UAVObjSave((UAVObjHandle) obj, 0) ==
+				-1) {
+				goto unlock_exit;
+			}
+		}
+	}
+
+	rc = 0;
+
+unlock_exit:
+	xSemaphoreGiveRecursive(mutex);
+	return rc;
 }
 
 /**
@@ -846,27 +1053,30 @@ int32_t UAVObjSaveSettings()
  */
 int32_t UAVObjLoadSettings()
 {
-	  ObjectList *objEntry;
+	struct UAVOData *obj;
 
-	  // Get lock
+	// Get lock
-	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
+	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
 
-	  // Load all settings objects
+	int32_t rc = -1;
-	  LL_FOREACH(objList, objEntry) {
-		    // Check if this is a settings object
-		    if (OLGetIsSettings(objEntry)) {
-			      // Load object
-			      if (UAVObjLoad((UAVObjHandle) objEntry, 0) ==
-				  -1) {
-					xSemaphoreGiveRecursive(mutex);
-					return -1;
-			      }
-		    }
-	  }
 
-	  // Done
+	// Load all settings objects
-	  xSemaphoreGiveRecursive(mutex);
+	LL_FOREACH(uavo_list, obj) {
-	  return 0;
+		// Check if this is a settings object
+		if (UAVObjIsSettings(obj)) {
+			// Load object
+			if (UAVObjLoad((UAVObjHandle) obj, 0) ==
+				-1) {
+				goto unlock_exit;
+			}
+		}
+	}
+
+	rc = 0;
+
+unlock_exit:
+	xSemaphoreGiveRecursive(mutex);
+	return rc;
 }
 
 /**
@@ -875,27 +1085,30 @@ int32_t UAVObjLoadSettings()
  */
 int32_t UAVObjDeleteSettings()
 {
-	  ObjectList *objEntry;
+	struct UAVOData *obj;
 
-	  // Get lock
+	// Get lock
-	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
+	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
 
-	  // Save all settings objects
+	int32_t rc = -1;
-	  LL_FOREACH(objList, objEntry) {
-		    // Check if this is a settings object
-		    if (OLGetIsSettings(objEntry)) {
-			      // Save object
-			      if (UAVObjDelete((UAVObjHandle) objEntry, 0)
-				  == -1) {
-					xSemaphoreGiveRecursive(mutex);
-					return -1;
-			      }
-		    }
-	  }
 
-	  // Done
+	// Save all settings objects
-	  xSemaphoreGiveRecursive(mutex);
+	LL_FOREACH(uavo_list, obj) {
-	  return 0;
+		// Check if this is a settings object
+		if (UAVObjIsSettings(obj)) {
+			// Save object
+			if (UAVObjDelete((UAVObjHandle) obj, 0)
+				== -1) {
+				goto unlock_exit;
+			}
+		}
+	}
+
+	rc = 0;
+
+unlock_exit:
+	xSemaphoreGiveRecursive(mutex);
+	return rc;
 }
 
 /**
@@ -904,27 +1117,27 @@ int32_t UAVObjDeleteSettings()
  */
 int32_t UAVObjSaveMetaobjects()
 {
-	  ObjectList *objEntry;
+	struct UAVOData *obj;
 
-	  // Get lock
+	// Get lock
-	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
+	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
 
-	  // Save all settings objects
+	int32_t rc = -1;
-	  LL_FOREACH(objList, objEntry) {
-		    // Check if this is a settings object
-		    if (OLGetIsMetaobject(objEntry)) {
-			      // Save object
-			      if (UAVObjSave((UAVObjHandle) objEntry, 0) ==
-				  -1) {
-					xSemaphoreGiveRecursive(mutex);
-					return -1;
-			      }
-		    }
-	  }
 
-	  // Done
+	// Save all settings objects
-	  xSemaphoreGiveRecursive(mutex);
+	LL_FOREACH(uavo_list, obj) {
-	  return 0;
+		// Save object
+		if (UAVObjSave( (UAVObjHandle) MetaObjectPtr(obj), 0) ==
+			-1) {
+			goto unlock_exit;
+		}
+	}
+
+	rc = 0;
+
+unlock_exit:
+	xSemaphoreGiveRecursive(mutex);
+	return rc;
 }
 
 /**
@@ -933,27 +1146,27 @@ int32_t UAVObjSaveMetaobjects()
  */
 int32_t UAVObjLoadMetaobjects()
 {
-	  ObjectList *objEntry;
+	struct UAVOData *obj;
 
-	  // Get lock
+	// Get lock
-	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
+	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
 
-	  // Load all settings objects
+	int32_t rc = -1;
-	  LL_FOREACH(objList, objEntry) {
-		    // Check if this is a settings object
-		    if (OLGetIsMetaobject(objEntry)) {
-			      // Load object
-			      if (UAVObjLoad((UAVObjHandle) objEntry, 0) ==
-				  -1) {
-					xSemaphoreGiveRecursive(mutex);
-					return -1;
-			      }
-		    }
-	  }
 
-	  // Done
+	// Load all settings objects
-	  xSemaphoreGiveRecursive(mutex);
+	LL_FOREACH(uavo_list, obj) {
-	  return 0;
+		// Load object
+		if (UAVObjLoad((UAVObjHandle) MetaObjectPtr(obj), 0) ==
+			-1) {
+			goto unlock_exit;
+		}
+	}
+
+	rc = 0;
+
+unlock_exit:
+	xSemaphoreGiveRecursive(mutex);
+	return rc;
 }
 
 /**
@@ -962,253 +1175,293 @@ int32_t UAVObjLoadMetaobjects()
  */
 int32_t UAVObjDeleteMetaobjects()
 {
-	  ObjectList *objEntry;
+	struct UAVOData *obj;
 
-	  // Get lock
+	// Get lock
-	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
-
-	  // Load all settings objects
-	  LL_FOREACH(objList, objEntry) {
-		    // Check if this is a settings object
-		    if (OLGetIsMetaobject(objEntry)) {
-			      // Load object
-			      if (UAVObjDelete((UAVObjHandle) objEntry, 0)
-				  == -1) {
-					xSemaphoreGiveRecursive(mutex);
-					return -1;
-			      }
-		    }
-	  }
-
-	  // Done
-	  xSemaphoreGiveRecursive(mutex);
-	  return 0;
-}
-
-/**
- * Set the object data
- * \param[in] obj The object handle
- * \param[in] dataIn The object's data structure
- * \return 0 if success or -1 if failure
- */
-int32_t UAVObjSetData(UAVObjHandle obj, const void *dataIn)
-{
-	  return UAVObjSetInstanceData(obj, 0, dataIn);
-}
-
-/**
- * Set the object data
- * \param[in] obj The object handle
- * \param[in] dataIn The object's data structure
- * \return 0 if success or -1 if failure
- */
-int32_t UAVObjSetDataField(UAVObjHandle obj, const void* dataIn, uint32_t offset, uint32_t size)
-{
-	return UAVObjSetInstanceDataField(obj, 0, dataIn, offset, size);
-}
-
-/**
- * Get the object data
- * \param[in] obj The object handle
- * \param[out] dataOut The object's data structure
- * \return 0 if success or -1 if failure
- */
-int32_t UAVObjGetData(UAVObjHandle obj, void *dataOut)
-{
-	  return UAVObjGetInstanceData(obj, 0, dataOut);
-}
-
-/**
- * Get the object data
- * \param[in] obj The object handle
- * \param[out] dataOut The object's data structure
- * \return 0 if success or -1 if failure
- */
-int32_t UAVObjGetDataField(UAVObjHandle obj, void* dataOut, uint32_t offset, uint32_t size)
-{
-	return UAVObjGetInstanceDataField(obj, 0, dataOut, offset, size);
-}
-
-/**
- * Set the data of a specific object instance
- * \param[in] obj The object handle
- * \param[in] instId The object instance ID
- * \param[in] dataIn The object's data structure
- * \return 0 if success or -1 if failure
- */
-int32_t UAVObjSetInstanceData(UAVObjHandle obj, uint16_t instId,
-			      const void *dataIn)
-{
-	  ObjectList *objEntry;
-	  ObjectInstList *instEntry;
-	  UAVObjMetadata *mdata;
-
-	  // Lock
-	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
-
-	  // Cast to object info
-	  objEntry = (ObjectList *) obj;
-
-	  // Check access level
-	  if (!OLGetIsMetaobject(objEntry)) {
-		    mdata =
-			(UAVObjMetadata *) (objEntry->linkedObj->instances.
-					    data);
-		    if (UAVObjGetAccess(mdata) == ACCESS_READONLY) {
-			      xSemaphoreGiveRecursive(mutex);
-			      return -1;
-		    }
-	  }
-	  // Get instance information
-	  instEntry = getInstance(objEntry, instId);
-	  if (instEntry == NULL) {
-		    // Error, unlock and return
-		    xSemaphoreGiveRecursive(mutex);
-		    return -1;
-	  }
-	  // Set data
-	  memcpy(instEntry->data, dataIn, objEntry->numBytes);
-
-	  // Fire event
-	  sendEvent(objEntry, instId, EV_UPDATED);
-
-	  // Unlock
-	  xSemaphoreGiveRecursive(mutex);
-	  return 0;
-}
-
-/**
- * Set the data of a specific object instance
- * \param[in] obj The object handle
- * \param[in] instId The object instance ID
- * \param[in] dataIn The object's data structure
- * \return 0 if success or -1 if failure
- */
-int32_t UAVObjSetInstanceDataField(UAVObjHandle obj, uint16_t instId, const void* dataIn, uint32_t offset, uint32_t size)
-{
-	ObjectList* objEntry;
-	ObjectInstList* instEntry;
-	UAVObjMetadata* mdata;
-
-	// Lock
 	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
 
-	// Cast to object info
+	int32_t rc = -1;
-	objEntry = (ObjectList*)obj;
 
-	// Check access level
+	// Load all settings objects
-	if ( !OLGetIsMetaobject(objEntry) )
+	LL_FOREACH(uavo_list, obj) {
-	{
+		// Load object
-		mdata = (UAVObjMetadata*)(objEntry->linkedObj->instances.data);
+		if (UAVObjDelete((UAVObjHandle) MetaObjectPtr(obj), 0)
-		if ( UAVObjGetAccess(mdata) == ACCESS_READONLY )
+			== -1) {
-		{
+			goto unlock_exit;
-			xSemaphoreGiveRecursive(mutex);
-			return -1;
 		}
 	}
 
-	// Get instance information
+	rc = 0;
-	instEntry = getInstance(objEntry, instId);
-	if ( instEntry == NULL )
-	{
-		// Error, unlock and return
-		xSemaphoreGiveRecursive(mutex);
-		return -1;
-	}
 
-	// return if we set too much of what we have
+unlock_exit:
-	if ( (size + offset) > objEntry->numBytes) {
-		// Error, unlock and return
-		xSemaphoreGiveRecursive(mutex);		
-		return -1;
-	}
-
-	// Set data
-	memcpy(instEntry->data + offset, dataIn, size);
-
-	// Fire event
-	sendEvent(objEntry, instId, EV_UPDATED);
-
-	// Unlock
 	xSemaphoreGiveRecursive(mutex);
-	return 0;
+	return rc;
 }
 
 /**
- * Get the data of a specific object instance
+ * Set the object data
  * \param[in] obj The object handle
- * \param[in] instId The object instance ID
+ * \param[in] dataIn The object's data structure
- * \param[out] dataOut The object's data structure
  * \return 0 if success or -1 if failure
  */
-int32_t UAVObjGetInstanceData(UAVObjHandle obj, uint16_t instId,
+int32_t UAVObjSetData(UAVObjHandle obj_handle, const void *dataIn)
-			      void *dataOut)
 {
-	  ObjectList *objEntry;
+	return UAVObjSetInstanceData(obj_handle, 0, dataIn);
-	  ObjectInstList *instEntry;
-
-	  // Lock
-	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
-
-	  // Cast to object info
-	  objEntry = (ObjectList *) obj;
-
-	  // Get instance information
-	  instEntry = getInstance(objEntry, instId);
-	  if (instEntry == NULL) {
-		    // Error, unlock and return
-		    xSemaphoreGiveRecursive(mutex);
-		    return -1;
-	  }
-	  // Set data
-	  memcpy(dataOut, instEntry->data, objEntry->numBytes);
-
-	  // Unlock
-	  xSemaphoreGiveRecursive(mutex);
-	  return 0;
 }
 
 /**
- * Get the data of a specific object instance
+ * Set the object data
+ * \param[in] obj The object handle
+ * \param[in] dataIn The object's data structure
+ * \return 0 if success or -1 if failure
+ */
+int32_t UAVObjSetDataField(UAVObjHandle obj_handle, const void* dataIn, uint32_t offset, uint32_t size)
+{
+	return UAVObjSetInstanceDataField(obj_handle, 0, dataIn, offset, size);
+}
+
+/**
+ * Get the object data
  * \param[in] obj The object handle
- * \param[in] instId The object instance ID
  * \param[out] dataOut The object's data structure
  * \return 0 if success or -1 if failure
  */
-int32_t UAVObjGetInstanceDataField(UAVObjHandle obj, uint16_t instId, void* dataOut, uint32_t offset, uint32_t size)
+int32_t UAVObjGetData(UAVObjHandle obj_handle, void *dataOut)
 {
-	ObjectList* objEntry;
+	return UAVObjGetInstanceData(obj_handle, 0, dataOut);
-	ObjectInstList* instEntry;
+}
+
+/**
+ * Get the object data
+ * \param[in] obj The object handle
+ * \param[out] dataOut The object's data structure
+ * \return 0 if success or -1 if failure
+ */
+int32_t UAVObjGetDataField(UAVObjHandle obj_handle, void* dataOut, uint32_t offset, uint32_t size)
+{
+	return UAVObjGetInstanceDataField(obj_handle, 0, dataOut, offset, size);
+}
+
+/**
+ * Set the data of a specific object instance
+ * \param[in] obj The object handle
+ * \param[in] instId The object instance ID
+ * \param[in] dataIn The object's data structure
+ * \return 0 if success or -1 if failure
+ */
+int32_t UAVObjSetInstanceData(UAVObjHandle obj_handle, uint16_t instId,
+			const void *dataIn)
+{
+	PIOS_Assert(obj_handle);
 
 	// Lock
 	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
 
-	// Cast to object info
+	int32_t rc = -1;
-	objEntry = (ObjectList*)obj;
 
-	// Get instance information
+	if (UAVObjIsMetaobject(obj_handle)) {
-	instEntry = getInstance(objEntry, instId);
+		if (instId != 0) {
-	if ( instEntry == NULL )
+			goto unlock_exit;
-	{
+		}
-		// Error, unlock and return
+		memcpy(MetaDataPtr((struct UAVOMeta *)obj_handle), dataIn, MetaNumBytes);
-		xSemaphoreGiveRecursive(mutex);
+	} else {
-		return -1;
+		struct UAVOData *obj;
+		InstanceHandle instEntry;
+
+		// Cast to object info
+		obj = (struct UAVOData *) obj_handle;
+
+		// Check access level
+		if (UAVObjReadOnly(obj_handle)) {
+			goto unlock_exit;
+		}
+		// Get instance information
+		instEntry = getInstance(obj, instId);
+		if (instEntry == NULL) {
+			goto unlock_exit;
+		}
+		// Set data
+		memcpy(InstanceData(instEntry), dataIn, obj->instance_size);
 	}
 
-	// return if we request too much of what we can give
+	// Fire event
-	if ( (size + offset) > objEntry->numBytes) 
+	sendEvent((struct UAVOBase *)obj_handle, instId, EV_UPDATED);
-	{
+	rc = 0;
-		// Error, unlock and return
-		xSemaphoreGiveRecursive(mutex);
-		return -1;
-	}
 
-	// Set data
+unlock_exit:
-	memcpy(dataOut, instEntry->data + offset, size);
-
-	// Unlock
 	xSemaphoreGiveRecursive(mutex);
-	return 0;
+	return rc;
+}
+
+/**
+ * Set the data of a specific object instance
+ * \param[in] obj The object handle
+ * \param[in] instId The object instance ID
+ * \param[in] dataIn The object's data structure
+ * \return 0 if success or -1 if failure
+ */
+int32_t UAVObjSetInstanceDataField(UAVObjHandle obj_handle, uint16_t instId, const void* dataIn, uint32_t offset, uint32_t size)
+{
+	PIOS_Assert(obj_handle);
+
+	// Lock
+	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
+
+	int32_t rc = -1;
+
+	if (UAVObjIsMetaobject(obj_handle)) {
+		// Get instance information
+		if (instId != 0) {
+			goto unlock_exit;
+		}
+
+		// Check for overrun
+		if ((size + offset) > MetaNumBytes) {
+			goto unlock_exit;
+		}
+
+		// Set data
+		memcpy(MetaDataPtr((struct UAVOMeta *)obj_handle) + offset, dataIn, size);
+	} else {
+		struct UAVOData * obj;
+		InstanceHandle instEntry;
+
+		// Cast to object info
+		obj = (struct UAVOData *)obj_handle;
+
+		// Check access level
+		if (UAVObjReadOnly(obj_handle)) {
+			goto unlock_exit;
+		}
+
+		// Get instance information
+		instEntry = getInstance(obj, instId);
+		if (instEntry == NULL) {
+			goto unlock_exit;
+		}
+
+		// Check for overrun
+		if ((size + offset) > obj->instance_size) {
+			goto unlock_exit;
+		}
+
+		// Set data
+		memcpy(InstanceData(instEntry) + offset, dataIn, size);
+	}
+
+
+	// Fire event
+	sendEvent((struct UAVOBase *)obj_handle, instId, EV_UPDATED);
+	rc = 0;
+
+unlock_exit:
+	xSemaphoreGiveRecursive(mutex);
+	return rc;
+}
+
+/**
+ * Get the data of a specific object instance
+ * \param[in] obj The object handle
+ * \param[in] instId The object instance ID
+ * \param[out] dataOut The object's data structure
+ * \return 0 if success or -1 if failure
+ */
+int32_t UAVObjGetInstanceData(UAVObjHandle obj_handle, uint16_t instId,
+			void *dataOut)
+{
+	PIOS_Assert(obj_handle);
+
+	// Lock
+	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
+
+	int32_t rc = -1;
+
+	if (UAVObjIsMetaobject(obj_handle)) {
+		// Get instance information
+		if (instId != 0) {
+			goto unlock_exit;
+		}
+		// Set data
+		memcpy(dataOut, MetaDataPtr((struct UAVOMeta *)obj_handle), MetaNumBytes);
+	} else {
+		struct UAVOData *obj;
+		InstanceHandle instEntry;
+
+		// Cast to object info
+		obj = (struct UAVOData *) obj_handle;
+
+		// Get instance information
+		instEntry = getInstance(obj, instId);
+		if (instEntry == NULL) {
+			goto unlock_exit;
+		}
+		// Set data
+		memcpy(dataOut, InstanceData(instEntry), obj->instance_size);
+	}
+
+	rc = 0;
+
+unlock_exit:
+	xSemaphoreGiveRecursive(mutex);
+	return rc;
+}
+
+/**
+ * Get the data of a specific object instance
+ * \param[in] obj The object handle
+ * \param[in] instId The object instance ID
+ * \param[out] dataOut The object's data structure
+ * \return 0 if success or -1 if failure
+ */
+int32_t UAVObjGetInstanceDataField(UAVObjHandle obj_handle, uint16_t instId, void* dataOut, uint32_t offset, uint32_t size)
+{
+	PIOS_Assert(obj_handle);
+
+	// Lock
+	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
+
+	int32_t rc = -1;
+
+	if (UAVObjIsMetaobject(obj_handle)) {
+		// Get instance information
+		if (instId != 0) {
+			goto unlock_exit;
+		}
+
+		// Check for overrun
+		if ((size + offset) > MetaNumBytes) {
+			goto unlock_exit;
+		}
+
+		// Set data
+		memcpy(dataOut, MetaDataPtr((struct UAVOMeta *)obj_handle) + offset, size);
+	} else {
+		struct UAVOData * obj;
+		InstanceHandle instEntry;
+
+		// Cast to object info
+		obj = (struct UAVOData *)obj_handle;
+
+		// Get instance information
+		instEntry = getInstance(obj, instId);
+		if (instEntry == NULL) {
+			goto unlock_exit;
+		}
+
+		// Check for overrun
+		if ((size + offset) > obj->instance_size) {
+			goto unlock_exit;
+		}
+		
+		// Set data
+		memcpy(dataOut, InstanceData(instEntry) + offset, size);
+	}
+
+	rc = 0;
+
+unlock_exit:
+	xSemaphoreGiveRecursive(mutex);
+	return rc;
 }
 
 /**
@@ -1217,25 +1470,21 @@ int32_t UAVObjGetInstanceDataField(UAVObjHandle obj, uint16_t instId, void* data
  * \param[in] dataIn The object's metadata structure
  * \return 0 if success or -1 if failure
  */
-int32_t UAVObjSetMetadata(UAVObjHandle obj, const UAVObjMetadata * dataIn)
+int32_t UAVObjSetMetadata(UAVObjHandle obj_handle, const UAVObjMetadata * dataIn)
 {
-	  ObjectList *objEntry;
+	PIOS_Assert(obj_handle);
 
-	  // Lock
+	// Set metadata (metadata of metaobjects can not be modified)
-	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
+	if (UAVObjIsMetaobject(obj_handle)) {
+		return -1;
+	}
 
-	  // Set metadata (metadata of metaobjects can not be modified)
+	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
-	  objEntry = (ObjectList *) obj;
-	  if (!OLGetIsMetaobject(objEntry)) {
-		    UAVObjSetData((UAVObjHandle) objEntry->linkedObj,
-				  dataIn);
-	  } else {
-		    return -1;
-	  }
 
-	  // Unlock
+	UAVObjSetData((UAVObjHandle) MetaObjectPtr((struct UAVOData *)obj_handle), dataIn);
-	  xSemaphoreGiveRecursive(mutex);
+
-	  return 0;
+	xSemaphoreGiveRecursive(mutex);
+	return 0;
 }
 
 /**
@@ -1244,44 +1493,29 @@ int32_t UAVObjSetMetadata(UAVObjHandle obj, const UAVObjMetadata * dataIn)
  * \param[out] dataOut The object's metadata structure
  * \return 0 if success or -1 if failure
  */
-int32_t UAVObjGetMetadata(UAVObjHandle obj, UAVObjMetadata * dataOut)
+int32_t UAVObjGetMetadata(UAVObjHandle obj_handle, UAVObjMetadata * dataOut)
 {
-	  ObjectList *objEntry;
+	PIOS_Assert(obj_handle);
 
-	  // Lock
+	// Lock
-	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
+	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
 
-	  // Get metadata
+	// Get metadata
-	  objEntry = (ObjectList *) obj;
+	if (UAVObjIsMetaobject(obj_handle)) {
-	  if (OLGetIsMetaobject(objEntry)) {
+		memcpy(dataOut, &defMetadata, sizeof(UAVObjMetadata));
-		    memcpy(dataOut, &defMetadata, sizeof(UAVObjMetadata));
+	} else {
-	  } else {
+		UAVObjGetData((UAVObjHandle) MetaObjectPtr( (struct UAVOData *)obj_handle ),
-		    UAVObjGetData((UAVObjHandle) objEntry->linkedObj,
+			dataOut);
-				  dataOut);
+	}
-	  }
 
-	  // Unlock
+	// Unlock
-	  xSemaphoreGiveRecursive(mutex);
+	xSemaphoreGiveRecursive(mutex);
-	  return 0;
+	return 0;
 }
 
-/**
+/*******************************
- * Initialize a UAVObjMetadata object.
+ * Object Metadata Manipulation
- * \param[in] metadata The metadata object
+ ******************************/
- */
-void UAVObjMetadataInitialize(UAVObjMetadata* metadata)
-{
-	metadata->flags =
-		ACCESS_READWRITE << UAVOBJ_ACCESS_SHIFT |
-		ACCESS_READWRITE << UAVOBJ_GCS_ACCESS_SHIFT |
-		1 << UAVOBJ_TELEMETRY_ACKED_SHIFT |
-		1 << UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT |
-		UPDATEMODE_ONCHANGE << UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT |
-		UPDATEMODE_ONCHANGE << UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT;
-	metadata->telemetryUpdatePeriod = 0;
-	metadata->gcsTelemetryUpdatePeriod = 0;
-	metadata->loggingUpdatePeriod = 0;
-}
 
 /**
  * Get the UAVObject metadata access member
@@ -1290,6 +1524,7 @@ void UAVObjMetadataInitialize(UAVObjMetadata* metadata)
  */
 UAVObjAccessType UAVObjGetAccess(const UAVObjMetadata* metadata)
 {
+	PIOS_Assert(metadata);
 	return (metadata->flags >> UAVOBJ_ACCESS_SHIFT) & 1;
 }
 
@@ -1300,6 +1535,7 @@ UAVObjAccessType UAVObjGetAccess(const UAVObjMetadata* metadata)
  */
 void UAVObjSetAccess(UAVObjMetadata* metadata, UAVObjAccessType mode)
 {
+	PIOS_Assert(metadata);
 	SET_BITS(metadata->flags, UAVOBJ_ACCESS_SHIFT, mode, 1);
 }
 
@@ -1310,6 +1546,7 @@ void UAVObjSetAccess(UAVObjMetadata* metadata, UAVObjAccessType mode)
  */
 UAVObjAccessType UAVObjGetGcsAccess(const UAVObjMetadata* metadata)
 {
+	PIOS_Assert(metadata);
 	return (metadata->flags >> UAVOBJ_GCS_ACCESS_SHIFT) & 1;
 }
 
@@ -1319,6 +1556,7 @@ UAVObjAccessType UAVObjGetGcsAccess(const UAVObjMetadata* metadata)
  * \param[in] mode The access mode
  */
 void UAVObjSetGcsAccess(UAVObjMetadata* metadata, UAVObjAccessType mode) {
+	PIOS_Assert(metadata);
 	SET_BITS(metadata->flags, UAVOBJ_GCS_ACCESS_SHIFT, mode, 1);
 }
 
@@ -1328,6 +1566,7 @@ void UAVObjSetGcsAccess(UAVObjMetadata* metadata, UAVObjAccessType mode) {
  * \return the telemetry acked boolean
  */
 uint8_t UAVObjGetTelemetryAcked(const UAVObjMetadata* metadata) {
+	PIOS_Assert(metadata);
 	return (metadata->flags >> UAVOBJ_TELEMETRY_ACKED_SHIFT) & 1;
 }
 
@@ -1337,6 +1576,7 @@ uint8_t UAVObjGetTelemetryAcked(const UAVObjMetadata* metadata) {
  * \param[in] val The telemetry acked boolean
  */
 void UAVObjSetTelemetryAcked(UAVObjMetadata* metadata, uint8_t val) {
+	PIOS_Assert(metadata);
 	SET_BITS(metadata->flags, UAVOBJ_TELEMETRY_ACKED_SHIFT, val, 1);
 }
 
@@ -1346,6 +1586,7 @@ void UAVObjSetTelemetryAcked(UAVObjMetadata* metadata, uint8_t val) {
  * \return the telemetry acked boolean
  */
 uint8_t UAVObjGetGcsTelemetryAcked(const UAVObjMetadata* metadata) {
+	PIOS_Assert(metadata);
 	return (metadata->flags >> UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT) & 1;
 }
 
@@ -1355,6 +1596,7 @@ uint8_t UAVObjGetGcsTelemetryAcked(const UAVObjMetadata* metadata) {
  * \param[in] val The GCS telemetry acked boolean
  */
 void UAVObjSetGcsTelemetryAcked(UAVObjMetadata* metadata, uint8_t val) {
+	PIOS_Assert(metadata);
 	SET_BITS(metadata->flags, UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT, val, 1);
 }
 
@@ -1364,6 +1606,7 @@ void UAVObjSetGcsTelemetryAcked(UAVObjMetadata* metadata, uint8_t val) {
  * \return the telemetry update mode
  */
 UAVObjUpdateMode UAVObjGetTelemetryUpdateMode(const UAVObjMetadata* metadata) {
+	PIOS_Assert(metadata);
 	return (metadata->flags >> UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT) & UAVOBJ_UPDATE_MODE_MASK;
 }
 
@@ -1373,6 +1616,7 @@ UAVObjUpdateMode UAVObjGetTelemetryUpdateMode(const UAVObjMetadata* metadata) {
  * \param[in] val The telemetry update mode
  */
 void UAVObjSetTelemetryUpdateMode(UAVObjMetadata* metadata, UAVObjUpdateMode val) {
+	PIOS_Assert(metadata);
 	SET_BITS(metadata->flags, UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT, val, UAVOBJ_UPDATE_MODE_MASK);
 }
 
@@ -1382,6 +1626,7 @@ void UAVObjSetTelemetryUpdateMode(UAVObjMetadata* metadata, UAVObjUpdateMode val
  * \return the GCS telemetry update mode
  */
 UAVObjUpdateMode UAVObjGetGcsTelemetryUpdateMode(const UAVObjMetadata* metadata) {
+	PIOS_Assert(metadata);
 	return (metadata->flags >> UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT) & UAVOBJ_UPDATE_MODE_MASK;
 }
 
@@ -1391,6 +1636,7 @@ UAVObjUpdateMode UAVObjGetGcsTelemetryUpdateMode(const UAVObjMetadata* metadata)
  * \param[in] val The GCS telemetry update mode
  */
 void UAVObjSetGcsTelemetryUpdateMode(UAVObjMetadata* metadata, UAVObjUpdateMode val) {
+	PIOS_Assert(metadata);
 	SET_BITS(metadata->flags, UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT, val, UAVOBJ_UPDATE_MODE_MASK);
 }
 
@@ -1403,22 +1649,13 @@ void UAVObjSetGcsTelemetryUpdateMode(UAVObjMetadata* metadata, UAVObjUpdateMode
  *   \arg 1 if read only
  *   \arg -1 if unable to get meta data
  */
-int8_t UAVObjReadOnly(UAVObjHandle obj)
+int8_t UAVObjReadOnly(UAVObjHandle obj_handle)
 {
-	  ObjectList *objEntry;
+	PIOS_Assert(obj_handle);
-	  UAVObjMetadata *mdata;
+	if (!UAVObjIsMetaobject(obj_handle)) {
-
+		return UAVObjGetAccess(LinkedMetaDataPtr( (struct UAVOData *)obj_handle)) == ACCESS_READONLY;
-	  // Cast to object info
+	}
-	  objEntry = (ObjectList *) obj;
+	return -1;
-
-	  // Check access level
-	  if (!OLGetIsMetaobject(objEntry)) {
-		    mdata =
-			(UAVObjMetadata *) (objEntry->linkedObj->instances.
-					    data);
-		    return UAVObjGetAccess(mdata) == ACCESS_READONLY;
-	  }
-	  return -1;
 }
 
 /**
@@ -1429,14 +1666,16 @@ int8_t UAVObjReadOnly(UAVObjHandle obj)
  * \param[in] eventMask The event mask, if EV_MASK_ALL then all events are enabled (e.g. EV_UPDATED | EV_UPDATED_MANUAL)
  * \return 0 if success or -1 if failure
  */
-int32_t UAVObjConnectQueue(UAVObjHandle obj, xQueueHandle queue,
+int32_t UAVObjConnectQueue(UAVObjHandle obj_handle, xQueueHandle queue,
-			   uint8_t eventMask)
+			uint8_t eventMask)
 {
-	  int32_t res;
+	PIOS_Assert(obj_handle);
-	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
+	PIOS_Assert(queue);
-	  res = connectObj(obj, queue, 0, eventMask);
+	int32_t res;
-	  xSemaphoreGiveRecursive(mutex);
+	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
-	  return res;
+	res = connectObj(obj_handle, queue, 0, eventMask);
+	xSemaphoreGiveRecursive(mutex);
+	return res;
 }
 
 /**
@@ -1445,13 +1684,15 @@ int32_t UAVObjConnectQueue(UAVObjHandle obj, xQueueHandle queue,
  * \param[in] queue The event queue
  * \return 0 if success or -1 if failure
  */
-int32_t UAVObjDisconnectQueue(UAVObjHandle obj, xQueueHandle queue)
+int32_t UAVObjDisconnectQueue(UAVObjHandle obj_handle, xQueueHandle queue)
 {
-	  int32_t res;
+	PIOS_Assert(obj_handle);
-	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
+	PIOS_Assert(queue);
-	  res = disconnectObj(obj, queue, 0);
+	int32_t res;
-	  xSemaphoreGiveRecursive(mutex);
+	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
-	  return res;
+	res = disconnectObj(obj_handle, queue, 0);
+	xSemaphoreGiveRecursive(mutex);
+	return res;
 }
 
 /**
@@ -1462,14 +1703,15 @@ int32_t UAVObjDisconnectQueue(UAVObjHandle obj, xQueueHandle queue)
  * \param[in] eventMask The event mask, if EV_MASK_ALL then all events are enabled (e.g. EV_UPDATED | EV_UPDATED_MANUAL)
  * \return 0 if success or -1 if failure
  */
-int32_t UAVObjConnectCallback(UAVObjHandle obj, UAVObjEventCallback cb,
+int32_t UAVObjConnectCallback(UAVObjHandle obj_handle, UAVObjEventCallback cb,
-			      uint8_t eventMask)
+			uint8_t eventMask)
 {
-	  int32_t res;
+	PIOS_Assert(obj_handle);
-	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
+	int32_t res;
-	  res = connectObj(obj, 0, cb, eventMask);
+	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
-	  xSemaphoreGiveRecursive(mutex);
+	res = connectObj(obj_handle, 0, cb, eventMask);
-	  return res;
+	xSemaphoreGiveRecursive(mutex);
+	return res;
 }
 
 /**
@@ -1478,13 +1720,14 @@ int32_t UAVObjConnectCallback(UAVObjHandle obj, UAVObjEventCallback cb,
  * \param[in] cb The event callback
  * \return 0 if success or -1 if failure
  */
-int32_t UAVObjDisconnectCallback(UAVObjHandle obj, UAVObjEventCallback cb)
+int32_t UAVObjDisconnectCallback(UAVObjHandle obj_handle, UAVObjEventCallback cb)
 {
-	  int32_t res;
+	PIOS_Assert(obj_handle);
-	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
+	int32_t res;
-	  res = disconnectObj(obj, 0, cb);
+	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
-	  xSemaphoreGiveRecursive(mutex);
+	res = disconnectObj(obj_handle, 0, cb);
-	  return res;
+	xSemaphoreGiveRecursive(mutex);
+	return res;
 }
 
 /**
@@ -1492,9 +1735,9 @@ int32_t UAVObjDisconnectCallback(UAVObjHandle obj, UAVObjEventCallback cb)
  * will be generated as soon as the object is updated.
  * \param[in] obj The object handle
  */
-void UAVObjRequestUpdate(UAVObjHandle obj)
+void UAVObjRequestUpdate(UAVObjHandle obj_handle)
 {
-	  UAVObjRequestInstanceUpdate(obj, UAVOBJ_ALL_INSTANCES);
+	UAVObjRequestInstanceUpdate(obj_handle, UAVOBJ_ALL_INSTANCES);
 }
 
 /**
@@ -1503,20 +1746,21 @@ void UAVObjRequestUpdate(UAVObjHandle obj)
  * \param[in] obj The object handle
  * \param[in] instId Object instance ID to update
  */
-void UAVObjRequestInstanceUpdate(UAVObjHandle obj, uint16_t instId)
+void UAVObjRequestInstanceUpdate(UAVObjHandle obj_handle, uint16_t instId)
 {
-	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
+	PIOS_Assert(obj_handle);
-	  sendEvent((ObjectList *) obj, instId, EV_UPDATE_REQ);
+	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
-	  xSemaphoreGiveRecursive(mutex);
+	sendEvent((struct UAVOBase *) obj_handle, instId, EV_UPDATE_REQ);
+	xSemaphoreGiveRecursive(mutex);
 }
 
 /**
  * Send the object's data to the GCS (triggers a EV_UPDATED_MANUAL event on this object).
  * \param[in] obj The object handle
  */
-void UAVObjUpdated(UAVObjHandle obj)
+void UAVObjUpdated(UAVObjHandle obj_handle)
 {
-	  UAVObjInstanceUpdated(obj, UAVOBJ_ALL_INSTANCES);
+	UAVObjInstanceUpdated(obj_handle, UAVOBJ_ALL_INSTANCES);
 }
 
 /**
@@ -1524,11 +1768,12 @@ void UAVObjUpdated(UAVObjHandle obj)
  * \param[in] obj The object handle
  * \param[in] instId The object instance ID
  */
-void UAVObjInstanceUpdated(UAVObjHandle obj, uint16_t instId)
+void UAVObjInstanceUpdated(UAVObjHandle obj_handle, uint16_t instId)
 {
-	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
+	PIOS_Assert(obj_handle);
-	  sendEvent((ObjectList *) obj, instId, EV_UPDATED_MANUAL);
+	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
-	  xSemaphoreGiveRecursive(mutex);
+	sendEvent((struct UAVOBase *) obj_handle, instId, EV_UPDATED_MANUAL);
+	xSemaphoreGiveRecursive(mutex);
 }
 
 /**
@@ -1538,133 +1783,152 @@ void UAVObjInstanceUpdated(UAVObjHandle obj, uint16_t instId)
  */
 void UAVObjIterate(void (*iterator) (UAVObjHandle obj))
 {
-	  ObjectList *objEntry;
+	PIOS_Assert(iterator);
 
-	  // Get lock
+	// Get lock
-	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
+	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
 
-	  // Iterate through the list and invoke iterator for each object
+	// Iterate through the list and invoke iterator for each object
-	  LL_FOREACH(objList, objEntry) {
+	struct UAVOData *obj;
-		    (*iterator) ((UAVObjHandle) objEntry);
+	LL_FOREACH(uavo_list, obj) {
-	  }
+		(*iterator) ((UAVObjHandle) obj);
+		(*iterator) ((UAVObjHandle) &obj->metaObj);
+	}
 
-	  // Release lock
+	// Release lock
-	  xSemaphoreGiveRecursive(mutex);
+	xSemaphoreGiveRecursive(mutex);
 }
 
 /**
- * Send an event to all event queues registered on the object.
+ * Send a triggered event to all event queues registered on the object.
  */
-static int32_t sendEvent(ObjectList * obj, uint16_t instId,
+static int32_t sendEvent(struct UAVOBase * obj, uint16_t instId,
-			 UAVObjEventType event)
+			UAVObjEventType triggered_event)
 {
-	  ObjectEventList *eventEntry;
+	/* Set up the message that will be sent to all registered listeners */
-	  UAVObjEvent msg;
+	UAVObjEvent msg = {
+		.obj    = (UAVObjHandle) obj,
+		.event  = triggered_event,
+		.instId = instId,
+	};
 
-	  // Setup event
+	// Go through each object and push the event message in the queue (if event is activated for the queue)
-	  msg.obj = (UAVObjHandle) obj;
+	struct ObjectEventEntry *event;
-	  msg.event = event;
+	LL_FOREACH(obj->next_event, event) {
-	  msg.instId = instId;
+		if (event->eventMask == 0
+			|| (event->eventMask & triggered_event) != 0) {
+			// Send to queue if a valid queue is registered
+			if (event->queue) {
+				// will not block
+				if (xQueueSend(event->queue, &msg, 0) != pdTRUE) {
+					stats.lastQueueErrorID = UAVObjGetID(obj);
+					++stats.eventQueueErrors;
+				}
+			}
 
-	  // Go through each object and push the event message in the queue (if event is activated for the queue)
+			// Invoke callback (from event task) if a valid one is registered
-	  LL_FOREACH(obj->events, eventEntry) {
+			if (event->cb) {
-		    if (eventEntry->eventMask == 0
+				// invoke callback from the event task, will not block
-			|| (eventEntry->eventMask & event) != 0) {
+				if (EventCallbackDispatch(&msg, event->cb) != pdTRUE) {
-			      // Send to queue if a valid queue is registered
+					++stats.eventCallbackErrors;
-			      if (eventEntry->queue != 0) {
+					stats.lastCallbackErrorID = UAVObjGetID(obj);
-					if (xQueueSend(eventEntry->queue, &msg, 0) != pdTRUE)	// will not block
+				}
-					{
+			}
-						stats.lastQueueErrorID = UAVObjGetID(obj);
+		}
-						  ++stats.eventQueueErrors;
+	}
-					}
-			      }
-			      // Invoke callback (from event task) if a valid one is registered
-			      if (eventEntry->cb != 0) {
-					if (EventCallbackDispatch(&msg, eventEntry->cb) != pdTRUE)	// invoke callback from the event task, will not block
-					{
-						  ++stats.eventCallbackErrors;
-						  stats.lastCallbackErrorID = UAVObjGetID(obj);
-					}
-			      }
-		    }
-	  }
 
-	  // Done
+	return 0;
-	  return 0;
 }
 
 /**
  * Create a new object instance, return the instance info or NULL if failure.
  */
-static ObjectInstList *createInstance(ObjectList * obj, uint16_t instId)
+static InstanceHandle createInstance(struct UAVOData * obj, uint16_t instId)
 {
-	  ObjectInstList *instEntry;
+	struct UAVOMultiInst *instEntry;
-	  int32_t n;
 
-	  // For single instance objects, only instance zero is allowed
+	/* Don't allow more than one instance for single instance objects */
-	  if (OLGetIsSingleInstance(obj) && instId != 0) {
+	if (UAVObjIsSingleInstance(&(obj->base))) {
-		    return NULL;
+		PIOS_Assert(0);
-	  }
+		return NULL;
-	  // Make sure that the instance ID is within limits
+	}
-	  if (instId >= UAVOBJ_MAX_INSTANCES) {
-		    return NULL;
-	  }
-	  // Check if the instance already exists
-	  if (getInstance(obj, instId) != NULL) {
-		    return NULL;
-	  }
-	  // Create any missing instances (all instance IDs must be sequential)
-	  for (n = obj->numInstances; n < instId; ++n) {
-		    if (createInstance(obj, n) == NULL) {
-			      return NULL;
-		    }
-	  }
 
-	  if (instId == 0) {	/* Instance 0 ObjectInstList allocated with ObjectList element */
+	/* Don't create more than the allowed number of instances */
-		    instEntry = &obj->instances;
+	if (instId >= UAVOBJ_MAX_INSTANCES) {
-		    instEntry->data = pvPortMalloc(obj->numBytes);
+		return NULL;
-		    if (instEntry->data == NULL)
+	}
-			      return NULL;
-		    memset(instEntry->data, 0, obj->numBytes);
-		    instEntry->instId = instId;
-	  } else {
-		    // Create the actual instance
-		    instEntry =
-			(ObjectInstList *)
-			pvPortMalloc(sizeof(ObjectInstList));
-		    if (instEntry == NULL)
-			      return NULL;
-		    instEntry->data = pvPortMalloc(obj->numBytes);
-		    if (instEntry->data == NULL)
-			      return NULL;
-		    memset(instEntry->data, 0, obj->numBytes);
-		    instEntry->instId = instId;
-		    LL_APPEND(obj->instances.next, instEntry);
-	  }
-	  ++obj->numInstances;
 
-	  // Fire event
+	/* Don't allow duplicate instances */
-	  UAVObjInstanceUpdated((UAVObjHandle) obj, instId);
+	if (instId < UAVObjGetNumInstances(&(obj->base))) {
+		return NULL;
+	}
 
-	  // Done
+	// Create any missing instances (all instance IDs must be sequential)
-	  return instEntry;
+	for (uint16_t n = UAVObjGetNumInstances(&(obj->base)); n < instId; ++n) {
+		if (createInstance(obj, n) == NULL) {
+			return NULL;
+		}
+	}
+
+	/* Create the actual instance */
+	instEntry = (struct UAVOMultiInst *) pvPortMalloc(sizeof(struct UAVOMultiInst)+obj->instance_size);
+	if (!instEntry)
+		return NULL;
+	memset(InstanceDataOffset(instEntry), 0, obj->instance_size);
+	LL_APPEND(( (struct UAVOMulti*)obj )->instance0.next, instEntry);
+
+	( (struct UAVOMulti*)obj )->num_instances++;
+
+	// Fire event
+	UAVObjInstanceUpdated((UAVObjHandle) obj, instId);
+
+	// Done
+	return InstanceDataOffset(instEntry);
 }
 
 /**
  * Get the instance information or NULL if the instance does not exist
  */
-static ObjectInstList *getInstance(ObjectList * obj, uint16_t instId)
+static InstanceHandle getInstance(struct UAVOData * obj, uint16_t instId)
 {
-	  ObjectInstList *instEntry;
+	if (UAVObjIsMetaobject(&obj->base)) {
+		/* Metadata Instance */
 
-	  // Look for specified instance ID
+		if (instId != 0)
-	  LL_FOREACH(&(obj->instances), instEntry) {
+			return NULL;
-		    if (instEntry->instId == instId) {
+
-			      return instEntry;
+		/* Augment our pointer to reflect the proper type */
-		    }
+		struct UAVOMeta * uavo_meta = (struct UAVOMeta *) obj;
-	  }
+		return (&(uavo_meta->instance0));
-	  // If this point is reached then instance id was not found
+
-	  return NULL;
+	} else if (UAVObjIsSingleInstance(&(obj->base))) {
+		/* Single Instance */
+
+		if (instId != 0)
+			return NULL;
+
+		/* Augment our pointer to reflect the proper type */
+		struct UAVOSingle * uavo_single = (struct UAVOSingle *) obj;
+		return (&(uavo_single->instance0));
+	} else {
+		/* Multi Instance */
+		/* Augment our pointer to reflect the proper type */
+		struct UAVOMulti * uavo_multi = (struct UAVOMulti *) obj;
+		if (instId >= uavo_multi->num_instances)
+			return NULL;
+
+		// Look for specified instance ID
+		uint16_t instance = 0;
+		struct UAVOMultiInst *instEntry;
+		LL_FOREACH(&(uavo_multi->instance0), instEntry) {
+			if (instance++ == instId) {
+				/* Found it */
+				return &(instEntry->instance);
+			}
+		}
+		/* Instance was not found */
+		return NULL;
+	}
 }
 
 /**
@@ -1675,35 +1939,34 @@ static ObjectInstList *getInstance(ObjectList * obj, uint16_t instId)
  * \param[in] eventMask The event mask, if EV_MASK_ALL then all events are enabled (e.g. EV_UPDATED | EV_UPDATED_MANUAL)
  * \return 0 if success or -1 if failure
  */
-static int32_t connectObj(UAVObjHandle obj, xQueueHandle queue,
+static int32_t connectObj(UAVObjHandle obj_handle, xQueueHandle queue,
-			  UAVObjEventCallback cb, uint8_t eventMask)
+			UAVObjEventCallback cb, uint8_t eventMask)
 {
-	  ObjectEventList *eventEntry;
+	struct ObjectEventEntry *event;
-	  ObjectList *objEntry;
+	struct UAVOBase *obj;
 
-	  // Check that the queue is not already connected, if it is simply update event mask
+	// Check that the queue is not already connected, if it is simply update event mask
-	  objEntry = (ObjectList *) obj;
+	obj = (struct UAVOBase *) obj_handle;
-	  LL_FOREACH(objEntry->events, eventEntry) {
+	LL_FOREACH(obj->next_event, event) {
-		    if (eventEntry->queue == queue && eventEntry->cb == cb) {
+		if (event->queue == queue && event->cb == cb) {
-			      // Already connected, update event mask and return
+			// Already connected, update event mask and return
-			      eventEntry->eventMask = eventMask;
+			event->eventMask = eventMask;
-			      return 0;
+			return 0;
-		    }
+		}
-	  }
+	}
 
-	  // Add queue to list
+	// Add queue to list
-	  eventEntry =
+	event =	(struct ObjectEventEntry *) pvPortMalloc(sizeof(struct ObjectEventEntry));
-	      (ObjectEventList *) pvPortMalloc(sizeof(ObjectEventList));
+	if (event == NULL) {
-	  if (eventEntry == NULL) {
+		return -1;
-		    return -1;
+	}
-	  }
+	event->queue = queue;
-	  eventEntry->queue = queue;
+	event->cb = cb;
-	  eventEntry->cb = cb;
+	event->eventMask = eventMask;
-	  eventEntry->eventMask = eventMask;
+	LL_APPEND(obj->next_event, event);
-	  LL_APPEND(objEntry->events, eventEntry);
 
-	  // Done
+	// Done
-	  return 0;
+	return 0;
 }
 
 /**
@@ -1713,25 +1976,25 @@ static int32_t connectObj(UAVObjHandle obj, xQueueHandle queue,
  * \param[in] cb The event callback
  * \return 0 if success or -1 if failure
  */
-static int32_t disconnectObj(UAVObjHandle obj, xQueueHandle queue,
+static int32_t disconnectObj(UAVObjHandle obj_handle, xQueueHandle queue,
-			     UAVObjEventCallback cb)
+			UAVObjEventCallback cb)
 {
-	  ObjectEventList *eventEntry;
+	struct ObjectEventEntry *event;
-	  ObjectList *objEntry;
+	struct UAVOBase *obj;
 
-	  // Find queue and remove it
+	// Find queue and remove it
-	  objEntry = (ObjectList *) obj;
+	obj = (struct UAVOBase *) obj_handle;
-	  LL_FOREACH(objEntry->events, eventEntry) {
+	LL_FOREACH(obj->next_event, event) {
-		    if ((eventEntry->queue == queue
+		if ((event->queue == queue
-			 && eventEntry->cb == cb)) {
+				&& event->cb == cb)) {
-			      LL_DELETE(objEntry->events, eventEntry);
+			LL_DELETE(obj->next_event, event);
-			      vPortFree(eventEntry);
+			vPortFree(event);
-			      return 0;
+			return 0;
-		    }
+		}
-	  }
+	}
 
-	  // If this point is reached the queue was not found
+	// If this point is reached the queue was not found
-	  return -1;
+	return -1;
 }
 
 #if defined(PIOS_INCLUDE_SDCARD)
@@ -1740,16 +2003,16 @@ static int32_t disconnectObj(UAVObjHandle obj, xQueueHandle queue,
  */
 static void customSPrintf(uint8_t * buffer, uint8_t * format, ...)
 {
-	  va_list args;
+	va_list args;
-	  va_start(args, format);
+	va_start(args, format);
-	  vsprintf((char *)buffer, (char *)format, args);
+	vsprintf((char *)buffer, (char *)format, args);
 }
 
 /**
  * Get an 8 character (plus extension) filename for the object.
  */
-static void objectFilename(ObjectList * obj, uint8_t * filename)
+static void objectFilename(UAVObjHandle obj_handle, uint8_t * filename)
 {
-	  customSPrintf(filename, (uint8_t *) "%X.obj", obj->id);
+	customSPrintf(filename, (uint8_t *) "%X.obj", UAVObjGetID(obj_handle));
 }
 #endif /* PIOS_INCLUDE_SDCARD */

flight/UAVObjects/uavobjecttemplate.c

@@ -54,7 +54,7 @@ int32_t $(NAME)Initialize(void)
 		return -2;
 	
 	// Register object with the object manager
-	handle = UAVObjRegister($(NAMEUC)_OBJID, $(NAMEUC)_NAME, $(NAMEUC)_METANAME, 0,
+	handle = UAVObjRegister($(NAMEUC)_OBJID,
 			$(NAMEUC)_ISSINGLEINST, $(NAMEUC)_ISSETTINGS, $(NAMEUC)_NUMBYTES, &$(NAME)SetDefaults);
 
 	// Done

ground/openpilotgcs/src/plugins/config/airframe.ui

@@ -2788,7 +2788,7 @@ p, li { white-space: pre-wrap; }
 <table border="0" style="-qt-table-type: root; margin-top:4px; margin-bottom:4px; margin-left:4px; margin-right:4px;">
 <tr>
 <td style="border: none;">
-<p align="center" style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:14pt; font-weight:600; color:#ff0000;">SETTING UP FEED FORWARD IS DANGEROUS</span></p>
+<p align="center" style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:14pt; font-weight:600; color:#ff0000;">SETTING UP FEED FORWARD NEEDS CAUTION</span></p>
 <p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu'; font-size:11pt;"></p>
 <p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Beware: Feed Forward Tuning will launch all engines around mid-throttle, you have been warned!</p>
 &lt;p style=&quot; margin-top:12px; margin-bottom:12px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;Remove your props initially, and for fine-tuning, make sure your airframe is safely held in place. Wear glasses and protect your face and body.&lt;/p&gt;&lt;/td&gt;&lt;/tr&gt;&lt;/table&gt;&lt;/body&gt;&lt;/html&gt;</string>

ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.cpp

@@ -274,6 +274,8 @@ QStringList ConfigMultiRotorWidget::getChannelDescriptions()
         channelDesc[multi.VTOLMotorW-1] = QString("VTOLMotorW");
     if (multi.VTOLMotorE > 0 && multi.VTOLMotorE < ConfigMultiRotorWidget::CHANNEL_NUMELEM)
         channelDesc[multi.VTOLMotorE-1] = QString("VTOLMotorE");
+    if (multi.TRIYaw > 0 && multi.TRIYaw <= ConfigMultiRotorWidget::CHANNEL_NUMELEM)
+        channelDesc[multi.TRIYaw-1] = QString("Tri-Yaw");
 
     return channelDesc;
 }

ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp

@@ -71,6 +71,8 @@ ConfigPipXtremeWidget::ConfigPipXtremeWidget(QWidget *parent) : ConfigTaskWidget
 	addUAVObjectToWidgetRelation("PipXStatus", "MinFrequency", m_pipx->MinFrequency);
 	addUAVObjectToWidgetRelation("PipXStatus", "MaxFrequency", m_pipx->MaxFrequency);
 	addUAVObjectToWidgetRelation("PipXStatus", "FrequencyStepSize", m_pipx->FrequencyStepSize);
+	addUAVObjectToWidgetRelation("PipXStatus", "FrequencyBand", m_pipx->FreqBand);
+	addUAVObjectToWidgetRelation("PipXStatus", "RSSI", m_pipx->RSSI);
 	addUAVObjectToWidgetRelation("PipXStatus", "AFC", m_pipx->RxAFC);
 	addUAVObjectToWidgetRelation("PipXStatus", "Retries", m_pipx->Retries);
 	addUAVObjectToWidgetRelation("PipXStatus", "Errors", m_pipx->Errors);
@@ -81,6 +83,7 @@ ConfigPipXtremeWidget::ConfigPipXtremeWidget(QWidget *parent) : ConfigTaskWidget
 	addUAVObjectToWidgetRelation("PipXStatus", "TXRate", m_pipx->TXRate);
 
 	// Connect to the pair ID radio buttons.
+	connect(m_pipx->PairSelectB, SIGNAL(toggled(bool)), this, SLOT(pairBToggled(bool)));
 	connect(m_pipx->PairSelect1, SIGNAL(toggled(bool)), this, SLOT(pair1Toggled(bool)));
 	connect(m_pipx->PairSelect2, SIGNAL(toggled(bool)), this, SLOT(pair2Toggled(bool)));
 	connect(m_pipx->PairSelect3, SIGNAL(toggled(bool)), this, SLOT(pair3Toggled(bool)));
@@ -89,6 +92,7 @@ ConfigPipXtremeWidget::ConfigPipXtremeWidget(QWidget *parent) : ConfigTaskWidget
 	//Add scroll bar when necessary
 	QScrollArea *scroll = new QScrollArea;
 	scroll->setWidget(m_pipx->frame_3);
+	scroll->setWidgetResizable(true);
 	m_pipx->verticalLayout_3->addWidget(scroll);
 
 	// Request and update of the setting object.
@@ -148,7 +152,7 @@ void ConfigPipXtremeWidget::updateStatus(UAVObject *object)
 	PipXSettings *pipxSettings = PipXSettings::GetInstance(getObjectManager());
 	quint32 pairID = 0;
 	if (pipxSettings)
-		pipxSettings->getPairID();
+		pairID = pipxSettings->getPairID();
 
 	// Update the detected devices.
 	UAVObjectField* pairIdField = object->getField("PairIDs");
@@ -245,6 +249,9 @@ void ConfigPipXtremeWidget::updateStatus(UAVObject *object)
 		qDebug() << "PipXtremeGadgetWidget: Count not read DeviceID field.";
 	}
 
+	// Update the PairID field
+	m_pipx->PairID->setText(QString::number(pairID, 16).toUpper());
+
  	// Update the link state
  	UAVObjectField* linkField = object->getField("LinkState");
  	if (linkField) {
@@ -284,9 +291,16 @@ void ConfigPipXtremeWidget::pairIDToggled(bool checked, quint8 idx)
 
 		if (pipxStatus && pipxSettings)
 		{
-			quint32 pairID = pipxStatus->getPairIDs(idx);
+			if (idx == 4)
-			if (pairID)
+			{
-				pipxSettings->setPairID(pairID);
+				pipxSettings->setPairID(0);
+			}
+			else
+			{
+				quint32 pairID = pipxStatus->getPairIDs(idx);
+				if (pairID)
+					pipxSettings->setPairID(pairID);
+			}
 		}
 	}
 }
@@ -311,6 +325,11 @@ void ConfigPipXtremeWidget::pair4Toggled(bool checked)
 	pairIDToggled(checked, 3);
 }
 
+void ConfigPipXtremeWidget::pairBToggled(bool checked)
+{
+	pairIDToggled(checked, 4);
+}
+
 /**
    @}
    @}

ground/openpilotgcs/src/plugins/config/configpipxtremewidget.h

@@ -63,6 +63,7 @@ private slots:
 	void pair2Toggled(bool checked);
 	void pair3Toggled(bool checked);
 	void pair4Toggled(bool checked);
+	void pairBToggled(bool checked);
 };
 
 #endif // CONFIGTXPIDWIDGET_H

ground/openpilotgcs/src/plugins/config/input.ui

@@ -17,7 +17,7 @@
    <item>
     <widget class="QTabWidget" name="tabWidget">
      <property name="currentIndex">
-      <number>1</number>
+      <number>0</number>
      </property>
      <widget class="QWidget" name="RCInput">
       <attribute name="title">

ground/openpilotgcs/src/plugins/config/pipxtreme.ui

@@ -6,8 +6,8 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>840</width>
+    <width>834</width>
-    <height>834</height>
+    <height>772</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -81,7 +81,7 @@
             <property name="sizeHint" stdset="0">
              <size>
               <width>40</width>
-              <height>20</height>
+              <height>5</height>
              </size>
             </property>
            </spacer>
@@ -135,16 +135,56 @@
            </property>
            <layout class="QGridLayout" name="gridLayout_3">
             <item row="0" column="0">
+             <widget class="QRadioButton" name="PairSelectB">
+              <property name="text">
+               <string/>
+              </property>
+             </widget>
+            </item>
+            <item row="0" column="1">
+             <widget class="QLineEdit" name="PairIDB">
+              <property name="enabled">
+               <bool>false</bool>
+              </property>
+              <property name="maximumSize">
+               <size>
+                <width>100</width>
+                <height>16777215</height>
+               </size>
+              </property>
+              <property name="text">
+               <string>Broadcast</string>
+              </property>
+             </widget>
+            </item>
+            <item row="0" column="2">
+             <widget class="QLabel" name="PaidIDBLabel">
+              <property name="text">
+               <string>Broadcast Address</string>
+              </property>
+             </widget>
+            </item>
+            <item row="1" column="0">
              <widget class="QRadioButton" name="PairSelect1">
               <property name="text">
                <string/>
               </property>
              </widget>
             </item>
-            <item row="0" column="1" colspan="2">
+            <item row="1" column="1">
-             <widget class="QLineEdit" name="PairID1"/>
+             <widget class="QLineEdit" name="PairID1">
+              <property name="maximumSize">
+               <size>
+                <width>100</width>
+                <height>16777215</height>
+               </size>
+              </property>
+              <property name="placeholderText">
+               <string>12345678</string>
+              </property>
+             </widget>
             </item>
-            <item row="0" column="3">
+            <item row="1" column="2">
              <widget class="QProgressBar" name="PairSignalStrengthBar1">
               <property name="minimum">
                <number>-127</number>
@@ -163,17 +203,31 @@
               </property>
              </widget>
             </item>
-            <item row="1" column="0">
+            <item row="1" column="3">
+             <widget class="QLabel" name="PairSignalStrengthLabel1">
+              <property name="text">
+               <string>-100dB</string>
+              </property>
+             </widget>
+            </item>
+            <item row="2" column="0">
              <widget class="QRadioButton" name="PairSelect2">
               <property name="text">
                <string/>
               </property>
              </widget>
             </item>
-            <item row="1" column="1" colspan="2">
+            <item row="2" column="1">
-             <widget class="QLineEdit" name="PairID2"/>
+             <widget class="QLineEdit" name="PairID2">
+              <property name="maximumSize">
+               <size>
+                <width>100</width>
+                <height>16777215</height>
+               </size>
+              </property>
+             </widget>
             </item>
-            <item row="1" column="3">
+            <item row="2" column="2">
              <widget class="QProgressBar" name="PairSignalStrengthBar2">
               <property name="minimum">
                <number>-127</number>
@@ -192,17 +246,31 @@
               </property>
              </widget>
             </item>
-            <item row="2" column="0">
+            <item row="2" column="3">
+             <widget class="QLabel" name="PairSignalStrengthLabel2">
+              <property name="text">
+               <string>-100dB</string>
+              </property>
+             </widget>
+            </item>
+            <item row="3" column="0">
              <widget class="QRadioButton" name="PairSelect3">
               <property name="text">
                <string/>
               </property>
              </widget>
             </item>
-            <item row="2" column="1" colspan="2">
+            <item row="3" column="1">
-             <widget class="QLineEdit" name="PairID3"/>
+             <widget class="QLineEdit" name="PairID3">
+              <property name="maximumSize">
+               <size>
+                <width>100</width>
+                <height>16777215</height>
+               </size>
+              </property>
+             </widget>
             </item>
-            <item row="2" column="3">
+            <item row="3" column="2">
              <widget class="QProgressBar" name="PairSignalStrengthBar3">
               <property name="minimum">
                <number>-127</number>
@@ -221,17 +289,31 @@
               </property>
              </widget>
             </item>
-            <item row="3" column="0">
+            <item row="3" column="3">
+             <widget class="QLabel" name="PairSignalStrengthLabel3">
+              <property name="text">
+               <string>-100dB</string>
+              </property>
+             </widget>
+            </item>
+            <item row="4" column="0">
              <widget class="QRadioButton" name="PairSelect4">
               <property name="text">
                <string/>
               </property>
              </widget>
             </item>
-            <item row="3" column="1" colspan="2">
+            <item row="4" column="1">
-             <widget class="QLineEdit" name="PairID4"/>
+             <widget class="QLineEdit" name="PairID4">
+              <property name="maximumSize">
+               <size>
+                <width>100</width>
+                <height>16777215</height>
+               </size>
+              </property>
+             </widget>
             </item>
-            <item row="3" column="3">
+            <item row="4" column="2">
              <widget class="QProgressBar" name="PairSignalStrengthBar4">
               <property name="minimum">
                <number>-127</number>
@@ -250,28 +332,7 @@
               </property>
              </widget>
             </item>
-            <item row="0" column="4">
+            <item row="4" column="3">
-             <widget class="QLabel" name="PairSignalStrengthLabel1">
-              <property name="text">
-               <string>-100dB</string>
-              </property>
-             </widget>
-            </item>
-            <item row="1" column="4">
-             <widget class="QLabel" name="PairSignalStrengthLabel2">
-              <property name="text">
-               <string>-100dB</string>
-              </property>
-             </widget>
-            </item>
-            <item row="2" column="4">
-             <widget class="QLabel" name="PairSignalStrengthLabel3">
-              <property name="text">
-               <string>-100dB</string>
-              </property>
-             </widget>
-            </item>
-            <item row="3" column="4">
              <widget class="QLabel" name="PairSignalStrengthLabel4">
               <property name="text">
                <string>-100dB</string>
@@ -285,7 +346,7 @@
           <widget class="QGroupBox" name="groupBox_3">
            <property name="minimumSize">
             <size>
-             <width>400</width>
+             <width>430</width>
              <height>0</height>
             </size>
            </property>
@@ -302,15 +363,27 @@
             <item row="0" column="0">
              <widget class="QLabel" name="label_9">
               <property name="text">
-               <string>Firmware Version</string>
+               <string>Firmware Ver.</string>
               </property>
               <property name="alignment">
                <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
               </property>
              </widget>
             </item>
-            <item row="0" column="1">
+            <item row="0" column="1" colspan="3">
              <widget class="QLineEdit" name="FirmwareVersion">
+              <property name="minimumSize">
+               <size>
+                <width>0</width>
+                <height>0</height>
+               </size>
+              </property>
+              <property name="maximumSize">
+               <size>
+                <width>16777215</width>
+                <height>16777215</height>
+               </size>
+              </property>
               <property name="font">
                <font>
                 <weight>75</weight>
@@ -321,7 +394,7 @@
                <string notr="true">QLineEdit {
   border: none;
   border-radius: 1px;
-  padding: 0 8px;
+  padding: 0 4px;
   background: rgba(0, 0, 0, 16);
 /*  background: transparent; */
 /*     selection-background-color: darkgray;*/
@@ -345,7 +418,7 @@
               </property>
              </widget>
             </item>
-            <item row="2" column="1">
+            <item row="2" column="1" colspan="3">
              <widget class="QLineEdit" name="SerialNumber">
               <property name="sizePolicy">
                <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
@@ -353,6 +426,12 @@
                 <verstretch>0</verstretch>
                </sizepolicy>
               </property>
+              <property name="minimumSize">
+               <size>
+                <width>0</width>
+                <height>0</height>
+               </size>
+              </property>
               <property name="maximumSize">
                <size>
                 <width>16777215</width>
@@ -376,7 +455,7 @@
                <string notr="true">QLineEdit {
   border: none;
   border-radius: 1px;
-  padding: 0 8px;
+  padding: 0 4px;
   background: rgba(0, 0, 0, 16);
 /*  background: transparent; */
 /*     selection-background-color: darkgray;*/
@@ -393,6 +472,108 @@
               </property>
              </widget>
             </item>
+            <item row="3" column="0">
+             <widget class="QLabel" name="DeviceIDLabel">
+              <property name="text">
+               <string>Device ID</string>
+              </property>
+              <property name="alignment">
+               <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+              </property>
+             </widget>
+            </item>
+            <item row="3" column="1">
+             <widget class="QLineEdit" name="DeviceID">
+              <property name="minimumSize">
+               <size>
+                <width>0</width>
+                <height>0</height>
+               </size>
+              </property>
+              <property name="maximumSize">
+               <size>
+                <width>101</width>
+                <height>16777215</height>
+               </size>
+              </property>
+              <property name="font">
+               <font>
+                <weight>75</weight>
+                <bold>true</bold>
+               </font>
+              </property>
+              <property name="styleSheet">
+               <string notr="true">QLineEdit {
+  border: none;
+  border-radius: 1px;
+  padding: 0 4px;
+  background: rgba(0, 0, 0, 16);
+/*  background: transparent; */
+/*     selection-background-color: darkgray;*/
+}</string>
+              </property>
+              <property name="frame">
+               <bool>false</bool>
+              </property>
+              <property name="alignment">
+               <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+              </property>
+              <property name="readOnly">
+               <bool>true</bool>
+              </property>
+              <property name="placeholderText">
+               <string>12345678</string>
+              </property>
+             </widget>
+            </item>
+            <item row="3" column="2">
+             <widget class="QLabel" name="PairIDLabel">
+              <property name="text">
+               <string>Pair ID</string>
+              </property>
+              <property name="alignment">
+               <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+              </property>
+             </widget>
+            </item>
+            <item row="3" column="3">
+             <widget class="QLineEdit" name="PairID">
+              <property name="maximumSize">
+               <size>
+                <width>101</width>
+                <height>16777215</height>
+               </size>
+              </property>
+              <property name="font">
+               <font>
+                <weight>75</weight>
+                <bold>true</bold>
+               </font>
+              </property>
+              <property name="styleSheet">
+               <string notr="true">QLineEdit {
+  border: none;
+  border-radius: 1px;
+  padding: 0 4px;
+  background: rgba(0, 0, 0, 16);
+/*  background: transparent; */
+/*     selection-background-color: darkgray;*/
+}</string>
+              </property>
+              <property name="frame">
+               <bool>false</bool>
+              </property>
+              <property name="alignment">
+               <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+              </property>
+              <property name="readOnly">
+               <bool>true</bool>
+              </property>
+              <property name="placeholderText">
+               <string>90ABCDEF</string>
+              </property>
+             </widget>
+            </item>
             <item row="4" column="0">
              <widget class="QLabel" name="label_3">
               <property name="text">
@@ -411,9 +592,15 @@
                 <verstretch>0</verstretch>
                </sizepolicy>
               </property>
+              <property name="minimumSize">
+               <size>
+                <width>0</width>
+                <height>0</height>
+               </size>
+              </property>
               <property name="maximumSize">
                <size>
-                <width>16777215</width>
+                <width>101</width>
                 <height>16777215</height>
                </size>
               </property>
@@ -430,7 +617,7 @@
                <string notr="true">QLineEdit {
   border: none;
   border-radius: 1px;
-  padding: 0 8px;
+  padding: 0 4px;
   background: rgba(0, 0, 0, 16);
 /*  background: transparent; */
 /*     selection-background-color: darkgray;*/
@@ -444,7 +631,7 @@
               </property>
              </widget>
             </item>
-            <item row="5" column="0">
+            <item row="4" column="2">
              <widget class="QLabel" name="label_4">
               <property name="text">
                <string>Max Frequency</string>
@@ -454,7 +641,7 @@
               </property>
              </widget>
             </item>
-            <item row="5" column="1">
+            <item row="4" column="3">
              <widget class="QLineEdit" name="MaxFrequency">
               <property name="sizePolicy">
                <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
@@ -464,7 +651,7 @@
               </property>
               <property name="maximumSize">
                <size>
-                <width>16777215</width>
+                <width>101</width>
                 <height>16777215</height>
                </size>
               </property>
@@ -481,7 +668,7 @@
                <string notr="true">QLineEdit {
   border: none;
   border-radius: 1px;
-  padding: 0 8px;
+  padding: 0 4px;
   background: rgba(0, 0, 0, 16);
 /*  background: transparent; */
 /*     selection-background-color: darkgray;*/
@@ -495,7 +682,7 @@
               </property>
              </widget>
             </item>
-            <item row="6" column="0">
+            <item row="5" column="0">
              <widget class="QLabel" name="label_12">
               <property name="text">
                <string>Freq. Step Size</string>
@@ -505,7 +692,7 @@
               </property>
              </widget>
             </item>
-            <item row="6" column="1">
+            <item row="5" column="1">
              <widget class="QLineEdit" name="FrequencyStepSize">
               <property name="sizePolicy">
                <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
@@ -513,9 +700,15 @@
                 <verstretch>0</verstretch>
                </sizepolicy>
               </property>
+              <property name="minimumSize">
+               <size>
+                <width>0</width>
+                <height>0</height>
+               </size>
+              </property>
               <property name="maximumSize">
                <size>
-                <width>16777215</width>
+                <width>101</width>
                 <height>16777215</height>
                </size>
               </property>
@@ -532,7 +725,7 @@
                <string notr="true">QLineEdit {
   border: none;
   border-radius: 1px;
-  padding: 0 8px;
+  padding: 0 4px;
   background: rgba(0, 0, 0, 16);
 /*  background: transparent; */
 /*     selection-background-color: darkgray;*/
@@ -546,7 +739,232 @@
               </property>
              </widget>
             </item>
+            <item row="5" column="2">
+             <widget class="QLabel" name="FreqBandLabel">
+              <property name="text">
+               <string>Freq. Band</string>
+              </property>
+              <property name="alignment">
+               <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+              </property>
+             </widget>
+            </item>
+            <item row="5" column="3">
+             <widget class="QLineEdit" name="FreqBand">
+              <property name="sizePolicy">
+               <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
+                <horstretch>0</horstretch>
+                <verstretch>0</verstretch>
+               </sizepolicy>
+              </property>
+              <property name="maximumSize">
+               <size>
+                <width>101</width>
+                <height>16777215</height>
+               </size>
+              </property>
+              <property name="font">
+               <font>
+                <weight>75</weight>
+                <bold>true</bold>
+               </font>
+              </property>
+              <property name="toolTip">
+               <string>The current frequency band</string>
+              </property>
+              <property name="styleSheet">
+               <string notr="true">QLineEdit {
+  border: none;
+  border-radius: 1px;
+  padding: 0 4px;
+  background: rgba(0, 0, 0, 16);
+/*  background: transparent; */
+/*     selection-background-color: darkgray;*/
+}</string>
+              </property>
+              <property name="alignment">
+               <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+              </property>
+              <property name="readOnly">
+               <bool>true</bool>
+              </property>
+             </widget>
+            </item>
+            <item row="6" column="0">
+             <widget class="QLabel" name="RSSILabel">
+              <property name="text">
+               <string>RSSI</string>
+              </property>
+              <property name="alignment">
+               <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+              </property>
+             </widget>
+            </item>
+            <item row="6" column="1">
+             <widget class="QLineEdit" name="RSSI">
+              <property name="minimumSize">
+               <size>
+                <width>0</width>
+                <height>0</height>
+               </size>
+              </property>
+              <property name="maximumSize">
+               <size>
+                <width>101</width>
+                <height>16777215</height>
+               </size>
+              </property>
+              <property name="font">
+               <font>
+                <weight>75</weight>
+                <bold>true</bold>
+               </font>
+              </property>
+              <property name="styleSheet">
+               <string notr="true">QLineEdit {
+  border: none;
+  border-radius: 1px;
+  padding: 0 4px;
+  background: rgba(0, 0, 0, 16);
+/*  background: transparent; */
+/*     selection-background-color: darkgray;*/
+}</string>
+              </property>
+              <property name="readOnly">
+               <bool>true</bool>
+              </property>
+             </widget>
+            </item>
+            <item row="6" column="2">
+             <widget class="QLabel" name="RxAFCLabel">
+              <property name="text">
+               <string>Rx AFC</string>
+              </property>
+              <property name="alignment">
+               <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+              </property>
+             </widget>
+            </item>
+            <item row="6" column="3">
+             <widget class="QLineEdit" name="RxAFC">
+              <property name="maximumSize">
+               <size>
+                <width>101</width>
+                <height>16777215</height>
+               </size>
+              </property>
+              <property name="font">
+               <font>
+                <weight>75</weight>
+                <bold>true</bold>
+               </font>
+              </property>
+              <property name="styleSheet">
+               <string notr="true">QLineEdit {
+  border: none;
+  border-radius: 1px;
+  padding: 0 4px;
+  background: rgba(0, 0, 0, 16);
+/*  background: transparent; */
+/*     selection-background-color: darkgray;*/
+}</string>
+              </property>
+              <property name="readOnly">
+               <bool>true</bool>
+              </property>
+             </widget>
+            </item>
             <item row="7" column="0">
+             <widget class="QLabel" name="TXRateLabel">
+              <property name="text">
+               <string>TX Rate (B/s)</string>
+              </property>
+              <property name="alignment">
+               <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+              </property>
+             </widget>
+            </item>
+            <item row="7" column="1">
+             <widget class="QLineEdit" name="TXRate">
+              <property name="minimumSize">
+               <size>
+                <width>0</width>
+                <height>0</height>
+               </size>
+              </property>
+              <property name="maximumSize">
+               <size>
+                <width>101</width>
+                <height>16777215</height>
+               </size>
+              </property>
+              <property name="font">
+               <font>
+                <weight>75</weight>
+                <bold>true</bold>
+               </font>
+              </property>
+              <property name="styleSheet">
+               <string notr="true">QLineEdit {
+  border: none;
+  border-radius: 1px;
+  padding: 0 4px;
+  background: rgba(0, 0, 0, 16);
+/*  background: transparent; */
+/*     selection-background-color: darkgray;*/
+}</string>
+              </property>
+              <property name="frame">
+               <bool>false</bool>
+              </property>
+              <property name="readOnly">
+               <bool>true</bool>
+              </property>
+             </widget>
+            </item>
+            <item row="7" column="2">
+             <widget class="QLabel" name="RXRateLabel">
+              <property name="text">
+               <string>RX Rate (B/s)</string>
+              </property>
+              <property name="alignment">
+               <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+              </property>
+             </widget>
+            </item>
+            <item row="7" column="3">
+             <widget class="QLineEdit" name="RXRate">
+              <property name="maximumSize">
+               <size>
+                <width>101</width>
+                <height>16777215</height>
+               </size>
+              </property>
+              <property name="font">
+               <font>
+                <weight>75</weight>
+                <bold>true</bold>
+               </font>
+              </property>
+              <property name="styleSheet">
+               <string notr="true">QLineEdit {
+  border: none;
+  border-radius: 1px;
+  padding: 0 4px;
+  background: rgba(0, 0, 0, 16);
+/*  background: transparent; */
+/*     selection-background-color: darkgray;*/
+}</string>
+              </property>
+              <property name="frame">
+               <bool>false</bool>
+              </property>
+              <property name="readOnly">
+               <bool>true</bool>
+              </property>
+             </widget>
+            </item>
+            <item row="8" column="0">
              <widget class="QLabel" name="label_11">
               <property name="text">
                <string>Link State</string>
@@ -556,17 +974,23 @@
               </property>
              </widget>
             </item>
-            <item row="7" column="1">
+            <item row="8" column="1">
              <widget class="QLineEdit" name="LinkState">
               <property name="sizePolicy">
-               <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
+               <sizepolicy hsizetype="Expanding" vsizetype="Fixed">
                 <horstretch>0</horstretch>
                 <verstretch>0</verstretch>
                </sizepolicy>
               </property>
+              <property name="minimumSize">
+               <size>
+                <width>0</width>
+                <height>0</height>
+               </size>
+              </property>
               <property name="maximumSize">
                <size>
-                <width>16777215</width>
+                <width>101</width>
                 <height>16777215</height>
                </size>
               </property>
@@ -583,7 +1007,7 @@
                <string notr="true">QLineEdit {
   border: none;
   border-radius: 1px;
-  padding: 0 8px;
+  padding: 0 3px;
   background: rgba(0, 0, 0, 16);
 /*  background: transparent; */
 /*     selection-background-color: darkgray;*/
@@ -595,20 +1019,29 @@
               <property name="readOnly">
                <bool>true</bool>
               </property>
+              <property name="placeholderText">
+               <string>Disconnected</string>
+              </property>
              </widget>
             </item>
-            <item row="8" column="0">
+            <item row="8" column="2">
-             <widget class="QLabel" name="label_17">
+             <widget class="QLabel" name="label_19">
               <property name="text">
-               <string>Rx AFC</string>
+               <string>Errors</string>
               </property>
               <property name="alignment">
                <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
               </property>
              </widget>
             </item>
-            <item row="8" column="1">
+            <item row="8" column="3">
-             <widget class="QLineEdit" name="RxAFC">
+             <widget class="QLineEdit" name="Errors">
+              <property name="maximumSize">
+               <size>
+                <width>101</width>
+                <height>16777215</height>
+               </size>
+              </property>
               <property name="font">
                <font>
                 <weight>75</weight>
@@ -619,12 +1052,15 @@
                <string notr="true">QLineEdit {
   border: none;
   border-radius: 1px;
-  padding: 0 8px;
+  padding: 0 4px;
   background: rgba(0, 0, 0, 16);
 /*  background: transparent; */
 /*     selection-background-color: darkgray;*/
 }</string>
               </property>
+              <property name="frame">
+               <bool>false</bool>
+              </property>
               <property name="readOnly">
                <bool>true</bool>
               </property>
@@ -642,6 +1078,18 @@
             </item>
             <item row="9" column="1">
              <widget class="QLineEdit" name="Retries">
+              <property name="minimumSize">
+               <size>
+                <width>0</width>
+                <height>0</height>
+               </size>
+              </property>
+              <property name="maximumSize">
+               <size>
+                <width>101</width>
+                <height>16777215</height>
+               </size>
+              </property>
               <property name="font">
                <font>
                 <weight>75</weight>
@@ -652,7 +1100,7 @@
                <string notr="true">QLineEdit {
   border: none;
   border-radius: 1px;
-  padding: 0 8px;
+  padding: 0 4px;
   background: rgba(0, 0, 0, 16);
 /*  background: transparent; */
 /*     selection-background-color: darkgray;*/
@@ -666,8 +1114,24 @@
               </property>
              </widget>
             </item>
-            <item row="10" column="1">
+            <item row="9" column="2">
-             <widget class="QLineEdit" name="Errors">
+             <widget class="QLabel" name="UAVTalkErrorsLabel">
+              <property name="text">
+               <string>UAVTalk Errors</string>
+              </property>
+              <property name="alignment">
+               <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+              </property>
+             </widget>
+            </item>
+            <item row="9" column="3">
+             <widget class="QLineEdit" name="UAVTalkErrors">
+              <property name="maximumSize">
+               <size>
+                <width>101</width>
+                <height>16777215</height>
+               </size>
+              </property>
               <property name="font">
                <font>
                 <weight>75</weight>
@@ -678,7 +1142,7 @@
                <string notr="true">QLineEdit {
   border: none;
   border-radius: 1px;
-  padding: 0 8px;
+  padding: 0 4px;
   background: rgba(0, 0, 0, 16);
 /*  background: transparent; */
 /*     selection-background-color: darkgray;*/
@@ -693,52 +1157,6 @@
              </widget>
             </item>
             <item row="10" column="0">
-             <widget class="QLabel" name="label_19">
-              <property name="text">
-               <string>Errors</string>
-              </property>
-              <property name="alignment">
-               <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
-              </property>
-             </widget>
-            </item>
-            <item row="11" column="0">
-             <widget class="QLabel" name="UAVTalkErrorsLabel">
-              <property name="text">
-               <string>UAVTalk Errors</string>
-              </property>
-              <property name="alignment">
-               <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
-              </property>
-             </widget>
-            </item>
-            <item row="11" column="1">
-             <widget class="QLineEdit" name="UAVTalkErrors">
-              <property name="font">
-               <font>
-                <weight>75</weight>
-                <bold>true</bold>
-               </font>
-              </property>
-              <property name="styleSheet">
-               <string notr="true">QLineEdit {
-  border: none;
-  border-radius: 1px;
-  padding: 0 8px;
-  background: rgba(0, 0, 0, 16);
-/*  background: transparent; */
-/*     selection-background-color: darkgray;*/
-}</string>
-              </property>
-              <property name="frame">
-               <bool>false</bool>
-              </property>
-              <property name="readOnly">
-               <bool>true</bool>
-              </property>
-             </widget>
-            </item>
-            <item row="12" column="0">
              <widget class="QLabel" name="ResetsLabel">
               <property name="text">
                <string>Resets</string>
@@ -748,8 +1166,20 @@
               </property>
              </widget>
             </item>
-            <item row="12" column="1">
+            <item row="10" column="1">
              <widget class="QLineEdit" name="Resets">
+              <property name="minimumSize">
+               <size>
+                <width>0</width>
+                <height>0</height>
+               </size>
+              </property>
+              <property name="maximumSize">
+               <size>
+                <width>101</width>
+                <height>16777215</height>
+               </size>
+              </property>
               <property name="font">
                <font>
                 <weight>75</weight>
@@ -760,7 +1190,7 @@
                <string notr="true">QLineEdit {
   border: none;
   border-radius: 1px;
-  padding: 0 8px;
+  padding: 0 4px;
   background: rgba(0, 0, 0, 16);
 /*  background: transparent; */
 /*     selection-background-color: darkgray;*/
@@ -774,7 +1204,7 @@
               </property>
              </widget>
             </item>
-            <item row="13" column="0">
+            <item row="10" column="2">
              <widget class="QLabel" name="DroppedLabel">
               <property name="text">
                <string>Dropped</string>
@@ -784,8 +1214,14 @@
               </property>
              </widget>
             </item>
-            <item row="13" column="1">
+            <item row="10" column="3">
              <widget class="QLineEdit" name="Dropped">
+              <property name="maximumSize">
+               <size>
+                <width>101</width>
+                <height>16777215</height>
+               </size>
+              </property>
               <property name="font">
                <font>
                 <weight>75</weight>
@@ -796,7 +1232,7 @@
                <string notr="true">QLineEdit {
   border: none;
   border-radius: 1px;
-  padding: 0 8px;
+  padding: 0 4px;
   background: rgba(0, 0, 0, 16);
 /*  background: transparent; */
 /*     selection-background-color: darkgray;*/
@@ -810,114 +1246,6 @@
               </property>
              </widget>
             </item>
-            <item row="14" column="0">
-             <widget class="QLabel" name="TXRateLabel">
-              <property name="text">
-               <string>TX Rate (B/s)</string>
-              </property>
-              <property name="alignment">
-               <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
-              </property>
-             </widget>
-            </item>
-            <item row="14" column="1">
-             <widget class="QLineEdit" name="TXRate">
-              <property name="font">
-               <font>
-                <weight>75</weight>
-                <bold>true</bold>
-               </font>
-              </property>
-              <property name="styleSheet">
-               <string notr="true">QLineEdit {
-  border: none;
-  border-radius: 1px;
-  padding: 0 8px;
-  background: rgba(0, 0, 0, 16);
-/*  background: transparent; */
-/*     selection-background-color: darkgray;*/
-}</string>
-              </property>
-              <property name="frame">
-               <bool>false</bool>
-              </property>
-              <property name="readOnly">
-               <bool>true</bool>
-              </property>
-             </widget>
-            </item>
-            <item row="15" column="0">
-             <widget class="QLabel" name="RXRateLabel">
-              <property name="text">
-               <string>RX Rate (B/s)</string>
-              </property>
-              <property name="alignment">
-               <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
-              </property>
-             </widget>
-            </item>
-            <item row="15" column="1">
-             <widget class="QLineEdit" name="RXRate">
-              <property name="font">
-               <font>
-                <weight>75</weight>
-                <bold>true</bold>
-               </font>
-              </property>
-              <property name="styleSheet">
-               <string notr="true">QLineEdit {
-  border: none;
-  border-radius: 1px;
-  padding: 0 8px;
-  background: rgba(0, 0, 0, 16);
-/*  background: transparent; */
-/*     selection-background-color: darkgray;*/
-}</string>
-              </property>
-              <property name="frame">
-               <bool>false</bool>
-              </property>
-              <property name="readOnly">
-               <bool>true</bool>
-              </property>
-             </widget>
-            </item>
-            <item row="3" column="1">
-             <widget class="QLineEdit" name="DeviceID">
-              <property name="font">
-               <font>
-                <weight>75</weight>
-                <bold>true</bold>
-               </font>
-              </property>
-              <property name="styleSheet">
-               <string notr="true">QLineEdit {
-  border: none;
-  border-radius: 1px;
-  padding: 0 8px;
-  background: rgba(0, 0, 0, 16);
-/*  background: transparent; */
-/*     selection-background-color: darkgray;*/
-}</string>
-              </property>
-              <property name="frame">
-               <bool>false</bool>
-              </property>
-              <property name="readOnly">
-               <bool>true</bool>
-              </property>
-             </widget>
-            </item>
-            <item row="3" column="0">
-             <widget class="QLabel" name="label_20">
-              <property name="text">
-               <string>Device ID</string>
-              </property>
-              <property name="alignment">
-               <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
-              </property>
-             </widget>
-            </item>
            </layout>
           </widget>
          </item>
@@ -926,6 +1254,12 @@
       </item>
       <item row="0" column="1">
        <widget class="QGroupBox" name="groupBox">
+        <property name="sizePolicy">
+         <sizepolicy hsizetype="MinimumExpanding" vsizetype="Preferred">
+          <horstretch>0</horstretch>
+          <verstretch>0</verstretch>
+         </sizepolicy>
+        </property>
         <property name="font">
          <font>
           <weight>75</weight>

ground/openpilotgcs/src/plugins/gcscontrol/Matlab/GCSControl.m

@@ -0,0 +1,48 @@
+% GCSCONTROL
+%  This class allows the user to send 4-axis stick commands to OpenPilot
+%  GCS.
+%
+% Create class by
+%    control = GCSControl
+%
+% Open connection by 
+%    control.connect('01.23.45.67', 89)
+% where the first value is the IP address of the computer running GCS and
+% the second value is the port on which GCS is listening.
+%
+% Send command by
+%    control.command(pitch, yaw, roll, throttle)
+% where all variables are between [-1,1]
+%
+% Close connection by
+%    control.close()
+
+classdef GCSControl < handle
+    
+    properties
+        udpObj;
+        isConnected=false;
+    end
+    
+    methods
+        function obj=GCSControl()
+            obj.isConnected = false;
+        end
+        function obj=connect(obj,rhost,rport)
+            obj.udpObj = udp(rhost,rport);
+            fopen(obj.udpObj);
+            obj.isConnected = true;
+        end
+        function obj=command(obj,pitch,yaw,roll,throttle)
+            if(obj.isConnected)
+                fwrite(obj.udpObj,[42,pitch,yaw,roll,throttle,36],'double')
+            end
+        end
+        function obj=close(obj)
+            if(obj.isConnected)
+                fclose(obj.udpObj);
+                obj.isConnected = false;
+            end
+        end
+    end
+end

ground/openpilotgcs/src/plugins/gcscontrol/gcscontrol.pro

@@ -2,6 +2,7 @@ TEMPLATE = lib
 TARGET = GCSControl 
 QT += svg
 QT += opengl
+QT += network
 
 include(../../openpilotgcsplugin.pri) 
 include(../../plugins/coreplugin/coreplugin.pri) 

ground/openpilotgcs/src/plugins/gcscontrol/gcscontrol.ui

@@ -41,8 +41,21 @@
        </property>
       </widget>
      </item>
+     <item>
+      <widget class="QCheckBox" name="checkBoxUDPControl">
+       <property name="enabled">
+        <bool>false</bool>
+       </property>
+       <property name="text">
+        <string>UDP Control</string>
+       </property>
+      </widget>
+     </item>
      <item>
       <widget class="QCheckBox" name="checkBoxArmed">
+       <property name="enabled">
+        <bool>true</bool>
+       </property>
        <property name="text">
         <string>Armed</string>
        </property>

ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp

@@ -44,6 +44,10 @@ GCSControlGadget::GCSControlGadget(QString classId, GCSControlGadgetWidget *widg
 
     manualControlCommandUpdated(getManualControlCommand());
 
+    control_sock = new QUdpSocket(this);
+
+    connect(control_sock,SIGNAL(readyRead()),this,SLOT(readUDPCommand()));
+
     joystickTime.start();
     GCSControlPlugin *pl = dynamic_cast<GCSControlPlugin*>(plugin);
     connect(pl->sdlGamepad,SIGNAL(gamepads(quint8)),this,SLOT(gamepads(quint8)));
@@ -67,6 +71,12 @@ void GCSControlGadget::loadConfiguration(IUAVGadgetConfiguration* config)
     yawChannel = ql.at(2);
     throttleChannel = ql.at(3);
 
+ //   if(control_sock->isOpen())
+ //       control_sock->close();
+    control_sock->bind(GCSControlConfig->getUDPControlHost(), GCSControlConfig->getUDPControlPort(),QUdpSocket::ShareAddress);
+
+
+
     controlsMode = GCSControlConfig->getControlsMode();
 
     int i;
@@ -174,7 +184,8 @@ void GCSControlGadget::sticksChangedLocally(double leftX, double leftY, double r
     }
 
     //if we are not in local gcs control mode, ignore the joystick input
-    if (((GCSControlGadgetWidget *)m_widget)->getGCSControl()==false)return;
+    if (((GCSControlGadgetWidget *)m_widget)->getGCSControl()==false || ((GCSControlGadgetWidget *)m_widget)->getUDPControl())
+        return;
 
     if((newThrottle != oldThrottle) || (newPitch != oldPitch) || (newYaw != oldYaw) || (newRoll != oldRoll)) {
         if (buttonRollControl==0)obj->getField("Roll")->setDouble(newRoll);
@@ -191,6 +202,93 @@ void GCSControlGadget::gamepads(quint8 count)
 //    sdlGamepad.setTickRate(JOYSTICK_UPDATE_RATE);
 }
 
+void GCSControlGadget::readUDPCommand()
+{
+    double pitch, yaw, roll, throttle;
+    while (control_sock->hasPendingDatagrams()) {
+        QByteArray datagram;
+        datagram.resize(control_sock->pendingDatagramSize());
+        control_sock->readDatagram(datagram.data(), datagram.size());
+        QDataStream readData(datagram);
+        bool badPack = false;
+        int state = 0;
+        while(!readData.atEnd() && !badPack)
+        {
+            double buffer;
+            readData >> buffer;
+            switch(state)
+            {
+            case 0:
+                if(buffer == 42){
+                    state = 1;
+                }else{
+                    state = 0;
+                    badPack = true;
+                }
+                break;
+            case 1:
+                pitch = buffer;
+                state = 2;
+                break;
+            case 2:
+                yaw = buffer;
+                state = 3;
+                break;
+            case 3:
+                roll = buffer;
+                state = 4;
+                break;
+            case 4:
+                throttle = buffer;
+                state = 5;
+                break;
+            case 5:
+                if(buffer != 36 || !readData.atEnd())
+                    badPack=true;
+                break;
+            }
+
+        }
+        if(!badPack && ((GCSControlGadgetWidget *)m_widget)->getUDPControl())
+        {
+             ManualControlCommand * obj = getManualControlCommand();
+             bool update = false;
+
+             if(pitch != obj->getField("Pitch")->getDouble()){
+                 obj->getField("Pitch")->setDouble(constrain(pitch));
+                 update = true;
+             }
+             if(yaw != obj->getField("Yaw")->getDouble()){
+                 obj->getField("Yaw")->setDouble(constrain(yaw));
+                 update = true;
+             }
+             if(roll != obj->getField("Roll")->getDouble()){
+                 obj->getField("Roll")->setDouble(constrain(roll));
+                 update = true;
+             }
+             if(throttle != obj->getField("Throttle")->getDouble()){
+                obj->getField("Throttle")->setDouble(constrain(throttle));
+                update = true;
+             }
+             if(update)
+                 obj->updated();
+        }
+    }
+
+    qDebug() << "Pitch: " << pitch << " Yaw: " << yaw << " Roll: " << roll << " Throttle: " << throttle;
+
+
+}
+
+double GCSControlGadget::constrain(double value)
+{
+    if(value < -1)
+        return -1;
+    if(value > 1)
+        return 1;
+    return value;
+}
+
 void GCSControlGadget::buttonState(ButtonNumber number, bool pressed)
 {
     int state;
@@ -200,6 +298,7 @@ void GCSControlGadget::buttonState(ButtonNumber number, bool pressed)
         UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
         UAVDataObject* obj = dynamic_cast<UAVDataObject*>( objManager->getObject(QString("ManualControlCommand")) );
         bool currentCGSControl = ((GCSControlGadgetWidget *)m_widget)->getGCSControl();
+        bool currentUDPControl = ((GCSControlGadgetWidget *)m_widget)->getUDPControl();
 
         switch (buttonSettings[number].ActionID)
         {
@@ -268,6 +367,11 @@ void GCSControlGadget::buttonState(ButtonNumber number, bool pressed)
                 ((GCSControlGadgetWidget *)m_widget)->setGCSControl(!currentCGSControl);
 
                break;
+            case 3: //UDP Control
+                    if(currentCGSControl)
+                        ((GCSControlGadgetWidget *)m_widget)->setUDPControl(!currentUDPControl);
+
+                    break;
             }
 
             break;

ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.h

@@ -34,6 +34,9 @@
 #include "sdlgamepad/sdlgamepad.h"
 #include <QTime>
 #include "gcscontrolplugin.h"
+#include <QUdpSocket>
+#include <QHostAddress>
+
 
 namespace Core {
 class IUAVGadget;
@@ -59,6 +62,7 @@ public:
 
 private:
     ManualControlCommand* getManualControlCommand();
+    double constrain(double value);
     QTime joystickTime;
     QWidget *m_widget;
     QList<int> m_context;
@@ -72,6 +76,8 @@ private:
     double bound(double input);
     double wrap(double input);
     bool channelReverse[8];
+    QUdpSocket *control_sock;
+
 
 signals:
     void sticksChangedRemotely(double leftX, double leftY, double rightX, double rightY);
@@ -79,6 +85,7 @@ signals:
 protected slots:
     void manualControlCommandUpdated(UAVObject *);
     void sticksChangedLocally(double leftX, double leftY, double rightX, double rightY);
+    void readUDPCommand();
 
     // signals from joystick
     void gamepads(quint8 count);

ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.cpp

@@ -54,6 +54,9 @@ GCSControlGadgetConfiguration::GCSControlGadgetConfiguration(QString classId, QS
         yawChannel = qSettings->value("yawChannel").toInt();
         throttleChannel = qSettings->value("throttleChannel").toInt();
 
+        udp_port = qSettings->value("controlPortUDP").toUInt();
+        udp_host = QHostAddress(qSettings->value("controlHostUDP").toString());
+
         int i;
         for (i=0;i<8;i++)
         {
@@ -66,6 +69,21 @@ GCSControlGadgetConfiguration::GCSControlGadgetConfiguration(QString classId, QS
 
 }
 
+void GCSControlGadgetConfiguration::setUDPControlSettings(int port, QString host)
+{
+    udp_port = port;
+    udp_host = QHostAddress(host);
+}
+
+int GCSControlGadgetConfiguration::getUDPControlPort()
+{
+    return udp_port;
+}
+QHostAddress GCSControlGadgetConfiguration::getUDPControlHost()
+{
+    return udp_host;
+}
+
 void GCSControlGadgetConfiguration::setRPYTchannels(int roll, int pitch, int yaw, int throttle) {
     rollChannel = roll;
     pitchChannel = pitch;
@@ -102,6 +120,9 @@ IUAVGadgetConfiguration *GCSControlGadgetConfiguration::clone()
     m->yawChannel = yawChannel;
     m->throttleChannel = throttleChannel;
 
+    m->udp_host = udp_host;
+    m->udp_port = udp_port;
+
     int i;
     for (i=0;i<8;i++)
     {
@@ -126,6 +147,9 @@ void GCSControlGadgetConfiguration::saveConfig(QSettings* settings) const {
     settings->setValue("yawChannel", yawChannel);
     settings->setValue("throttleChannel", throttleChannel);
 
+    settings->setValue("controlPortUDP",QString::number(udp_port));
+    settings->setValue("controlHostUDP",udp_host.toString());
+
     int i;
     for (i=0;i<8;i++)
     {

ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.h

@@ -29,6 +29,7 @@
 #define GCSCONTROLGADGETCONFIGURATION_H
 
 #include <coreplugin/iuavgadgetconfiguration.h>
+#include <QtNetwork/QHostAddress>
 
 typedef struct{
     int     ActionID;
@@ -36,6 +37,11 @@ typedef struct{
     double  Amount;
 }buttonSettingsStruct;
 
+typedef struct{
+    int port;
+    QHostAddress address;
+}portSettingsStruct;
+
 
 using namespace Core;
 
@@ -49,6 +55,9 @@ class GCSControlGadgetConfiguration : public IUAVGadgetConfiguration
 
     void setControlsMode(int mode) { controlsMode = mode; }
     void setRPYTchannels(int roll, int pitch, int yaw, int throttle);
+    void setUDPControlSettings(int port, QString host);
+    int getUDPControlPort();
+    QHostAddress getUDPControlHost();
     int getControlsMode() { return controlsMode; }
     QList<int>  getChannelsMapping();
     QList<bool>  getChannelsReverse();
@@ -72,6 +81,8 @@ class GCSControlGadgetConfiguration : public IUAVGadgetConfiguration
         int throttleChannel;
         buttonSettingsStruct buttonSettings[8];
         bool channelReverse[8];
+        int udp_port;
+        QHostAddress udp_host;
 
 
 };

ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.cpp

@@ -137,7 +137,7 @@ QWidget *GCSControlGadgetOptionsPage::createPage(QWidget *parent)
               options_page->buttonFunction4 << options_page->buttonFunction5 <<
               options_page->buttonFunction6 << options_page->buttonFunction7;
     QStringList buttonOptions;
-    buttonOptions <<"-" << "Roll" << "Pitch" << "Yaw" << "Throttle" << "Armed" << "GCS Control" ;
+    buttonOptions <<"-" << "Roll" << "Pitch" << "Yaw" << "Throttle" << "Armed" << "GCS Control"; //added UDP control to action list
     foreach (QComboBox* qb, buttonFunctionList) {
         qb->addItems(buttonOptions);
     }
@@ -187,6 +187,9 @@ QWidget *GCSControlGadgetOptionsPage::createPage(QWidget *parent)
 
     //updateButtonFunction();
 
+    options_page->udp_host->setText(m_config->getUDPControlHost().toString());
+    options_page->udp_port->setText(QString::number(m_config->getUDPControlPort()));
+
 
     // Controls mode are from 1 to 4.
     if (m_config->getControlsMode()>0 && m_config->getControlsMode() < 5)
@@ -262,6 +265,9 @@ void GCSControlGadgetOptionsPage::apply()
    }
    m_config->setRPYTchannels(roll,pitch,yaw,throttle);
 
+   m_config->setUDPControlSettings(options_page->udp_port->text().toInt(),options_page->udp_host->text());
+
+
    int j;
    for (j=0;j<8;j++)
    {
@@ -271,6 +277,7 @@ void GCSControlGadgetOptionsPage::apply()
        m_config->setChannelReverse(j,chRevList.at(j)->isChecked());
    }
 
+
 }
 
 void GCSControlGadgetOptionsPage::finish()
@@ -369,7 +376,7 @@ void GCSControlGadgetOptionsPage::updateButtonAction(int controlID)
         if (buttonActionList.at(i)->currentText().compare("Toggles")==0)
         {
             disconnect(buttonFunctionList.at(i),SIGNAL(currentIndexChanged(int)),this,SLOT(updateButtonFunction()));
-            buttonOptions <<"-" << "Armed" << "GCS Control" ;
+            buttonOptions <<"-" << "Armed" << "GCS Control" << "UDP Control";
             buttonFunctionList.at(i)->clear();
             buttonFunctionList.at(i)->insertItems(-1,buttonOptions);
 

ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.ui

@@ -141,7 +141,7 @@
        </widget>
       </item>
       <item>
-       <widget class="QTabWidget" name="tabWidget">
+       <widget class="QTabWidget" name="UDPSetup">
         <property name="currentIndex">
          <number>0</number>
         </property>
@@ -1011,6 +1011,66 @@
           </item>
          </layout>
         </widget>
+        <widget class="QWidget" name="UDPSetup_2">
+         <attribute name="title">
+          <string>UDP Setup</string>
+         </attribute>
+         <widget class="QGroupBox" name="groupBox">
+          <property name="geometry">
+           <rect>
+            <x>20</x>
+            <y>20</y>
+            <width>301</width>
+            <height>71</height>
+           </rect>
+          </property>
+          <property name="title">
+           <string>UDP Port Configuration</string>
+          </property>
+          <layout class="QHBoxLayout" name="horizontalLayout_2">
+           <item>
+            <widget class="QLabel" name="label_7">
+             <property name="text">
+              <string>Host:</string>
+             </property>
+            </widget>
+           </item>
+           <item>
+            <widget class="QLineEdit" name="udp_host">
+             <property name="maximumSize">
+              <size>
+               <width>120</width>
+               <height>16777215</height>
+              </size>
+             </property>
+             <property name="text">
+              <string>127.0.0.1</string>
+             </property>
+            </widget>
+           </item>
+           <item>
+            <widget class="QLabel" name="label_6">
+             <property name="text">
+              <string>Port:</string>
+             </property>
+            </widget>
+           </item>
+           <item>
+            <widget class="QLineEdit" name="udp_port">
+             <property name="maximumSize">
+              <size>
+               <width>50</width>
+               <height>16777215</height>
+              </size>
+             </property>
+             <property name="text">
+              <string>2323</string>
+             </property>
+            </widget>
+           </item>
+          </layout>
+         </widget>
+        </widget>
        </widget>
       </item>
      </layout>

ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.cpp

@@ -34,6 +34,7 @@
 #include <QtGui/QVBoxLayout>
 #include <QtGui/QPushButton>
 
+
 #include "uavobject.h"
 #include "uavobjectmanager.h"
 #include "manualcontrolcommand.h"
@@ -64,9 +65,14 @@ GCSControlGadgetWidget::GCSControlGadgetWidget(QWidget *parent) : QLabel(parent)
     connect(m_gcscontrol->checkBoxArmed, SIGNAL(stateChanged(int)), this, SLOT(toggleArmed(int)));
     connect(m_gcscontrol->comboBoxFlightMode, SIGNAL(currentIndexChanged(int)), this, SLOT(selectFlightMode(int)));
 
+    connect(m_gcscontrol->checkBoxUDPControl, SIGNAL(stateChanged(int)),this,SLOT(toggleUDPControl(int))); //UDP control checkbox
+
     // Connect object updated event from UAVObject to also update check boxes and dropdown
     connect(obj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(mccChanged(UAVObject*)));
 
+
+
+
     leftX = 0;
     leftY = 0;
     rightX = 0;
@@ -122,11 +128,14 @@ void GCSControlGadgetWidget::toggleControl(int state)
         UAVObject::SetGcsTelemetryAcked(mdata, false);
         UAVObject::SetGcsTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_ONCHANGE);
         mdata.gcsTelemetryUpdatePeriod = 100;
+        m_gcscontrol->checkBoxUDPControl->setEnabled(true);
 
     }
     else
     {
         mdata = mccInitialData;
+        toggleUDPControl(false);
+        m_gcscontrol->checkBoxUDPControl->setEnabled(false);
     }
     obj->setMetadata(mdata);
 }
@@ -152,6 +161,16 @@ void GCSControlGadgetWidget::mccChanged(UAVObject * obj)
     m_gcscontrol->checkBoxArmed->setChecked(flightStatus->getField("Armed")->getValue() == "Armed");
 }
 
+void GCSControlGadgetWidget::toggleUDPControl(int state)
+{
+    if(state)
+    {
+        setUDPControl(true);
+    }else{
+        setUDPControl(false);
+    }
+}
+
 /*!
   \brief Called when the flight mode drop down is changed and sets the ManualControlCommand->FlightMode accordingly
   */
@@ -168,11 +187,21 @@ void GCSControlGadgetWidget::selectFlightMode(int state)
 void GCSControlGadgetWidget::setGCSControl(bool newState)
 {
     m_gcscontrol->checkBoxGcsControl->setChecked(newState);
-};
+}
 bool GCSControlGadgetWidget::getGCSControl(void)
 {
     return m_gcscontrol->checkBoxGcsControl->isChecked();
-};
+}
+
+void GCSControlGadgetWidget::setUDPControl(bool newState)
+{
+    m_gcscontrol->checkBoxUDPControl->setChecked(newState);
+}
+
+bool GCSControlGadgetWidget::getUDPControl(void)
+{
+    return m_gcscontrol->checkBoxUDPControl->isChecked();
+}
 
 
 /**

ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.h

@@ -31,6 +31,8 @@
 #include <QtGui/QLabel>
 #include "manualcontrolcommand.h"
 
+#define UDP_PORT 2323
+
 class Ui_GCSControl;
 
 class GCSControlGadgetWidget : public QLabel
@@ -42,6 +44,8 @@ public:
     ~GCSControlGadgetWidget();
     void setGCSControl(bool newState);
     bool getGCSControl(void);
+    void setUDPControl(bool newState);
+    bool getUDPControl(void);
 
 signals:
     void sticksChanged(double leftX, double leftY, double rightX, double rightY);
@@ -59,6 +63,7 @@ protected slots:
     void toggleArmed(int state);
     void selectFlightMode(int state);
     void mccChanged(UAVObject *);
+    void toggleUDPControl(int state);
 
 private:
     Ui_GCSControl *m_gcscontrol;

ground/openpilotgcs/src/plugins/gpsdisplay/gpsconstellationwidget.cpp

@@ -123,7 +123,7 @@ void GpsConstellationWidget::updateSat(int index, int prn, int elevation, int az
     satellites[index][2] = azimuth;
     satellites[index][3] = snr;
 
-    if (prn) {
+    if (prn && elevation >= 0) {
         QPointF opd = polarToCoord(elevation,azimuth);
         opd += QPointF(-satIcons[index]->boundingRect().center().x(),
                        -satIcons[index]->boundingRect().center().y());

ground/openpilotgcs/src/plugins/hitlv2/aerosimrc.cpp

@@ -1,3 +1,30 @@
+/**
+ ******************************************************************************
+ *
+ * @file       aerosimrc.cpp
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
+ * @addtogroup GCSPlugins GCS Plugins
+ * @{
+ * @addtogroup HITLPlugin HITLv2 Plugin
+ * @{
+ * @brief The Hardware In The Loop plugin version 2
+ *****************************************************************************/
+/*
+ * 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 "aerosimrc.h"
 #include <extensionsystem/pluginmanager.h>
 #include <coreplugin/icore.h>

ground/openpilotgcs/src/plugins/hitlv2/aerosimrc.h

@@ -1,3 +1,30 @@
+/**
+ ******************************************************************************
+ *
+ * @file       aerosimrc.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
+ * @addtogroup GCSPlugins GCS Plugins
+ * @{
+ * @addtogroup HITLPlugin HITLv2 Plugin
+ * @{
+ * @brief The Hardware In The Loop plugin version 2
+ *****************************************************************************/
+/*
+ * 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 AEROSIMRC_H
 #define AEROSIMRC_H
 

ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/aerosimrc.pro

@@ -0,0 +1,10 @@
+TEMPLATE  = subdirs
+
+win32 {
+    SUBDIRS += plugin
+}
+
+SUBDIRS += udptest
+
+plugin.file   = src/plugin.pro
+udptest.file  = src/udptest.pro

ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/aerosimrcdatastruct.h

@@ -0,0 +1,215 @@
+/**
+ ******************************************************************************
+ *
+ * @file       aerosimrcdatastruct.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
+ * @addtogroup 3rdParty Third-party integration
+ * @{
+ * @addtogroup AeroSimRC AeroSimRC proxy plugin
+ * @{
+ * @brief AeroSimRC simulator to HITL proxy plugin
+ *****************************************************************************/
+/*
+ * 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 AEROSIMRCDATASTRUCT_H
+#define AEROSIMRCDATASTRUCT_H
+
+#include <QtCore>
+
+const quint8 AEROSIMRC_MAX_CHANNELS = 39;
+const quint16 DBG_BUFFER_MAX_SIZE = 4096;
+
+#define MAX_DLL_USER_MENU_ITEMS 16
+#define OBSOLETE_MIT_COMMAND    (1 << 0)
+#define OBSOLETE_MIT_CHECKBOX   (1 << 1)
+#define OBSOLETE_MIT_SEPARATOR  (1 << 7)
+
+#if defined(Q_CC_MSVC)
+#define PACK_STRUCT
+#define MAX_PATH 260
+#pragma pack (push, r1, 1)
+#elif defined(Q_CC_GNU)
+#define PACK_STRUCT __attribute__((packed))
+#endif
+
+struct simToPlugin
+{
+    quint16 structSize;
+    float simTimeStep;
+    float chSimTX[AEROSIMRC_MAX_CHANNELS];
+    float chSimRX[AEROSIMRC_MAX_CHANNELS];
+    uchar *OSDVideoBuf;
+    quint32 simMenuStatus;
+    float initPosX;
+    float initPosY;
+    float initPosZ;
+    float initHeading;
+    float initPitch;
+    float initRoll;
+    float wpHomeX;
+    float wpHomeY;
+    float wpHomeLat;
+    float wpHomeLong;
+    const char *wpHomeDesc; // (m, deg, string)
+    float wpAX;
+    float wpAY;
+    float wpALat;
+    float wpALong;
+    const char *wpADesc;    // (m, deg, string)
+    float wpBX;
+    float wpBY;
+    float wpBLat;
+    float wpBLong;
+    const char *wpBDesc;    // (m, deg, string)
+    float wpCX;
+    float wpCY;
+    float wpCLat;
+    float wpCLong;
+    const char *wpCDesc;    // (m, deg, string)
+    float wpDX;
+    float wpDY;
+    float wpDLat;
+    float wpDLong;
+    const char *wpDDesc;    // (m, deg, string)
+    float posX;
+    float posY;
+    float posZ;
+    float velX;
+    float velY;
+    float velZ;
+    float angVelX;
+    float angVelY;
+    float angVelZ;
+    float accelX;
+    float accelY;
+    float accelZ;
+    qreal latitude;
+    qreal longitude;
+    float AGL;
+    float heading;
+    float pitch;
+    float roll;
+    float windVelX;
+    float windVelY;
+    float windVelZ;
+    float eng1RPM;
+    float eng2RPM;
+    float eng3RPM;
+    float eng4RPM;
+    float voltage;          // V
+    float current;          // A
+    float consumedCharge;   // Ah
+    float capacity;         // Ah
+    float fuelConsumed;     // l
+    float fuelTankCapacity; // l
+    // ver 3.83
+    qint16 screenW;
+    qint16 screenH;
+    // Model Orientation Matrix (X=Right, Y=Front, Z=Up)
+    float axisXx;
+    float axisXy;
+    float axisXz;
+    float axisYx;
+    float axisYy;
+    float axisYz;
+    float axisZx;
+    float axisZy;
+    float axisZz;
+    // Model data in body frame coordinates (X=Right, Y=Front, Z=Up)
+    float velXm;    // m/s   Model velocity in body coordinates
+    float velYm;
+    float velZm;
+    float angVelXm; // rad/s Model angular velocity in body coordinates
+    float angVelYm;
+    float angVelZm;
+    float accelXm;  // m/s/s Model acceleration in body coordinates
+    float accelYm;
+    float accelZm;
+    // ver 3.90
+    quint32 OSDVideoBufSize;
+} PACK_STRUCT ;     // normal - 592, packed - 582 OK (3.81)
+                    // normal - ???, packed - 658 OK (3.83)
+                    // normal - ???, packed - 662 OK (3.90)
+
+struct pluginToSim
+{
+    quint16 structSize;
+    const char *dbgInfoText;
+    uchar chOverTX[AEROSIMRC_MAX_CHANNELS];
+    float chNewTX[AEROSIMRC_MAX_CHANNELS];
+    uchar chOverRX[AEROSIMRC_MAX_CHANNELS];
+    float chNewRX[AEROSIMRC_MAX_CHANNELS];
+    float newPosX;      // m
+    float newPosY;
+    float newPosZ;
+    float newVelX;      // m/s
+    float newVelY;
+    float newVelZ;
+    float newAngVelX;   // rad/s
+    float newAngVelY;
+    float newAngVelZ;
+    float newHeading;   // rad
+    float newPitch;
+    float newRoll;
+    quint32 modelOverrideFlags;
+    quint32 newMenuStatus;
+    quint8 isOSDShow;
+    quint8 isOSDChanged;
+    quint16 OSDWindow_DX;
+    quint16 OSDWindow_DY;
+    float OSDScale;
+    float newWindVelX;
+    float newWindVelY;
+    float newWindVelZ;
+    float newEng1RPM;
+    float newEng2RPM;
+    float newEng3RPM;
+    float newEng4RPM;
+    float newVoltage;
+    float newCurrent;
+    float newConsumedCharge;
+    float newFuelConsumed;
+    quint8 modelCrashInhibit;
+    // ver 3.83
+    qint16 newScreenW;     // Simulator window position and size on screen
+    qint16 newScreenH;
+    qint16 newScreenX;
+    qint16 newScreenY;
+} PACK_STRUCT ;     // normal 516, packed 507 OK (3.81)
+                    // normal ???, packed 515 OK (3.83 & 3.90)
+
+struct TPluginMenuItem
+{
+    quint32 OBSOLETE_eType;
+    char *OBSOLETE_strName;
+} PACK_STRUCT ;
+
+struct pluginInit
+{
+    quint32 nStructSize;
+    char *OBSOLETE_strMenuTitle;
+    TPluginMenuItem OBSOLETE_atMenuItem[MAX_DLL_USER_MENU_ITEMS];
+    const char *strPluginFolder;
+    const char *strOutputFolder;
+} PACK_STRUCT ;     // normal - 144, packed - 144 OK (3.81 & 3.83 & 3.90)
+
+#ifdef Q_CC_MSVC
+#pragma pack (pop, r1)
+#endif
+#undef PACK_STRUCT
+
+#endif // AEROSIMRCDATASTRUCT_H

ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/enums.h

@@ -0,0 +1,102 @@
+/**
+ ******************************************************************************
+ *
+ * @file       enums.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
+ * @addtogroup 3rdParty Third-party integration
+ * @{
+ * @addtogroup AeroSimRC AeroSimRC proxy plugin
+ * @{
+ * @brief AeroSimRC simulator to HITL proxy plugin
+ *****************************************************************************/
+/*
+ * 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 ENUMS_H
+#define ENUMS_H
+
+// Custom Menu Item masks
+enum MenuMasks {
+    MenuEnable      = (1 << 0),
+    MenuTx          = (1 << 1),
+    MenuRx          = (1 << 2),
+    MenuScreen      = (1 << 3),
+    MenuNextWpt     = (1 << 4),
+    MenuCmdReset    = (1 << 5),
+    MenuLedBlue     = (1 << 6),
+    MenuLedGreen    = (1 << 7),
+    MenuFMode1      = (1 << 8),
+    MenuFMode2      = (1 << 9),
+    MenuFMode3      = (1 << 10)
+};
+
+enum EOverrideFlags
+{
+    OVR_POS             = (1 << 0),
+    OVR_VEL             = (1 << 1),
+    OVR_ANG_VEL         = (1 << 2),
+    OVR_HPR             = (1 << 3),  // Override Heading, Pitch and Roll
+    OVR_WIND_VEL        = (1 << 4),  // Override Wind velocity at model
+    OVR_ENGINE_RPM      = (1 << 5),  // Override RPM of all Engines or Motors
+    OVR_BAT_VOLT        = (1 << 6),  // Override motor Battery Voltage
+    OVR_BAT_AMP         = (1 << 7),  // Override motor Battery current
+    OVR_BAT_AH_CONSUMED = (1 << 8),  // Override motor Battery AmpsHour consumed
+    OVR_FUEL_CONSUMED   = (1 << 9)   // Override Fuel consumed (gas & jet engines)
+};
+
+enum Channels {
+    Ch1Aileron,
+    Ch2Elevator,
+    Ch3Throttle,
+    Ch4Rudder,
+    Ch5,
+    Ch6,
+    Ch7,
+    Ch8,
+    Ch9,
+    Ch10Retracts,
+    Ch11Flaps,
+    Ch12FPVCamPan,
+    Ch13FPVCamTilt,
+    Ch14Brakes,
+    Ch15Spoilers,
+    Ch16Smoke,
+    Ch17Fire,
+    Ch18FlightMode,
+    Ch19ALTHold,
+    Ch20FPVTiltHold,
+    Ch21ResetModel,
+    Ch22MouseTX,
+    Ch23Plugin1,
+    Ch24Plugin2,
+    Ch25ThrottleHold,
+    Ch26CareFree,
+    Ch27FPVCamRoll,
+    Ch28LMotorDual,
+    Ch29RMotorDual,
+    Ch30Mix,
+    Ch31Mix,
+    Ch32Mix,
+    Ch33Mix,
+    Ch34Mix,
+    Ch35Mix,
+    Ch36Mix,
+    Ch37Mix,
+    Ch38Mix,
+    Ch39Mix
+};
+
+#endif // ENUMS_H

ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/plugin.cpp

@@ -0,0 +1,394 @@
+/**
+ ******************************************************************************
+ *
+ * @file       plugin.cpp
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
+ * @addtogroup 3rdParty Third-party integration
+ * @{
+ * @addtogroup AeroSimRC AeroSimRC proxy plugin
+ * @{
+ * @brief AeroSimRC simulator to HITL proxy plugin
+ *****************************************************************************/
+/*
+ * 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 "plugin.h"
+#include "udpconnect.h"
+#include "qdebughandler.h"
+#include "enums.h"
+#include "settings.h"
+
+bool isFirstRun = true;
+QString debugInfo(DBG_BUFFER_MAX_SIZE, ' ');
+QString pluginFolder(MAX_PATH, ' ');
+QString outputFolder(MAX_PATH, ' ');
+
+QList<quint16> videoModes;
+QTime ledTimer;
+
+UdpSender *sndr;
+UdpReceiver *rcvr;
+
+const float RAD2DEG = (float)(180.0 / M_PI);
+const float DEG2RAD = (float)(M_PI / 180.0);
+
+//extern "C" int __stdcall DllMain(HINSTANCE hinstDLL, DWORD fdwReason, LPVOID lpvReserved)
+extern "C" int __stdcall DllMain(void*, quint32 fdwReason, void*)
+{
+    switch (fdwReason) {
+    case 0:
+//        qDebug() << hinstDLL << "DLL_PROCESS_DETACH " << lpvReserved;
+// free resources here
+        rcvr->stop();
+        rcvr->wait(500);
+        delete rcvr;
+        delete sndr;
+        qDebug("------");
+        break;
+    case 1:
+//        qDebug() << hinstDLL << " DLL_PROCESS_ATTACH " << lpvReserved;
+        break;
+    case 2:
+//        qDebug() << hinstDLL << "DLL_THREAD_ATTACH " << lpvReserved;
+        break;
+    case 3:
+//        qDebug() << hinstDLL << "DLL_THREAD_DETACH " << lpvReserved;
+        break;
+    }
+    return true;
+}
+
+SIM_DLL_EXPORT void AeroSIMRC_Plugin_ReportStructSizes(quint32 *sizeSimToPlugin,
+                                                       quint32 *sizePluginToSim,
+                                                       quint32 *sizePluginInit)
+{
+    // debug redirection
+    qInstallMsgHandler(myQDebugHandler);
+
+    qDebug() << "AeroSIMRC_Plugin_ReportStructSizes";
+    *sizeSimToPlugin = sizeof(simToPlugin);
+    *sizePluginToSim = sizeof(pluginToSim);
+    *sizePluginInit  = sizeof(pluginInit);
+    qDebug() << "sizeSimToPlugin = " << *sizeSimToPlugin;
+    qDebug() << "sizePluginToSim = " << *sizePluginToSim;
+    qDebug() << "sizePluginInit  = " << *sizePluginInit;
+}
+
+SIM_DLL_EXPORT void AeroSIMRC_Plugin_Init(pluginInit *p)
+{
+    qDebug() << "AeroSIMRC_Plugin_Init begin";
+
+    pluginFolder = p->strPluginFolder;
+    outputFolder = p->strOutputFolder;
+
+    ledTimer.restart();
+
+    Settings *ini = new Settings(pluginFolder);
+    ini->read();
+
+    videoModes = ini->getVideoModes();
+
+    sndr = new UdpSender(ini->getOutputMap(), ini->isFromTX());
+    sndr->init(ini->remoteHost(), ini->remotePort());
+
+    rcvr = new UdpReceiver(ini->getInputMap(), ini->isToRX());
+    rcvr->init(ini->localHost(), ini->localPort());
+
+    // run thread
+    rcvr->start();
+
+    delete ini;
+
+    qDebug() << "AeroSIMRC_Plugin_Init done";
+}
+
+//-----------------------------------------------------------------------------
+
+void Run_Command_Reset(/*const simToPlugin *stp,
+                               pluginToSim *pts*/)
+{
+    // Print some debug info, although it will only be seen during one frame
+    debugInfo.append("\nRESET");
+}
+
+void Run_Command_WindowSizeAndPos(const simToPlugin *stp,
+                                        pluginToSim *pts)
+{
+    static quint8 snSequence = 0;
+    quint8 idx = snSequence * 4;
+
+    if (snSequence >= videoModes.at(0)) {   // set fullscreen
+        pts->newScreenX = 0;
+        pts->newScreenY = 0;
+        pts->newScreenW = stp->screenW;
+        pts->newScreenH = stp->screenH;
+        snSequence = 0;
+    } else {                                // set video mode from config
+        pts->newScreenX = videoModes.at(idx + 1);
+        pts->newScreenY = videoModes.at(idx + 2);
+        pts->newScreenW = videoModes.at(idx + 3);
+        pts->newScreenH = videoModes.at(idx + 4);
+        snSequence++;
+    }
+}
+
+void Run_Command_MoveToNextWaypoint(const simToPlugin *stp,
+                                          pluginToSim *pts)
+{
+    static quint8 snSequence = 0;
+
+    switch(snSequence) {
+    case 0:
+        pts->newPosX = stp->wpAX;
+        pts->newPosY = stp->wpAY;
+        pts->newPosZ = 100.0;
+        break;
+    case 1:
+        pts->newPosX = stp->wpBX;
+        pts->newPosY = stp->wpBY;
+        pts->newPosZ = 100.0;
+        break;
+    case 2:
+        pts->newPosX = stp->wpCX;
+        pts->newPosY = stp->wpCY;
+        pts->newPosZ = 100.0;
+        break;
+    case 3:
+        pts->newPosX = stp->wpDX;
+        pts->newPosY = stp->wpDY;
+        pts->newPosZ = 100.0;
+        break;
+    case 4:
+        pts->newPosX = stp->wpHomeX;
+        pts->newPosY = stp->wpHomeY;
+        pts->newPosZ = 100.0;
+        break;
+    default:
+        qFatal("Run_Command_MoveToNextWaypoint switch error");
+    }
+    pts->modelOverrideFlags = 0;
+    pts->modelOverrideFlags |= OVR_POS;
+
+    snSequence++;
+    if(snSequence > 4)
+        snSequence = 0;
+}
+
+void Run_BlinkLEDs(const simToPlugin *stp,
+                         pluginToSim *pts)
+{
+    if ((stp->simMenuStatus & MenuEnable) != 0) {
+        pts->newMenuStatus |= MenuLedGreen;
+        int timeout;
+        quint8 armed;
+        quint8 mode;
+        rcvr->getFlighStatus(armed, mode);
+        debugInfo.append(QString("armed: %1, mode: %2\n").arg(armed).arg(mode));
+
+        if (armed == 0)         // disarm
+            timeout = 1000;
+        else if (armed == 1)    // arming
+            timeout = 40;
+        else if (armed == 2)    // armed
+            timeout = 100;
+        else                    // unknown
+            timeout = 2000;
+        if (ledTimer.elapsed() > timeout) {
+            ledTimer.restart();
+            pts->newMenuStatus ^= MenuLedBlue;
+        }
+
+        if (mode == 6) {
+            pts->newMenuStatus |= MenuFMode3;
+            pts->newMenuStatus |= MenuFMode2;
+            pts->newMenuStatus |= MenuFMode1;
+        } else if (mode == 5) {
+            pts->newMenuStatus |= MenuFMode3;
+            pts->newMenuStatus |= MenuFMode2;
+            pts->newMenuStatus &= ~MenuFMode1;
+        } else if (mode == 4) {
+            pts->newMenuStatus |= MenuFMode3;
+            pts->newMenuStatus &= ~MenuFMode2;
+            pts->newMenuStatus |= MenuFMode1;
+        } else if (mode == 3) {
+            pts->newMenuStatus |= MenuFMode3;
+            pts->newMenuStatus &= ~MenuFMode2;
+            pts->newMenuStatus &= ~MenuFMode1;
+        } else if (mode == 2) {
+            pts->newMenuStatus &= ~MenuFMode3;
+            pts->newMenuStatus |= MenuFMode2;
+            pts->newMenuStatus &= ~MenuFMode1;
+        } else if (mode == 1) {
+            pts->newMenuStatus &= ~MenuFMode3;
+            pts->newMenuStatus &= ~MenuFMode2;
+            pts->newMenuStatus |= MenuFMode1;
+        } else /*(mode == 0)*/ {
+            pts->newMenuStatus &= ~MenuFMode3;
+            pts->newMenuStatus &= ~MenuFMode2;
+            pts->newMenuStatus &= ~MenuFMode1;
+        }
+    } else {
+        pts->newMenuStatus = 0;
+    }
+}
+
+void InfoText(const simToPlugin *stp,
+                    pluginToSim *pts)
+{
+    debugInfo.append(
+                QString(
+                    "Plugin Folder = %1\n"
+                    "Output Folder = %2\n"
+                    "nStructSize = %3  "
+                    "fIntegrationTimeStep = %4\n"
+                    "\n"
+                    "Aileron   TX = %5  RX = %6  RCMD TX = %7  RX = %8\n"
+                    "Elevator  TX = %9  RX = %10  RCMD TX = %11  RX = %12\n"
+                    "Throttle  TX = %13  RX = %14  RCMD TX = %15  RX = %16\n"
+                    "Rudder    TX = %17  RX = %18  RCMD TX = %19  RX = %20\n"
+                    "Channel5  TX = %21  RX = %22  RCMD TX = %23  RX = %24\n"
+                    "Channel6  TX = %25  RX = %26  RCMD TX = %27  RX = %28\n"
+                    "Channel7  TX = %29  RX = %30  RCMD TX = %31  RX = %32\n"
+                    "PluginCh1 TX = %33  RX = %34  RCMD TX = %35  RX = %36\n"
+                    "PluginCh2 TX = %37  RX = %38  RCMD TX = %39  RX = %40\n"
+                    "FPVCamPan TX = %41  RX = %42  RCMD TX = %43  RX = %44\n"
+                    "FPVCamTil TX = %45  RX = %46  RCMD TX = %47  RX = %48\n"
+                    "\n"
+                    "MenuItems = %49\n"
+                    // Model data
+                    "\n"
+                    "fPosX,Y,Z    = (%50, %51, %52)\n"
+                    "fVelX,Y,Z    = (%53, %54, %55)\n"
+                    "fAngVelX,Y,Z = (%56, %57, %58)\n"
+                    "fAccelX,Y,Z  = (%59, %60, %61)\n"
+                    "\n"
+                    "Lat, Long    = %62, %63\n"
+                    "fHeightAboveTerrain = %64\n"
+                    "\n"
+                    "fHeading = %65   fPitch = %66   fRoll = %67\n"
+                    )
+                .arg(pluginFolder)
+                .arg(outputFolder)
+                .arg(stp->structSize)
+                .arg(1.0 / stp->simTimeStep, 4, 'f', 1)
+                .arg(stp->chSimTX[Ch1Aileron], 5, 'f', 2)
+                .arg(stp->chSimRX[Ch1Aileron], 5, 'f', 2)
+                .arg(pts->chNewTX[Ch1Aileron], 5, 'f', 2)
+                .arg(pts->chNewRX[Ch1Aileron], 5, 'f', 2)
+                .arg(stp->chSimTX[Ch2Elevator], 5, 'f', 2)
+                .arg(stp->chSimRX[Ch2Elevator], 5, 'f', 2)
+                .arg(pts->chNewTX[Ch2Elevator], 5, 'f', 2)
+                .arg(pts->chNewRX[Ch2Elevator], 5, 'f', 2)
+                .arg(stp->chSimTX[Ch3Throttle], 5, 'f', 2)
+                .arg(stp->chSimRX[Ch3Throttle], 5, 'f', 2)
+                .arg(pts->chNewTX[Ch3Throttle], 5, 'f', 2)
+                .arg(pts->chNewRX[Ch3Throttle], 5, 'f', 2)
+                .arg(stp->chSimTX[Ch4Rudder], 5, 'f', 2)
+                .arg(stp->chSimRX[Ch4Rudder], 5, 'f', 2)
+                .arg(pts->chNewTX[Ch4Rudder], 5, 'f', 2)
+                .arg(pts->chNewRX[Ch4Rudder], 5, 'f', 2)
+                .arg(stp->chSimTX[Ch5], 5, 'f', 2)
+                .arg(stp->chSimRX[Ch5], 5, 'f', 2)
+                .arg(pts->chNewTX[Ch5], 5, 'f', 2)
+                .arg(pts->chNewRX[Ch5], 5, 'f', 2)
+                .arg(stp->chSimTX[Ch6], 5, 'f', 2)
+                .arg(stp->chSimRX[Ch6], 5, 'f', 2)
+                .arg(pts->chNewTX[Ch6], 5, 'f', 2)
+                .arg(pts->chNewRX[Ch6], 5, 'f', 2)
+                .arg(stp->chSimTX[Ch7], 5, 'f', 2)
+                .arg(stp->chSimRX[Ch7], 5, 'f', 2)
+                .arg(pts->chNewTX[Ch7], 5, 'f', 2)
+                .arg(pts->chNewRX[Ch7], 5, 'f', 2)
+                .arg(stp->chSimTX[Ch23Plugin1], 5, 'f', 2)
+                .arg(stp->chSimRX[Ch23Plugin1], 5, 'f', 2)
+                .arg(pts->chNewTX[Ch23Plugin1], 5, 'f', 2)
+                .arg(pts->chNewRX[Ch23Plugin1], 5, 'f', 2)
+                .arg(stp->chSimTX[Ch24Plugin2], 5, 'f', 2)
+                .arg(stp->chSimRX[Ch24Plugin2], 5, 'f', 2)
+                .arg(pts->chNewTX[Ch24Plugin2], 5, 'f', 2)
+                .arg(pts->chNewRX[Ch24Plugin2], 5, 'f', 2)
+                .arg(stp->chSimTX[Ch12FPVCamPan], 5, 'f', 2)
+                .arg(stp->chSimRX[Ch12FPVCamPan], 5, 'f', 2)
+                .arg(pts->chNewTX[Ch12FPVCamPan], 5, 'f', 2)
+                .arg(pts->chNewRX[Ch12FPVCamPan], 5, 'f', 2)
+                .arg(stp->chSimTX[Ch13FPVCamTilt], 5, 'f', 2)
+                .arg(stp->chSimRX[Ch13FPVCamTilt], 5, 'f', 2)
+                .arg(pts->chNewTX[Ch13FPVCamTilt], 5, 'f', 2)
+                .arg(pts->chNewRX[Ch13FPVCamTilt], 5, 'f', 2)
+                .arg(stp->simMenuStatus)
+                .arg(stp->posX, 5, 'f', 2)
+                .arg(stp->posY, 5, 'f', 2)
+                .arg(stp->posZ, 5, 'f', 2)
+                .arg(stp->velX, 5, 'f', 2)
+                .arg(stp->velY, 5, 'f', 2)
+                .arg(stp->velZ, 5, 'f', 2)
+                .arg(stp->angVelXm, 5, 'f', 2)
+                .arg(stp->angVelYm, 5, 'f', 2)
+                .arg(stp->angVelZm, 5, 'f', 2)
+                .arg(stp->accelXm, 5, 'f', 2)
+                .arg(stp->accelYm, 5, 'f', 2)
+                .arg(stp->accelZm, 5, 'f', 2)
+                .arg(stp->latitude, 5, 'f', 2)
+                .arg(stp->longitude, 5, 'f', 2)
+                .arg(stp->AGL, 5, 'f', 2)
+                .arg(stp->heading*RAD2DEG, 5, 'f', 2)
+                .arg(stp->pitch*RAD2DEG, 5, 'f', 2)
+                .arg(stp->roll*RAD2DEG, 5, 'f', 2)
+    );
+}
+
+SIM_DLL_EXPORT void AeroSIMRC_Plugin_Run(const simToPlugin *stp,
+                                               pluginToSim *pts)
+{
+    debugInfo = "---\n";
+    // By default do not change the Menu Items of type CheckBox
+    pts->newMenuStatus = stp->simMenuStatus;
+    // Extract Menu Commands from Flags
+    bool isReset  = (stp->simMenuStatus & MenuCmdReset) != 0;
+    bool isEnable = (stp->simMenuStatus & MenuEnable) != 0;
+    bool isTxON   = (stp->simMenuStatus & MenuTx) != 0;
+    bool isRxON   = (stp->simMenuStatus & MenuRx) != 0;
+    bool isScreen = (stp->simMenuStatus & MenuScreen) != 0;
+    bool isNextWp = (stp->simMenuStatus & MenuNextWpt) != 0;
+    // Run commands
+    if (isReset) {
+        Run_Command_Reset(/*stp, pts*/);
+    } else if (isScreen) {
+        Run_Command_WindowSizeAndPos(stp, pts);
+    } else if (isNextWp) {
+        Run_Command_MoveToNextWaypoint(stp, pts);
+    } else {
+        Run_BlinkLEDs(stp, pts);
+        if (isEnable) {
+            if (isTxON)
+                sndr->sendDatagram(stp);
+            if (isRxON)
+                rcvr->setChannels(pts);
+        }
+
+        // network lag
+        debugInfo.append(QString("out: %1, inp: %2, delta: %3\n")
+                         .arg(sndr->pcks() - 1)
+                         .arg(rcvr->pcks())
+                         .arg(sndr->pcks() - rcvr->pcks() - 1)
+                         );
+    }
+
+    // debug info is shown on the screen
+    InfoText(stp, pts);
+    pts->dbgInfoText = debugInfo.toAscii();
+    isFirstRun = false;
+}

ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/plugin.h

@@ -0,0 +1,53 @@
+/**
+ ******************************************************************************
+ *
+ * @file       plugin.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
+ * @addtogroup 3rdParty Third-party integration
+ * @{
+ * @addtogroup AeroSimRC AeroSimRC proxy plugin
+ * @{
+ * @brief AeroSimRC simulator to HITL proxy plugin
+ *****************************************************************************/
+/*
+ * 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 PLUGIN_H
+#define PLUGIN_H
+
+#include <QtCore>
+#include <QTime>
+#include <QList>
+#include "aerosimrcdatastruct.h"
+
+#define SIM_DLL_EXPORT extern "C" __declspec(dllexport)
+
+SIM_DLL_EXPORT void AeroSIMRC_Plugin_ReportStructSizes(
+    quint32 *sizeSimToPlugin,
+    quint32 *sizePluginToSim,
+    quint32 *sizePluginInit
+);
+
+SIM_DLL_EXPORT void AeroSIMRC_Plugin_Init(
+    pluginInit *p
+);
+
+SIM_DLL_EXPORT void AeroSIMRC_Plugin_Run(
+    const simToPlugin *stp,
+    pluginToSim *pts
+);
+
+#endif // PLUGIN_H

ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/plugin.pro

@@ -0,0 +1,71 @@
+!win32 {
+    error("AeroSimRC plugin is only available for win32 platform")
+}
+
+include(../../../../../openpilotgcs.pri)
+
+QT += network
+QT -= gui
+
+TEMPLATE = lib
+TARGET = plugin_AeroSIMRC
+
+RES_DIR    = $${PWD}/resources
+SIM_DIR    = $$GCS_BUILD_TREE/../AeroSIM-RC
+PLUGIN_DIR = $$SIM_DIR/Plugin/CopterControl
+DLLDESTDIR = $$PLUGIN_DIR
+
+# Don't depend on MSVRT*.dll
+win32-msvc* {
+    QMAKE_CXXFLAGS_RELEASE -= -MD
+    QMAKE_CXXFLAGS_MT_DLL += -MT
+}
+
+HEADERS = \
+    aerosimrcdatastruct.h \
+    enums.h \
+    plugin.h \
+    qdebughandler.h \
+    udpconnect.h \
+    settings.h
+
+SOURCES = \
+    qdebughandler.cpp \
+    plugin.cpp \
+    udpconnect.cpp \
+    settings.cpp
+
+# Resemble the AeroSimRC directory structure and copy plugin files and resources
+equals(copydata, 1) {
+
+    # Windows release only
+    win32:CONFIG(release, debug|release) {
+
+        data_copy.commands += -@$(MKDIR) $$targetPath(\"$$PLUGIN_DIR\") $$addNewline()
+
+        # resources and sample configuration
+        PLUGIN_RESOURCES = \
+                cc_off.tga \
+                cc_off_hover.tga \
+                cc_on.tga \
+                cc_on_hover.tga \
+                cc_plugin.ini \
+                plugin.txt
+        for(res, PLUGIN_RESOURCES) {
+            data_copy.commands += $(COPY_FILE) $$targetPath(\"$$RES_DIR/$$res\") $$targetPath(\"$$PLUGIN_DIR/$$res\") $$addNewline()
+        }
+
+        # Qt DLLs
+        QT_DLLS = \
+                  libgcc_s_dw2-1.dll \
+                  mingwm10.dll \
+                  QtCore4.dll \
+                  QtNetwork4.dll
+        for(dll, QT_DLLS) {
+            data_copy.commands += $(COPY_FILE) $$targetPath(\"$$[QT_INSTALL_BINS]/$$dll\") $$targetPath(\"$$SIM_DIR/$$dll\") $$addNewline()
+        }
+
+        data_copy.target = FORCE
+        QMAKE_EXTRA_TARGETS += data_copy
+    }
+}

ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/qdebughandler.cpp

@@ -0,0 +1,64 @@
+/**
+ ******************************************************************************
+ *
+ * @file       qdebughandler.cpp
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
+ * @addtogroup 3rdParty Third-party integration
+ * @{
+ * @addtogroup AeroSimRC AeroSimRC proxy plugin
+ * @{
+ * @brief AeroSimRC simulator to HITL proxy plugin
+ *****************************************************************************/
+/*
+ * 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 "qdebughandler.h"
+
+void myQDebugHandler(QtMsgType type, const char *msg)
+{
+    static bool firstRun = true;
+    QString txt;
+
+    switch (type) {
+    case QtDebugMsg:
+        txt = QString("Debug: %1").arg(msg);
+        break;
+    case QtWarningMsg:
+        txt = QString("Warning: %1").arg(msg);
+        break;
+    case QtCriticalMsg:
+        txt = QString("Critical: %1").arg(msg);
+        break;
+    case QtFatalMsg:
+        txt = QString("Fatal: %1").arg(msg);
+        break;
+    }
+
+    QFile outFile("dbglog.txt");
+    outFile.open(QIODevice::WriteOnly | QIODevice::Append);
+    QTextStream ts(&outFile);
+    QTime time;
+
+    if (firstRun) {
+        ts << endl << endl;
+        firstRun = false;
+    }
+
+    ts << time.currentTime().toString("hh:mm:ss.zzz") << " " << txt << endl;
+
+    if (type == QtFatalMsg)
+        abort();
+}

ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/qdebughandler.h

@@ -0,0 +1,37 @@
+/**
+ ******************************************************************************
+ *
+ * @file       qdebughandler.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
+ * @addtogroup 3rdParty Third-party integration
+ * @{
+ * @addtogroup AeroSimRC AeroSimRC proxy plugin
+ * @{
+ * @brief AeroSimRC simulator to HITL proxy plugin
+ *****************************************************************************/
+/*
+ * 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 QDEBUGHANDLER_H
+#define QDEBUGHANDLER_H
+
+#include <QDebug>
+#include <QFile>
+#include <QTime>
+
+void myQDebugHandler(QtMsgType type, const char *msg);
+
+#endif // QDEBUGHANDLER_H

ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/resources/cc_plugin.ini

@@ -0,0 +1,55 @@
+[General]
+
+; Network settings
+listen_on_host = 127.0.0.1
+listen_on_port = 40200
+send_to_host = 127.0.0.1
+send_to_port = 40100
+
+; Channels enumerator, applicable for the AeroSIM RC version 3.90+
+all_channels = Ch1-Aileron Ch2-Elevator Ch3-Throttle Ch4-Rudder Ch5 Ch6 Ch7 Ch8 Ch9 Ch10-Retracts Ch11-Flaps Ch12-FPV-Pan Ch13-FPV-Tilt Ch14-Brakes Ch15-Spoilers Ch16-Smoke Ch17-Fire Ch18-Flight-Mode Ch19-ALT-Hold Ch20-FPV-Tilt-Hold Ch21-Reset-Model Ch22-MouseTX Ch23-Plugin-1 Ch24-Plugin-2 Ch25-Throttle-Hold Ch26-CareFree Ch27-FPV-Roll Ch28-L-Motor-Dual Ch29-R-Motor-Dual Ch30-Mix Ch31-Mix Ch32-Mix Ch33-Mix Ch34-Mix Ch35-Mix Ch36-Mix Ch37-Mix Ch38-Mix Ch39-Mix
+
+[Input]
+
+; Map CopterControl channels to simulator channels
+; To use internal simulator channels just comment the mapping here
+cc_channel_1 = Ch1-Aileron
+cc_channel_2 = Ch2-Elevator
+cc_channel_3 = Ch3-Throttle
+cc_channel_4 = Ch4-Rudder
+;cc_channel_5 = Ch27-FPV-Roll
+cc_channel_6 = Ch13-FPV-Tilt
+;cc_channel_7 = Ch12-FPV-Pan
+;cc_channel_8 =
+;cc_channel_9 =
+;cc_channel_10=
+
+; Control TX or RX (before or after mixes)
+send_to = RX
+
+[Output]
+
+; Map simulator channels to GCS HiTL/CopterControl channels
+; Only mapped channels will be sent to the GCS
+sim_channel_1 = Ch1-Aileron
+sim_channel_2 = Ch2-Elevator
+sim_channel_3 = Ch3-Throttle
+sim_channel_4 = Ch4-Rudder
+sim_channel_5 = Ch23-Plugin-1
+;sim_channel_6 = Ch27-FPV-Roll
+sim_channel_7 = Ch13-FPV-Tilt
+;sim_channel_8 = Ch12-FPV-Pan
+
+; take values from TX or RX (before or after mixes)
+take_from = TX
+
+[Video]
+
+; Windowed simulator mode configurations
+; Each resolution defines the upper left corner and width/hight.
+; User can cycle through all resolutions using the menu command.
+number_of_resolutions = 2
+
+; x, y, width, height
+resolution_1 = 50 50 640 720
+resolution_2 = 0 0 800 480

ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/resources/plugin.txt

@@ -0,0 +1,86 @@
+.NAME    "CopterControl"
+
+.HELP_EN "\
+OpenPilot CopterControl HiTL plugin for AeroSIM RC"
+
+.IMAGE_OFF             "cc_off.tga"
+.IMAGE_ON              "cc_on.tga"
+.IMAGE_OFF_MOUSE_HOVER "cc_off_hover.tga"
+.IMAGE_ON_MOUSE_HOVER  "cc_on_hover.tga"
+
+.MENU_ITEM_00_NAME              "Enable"
+.MENU_ITEM_00_TYPE              CHECKBOX
+.MENU_ITEM_00_HAS_SEPARATOR     0
+.MENU_ITEM_00_MOUSE_RECT_XY     89 91
+.MENU_ITEM_00_MOUSE_RECT_DXDY   48 48
+.MENU_ITEM_00_HIDE_MENU_ITEM    0
+
+.MENU_ITEM_01_NAME              "Transmit data"
+.MENU_ITEM_01_TYPE              CHECKBOX
+.MENU_ITEM_01_HAS_SEPARATOR     0
+.MENU_ITEM_01_MOUSE_RECT_XY     110 166
+.MENU_ITEM_01_MOUSE_RECT_DXDY   52 34
+.MENU_ITEM_01_HIDE_MENU_ITEM    0
+
+.MENU_ITEM_02_NAME              "Receive data"
+.MENU_ITEM_02_TYPE              CHECKBOX
+.MENU_ITEM_02_HAS_SEPARATOR     1
+.MENU_ITEM_02_MOUSE_RECT_XY     36 166
+.MENU_ITEM_02_MOUSE_RECT_DXDY   52 34
+.MENU_ITEM_02_HIDE_MENU_ITEM    0
+
+.MENU_ITEM_03_NAME              "Change window size and position"
+.MENU_ITEM_03_TYPE              COMMAND
+.MENU_ITEM_03_HAS_SEPARATOR     0
+.MENU_ITEM_03_MOUSE_RECT_XY     0 0
+.MENU_ITEM_03_MOUSE_RECT_DXDY   0 0
+.MENU_ITEM_03_HIDE_MENU_ITEM    0
+
+.MENU_ITEM_04_NAME              "Move to next waypoint"
+.MENU_ITEM_04_TYPE              COMMAND
+.MENU_ITEM_04_HAS_SEPARATOR     0
+.MENU_ITEM_04_MOUSE_RECT_XY     0 0
+.MENU_ITEM_04_MOUSE_RECT_DXDY   0 0
+.MENU_ITEM_04_HIDE_MENU_ITEM    0
+
+.MENU_ITEM_05_NAME              "no action"
+.MENU_ITEM_05_TYPE              COMMAND
+.MENU_ITEM_05_HAS_SEPARATOR     0
+.MENU_ITEM_05_MOUSE_RECT_XY     0 0
+.MENU_ITEM_05_MOUSE_RECT_DXDY   0 0
+.MENU_ITEM_05_HIDE_MENU_ITEM    0
+
+.MENU_ITEM_06_NAME              "Blue LED"
+.MENU_ITEM_06_TYPE              CHECKBOX
+.MENU_ITEM_06_HAS_SEPARATOR     0
+.MENU_ITEM_06_MOUSE_RECT_XY     6 40
+.MENU_ITEM_06_MOUSE_RECT_DXDY   15 12
+.MENU_ITEM_06_HIDE_MENU_ITEM    1
+
+.MENU_ITEM_07_NAME              "Green LED"
+.MENU_ITEM_07_TYPE              CHECKBOX
+.MENU_ITEM_07_HAS_SEPARATOR     0
+.MENU_ITEM_07_MOUSE_RECT_XY     6 52
+.MENU_ITEM_07_MOUSE_RECT_DXDY   15 12
+.MENU_ITEM_07_HIDE_MENU_ITEM    1
+
+.MENU_ITEM_08_NAME              "FlightMode 1"
+.MENU_ITEM_08_TYPE              CHECKBOX
+.MENU_ITEM_08_HAS_SEPARATOR     0
+.MENU_ITEM_08_MOUSE_RECT_XY     40 19
+.MENU_ITEM_08_MOUSE_RECT_DXDY   38 38
+.MENU_ITEM_08_HIDE_MENU_ITEM    1
+
+.MENU_ITEM_09_NAME              "FlightMode 2"
+.MENU_ITEM_09_TYPE              CHECKBOX
+.MENU_ITEM_09_HAS_SEPARATOR     0
+.MENU_ITEM_09_MOUSE_RECT_XY     78 19
+.MENU_ITEM_09_MOUSE_RECT_DXDY   38 38
+.MENU_ITEM_09_HIDE_MENU_ITEM    1
+
+.MENU_ITEM_10_NAME              "FlightMode 3"
+.MENU_ITEM_10_TYPE              CHECKBOX
+.MENU_ITEM_10_HAS_SEPARATOR     0
+.MENU_ITEM_10_MOUSE_RECT_XY     115 19
+.MENU_ITEM_10_MOUSE_RECT_DXDY   38 38
+.MENU_ITEM_10_HIDE_MENU_ITEM    1

ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/settings.cpp

@@ -0,0 +1,98 @@
+/**
+ ******************************************************************************
+ *
+ * @file       settings.cpp
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
+ * @addtogroup 3rdParty Third-party integration
+ * @{
+ * @addtogroup AeroSimRC AeroSimRC proxy plugin
+ * @{
+ * @brief AeroSimRC simulator to HITL proxy plugin
+ *****************************************************************************/
+/*
+ * 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 "settings.h"
+
+Settings::Settings(QString settingsPath, QObject *parent) :
+    QObject(parent)
+{
+   settings = new QSettings(settingsPath + "/cc_plugin.ini", QSettings::IniFormat);
+   // default settings
+   sendToHost = "127.0.0.1";
+   sendToPort = 40100;
+   listenOnHost = "127.0.0.1";
+   listenOnPort = 40200;
+   channels.reserve(60);
+   for (quint8 i = 0; i < 10; ++i)
+       inputMap << 255;
+   for (quint8 i = 0; i < 8; ++i)
+       outputMap << 255;
+   sendToRX = true;
+   takeFromTX = true;
+   videoModes << 1 << 50 << 50 << 800 << 600;
+}
+
+void Settings::read()
+{
+    // network
+    listenOnHost = settings->value("listen_on_host", listenOnHost).toString();
+    listenOnPort = settings->value("listen_on_port", listenOnPort).toInt();
+    sendToHost = settings->value("send_to_host", sendToHost).toString();
+    sendToPort = settings->value("send_to_port", sendToPort).toInt();
+
+    QString allChannels = settings->value("all_channels").toString();
+    QString chan;
+    int i = 0;
+    foreach (chan, allChannels.split(" "))
+        channels.insert(chan, i++);
+
+    // inputs
+    QString num = "";
+    QString map = "";
+    for (quint8 i = 0; i < 10; ++i) {
+        num = QString::number(i+1);
+        map = settings->value("Input/cc_channel_" + num).toString();
+        inputMap[i] = channels.value(map, inputMap.at(i));
+    }
+
+    QString sendTo = settings->value("Input/send_to", "RX").toString();
+    sendToRX = (sendTo == "RX") ? true : false;
+
+    // outputs
+    for (quint8 i = 0; i < 8; ++i) {
+        num = QString::number(i+1);
+        map = settings->value("Output/sim_channel_" + num).toString();
+        outputMap[i] = channels.value(map, outputMap.at(i));
+    }
+
+    QString takeFrom = settings->value("Output/take_from", "TX").toString();
+    takeFromTX = (takeFrom == "TX") ? true : false;
+
+    // video
+    quint8 resolutionNum = settings->value("Video/number_of_resolutions", 0).toInt();
+    if (resolutionNum > 0) {
+        videoModes.clear();
+        videoModes << resolutionNum;
+        for (quint8 i = 0; i < resolutionNum; ++i) {
+            num = QString::number(i+1);
+            QString modes = settings->value("Video/resolution_" + num, "0, 0, 640, 480").toString();
+            QString mode;
+            foreach (mode, modes.split(" "))
+                videoModes << mode.toInt();
+        }
+    }
+}

ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/settings.h

@@ -0,0 +1,67 @@
+/**
+ ******************************************************************************
+ *
+ * @file       settings.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
+ * @addtogroup 3rdParty Third-party integration
+ * @{
+ * @addtogroup AeroSimRC AeroSimRC proxy plugin
+ * @{
+ * @brief AeroSimRC simulator to HITL proxy plugin
+ *****************************************************************************/
+/*
+ * 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 SETTINGS_H
+#define SETTINGS_H
+
+#include <QObject>
+#include <QSettings>
+#include <QHash>
+#include <QList>
+#include <QStringList>
+#include <QDebug>
+
+class Settings : public QObject
+{
+public:
+    explicit Settings(QString settingsPath, QObject *parent = 0);
+    void read();
+    QString remoteHost() { return sendToHost; }
+    quint16 remotePort() { return sendToPort; }
+    QString localHost() { return listenOnHost; }
+    quint16 localPort() { return listenOnPort; }
+    QList<quint8> getInputMap() { return inputMap; }
+    QList<quint8> getOutputMap() { return outputMap; }
+    bool isToRX() { return sendToRX; }
+    bool isFromTX() { return takeFromTX; }
+    QList<quint16> getVideoModes() { return videoModes; }
+
+private:
+    QHash<QString, quint8> channels;
+    QSettings *settings;
+    QString sendToHost;
+    quint16 sendToPort;
+    QString listenOnHost;
+    quint16 listenOnPort;
+    QList<quint8> inputMap;
+    QList<quint8> outputMap;
+    bool sendToRX;
+    bool takeFromTX;
+    QList<quint16> videoModes;
+};
+
+#endif // SETTINGS_H

ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udpconnect.cpp

@@ -0,0 +1,228 @@
+/**
+ ******************************************************************************
+ *
+ * @file       udpconnect.cpp
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
+ * @addtogroup 3rdParty Third-party integration
+ * @{
+ * @addtogroup AeroSimRC AeroSimRC proxy plugin
+ * @{
+ * @brief AeroSimRC simulator to HITL proxy plugin
+ *****************************************************************************/
+/*
+ * 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 "udpconnect.h"
+#include "enums.h"
+
+UdpSender::UdpSender(const QList<quint8> map,
+                     bool isTX,
+                     QObject *parent)
+    : QObject(parent)
+{
+    qDebug() << this << "UdpSender::UdpSender thread:" << thread();
+    outSocket = NULL;
+    for (int i = 0; i < 8; ++i)
+        channels << 0.0;
+    channelsMap = map;
+    takeFromTX = isTX;
+    packetsSended = 0;
+}
+
+UdpSender::~UdpSender()
+{
+    qDebug() << this  << "UdpSender::~UdpSender";
+    if (outSocket)
+        delete outSocket;
+}
+
+// public
+void UdpSender::init(const QString &remoteHost, quint16 remotePort)
+{
+    qDebug() << this << "UdpSender::init";
+    outHost = remoteHost;
+    outPort = remotePort;
+    outSocket = new QUdpSocket();
+}
+
+void UdpSender::sendDatagram(const simToPlugin *stp)
+{
+    QByteArray data;
+    data.resize(188);
+    QDataStream out(&data, QIODevice::WriteOnly);
+    out.setFloatingPointPrecision(QDataStream::SinglePrecision);
+
+    // magic header, "AERO"
+    out << quint32(0x4153494D);
+    // simulation step
+    out << stp->simTimeStep;
+    // home location
+    out << stp->initPosX    << stp->initPosY    << stp->initPosZ;
+    out << stp->wpHomeX     << stp->wpHomeY     << stp->wpHomeLat   << stp->wpHomeLong;
+    // position
+    out << stp->posX        << stp->posY        << stp->posZ;
+    // velocity (world)
+    out << stp->velX        << stp->velY        << stp->velZ;
+    // angular velocity (model)
+    out << stp->angVelXm    << stp->angVelYm    << stp->angVelZm;
+    // acceleration (model)
+    out << stp->accelXm     << stp->accelYm     << stp->accelZm;
+    // coordinates
+    out << stp->latitude    << stp->longitude;
+    // sonar
+    out << stp->AGL;
+    // attitude
+    out << stp->heading     << stp->pitch       << stp->roll;
+    // electric
+    out << stp->voltage     << stp->current;
+    // matrix
+    out << stp->axisXx << stp->axisXy << stp->axisXz;
+    out << stp->axisYx << stp->axisYy << stp->axisYz;
+    out << stp->axisZx << stp->axisZy << stp->axisZz;
+    // channels
+    for (int i = 0; i < 8; ++i) {
+        quint8 mapTo = channelsMap.at(i);
+        if (mapTo == 255)       // unused channel
+            out << 0.0;
+        else if (takeFromTX)    // use values from simulators transmitter
+            out << stp->chSimTX[mapTo];
+        else                    // direct use values from ESC/motors/ailerons/etc
+            out << stp->chSimRX[mapTo];
+    }
+
+    // packet counter
+    out << packetsSended;
+
+    outSocket->writeDatagram(data, outHost, outPort);
+    ++packetsSended;
+}
+
+//-----------------------------------------------------------------------------
+
+UdpReceiver::UdpReceiver(const QList<quint8> map,
+                         bool isRX,
+                         QObject *parent)
+    : QThread(parent)
+{
+    qDebug() << this << "UdpReceiver::UdpReceiver thread:" << thread();
+
+    stopped = false;
+    inSocket = NULL;
+    for (int i = 0; i < 10; ++i)
+        channels << -1.0;
+    channelsMap = map;
+    sendToRX = isRX;
+    armed = 0;
+    mode = 0;
+    packetsRecived = 1;
+}
+
+UdpReceiver::~UdpReceiver()
+{
+    qDebug() << this  << "UdpReceiver::~UdpReceiver";
+    if (inSocket)
+        delete inSocket;
+}
+
+// public
+void UdpReceiver::init(const QString &localHost, quint16 localPort)
+{
+    qDebug() << this << "UdpReceiver::init";
+
+    inSocket = new QUdpSocket();
+    qDebug() << this << "inSocket constructed" << inSocket->thread();
+
+    inSocket->bind(QHostAddress(localHost), localPort);
+}
+
+void UdpReceiver::run()
+{
+    qDebug() << this << "UdpReceiver::run start";
+    while (!stopped)
+        onReadyRead();
+    qDebug() << this << "UdpReceiver::run ended";
+}
+
+void UdpReceiver::stop()
+{
+    qDebug() << this << "UdpReceiver::stop";
+    stopped = true;
+}
+
+void UdpReceiver::setChannels(pluginToSim *pts)
+{
+    QMutexLocker locker(&mutex);
+
+    for (int i = 0; i < 10; ++i) {
+        quint8 mapTo = channelsMap.at(i);
+        if (mapTo != 255) {
+            float channelValue = qBound(-1.0f, channels.at(i), 1.0f);
+            if (sendToRX) {
+                // direct connect to ESC/motors/ailerons/etc
+                pts->chNewRX[mapTo] = channelValue;
+                pts->chOverRX[mapTo] = true;
+            } else {
+                // replace simulators transmitter
+                pts->chNewTX[mapTo] = channelValue;
+                pts->chOverTX[mapTo] = true;
+            }
+        }
+    }
+}
+
+void UdpReceiver::getFlighStatus(quint8 &arm, quint8 &mod)
+{
+    QMutexLocker locker(&mutex);
+
+    arm = armed;
+    mod = mode;
+}
+
+// private
+void UdpReceiver::onReadyRead()
+{
+    if (!inSocket->waitForReadyRead(8)) // 1/60fps ~= 16.7ms, 1/120fps ~= 8.3ms
+        return;
+    // TODO: add failsafe
+    // if a command not recieved then slowly return all channel to neutral
+    //
+    while (inSocket->hasPendingDatagrams()) {
+        QByteArray datagram;
+        datagram.resize(inSocket->pendingDatagramSize());
+        quint64 datagramSize;
+        datagramSize = inSocket->readDatagram(datagram.data(), datagram.size());
+
+        processDatagram(datagram);
+    }
+}
+
+void UdpReceiver::processDatagram(QByteArray &datagram)
+{
+    QDataStream stream(datagram);
+    stream.setFloatingPointPrecision(QDataStream::SinglePrecision);
+    // check magic header
+    quint32 magic;
+    stream >> magic;
+    if (magic != 0x52434D44) // "RCMD"
+        return;
+    // read channels
+    for (int i = 0; i < 10; ++i)
+        stream >> channels[i];
+    // read flight mode
+    stream >> armed >> mode;
+    // read counter
+    stream >> packetsRecived;
+}

ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udpconnect.h

@@ -0,0 +1,90 @@
+/**
+ ******************************************************************************
+ *
+ * @file       udpconnect.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
+ * @addtogroup 3rdParty Third-party integration
+ * @{
+ * @addtogroup AeroSimRC AeroSimRC proxy plugin
+ * @{
+ * @brief AeroSimRC simulator to HITL proxy plugin
+ *****************************************************************************/
+/*
+ * 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 UDPCONNECT_H
+#define UDPCONNECT_H
+
+#include <QObject>
+#include <QUdpSocket>
+#include <QList>
+#include <QTime>
+#include <QMutex>
+#include <QMutexLocker>
+#include "aerosimrcdatastruct.h"
+
+class UdpSender : public QObject
+{
+//    Q_OBJECT
+public:
+    explicit UdpSender(const QList<quint8> map, bool isTX, QObject *parent = 0);
+    ~UdpSender();
+    void init(const QString &remoteHost, quint16 remotePort);
+    void sendDatagram(const simToPlugin *stp);
+    quint32 pcks() { return packetsSended; }
+
+private:
+    QUdpSocket *outSocket;
+    QHostAddress outHost;
+    quint16 outPort;
+    QList<float> channels;
+    QList<quint8> channelsMap;
+    bool takeFromTX;
+    quint32 packetsSended;
+};
+
+
+class UdpReceiver : public QThread
+{
+//    Q_OBJECT
+public:
+    explicit UdpReceiver(const QList<quint8> map, bool isRX, QObject *parent = 0);
+    ~UdpReceiver();
+    void init(const QString &localHost, quint16 localPort);
+    void run();
+    void stop();
+    // function getChannels for other threads
+    void setChannels(pluginToSim *pts);
+    void getFlighStatus(quint8 &arm, quint8 &mod);
+    quint8 getArmed() { return armed; }
+    quint8 getMode() { return mode; }
+    quint32 pcks() { return packetsRecived; }
+
+private:
+    volatile bool stopped;
+    QMutex mutex;
+    QUdpSocket *inSocket;
+    QList<float> channels;
+    QList<quint8> channelsMap;
+    bool sendToRX;
+    quint8 armed;
+    quint8 mode;
+    quint32 packetsRecived;
+    void onReadyRead();
+    void processDatagram(QByteArray &datagram);
+};
+
+#endif // UDPCONNECT_H

ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udptest.pro

@@ -0,0 +1,17 @@
+include(../../../../../openpilotgcs.pri)
+
+QT += core gui network
+
+TEMPLATE = app
+TARGET = udp_test
+DESTDIR = $$GCS_APP_PATH
+
+HEADERS += \
+    udptestwidget.h
+
+SOURCES += \
+    udptestmain.cpp \
+    udptestwidget.cpp
+
+FORMS += \
+    udptestwidget.ui

ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udptestmain.cpp

@@ -0,0 +1,38 @@
+/**
+ ******************************************************************************
+ *
+ * @file       udptestmain.cpp
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
+ * @addtogroup 3rdParty Third-party integration
+ * @{
+ * @addtogroup AeroSimRC AeroSimRC proxy plugin
+ * @{
+ * @brief AeroSimRC simulator to HITL proxy plugin test utility
+ *****************************************************************************/
+/*
+ * 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 <QApplication>
+#include "udptestwidget.h"
+
+int main(int argc, char *argv[])
+{
+    QApplication a(argc, argv);
+    Widget w;
+    w.show();
+
+    return a.exec();
+}

ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udptestwidget.cpp

@@ -0,0 +1,537 @@
+/**
+ ******************************************************************************
+ *
+ * @file       udptestwidget.cpp
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
+ * @addtogroup 3rdParty Third-party integration
+ * @{
+ * @addtogroup AeroSimRC AeroSimRC proxy plugin
+ * @{
+ * @brief AeroSimRC simulator to HITL proxy plugin test utility
+ *****************************************************************************/
+/*
+ * 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 "udptestwidget.h"
+#include "ui_udptestwidget.h"
+
+Widget::Widget(QWidget *parent) :
+    QWidget(parent),
+    ui(new Ui::Widget)
+{
+    ui->setupUi(this);
+
+    inSocket = NULL;
+    outSocket = NULL;
+    screenTimeout.start();
+    packetCounter = 0;
+
+    autoSendTimer = new QTimer(this);
+    connect(autoSendTimer, SIGNAL(timeout()), this, SLOT(sendDatagram()), Qt::DirectConnection);
+}
+
+Widget::~Widget()
+{
+    if(outSocket) {
+        delete outSocket;
+    }
+    if(inSocket) {
+        delete inSocket;
+    }
+    delete ui;
+}
+
+// private slots //////////////////////////////////////////////////////////////
+
+void Widget::on_btReciveStart_clicked()
+{
+    on_btReciveStop_clicked();
+
+    inSocket = new QUdpSocket();
+    QString host = ui->localHost->text();
+    quint16 port = ui->localPort->text().toInt();
+
+    if (inSocket->bind(QHostAddress(host), port)) {
+        connect(inSocket, SIGNAL(readyRead()),
+                this, SLOT(readDatagram()), Qt::DirectConnection);
+
+        ui->listWidget->addItem("bind ok");
+        ui->btReciveStop->setEnabled(1);
+        ui->localHost->setDisabled(1);
+        ui->localPort->setDisabled(1);
+        ui->btReciveStart->setDisabled(1);
+    } else {
+        ui->listWidget->addItem("bind error: " + inSocket->errorString());
+    }
+}
+
+void Widget::on_btReciveStop_clicked()
+{
+    if(inSocket) {
+        delete inSocket;
+        inSocket = NULL;
+        ui->listWidget->addItem("unbind ok");
+    } else {
+        ui->listWidget->addItem("socket not found");
+    }
+    ui->btReciveStart->setEnabled(1);
+    ui->localHost->setEnabled(1);
+    ui->localPort->setEnabled(1);
+    ui->btReciveStop->setDisabled(1);
+}
+
+void Widget::on_btTransmitStart_clicked()
+{
+    on_btTransmitStop_clicked();
+
+    outSocket = new QUdpSocket();
+    outHost = ui->simHost->text();
+    outPort = ui->simPort->text().toInt();
+
+    ui->listWidget->addItem("transmit started");
+    ui->btTransmitStop->setEnabled(1);
+    ui->simHost->setDisabled(1);
+    ui->simPort->setDisabled(1);
+    ui->btTransmitStart->setDisabled(1);
+}
+
+void Widget::on_btTransmitStop_clicked()
+{
+    if(outSocket) {
+        delete outSocket;
+        outSocket = NULL;
+        ui->listWidget->addItem("transmit stopped");
+    } else {
+        ui->listWidget->addItem("transmit socket not found");
+    }
+    ui->btTransmitStart->setEnabled(1);
+    ui->simHost->setEnabled(1);
+    ui->simPort->setEnabled(1);
+    ui->btTransmitStop->setDisabled(1);
+}
+
+void Widget::readDatagram()
+{
+    while (inSocket->hasPendingDatagrams()) {
+        QByteArray datagram;
+        datagram.resize(inSocket->pendingDatagramSize());
+        QHostAddress sender;
+        quint16 senderPort;
+        quint64 datagramSize = inSocket->readDatagram(datagram.data(), datagram.size(),
+                                                      &sender, &senderPort);
+        Q_UNUSED(datagramSize);
+
+        processDatagram(datagram);
+        if (ui->autoAnswer->isChecked())
+            sendDatagram();
+    }
+}
+
+// private ////////////////////////////////////////////////////////////////////
+
+void Widget::processDatagram(const QByteArray &data)
+{
+    QByteArray buf = data;
+    QDataStream stream(&buf, QIODevice::ReadOnly);
+    stream.setFloatingPointPrecision(QDataStream::SinglePrecision);
+
+    // check magic header
+    quint32 magic;
+    stream >> magic;
+    if (magic == 0x4153494D) {  // "AERO"
+        float   timeStep,
+                homeX, homeY, homeZ,
+                WpHX, WpHY, WpLat, WpLon,
+                posX, posY, posZ,
+                velX, velY, velZ,
+                angX, angY, angZ,
+                accX, accY, accZ,
+                lat, lon, alt,
+                head, pitch, roll,
+                volt, curr,
+                rx, ry, rz, fx, fy, fz, ux, uy, uz,
+                chAil, chEle, chThr, chRud, chPlg1, chPlg2, chFpv1, chFpv2;
+
+        stream >> timeStep;
+        stream >> homeX >> homeY >> homeZ;
+        stream >> WpHX >> WpHY >> WpLat >> WpLon;
+        stream >> posX >> posY >> posZ;
+        stream >> velX >> velY >> velZ;
+        stream >> angX >> angY >> angZ;
+        stream >> accX >> accY >> accZ;
+        stream >> lat >> lon >> alt;
+        stream >> head >> pitch >> roll;
+        stream >> volt >> curr;
+        stream >> rx >> ry >> rz >> fx >> fy >> fz >> ux >> uy >> uz;
+        stream >> chAil >> chEle >> chThr >> chRud >> chPlg1 >> chPlg2 >> chFpv1 >> chFpv2;
+        stream >> packetCounter;
+
+        if(ui->tabWidget->currentIndex() != 0)
+            return;
+
+        if (screenTimeout.elapsed() < 200)
+            return;
+
+        ui->listWidget->clear();
+        /*
+        ui->listWidget->addItem("time step (s)");
+        ui->listWidget->addItem(QString("%1")
+                                .arg(timeStep);
+        ui->listWidget->addItem("home location (m)");
+        ui->listWidget->addItem(QString("%1, %2, %3")
+                                .arg(homeX, 7, 'f', 4)
+                                .arg(homeY, 7, 'f', 4)
+                                .arg(homeZ, 7, 'f', 4));
+        ui->listWidget->addItem("home waypoint");
+        ui->listWidget->addItem(QString("%1, %2, %3, %4")
+                                .arg(WpHX, 7, 'f', 4)
+                                .arg(WpHY, 7, 'f', 4)
+                                .arg(WpLat, 7, 'f', 4)
+                                .arg(WpLon, 7, 'f', 4));
+        ui->listWidget->addItem("model position (m)");
+        ui->listWidget->addItem(QString("%1, %2, %3")
+                                .arg(posX, 7, 'f', 4)
+                                .arg(posY, 7, 'f', 4)
+                                .arg(posZ, 7, 'f', 4));
+        ui->listWidget->addItem("model velocity (m/s)");
+        ui->listWidget->addItem(QString("%1, %2, %3")
+                                .arg(velX, 7, 'f', 4)
+                                .arg(velY, 7, 'f', 4)
+                                .arg(velZ, 7, 'f', 4));
+        ui->listWidget->addItem("model angular velocity (rad/s)");
+        ui->listWidget->addItem(QString("%1, %2, %3")
+                                .arg(angX, 7, 'f', 4)
+                                .arg(angY, 7, 'f', 4)
+                                .arg(angZ, 7, 'f', 4));
+        ui->listWidget->addItem("model acceleration (m/s/s)");
+        ui->listWidget->addItem(QString("%1, %2, %3")
+                                .arg(accX, 7, 'f', 4)
+                                .arg(accY, 7, 'f', 4)
+                                .arg(accZ, 7, 'f', 4));
+        ui->listWidget->addItem("model coordinates (deg, deg, m)");
+        ui->listWidget->addItem(QString("%1, %2, %3")
+                                .arg(lat, 7, 'f', 4)
+                                .arg(lon, 7, 'f', 4)
+                                .arg(alt, 7, 'f', 4));
+        ui->listWidget->addItem("model electrics");
+        ui->listWidget->addItem(QString("%1V, %2A")
+                                .arg(volt, 7, 'f', 4)
+                                .arg(curr, 7, 'f', 4));
+        ui->listWidget->addItem("channels");
+        ui->listWidget->addItem(QString("%1 %2 %3 %4 %5 %6 %7 %8")
+                                .arg(chAil, 6, 'f', 3)
+                                .arg(chEle, 6, 'f', 3)
+                                .arg(chThr, 6, 'f', 3)
+                                .arg(chRud, 6, 'f', 3)
+                                .arg(chPlg1, 6, 'f', 3)
+                                .arg(chPlg2, 6, 'f', 3)
+                                .arg(chFpv1, 6, 'f', 3)
+                                .arg(chFpv2, 6, 'f', 3));
+        ui->listWidget->addItem("datagram size (bytes), packet counter");
+        ui->listWidget->addItem(QString("%1 %2")
+                                .arg(data.size())
+                                .arg(packetCounter));
+*/
+
+        // matrix calculation start
+        // model matrix
+        QMatrix4x4 m = QMatrix4x4( fy,  fx, -fz,  0.0,
+                                   ry,  rx, -rz,  0.0,
+                                  -uy, -ux,  uz,  0.0,
+                                  0.0, 0.0, 0.0,  1.0);
+        m.optimize();
+
+        // world matrix
+        QMatrix4x4 w = m.inverted();
+
+        // model quat
+        QQuaternion q;
+        asMatrix2Quat(m, q);
+
+        // model roll, pitch, yaw
+        QVector3D rpy;
+        asMatrix2RPY(m, rpy);
+
+        // sonar
+        float sAlt = 5.0;
+        if ((alt < (sAlt * 2.0)) && (roll < 0.35) && (pitch < 0.35)) {
+            float x = alt * qTan(roll);
+            float y = alt * qTan(pitch);
+            float h = QVector3D(x, y, alt).length();
+            sAlt = qMin(h, sAlt);
+        }
+
+        ui->listWidget->addItem("sonar altitude");
+        ui->listWidget->addItem(QString("%1")
+                                .arg(sAlt, 8, 'f', 5));
+        ui->listWidget->addItem("vectors");
+        ui->listWidget->addItem(QString("       X       Y       Z"));
+        ui->listWidget->addItem(QString("R: %1 %2 %3\nF: %4 %5 %6\nU: %7 %8 %9")
+                                .arg(rx, 8, 'f', 5).arg(ry, 8, 'f', 5).arg(rz, 8, 'f', 5)
+                                .arg(fx, 8, 'f', 5).arg(fy, 8, 'f', 5).arg(fz, 8, 'f', 5)
+                                .arg(ux, 8, 'f', 5).arg(uy, 8, 'f', 5).arg(uz, 8, 'f', 5));
+        ui->listWidget->addItem("CC model matrix");
+        ui->listWidget->addItem(QString("   %1 %2 %3\n   %4 %5 %6\n   %7 %8 %9")
+                                .arg(m(0, 0), 8, 'f', 5).arg(m(0, 1), 8, 'f', 5).arg(m(0, 2), 8, 'f', 5)
+                                .arg(m(1, 0), 8, 'f', 5).arg(m(1, 1), 8, 'f', 5).arg(m(1, 2), 8, 'f', 5)
+                                .arg(m(2, 0), 8, 'f', 5).arg(m(2, 1), 8, 'f', 5).arg(m(2, 2), 8, 'f', 5));
+        ui->listWidget->addItem("CC world matrix");
+        ui->listWidget->addItem(QString("   %1 %2 %3\n   %4 %5 %6\n   %7 %8 %9")
+                                .arg(w(0, 0), 8, 'f', 5).arg(w(0, 1), 8, 'f', 5).arg(w(0, 2), 8, 'f', 5)
+                                .arg(w(1, 0), 8, 'f', 5).arg(w(1, 1), 8, 'f', 5).arg(w(1, 2), 8, 'f', 5)
+                                .arg(w(2, 0), 8, 'f', 5).arg(w(2, 1), 8, 'f', 5).arg(w(2, 2), 8, 'f', 5));
+        ui->listWidget->addItem("CC quaternion");
+        ui->listWidget->addItem(QString("%1, %2, %3, %4")
+                                .arg(q.x(), 7, 'f', 4)
+                                .arg(q.y(), 7, 'f', 4)
+                                .arg(q.z(), 7, 'f', 4)
+                                .arg(q.scalar(), 7, 'f', 4));
+        ui->listWidget->addItem("model attitude (deg)");
+        ui->listWidget->addItem(QString("%1, %2, %3")
+                                .arg(roll*RAD2DEG, 7, 'f', 4)
+                                .arg(pitch*RAD2DEG, 7, 'f', 4)
+                                .arg(head*RAD2DEG, 7, 'f', 4));
+        ui->listWidget->addItem("CC attitude calculated (deg)");
+        ui->listWidget->addItem(QString("%1, %2, %3")
+                                .arg(rpy.x(), 7, 'f', 4)
+                                .arg(rpy.y(), 7, 'f', 4)
+                                .arg(rpy.z(), 7, 'f', 4));
+
+        screenTimeout.restart();
+
+    } else if (magic == 0x52434D44) { // "RCMD"
+        qreal ch1, ch2, ch3, ch4, ch5, ch6, ch7, ch8, ch9, ch10;
+        stream >> ch1 >> ch2 >> ch3 >> ch4 >> ch5 >> ch6 >> ch7 >> ch8 >> ch9 >> ch10;
+        quint8 armed, mode;
+        stream >> armed >> mode;
+
+        if(ui->tabWidget->currentIndex() == 0) {
+            if (screenTimeout.elapsed() < 200)
+                return;
+            ui->listWidget->clear();
+            ui->listWidget->addItem("channels");
+            ui->listWidget->addItem("CH1: " + QString::number(ch1));
+            ui->listWidget->addItem("CH2: " + QString::number(ch2));
+            ui->listWidget->addItem("CH3: " + QString::number(ch3));
+            ui->listWidget->addItem("CH4: " + QString::number(ch4));
+            ui->listWidget->addItem("CH5: " + QString::number(ch5));
+            ui->listWidget->addItem("CH6: " + QString::number(ch6));
+            ui->listWidget->addItem("CH7: " + QString::number(ch7));
+            ui->listWidget->addItem("CH8: " + QString::number(ch8));
+            ui->listWidget->addItem("CH9: " + QString::number(ch9));
+            ui->listWidget->addItem("CH10:" + QString::number(ch10));
+            ui->listWidget->addItem("armed:" + QString::number(armed));
+            ui->listWidget->addItem("fmode:" + QString::number(mode));
+        }
+        screenTimeout.restart();
+    } else {
+        qDebug() << "unknown magic:" << magic;
+    }
+}
+
+void Widget::sendDatagram()
+{
+    if(!outSocket)
+        return;
+
+    float ch[10];   // = {0,0,0,0,0,0,0,0,0,0};
+    quint8 armed;
+    quint8 fmode;
+    const float coeff = 1.0 / 512.0;
+
+    ch[0] = ui->ch1->value() * coeff;
+    ch[1] = ui->ch2->value() * coeff;
+    ch[2] = ui->ch3->value() * coeff;
+    ch[3] = ui->ch4->value() * coeff;
+    ch[4] = ui->ch5->value() * coeff;
+    ch[5] = ui->ch6->value() * coeff;
+    ch[6] = ui->ch7->value() * coeff;
+    ch[7] = ui->ch8->value() * coeff;
+    ch[8] = ui->ch9->value() * coeff;
+    ch[9] = ui->ch10->value() * coeff;
+
+    armed = (ui->disarmed->isChecked()) ? 0 : (ui->arming->isChecked()) ? 1 : 2;
+    fmode = ui->flightMode->value();
+
+    QByteArray data;
+    // 50 - current size of values, 4(quint32) + 10*4(float) + 2*1(quint8) + 4(quint32)
+    data.resize(50);
+    QDataStream stream(&data, QIODevice::WriteOnly);
+    stream.setFloatingPointPrecision(QDataStream::SinglePrecision);
+
+    // magic header, "RCMD"
+    stream << quint32(0x52434D44);
+    // send channels
+    for (int i = 0; i < 10; ++i) {
+        stream << ch[i];
+    }
+    // send armed and mode
+    stream << armed << fmode;
+    // send readed counter
+    stream << packetCounter;
+
+    if (outSocket->writeDatagram(data, outHost, outPort) == -1) {
+        qDebug() << "write failed: outHost" << outHost << " "
+                 << "outPort " << outPort << " "
+                 << outSocket->errorString();
+    }
+}
+
+void Widget::on_autoSend_clicked()
+{
+    autoSendTimer->start(100);
+    qDebug() << "timer start";
+}
+
+void Widget::on_autoAnswer_clicked()
+{
+    autoSendTimer->stop();
+    qDebug() << "timer stop";
+}
+
+// transfomations
+
+void Widget::asMatrix2Quat(const QMatrix4x4 &m, QQuaternion &q)
+{
+    qreal w, x, y, z;
+
+    // w always >= 0
+    w = qSqrt(qMax(0.0, 1.0 + m(0, 0) + m(1, 1) + m(2, 2))) / 2.0;
+    x = qSqrt(qMax(0.0, 1.0 + m(0, 0) - m(1, 1) - m(2, 2))) / 2.0;
+    y = qSqrt(qMax(0.0, 1.0 - m(0, 0) + m(1, 1) - m(2, 2))) / 2.0;
+    z = qSqrt(qMax(0.0, 1.0 - m(0, 0) - m(1, 1) + m(2, 2))) / 2.0;
+
+    x = copysign(x, (m(1, 2) - m(2, 1)));
+    y = copysign(y, (m(2, 0) - m(0, 2)));
+    z = copysign(z, (m(0, 1) - m(1, 0)));
+
+    q.setScalar(w);
+    q.setX(x);
+    q.setY(y);
+    q.setZ(z);
+}
+
+void Widget::asQuat2RPY(const QQuaternion &q, QVector3D &rpy)
+{
+    qreal roll;
+    qreal pitch;
+    qreal yaw;
+
+    const qreal d2 = 2.0;
+    const qreal qss = q.scalar() * q.scalar();
+    const qreal qxx = q.x() * q.x();
+    const qreal qyy = q.y() * q.y();
+    const qreal qzz = q.z() * q.z();
+
+    qreal test = -d2 * (q.x() * q.z() - q.scalar() * q.y());
+    if (qFabs(test) > 0.998) {
+        // ~86.3°, gimbal lock
+        qreal R10 = d2 * (q.x() * q.y() - q.scalar() * q.z());
+        qreal R11 = qss - qxx + qyy - qzz;
+
+        roll = 0.0;
+        pitch = copysign(M_PI_2, test);
+        yaw = qAtan2(-R10, R11);
+    } else {
+        qreal R12 = d2 * (q.y() * q.z() + q.scalar() * q.x());
+        qreal R22 = qss - qxx - qyy + qzz;
+        qreal R01 = d2 * (q.x() * q.y() + q.scalar() * q.z());
+        qreal R00 = qss + qxx - qyy - qzz;
+
+        roll    = qAtan2(R12, R22);
+        pitch   = qAsin(test);
+        yaw     = qAtan2(R01, R00);
+    }
+    rpy.setX(RAD2DEG * roll);
+    rpy.setY(RAD2DEG * pitch);
+    rpy.setZ(RAD2DEG * yaw);
+}
+
+void Widget::asMatrix2RPY(const QMatrix4x4 &m, QVector3D &rpy)
+{
+    qreal roll;
+    qreal pitch;
+    qreal yaw;
+
+    if (qFabs(m(0, 2)) > 0.998) {
+        // ~86.3°, gimbal lock
+        roll  = 0.0;
+        pitch = copysign(M_PI_2, -m(0, 2));
+        yaw   = qAtan2(-m(1, 0), m(1, 1));
+    } else {
+        roll  = qAtan2(m(1, 2), m(2, 2));
+        pitch = qAsin(-m(0, 2));
+        yaw   = qAtan2(m(0, 1), m(0, 0));
+    }
+
+    rpy.setX(roll  * RAD2DEG);
+    rpy.setY(pitch * RAD2DEG);
+    rpy.setZ(yaw   * RAD2DEG);
+}
+
+/* // not used
+
+void Widget::ccRPY2Quat(const QVector3D &rpy, QQuaternion &q)
+{
+    float phi, theta, psi;
+    float cphi, sphi, ctheta, stheta, cpsi, spsi;
+
+    phi    = rpy.x() / 2;
+    theta  = rpy.y() / 2;
+    psi    = rpy.z() / 2;
+    cphi   = cosf(phi);
+    sphi   = sinf(phi);
+    ctheta = cosf(theta);
+    stheta = sinf(theta);
+    cpsi   = cosf(psi);
+    spsi   = sinf(psi);
+
+    q.setScalar(cphi * ctheta * cpsi + sphi * stheta * spsi);
+    q.setX(sphi * ctheta * cpsi - cphi * stheta * spsi);
+    q.setY(cphi * stheta * cpsi + sphi * ctheta * spsi);
+    q.setZ(cphi * ctheta * spsi - sphi * stheta * cpsi);
+
+    if (q.scalar() < 0) {   // q0 always positive for uniqueness
+        q.setScalar(-q.scalar());
+        q.setX(-q.x());
+        q.setY(-q.y());
+        q.setZ(-q.z());
+    }
+}
+
+void Widget::ccQuat2Matrix(const QQuaternion &q, QMatrix4x4 &m)
+{
+    float q0s = q.scalar() * q.scalar();
+    float q1s = q.x() * q.x();
+    float q2s = q.y() * q.y();
+    float q3s = q.z() * q.z();
+
+    float m00 = q0s + q1s - q2s - q3s;
+    float m01 = 2 * (q.x() * q.y() + q.scalar() * q.z());
+    float m02 = 2 * (q.x() * q.z() - q.scalar() * q.y());
+    float m10 = 2 * (q.x() * q.y() - q.scalar() * q.z());
+    float m11 = q0s - q1s + q2s - q3s;
+    float m12 = 2 * (q.y() * q.z() + q.scalar() * q.x());
+    float m20 = 2 * (q.x() * q.z() + q.scalar() * q.y());
+    float m21 = 2 * (q.y() * q.z() - q.scalar() * q.x());
+    float m22 = q0s - q1s - q2s + q3s;
+
+    m = QMatrix4x4(m00, m01, m02, 0,
+                   m10, m11, m12, 0,
+                   m20, m21, m22, 0,
+                   0,   0,   0,   1);
+}
+*/

ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udptestwidget.h

@@ -0,0 +1,94 @@
+/**
+ ******************************************************************************
+ *
+ * @file       udptestwidget.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
+ * @addtogroup 3rdParty Third-party integration
+ * @{
+ * @addtogroup AeroSimRC AeroSimRC proxy plugin
+ * @{
+ * @brief AeroSimRC simulator to HITL proxy plugin test utility
+ *****************************************************************************/
+/*
+ * 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 UDPTESTWIDGET_H
+#define UDPTESTWIDGET_H
+
+#include <QWidget>
+#include <QUdpSocket>
+#include <QTime>
+#if defined(Q_CC_MSVC)
+#define _USE_MATH_DEFINES
+#endif
+#include <qmath.h>
+#include <QVector3D>
+#include <QMatrix4x4>
+#include <QDebug>
+#include <QTimer>
+
+namespace Ui {
+    class Widget;
+}
+
+const float RAD2DEG = (float)(180.0/M_PI);
+const float DEG2RAD = (float)(M_PI/180.0);
+
+class Widget : public QWidget
+{
+    Q_OBJECT
+
+public:
+    explicit Widget(QWidget *parent = 0);
+    ~Widget();
+
+private slots:
+    void on_btReciveStart_clicked();
+    void on_btReciveStop_clicked();
+    void on_btTransmitStart_clicked();
+    void on_btTransmitStop_clicked();
+
+    void readDatagram();
+    void sendDatagram();
+
+    void on_autoSend_clicked();
+
+    void on_autoAnswer_clicked();
+
+private:
+    Ui::Widget *ui;
+
+    QTime screenTimeout;
+    QUdpSocket *inSocket;
+    QUdpSocket *outSocket;
+    QHostAddress outHost;
+    quint16 outPort;
+    quint32 packetCounter;
+
+    void processDatagram(const QByteArray &data);
+    QTimer *autoSendTimer;
+
+    void asMatrix2Quat(const QMatrix4x4 &m, QQuaternion &q);
+    void asMatrix2RPY(const QMatrix4x4 &m, QVector3D &rpy);
+    void asQuat2RPY(const QQuaternion &q, QVector3D &rpy);
+
+/*  // not used
+ *  void ccRPY2Quat(const QVector3D &rpy, QQuaternion &q);
+ *  void ccQuat2Matrix(const QQuaternion &q, QMatrix4x4 &m);
+ */
+};
+
+#endif // UDPTESTWIDGET_H

ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udptestwidget.ui

@@ -0,0 +1,940 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>Widget</class>
+ <widget class="QWidget" name="Widget">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>440</width>
+    <height>525</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string notr="true">udp_test</string>
+  </property>
+  <property name="windowFilePath">
+   <string notr="true"/>
+  </property>
+  <layout class="QVBoxLayout" name="verticalLayout">
+   <item>
+    <layout class="QGridLayout" name="gridLayout">
+     <item row="0" column="4">
+      <widget class="QPushButton" name="btReciveStart">
+       <property name="minimumSize">
+        <size>
+         <width>60</width>
+         <height>0</height>
+        </size>
+       </property>
+       <property name="text">
+        <string notr="true">Connect</string>
+       </property>
+      </widget>
+     </item>
+     <item row="0" column="3">
+      <widget class="QLineEdit" name="localPort">
+       <property name="enabled">
+        <bool>true</bool>
+       </property>
+       <property name="sizePolicy">
+        <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
+         <horstretch>0</horstretch>
+         <verstretch>0</verstretch>
+        </sizepolicy>
+       </property>
+       <property name="minimumSize">
+        <size>
+         <width>50</width>
+         <height>0</height>
+        </size>
+       </property>
+       <property name="text">
+        <string notr="true">40100</string>
+       </property>
+       <property name="maxLength">
+        <number>5</number>
+       </property>
+      </widget>
+     </item>
+     <item row="0" column="1">
+      <widget class="QLineEdit" name="localHost">
+       <property name="enabled">
+        <bool>true</bool>
+       </property>
+       <property name="sizePolicy">
+        <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
+         <horstretch>0</horstretch>
+         <verstretch>0</verstretch>
+        </sizepolicy>
+       </property>
+       <property name="text">
+        <string notr="true">127.0.0.1</string>
+       </property>
+      </widget>
+     </item>
+     <item row="1" column="0">
+      <widget class="QLabel" name="lbLocalHost">
+       <property name="text">
+        <string notr="true">sim host</string>
+       </property>
+      </widget>
+     </item>
+     <item row="0" column="0">
+      <widget class="QLabel" name="lbSimHost">
+       <property name="text">
+        <string notr="true">local host:</string>
+       </property>
+      </widget>
+     </item>
+     <item row="0" column="2">
+      <widget class="QLabel" name="lbSimPort">
+       <property name="text">
+        <string notr="true">local port:</string>
+       </property>
+      </widget>
+     </item>
+     <item row="0" column="5">
+      <widget class="QPushButton" name="btReciveStop">
+       <property name="enabled">
+        <bool>false</bool>
+       </property>
+       <property name="minimumSize">
+        <size>
+         <width>60</width>
+         <height>0</height>
+        </size>
+       </property>
+       <property name="text">
+        <string notr="true">Disconnect</string>
+       </property>
+      </widget>
+     </item>
+     <item row="1" column="1">
+      <widget class="QLineEdit" name="simHost">
+       <property name="enabled">
+        <bool>true</bool>
+       </property>
+       <property name="sizePolicy">
+        <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
+         <horstretch>0</horstretch>
+         <verstretch>0</verstretch>
+        </sizepolicy>
+       </property>
+       <property name="text">
+        <string notr="true">127.0.0.1</string>
+       </property>
+      </widget>
+     </item>
+     <item row="1" column="2">
+      <widget class="QLabel" name="lbLocalPort">
+       <property name="text">
+        <string notr="true">sim port</string>
+       </property>
+      </widget>
+     </item>
+     <item row="1" column="3">
+      <widget class="QLineEdit" name="simPort">
+       <property name="sizePolicy">
+        <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
+         <horstretch>0</horstretch>
+         <verstretch>0</verstretch>
+        </sizepolicy>
+       </property>
+       <property name="minimumSize">
+        <size>
+         <width>50</width>
+         <height>0</height>
+        </size>
+       </property>
+       <property name="text">
+        <string notr="true">40200</string>
+       </property>
+       <property name="maxLength">
+        <number>5</number>
+       </property>
+      </widget>
+     </item>
+     <item row="1" column="4">
+      <widget class="QPushButton" name="btTransmitStart">
+       <property name="minimumSize">
+        <size>
+         <width>60</width>
+         <height>0</height>
+        </size>
+       </property>
+       <property name="text">
+        <string notr="true">Transmit</string>
+       </property>
+      </widget>
+     </item>
+     <item row="1" column="5">
+      <widget class="QPushButton" name="btTransmitStop">
+       <property name="enabled">
+        <bool>false</bool>
+       </property>
+       <property name="minimumSize">
+        <size>
+         <width>60</width>
+         <height>0</height>
+        </size>
+       </property>
+       <property name="text">
+        <string notr="true">Stop</string>
+       </property>
+      </widget>
+     </item>
+    </layout>
+   </item>
+   <item>
+    <widget class="QTabWidget" name="tabWidget">
+     <property name="currentIndex">
+      <number>0</number>
+     </property>
+     <widget class="QWidget" name="tab_1">
+      <attribute name="title">
+       <string notr="true">raw data</string>
+      </attribute>
+      <layout class="QVBoxLayout" name="verticalLayout_2">
+       <item>
+        <widget class="QListWidget" name="listWidget">
+         <property name="font">
+          <font>
+           <family>Terminal</family>
+           <pointsize>10</pointsize>
+          </font>
+         </property>
+        </widget>
+       </item>
+      </layout>
+     </widget>
+     <widget class="QWidget" name="tab_2">
+      <attribute name="title">
+       <string notr="true">channels</string>
+      </attribute>
+      <layout class="QVBoxLayout" name="verticalLayout_3">
+       <property name="spacing">
+        <number>0</number>
+       </property>
+       <property name="bottomMargin">
+        <number>9</number>
+       </property>
+       <item>
+        <widget class="QGroupBox" name="groupBox">
+         <property name="title">
+          <string>send data</string>
+         </property>
+         <layout class="QHBoxLayout" name="horizontalLayout_2">
+          <property name="spacing">
+           <number>6</number>
+          </property>
+          <property name="leftMargin">
+           <number>9</number>
+          </property>
+          <property name="topMargin">
+           <number>0</number>
+          </property>
+          <property name="bottomMargin">
+           <number>3</number>
+          </property>
+          <item>
+           <widget class="QRadioButton" name="autoAnswer">
+            <property name="enabled">
+             <bool>true</bool>
+            </property>
+            <property name="text">
+             <string notr="true">answer on recieve</string>
+            </property>
+            <property name="checked">
+             <bool>true</bool>
+            </property>
+           </widget>
+          </item>
+          <item>
+           <widget class="QRadioButton" name="autoSend">
+            <property name="enabled">
+             <bool>true</bool>
+            </property>
+            <property name="text">
+             <string notr="true">auto send</string>
+            </property>
+            <property name="checked">
+             <bool>false</bool>
+            </property>
+           </widget>
+          </item>
+         </layout>
+        </widget>
+       </item>
+       <item>
+        <widget class="QGroupBox" name="groupBox_2">
+         <property name="title">
+          <string>Flight mode</string>
+         </property>
+         <layout class="QHBoxLayout" name="horizontalLayout">
+          <property name="spacing">
+           <number>6</number>
+          </property>
+          <property name="leftMargin">
+           <number>9</number>
+          </property>
+          <property name="topMargin">
+           <number>0</number>
+          </property>
+          <property name="bottomMargin">
+           <number>3</number>
+          </property>
+          <item>
+           <widget class="QSlider" name="flightMode">
+            <property name="maximum">
+             <number>6</number>
+            </property>
+            <property name="pageStep">
+             <number>1</number>
+            </property>
+            <property name="orientation">
+             <enum>Qt::Horizontal</enum>
+            </property>
+            <property name="tickPosition">
+             <enum>QSlider::TicksAbove</enum>
+            </property>
+            <property name="tickInterval">
+             <number>1</number>
+            </property>
+           </widget>
+          </item>
+         </layout>
+        </widget>
+       </item>
+       <item>
+        <widget class="QGroupBox" name="groupBox_3">
+         <property name="title">
+          <string>Armed state</string>
+         </property>
+         <layout class="QHBoxLayout" name="horizontalLayout_3">
+          <property name="spacing">
+           <number>6</number>
+          </property>
+          <property name="leftMargin">
+           <number>9</number>
+          </property>
+          <property name="topMargin">
+           <number>0</number>
+          </property>
+          <property name="bottomMargin">
+           <number>3</number>
+          </property>
+          <item>
+           <widget class="QRadioButton" name="disarmed">
+            <property name="text">
+             <string>Disarmed</string>
+            </property>
+            <property name="checked">
+             <bool>true</bool>
+            </property>
+           </widget>
+          </item>
+          <item>
+           <widget class="QRadioButton" name="arming">
+            <property name="text">
+             <string>Arming</string>
+            </property>
+           </widget>
+          </item>
+          <item>
+           <widget class="QRadioButton" name="armed">
+            <property name="text">
+             <string>Armed</string>
+            </property>
+           </widget>
+          </item>
+         </layout>
+        </widget>
+       </item>
+       <item>
+        <widget class="QGroupBox" name="groupBox_4">
+         <property name="title">
+          <string>Channels</string>
+         </property>
+         <layout class="QGridLayout" name="gridLayout_3">
+          <property name="leftMargin">
+           <number>9</number>
+          </property>
+          <property name="topMargin">
+           <number>0</number>
+          </property>
+          <property name="rightMargin">
+           <number>9</number>
+          </property>
+          <property name="bottomMargin">
+           <number>3</number>
+          </property>
+          <property name="spacing">
+           <number>3</number>
+          </property>
+          <item row="0" column="0">
+           <widget class="QLabel" name="lbCH1n">
+            <property name="text">
+             <string notr="true">CH1</string>
+            </property>
+            <property name="alignment">
+             <set>Qt::AlignCenter</set>
+            </property>
+           </widget>
+          </item>
+          <item row="0" column="1">
+           <widget class="QSlider" name="ch1">
+            <property name="minimum">
+             <number>-511</number>
+            </property>
+            <property name="maximum">
+             <number>512</number>
+            </property>
+            <property name="singleStep">
+             <number>16</number>
+            </property>
+            <property name="pageStep">
+             <number>32</number>
+            </property>
+            <property name="orientation">
+             <enum>Qt::Horizontal</enum>
+            </property>
+            <property name="tickPosition">
+             <enum>QSlider::TicksAbove</enum>
+            </property>
+            <property name="tickInterval">
+             <number>128</number>
+            </property>
+           </widget>
+          </item>
+          <item row="0" column="2">
+           <widget class="QLabel" name="lbCH1">
+            <property name="sizePolicy">
+             <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+              <horstretch>0</horstretch>
+              <verstretch>0</verstretch>
+             </sizepolicy>
+            </property>
+            <property name="minimumSize">
+             <size>
+              <width>30</width>
+              <height>0</height>
+             </size>
+            </property>
+            <property name="text">
+             <string notr="true">0</string>
+            </property>
+           </widget>
+          </item>
+          <item row="1" column="0">
+           <widget class="QLabel" name="lbCH2n">
+            <property name="text">
+             <string notr="true">CH2</string>
+            </property>
+            <property name="alignment">
+             <set>Qt::AlignCenter</set>
+            </property>
+           </widget>
+          </item>
+          <item row="1" column="1">
+           <widget class="QSlider" name="ch2">
+            <property name="minimum">
+             <number>-511</number>
+            </property>
+            <property name="maximum">
+             <number>512</number>
+            </property>
+            <property name="singleStep">
+             <number>16</number>
+            </property>
+            <property name="pageStep">
+             <number>32</number>
+            </property>
+            <property name="orientation">
+             <enum>Qt::Horizontal</enum>
+            </property>
+            <property name="tickPosition">
+             <enum>QSlider::TicksAbove</enum>
+            </property>
+            <property name="tickInterval">
+             <number>128</number>
+            </property>
+           </widget>
+          </item>
+          <item row="1" column="2">
+           <widget class="QLabel" name="lbCH2">
+            <property name="sizePolicy">
+             <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+              <horstretch>0</horstretch>
+              <verstretch>0</verstretch>
+             </sizepolicy>
+            </property>
+            <property name="minimumSize">
+             <size>
+              <width>30</width>
+              <height>0</height>
+             </size>
+            </property>
+            <property name="text">
+             <string notr="true">0</string>
+            </property>
+           </widget>
+          </item>
+          <item row="2" column="0">
+           <widget class="QLabel" name="lbCH3n">
+            <property name="text">
+             <string notr="true">CH3</string>
+            </property>
+            <property name="alignment">
+             <set>Qt::AlignCenter</set>
+            </property>
+           </widget>
+          </item>
+          <item row="2" column="1">
+           <widget class="QSlider" name="ch3">
+            <property name="minimum">
+             <number>-511</number>
+            </property>
+            <property name="maximum">
+             <number>512</number>
+            </property>
+            <property name="singleStep">
+             <number>16</number>
+            </property>
+            <property name="pageStep">
+             <number>32</number>
+            </property>
+            <property name="orientation">
+             <enum>Qt::Horizontal</enum>
+            </property>
+            <property name="tickPosition">
+             <enum>QSlider::TicksAbove</enum>
+            </property>
+            <property name="tickInterval">
+             <number>128</number>
+            </property>
+           </widget>
+          </item>
+          <item row="2" column="2">
+           <widget class="QLabel" name="lbCH3">
+            <property name="sizePolicy">
+             <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+              <horstretch>0</horstretch>
+              <verstretch>0</verstretch>
+             </sizepolicy>
+            </property>
+            <property name="minimumSize">
+             <size>
+              <width>30</width>
+              <height>0</height>
+             </size>
+            </property>
+            <property name="text">
+             <string notr="true">0</string>
+            </property>
+           </widget>
+          </item>
+          <item row="3" column="0">
+           <widget class="QLabel" name="lbCH4n">
+            <property name="text">
+             <string notr="true">CH4</string>
+            </property>
+            <property name="alignment">
+             <set>Qt::AlignCenter</set>
+            </property>
+           </widget>
+          </item>
+          <item row="3" column="1">
+           <widget class="QSlider" name="ch4">
+            <property name="minimum">
+             <number>-511</number>
+            </property>
+            <property name="maximum">
+             <number>512</number>
+            </property>
+            <property name="singleStep">
+             <number>16</number>
+            </property>
+            <property name="pageStep">
+             <number>32</number>
+            </property>
+            <property name="orientation">
+             <enum>Qt::Horizontal</enum>
+            </property>
+            <property name="tickPosition">
+             <enum>QSlider::TicksAbove</enum>
+            </property>
+            <property name="tickInterval">
+             <number>128</number>
+            </property>
+           </widget>
+          </item>
+          <item row="3" column="2">
+           <widget class="QLabel" name="lbCH4">
+            <property name="sizePolicy">
+             <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+              <horstretch>0</horstretch>
+              <verstretch>0</verstretch>
+             </sizepolicy>
+            </property>
+            <property name="minimumSize">
+             <size>
+              <width>30</width>
+              <height>0</height>
+             </size>
+            </property>
+            <property name="text">
+             <string notr="true">0</string>
+            </property>
+           </widget>
+          </item>
+          <item row="4" column="0">
+           <widget class="QLabel" name="lbCH5n">
+            <property name="text">
+             <string notr="true">CH5</string>
+            </property>
+            <property name="alignment">
+             <set>Qt::AlignCenter</set>
+            </property>
+           </widget>
+          </item>
+          <item row="4" column="1">
+           <widget class="QSlider" name="ch5">
+            <property name="minimum">
+             <number>-511</number>
+            </property>
+            <property name="maximum">
+             <number>512</number>
+            </property>
+            <property name="singleStep">
+             <number>16</number>
+            </property>
+            <property name="pageStep">
+             <number>32</number>
+            </property>
+            <property name="orientation">
+             <enum>Qt::Horizontal</enum>
+            </property>
+            <property name="tickPosition">
+             <enum>QSlider::TicksAbove</enum>
+            </property>
+            <property name="tickInterval">
+             <number>128</number>
+            </property>
+           </widget>
+          </item>
+          <item row="4" column="2">
+           <widget class="QLabel" name="lbCH5">
+            <property name="sizePolicy">
+             <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+              <horstretch>0</horstretch>
+              <verstretch>0</verstretch>
+             </sizepolicy>
+            </property>
+            <property name="minimumSize">
+             <size>
+              <width>30</width>
+              <height>0</height>
+             </size>
+            </property>
+            <property name="text">
+             <string notr="true">0</string>
+            </property>
+           </widget>
+          </item>
+          <item row="5" column="0">
+           <widget class="QLabel" name="lbCH6n">
+            <property name="text">
+             <string notr="true">CH6</string>
+            </property>
+            <property name="alignment">
+             <set>Qt::AlignCenter</set>
+            </property>
+           </widget>
+          </item>
+          <item row="5" column="2">
+           <widget class="QLabel" name="lbCH6">
+            <property name="sizePolicy">
+             <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+              <horstretch>0</horstretch>
+              <verstretch>0</verstretch>
+             </sizepolicy>
+            </property>
+            <property name="minimumSize">
+             <size>
+              <width>30</width>
+              <height>0</height>
+             </size>
+            </property>
+            <property name="text">
+             <string notr="true">0</string>
+            </property>
+           </widget>
+          </item>
+          <item row="6" column="0">
+           <widget class="QLabel" name="lbCH7n">
+            <property name="text">
+             <string notr="true">Ch7</string>
+            </property>
+            <property name="alignment">
+             <set>Qt::AlignCenter</set>
+            </property>
+           </widget>
+          </item>
+          <item row="6" column="2">
+           <widget class="QLabel" name="lbCH7">
+            <property name="sizePolicy">
+             <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+              <horstretch>0</horstretch>
+              <verstretch>0</verstretch>
+             </sizepolicy>
+            </property>
+            <property name="minimumSize">
+             <size>
+              <width>30</width>
+              <height>0</height>
+             </size>
+            </property>
+            <property name="text">
+             <string notr="true">0</string>
+            </property>
+           </widget>
+          </item>
+          <item row="7" column="0">
+           <widget class="QLabel" name="lbCH8n">
+            <property name="text">
+             <string notr="true">Ch8</string>
+            </property>
+            <property name="alignment">
+             <set>Qt::AlignCenter</set>
+            </property>
+           </widget>
+          </item>
+          <item row="7" column="2">
+           <widget class="QLabel" name="lbCH8">
+            <property name="sizePolicy">
+             <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+              <horstretch>0</horstretch>
+              <verstretch>0</verstretch>
+             </sizepolicy>
+            </property>
+            <property name="minimumSize">
+             <size>
+              <width>30</width>
+              <height>0</height>
+             </size>
+            </property>
+            <property name="text">
+             <string notr="true">0</string>
+            </property>
+           </widget>
+          </item>
+          <item row="8" column="0">
+           <widget class="QLabel" name="lbCH9n">
+            <property name="text">
+             <string notr="true">Ch9</string>
+            </property>
+            <property name="alignment">
+             <set>Qt::AlignCenter</set>
+            </property>
+           </widget>
+          </item>
+          <item row="8" column="1">
+           <widget class="QSlider" name="ch9">
+            <property name="minimum">
+             <number>-511</number>
+            </property>
+            <property name="maximum">
+             <number>512</number>
+            </property>
+            <property name="singleStep">
+             <number>16</number>
+            </property>
+            <property name="pageStep">
+             <number>32</number>
+            </property>
+            <property name="orientation">
+             <enum>Qt::Horizontal</enum>
+            </property>
+            <property name="tickPosition">
+             <enum>QSlider::TicksAbove</enum>
+            </property>
+            <property name="tickInterval">
+             <number>128</number>
+            </property>
+           </widget>
+          </item>
+          <item row="8" column="2">
+           <widget class="QLabel" name="lbCH9">
+            <property name="sizePolicy">
+             <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+              <horstretch>0</horstretch>
+              <verstretch>0</verstretch>
+             </sizepolicy>
+            </property>
+            <property name="minimumSize">
+             <size>
+              <width>30</width>
+              <height>0</height>
+             </size>
+            </property>
+            <property name="text">
+             <string notr="true">0</string>
+            </property>
+           </widget>
+          </item>
+          <item row="9" column="0">
+           <widget class="QLabel" name="lbCH10n">
+            <property name="text">
+             <string notr="true">Ch10</string>
+            </property>
+            <property name="alignment">
+             <set>Qt::AlignCenter</set>
+            </property>
+           </widget>
+          </item>
+          <item row="9" column="1">
+           <widget class="QSlider" name="ch10">
+            <property name="minimum">
+             <number>-511</number>
+            </property>
+            <property name="maximum">
+             <number>512</number>
+            </property>
+            <property name="singleStep">
+             <number>16</number>
+            </property>
+            <property name="pageStep">
+             <number>32</number>
+            </property>
+            <property name="orientation">
+             <enum>Qt::Horizontal</enum>
+            </property>
+            <property name="tickPosition">
+             <enum>QSlider::TicksAbove</enum>
+            </property>
+            <property name="tickInterval">
+             <number>128</number>
+            </property>
+           </widget>
+          </item>
+          <item row="9" column="2">
+           <widget class="QLabel" name="lbCH10">
+            <property name="sizePolicy">
+             <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+              <horstretch>0</horstretch>
+              <verstretch>0</verstretch>
+             </sizepolicy>
+            </property>
+            <property name="minimumSize">
+             <size>
+              <width>30</width>
+              <height>0</height>
+             </size>
+            </property>
+            <property name="text">
+             <string notr="true">0</string>
+            </property>
+           </widget>
+          </item>
+          <item row="5" column="1">
+           <widget class="QSlider" name="ch6">
+            <property name="minimum">
+             <number>-511</number>
+            </property>
+            <property name="maximum">
+             <number>512</number>
+            </property>
+            <property name="singleStep">
+             <number>16</number>
+            </property>
+            <property name="pageStep">
+             <number>32</number>
+            </property>
+            <property name="orientation">
+             <enum>Qt::Horizontal</enum>
+            </property>
+            <property name="tickPosition">
+             <enum>QSlider::TicksAbove</enum>
+            </property>
+            <property name="tickInterval">
+             <number>128</number>
+            </property>
+           </widget>
+          </item>
+          <item row="6" column="1">
+           <widget class="QSlider" name="ch7">
+            <property name="minimum">
+             <number>-511</number>
+            </property>
+            <property name="maximum">
+             <number>512</number>
+            </property>
+            <property name="singleStep">
+             <number>16</number>
+            </property>
+            <property name="pageStep">
+             <number>32</number>
+            </property>
+            <property name="orientation">
+             <enum>Qt::Horizontal</enum>
+            </property>
+            <property name="tickPosition">
+             <enum>QSlider::TicksAbove</enum>
+            </property>
+            <property name="tickInterval">
+             <number>128</number>
+            </property>
+           </widget>
+          </item>
+          <item row="7" column="1">
+           <widget class="QSlider" name="ch8">
+            <property name="minimum">
+             <number>-511</number>
+            </property>
+            <property name="maximum">
+             <number>512</number>
+            </property>
+            <property name="singleStep">
+             <number>16</number>
+            </property>
+            <property name="pageStep">
+             <number>32</number>
+            </property>
+            <property name="orientation">
+             <enum>Qt::Horizontal</enum>
+            </property>
+            <property name="tickPosition">
+             <enum>QSlider::TicksAbove</enum>
+            </property>
+            <property name="tickInterval">
+             <number>128</number>
+            </property>
+           </widget>
+          </item>
+         </layout>
+        </widget>
+       </item>
+       <item>
+        <spacer name="verticalSpacer">
+         <property name="orientation">
+          <enum>Qt::Vertical</enum>
+         </property>
+         <property name="sizeHint" stdset="0">
+          <size>
+           <width>20</width>
+           <height>40</height>
+          </size>
+         </property>
+        </spacer>
+       </item>
+      </layout>
+     </widget>
+    </widget>
+   </item>
+  </layout>
+ </widget>
+ <layoutdefault spacing="6" margin="11"/>
+ <resources/>
+ <connections/>
+</ui>

ground/openpilotgcs/src/plugins/hitlv2/hitlv2.pro

@@ -1,27 +1,5 @@
-TEMPLATE = lib
+TEMPLATE = subdirs
-TARGET = HITLv2
+
-QT += network
+SUBDIRS = plugin aerosimrc
-include(../../openpilotgcsplugin.pri)
+
-include(hitlv2_dependencies.pri)
+plugin.file = plugin.pro
-HEADERS += \
-    aerosimrc.h \
-    hitlv2configuration.h \
-    hitlv2factory.h \
-    hitlv2gadget.h \
-    hitlv2optionspage.h \
-    hitlv2plugin.h \
-    hitlv2widget.h \
-    simulatorv2.h
-SOURCES += \
-    aerosimrc.cpp \
-    hitlv2configuration.cpp \
-    hitlv2factory.cpp \
-    hitlv2gadget.cpp \
-    hitlv2optionspage.cpp \
-    hitlv2plugin.cpp \
-    hitlv2widget.cpp \
-    simulatorv2.cpp
-OTHER_FILES += hitlv2.pluginspec
-FORMS += \
-    hitlv2optionspage.ui \
-    hitlv2widget.ui

ground/openpilotgcs/src/plugins/hitlv2/hitlv2configuration.cpp

@@ -1,13 +1,13 @@
 /**
  ******************************************************************************
  *
- * @file       hitlconfiguration.cpp
+ * @file       hitlv2configuration.cpp
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
  * @addtogroup GCSPlugins GCS Plugins
  * @{
- * @addtogroup HITLPlugin HITL Plugin
+ * @addtogroup HITLPlugin HITLv2 Plugin
  * @{
- * @brief The Hardware In The Loop plugin
+ * @brief The Hardware In The Loop plugin version 2
  *****************************************************************************/
 /*
  * This program is free software; you can redistribute it and/or modify

ground/openpilotgcs/src/plugins/hitlv2/hitlv2configuration.h

@@ -1,13 +1,13 @@
 /**
  ******************************************************************************
  *
- * @file       hitlconfiguration.h
+ * @file       hitlv2configuration.h
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
  * @addtogroup GCSPlugins GCS Plugins
  * @{
- * @addtogroup HITLPlugin HITL Plugin
+ * @addtogroup HITLPlugin HITLv2 Plugin
  * @{
- * @brief The Hardware In The Loop plugin
+ * @brief The Hardware In The Loop plugin version 2
  *****************************************************************************/
 /*
  * This program is free software; you can redistribute it and/or modify

ground/openpilotgcs/src/plugins/hitlv2/hitlv2factory.cpp

@@ -1,13 +1,13 @@
 /**
  ******************************************************************************
  *
- * @file       hitlfactory.cpp
+ * @file       hitlv2factory.cpp
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
  * @addtogroup GCSPlugins GCS Plugins
  * @{
- * @addtogroup HITLPlugin HITL Plugin
+ * @addtogroup HITLPlugin HITLv2 Plugin
  * @{
- * @brief The Hardware In The Loop plugin
+ * @brief The Hardware In The Loop plugin version 2
  *****************************************************************************/
 /*
  * This program is free software; you can redistribute it and/or modify
@@ -24,6 +24,7 @@
  * with this program; if not, write to the Free Software Foundation, Inc.,
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
+
 #include "hitlv2factory.h"
 #include "hitlv2widget.h"
 #include "hitlv2gadget.h"

ground/openpilotgcs/src/plugins/hitlv2/hitlv2factory.h

@@ -1,13 +1,13 @@
 /**
  ******************************************************************************
  *
- * @file       hitlfactory.h
+ * @file       hitlv2factory.h
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
  * @addtogroup GCSPlugins GCS Plugins
  * @{
- * @addtogroup HITLPlugin HITL Plugin
+ * @addtogroup HITLPlugin HITLv2 Plugin
  * @{
- * @brief The Hardware In The Loop plugin
+ * @brief The Hardware In The Loop plugin version 2
  *****************************************************************************/
 /*
  * This program is free software; you can redistribute it and/or modify

ground/openpilotgcs/src/plugins/hitlv2/hitlv2gadget.cpp

@@ -1,13 +1,13 @@
 /**
  ******************************************************************************
  *
- * @file       hitl.cpp
+ * @file       hitlv2gadget.cpp
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
  * @addtogroup GCSPlugins GCS Plugins
  * @{
- * @addtogroup HITLPlugin HITL Plugin
+ * @addtogroup HITLPlugin HITLv2 Plugin
  * @{
- * @brief The Hardware In The Loop plugin
+ * @brief The Hardware In The Loop plugin version 2
  *****************************************************************************/
 /*
  * This program is free software; you can redistribute it and/or modify
@@ -24,6 +24,7 @@
  * with this program; if not, write to the Free Software Foundation, Inc.,
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
+
 #include "hitlv2gadget.h"
 #include "hitlv2widget.h"
 #include "hitlv2configuration.h"

ground/openpilotgcs/src/plugins/hitlv2/hitlv2gadget.h

@@ -1,13 +1,13 @@
 /**
  ******************************************************************************
  *
- * @file       hitl.h
+ * @file       hitlv2gadget.h
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
  * @addtogroup GCSPlugins GCS Plugins
  * @{
- * @addtogroup HITLPlugin HITL Plugin
+ * @addtogroup HITLPlugin HITLv2 Plugin
  * @{
- * @brief The Hardware In The Loop plugin
+ * @brief The Hardware In The Loop plugin version 2
  *****************************************************************************/
 /*
  * This program is free software; you can redistribute it and/or modify

ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.cpp

@@ -1,13 +1,13 @@
 /**
  ******************************************************************************
  *
- * @file       hitloptionspage.cpp
+ * @file       hitlv2optionspage.cpp
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
  * @addtogroup GCSPlugins GCS Plugins
  * @{
- * @addtogroup HITLPlugin HITL Plugin
+ * @addtogroup HITLPlugin HITLv2 Plugin
  * @{
- * @brief The Hardware In The Loop plugin
+ * @brief The Hardware In The Loop plugin version 2
  *****************************************************************************/
 /*
  * This program is free software; you can redistribute it and/or modify

ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.h

@@ -1,13 +1,13 @@
 /**
  ******************************************************************************
  *
- * @file       hitloptionspage.h
+ * @file       hitlv2optionspage.h
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
  * @addtogroup GCSPlugins GCS Plugins
  * @{
- * @addtogroup HITLPlugin HITL Plugin
+ * @addtogroup HITLPlugin HITLv2 Plugin
  * @{
- * @brief The Hardware In The Loop plugin
+ * @brief The Hardware In The Loop plugin version 2
  *****************************************************************************/
 /*
  * This program is free software; you can redistribute it and/or modify

ground/openpilotgcs/src/plugins/hitlv2/hitlv2plugin.cpp

@@ -1,13 +1,13 @@
 /**
  ******************************************************************************
  *
- * @file       mapplugin.cpp
+ * @file       hitlv2plugin.cpp
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
  * @addtogroup GCSPlugins GCS Plugins
  * @{
- * @addtogroup HITLPlugin HITL Plugin
+ * @addtogroup HITLPlugin HITLv2 Plugin
  * @{
- * @brief The Hardware In The Loop plugin
+ * @brief The Hardware In The Loop plugin version 2
  *****************************************************************************/
 /*
  * This program is free software; you can redistribute it and/or modify
@@ -24,6 +24,7 @@
  * with this program; if not, write to the Free Software Foundation, Inc.,
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
+
 #include "hitlv2plugin.h"
 #include "hitlv2factory.h"
 #include <QtPlugin>

ground/openpilotgcs/src/plugins/hitlv2/hitlv2plugin.h

@@ -1,13 +1,13 @@
 /**
  ******************************************************************************
  *
- * @file       browserplugin.h
+ * @file       hitlv2plugin.h
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
  * @addtogroup GCSPlugins GCS Plugins
  * @{
- * @addtogroup HITLPlugin HITL Plugin
+ * @addtogroup HITLPlugin HITLv2 Plugin
  * @{
- * @brief The Hardware In The Loop plugin
+ * @brief The Hardware In The Loop plugin version 2
  *****************************************************************************/
 /*
  * This program is free software; you can redistribute it and/or modify

ground/openpilotgcs/src/plugins/hitlv2/hitlv2widget.cpp

@@ -1,13 +1,13 @@
 /**
  ******************************************************************************
  *
- * @file       hitlwidget.cpp
+ * @file       hitlv2widget.cpp
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
  * @addtogroup GCSPlugins GCS Plugins
  * @{
- * @addtogroup HITLPlugin HITL Plugin
+ * @addtogroup HITLPlugin HITLv2 Plugin
  * @{
- * @brief The Hardware In The Loop plugin
+ * @brief The Hardware In The Loop plugin version 2
  *****************************************************************************/
 /*
  * This program is free software; you can redistribute it and/or modify
@@ -24,6 +24,7 @@
  * with this program; if not, write to the Free Software Foundation, Inc.,
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
+
 #include "hitlv2widget.h"
 #include "ui_hitlv2widget.h"
 #include <QDebug>

ground/openpilotgcs/src/plugins/hitlv2/hitlv2widget.h

@@ -1,13 +1,13 @@
 /**
  ******************************************************************************
  *
- * @file       hitlwidget.h
+ * @file       hitlv2widget.h
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
  * @addtogroup GCSPlugins GCS Plugins
  * @{
- * @addtogroup HITLPlugin HITL Plugin
+ * @addtogroup HITLPlugin HITLv2 Plugin
  * @{
- * @brief The Hardware In The Loop plugin
+ * @brief The Hardware In The Loop plugin version 2
  *****************************************************************************/
 /*
  * This program is free software; you can redistribute it and/or modify

ground/openpilotgcs/src/plugins/hitlv2/plugin.pro

@@ -0,0 +1,32 @@
+TEMPLATE = lib
+TARGET = HITLv2
+QT += network
+
+include(../../openpilotgcsplugin.pri)
+include(hitlv2_dependencies.pri)
+
+HEADERS += \
+    aerosimrc.h \
+    hitlv2configuration.h \
+    hitlv2factory.h \
+    hitlv2gadget.h \
+    hitlv2optionspage.h \
+    hitlv2plugin.h \
+    hitlv2widget.h \
+    simulatorv2.h
+
+SOURCES += \
+    aerosimrc.cpp \
+    hitlv2configuration.cpp \
+    hitlv2factory.cpp \
+    hitlv2gadget.cpp \
+    hitlv2optionspage.cpp \
+    hitlv2plugin.cpp \
+    hitlv2widget.cpp \
+    simulatorv2.cpp
+
+FORMS += \
+    hitlv2optionspage.ui \
+    hitlv2widget.ui
+
+OTHER_FILES += hitlv2.pluginspec

ground/openpilotgcs/src/plugins/hitlv2/simulatorv2.cpp

@@ -1,13 +1,13 @@
 /**
  ******************************************************************************
  *
- * @file       simulator.cpp
+ * @file       simulatorv2.cpp
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
  * @addtogroup GCSPlugins GCS Plugins
  * @{
- * @addtogroup HITLPlugin HITL Plugin
+ * @addtogroup HITLPlugin HITLv2 Plugin
  * @{
- * @brief The Hardware In The Loop plugin
+ * @brief The Hardware In The Loop plugin version 2
  *****************************************************************************/
 /*
  * This program is free software; you can redistribute it and/or modify

ground/openpilotgcs/src/plugins/hitlv2/simulatorv2.h

@@ -1,13 +1,13 @@
 /**
  ******************************************************************************
  *
- * @file       simulator.h
+ * @file       simulatorv2.h
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
  * @addtogroup GCSPlugins GCS Plugins
  * @{
- * @addtogroup HITLPlugin HITL Plugin
+ * @addtogroup HITLPlugin HITLv2 Plugin
  * @{
- * @brief The Hardware In The Loop plugin
+ * @brief The Hardware In The Loop plugin version 2
  *****************************************************************************/
 /*
  * This program is free software; you can redistribute it and/or modify

ground/openpilotgcs/src/plugins/rawhid/rawhid.cpp

@@ -41,7 +41,7 @@ class IConnection;
 static const int READ_TIMEOUT = 200;
 static const int READ_SIZE = 64;
 
-static const int WRITE_TIMEOUT = 200;
+static const int WRITE_TIMEOUT = 1000;
 static const int WRITE_SIZE = 64;
 
 

ground/openpilotgcs/src/plugins/systemhealth/html/AlarmOK.html

@@ -0,0 +1,13 @@
+<html>
+  <head>
+    <title></title>
+    <meta content="">
+    <style></style>
+  </head>
+  <body>
+    <h1>Alarm: OK</h1>
+    <p>
+    There are no problems with this alarm.
+    </p>
+  </body>
+</html>

ground/openpilotgcs/src/plugins/systemhealth/systemhealth.qrc

@@ -3,5 +3,28 @@
         <file>html/Actuator-Critical.html</file>
         <file>html/ManualControl-Critical.html</file>
         <file>html/ManualControl-Warning.html</file>
+        <file>html/CPU-Critical.html</file>
+        <file>html/CPU-Warning.html</file>
+        <file>html/FlightTime-Error.html</file>
+        <file>html/Battery-Warning.html</file>
+        <file>html/BootFault-Critical.html</file>
+        <file>html/EventSystem-Warning.html</file>
+        <file>html/FlightTime-Critical.html</file>
+        <file>html/AlarmOK.html</file>
+        <file>html/Attitude-Critical.html</file>
+        <file>html/Attitude-Error.html</file>
+        <file>html/Battery-Critical.html</file>
+        <file>html/Battery-Error.html</file>
+        <file>html/FlightTime-Warning.html</file>
+        <file>html/GPS-Critical.html</file>
+        <file>html/GPS-Error.html</file>
+        <file>html/GPS-Warning.html</file>
+        <file>html/Guidance-Warning.html</file>
+        <file>html/Memory-Critical.html</file>
+        <file>html/Memory-Warning.html</file>
+        <file>html/Sensors-Critical.html</file>
+        <file>html/Stabilization-Warning.html</file>
+        <file>html/Stack-Critical.html</file>
+        <file>html/Telemetry-Error.html</file>
     </qresource>
 </RCC>

ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetwidget.cpp

@@ -63,6 +63,7 @@ SystemHealthGadgetWidget::SystemHealthGadgetWidget(QWidget *parent) : QGraphicsV
     connect(telMngr, SIGNAL(connected()), this, SLOT(onAutopilotConnect()));
     connect(telMngr, SIGNAL(disconnected()), this, SLOT(onAutopilotDisconnect()));
 
+    setToolTip(tr("Displays flight system errors. Click on an alarm for more information."));
 }
 
 /**
@@ -202,16 +203,67 @@ void SystemHealthGadgetWidget::mousePressEvent ( QMouseEvent * event )
     QGraphicsScene *graphicsScene = scene();
     if(graphicsScene){
         QPoint point = event->pos();
+        bool haveAlarmItem = false;
         foreach(QGraphicsItem* sceneItem, items(point)){
             QGraphicsSvgItem *clickedItem = dynamic_cast<QGraphicsSvgItem*>(sceneItem);
 
-            if(clickedItem && (clickedItem != foreground) && clickedItem != background){
+            if(clickedItem){
-	        QFile alarmDescription(":/systemhealth/html/" + clickedItem->elementId() + ".html");
+                if((clickedItem != foreground) && (clickedItem != background)){
-                if(alarmDescription.open(QIODevice::ReadOnly | QIODevice::Text)){
+                    // Clicked an actual alarm. We need to set haveAlarmItem to true
-                    QTextStream textStream(&alarmDescription);
+                    // as two of the items in this loop will always be foreground and
-                    QWhatsThis::showText(event->globalPos(), textStream.readAll());
+                    // background. Without this flag, at some point in the loop we
+                    // would always call showAllAlarmDescriptions...
+                    haveAlarmItem = true;
+                    QString itemId = clickedItem->elementId();
+                    if(itemId.contains("OK")){
+                        // No alarm set for this item
+                        showAlarmDescriptionForItemId("AlarmOK", event->globalPos());
+                    }else{
+                        // Warning, error or critical alarm
+                        showAlarmDescriptionForItemId(itemId, event->globalPos());
+                    }
+                }else if(!haveAlarmItem){
+                    // Clicked foreground or background
+                    showAllAlarmDescriptions(event->globalPos());
                 }
             }
         }
     }
 }
+
+void SystemHealthGadgetWidget::showAlarmDescriptionForItemId(const QString itemId, const QPoint& location){
+    QFile alarmDescription(":/systemhealth/html/" + itemId + ".html");
+    if(alarmDescription.open(QIODevice::ReadOnly | QIODevice::Text)){
+        QTextStream textStream(&alarmDescription);
+        QWhatsThis::showText(location, textStream.readAll());
+    }
+}
+
+void SystemHealthGadgetWidget::showAllAlarmDescriptions(const QPoint& location){
+    QGraphicsScene *graphicsScene = scene();
+    if(graphicsScene){
+        QString alarmsText;
+
+        // Loop through all items in the scene looking for svg items that represent alarms
+        foreach(QGraphicsItem* curItem, graphicsScene->items()){
+            QGraphicsSvgItem* curSvgItem = dynamic_cast<QGraphicsSvgItem*>(curItem);
+            if(curSvgItem && (curSvgItem != foreground) && (curSvgItem != background)){
+                QString elementId = curSvgItem->elementId();
+                if(!elementId.contains("OK")){
+                    // Found an alarm, get its corresponding alarm html file contents
+                    // and append to the cumulative string for all alarms.
+                    QFile alarmDescription(":/systemhealth/html/" + elementId + ".html");
+                    if(alarmDescription.open(QIODevice::ReadOnly | QIODevice::Text)){
+                        QTextStream textStream(&alarmDescription);
+                        alarmsText.append(textStream.readAll());
+                    }
+                }
+            }
+        }
+
+        // Show alarms text if we have any
+        if(alarmsText.length() > 0){
+            QWhatsThis::showText(location, alarmsText);
+        }
+    }
+}

ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetwidget.h

@@ -69,5 +69,8 @@ private:
                    // Simple flag to skip rendering if the
    bool fgenabled; // layer does not exist.
 
+   void showAlarmDescriptionForItemId(const QString itemId, const QPoint& location);
+   void showAllAlarmDescriptions(const QPoint &location);
+
 };
 #endif /* SYSTEMHEALTHGADGETWIDGET_H_ */

ground/openpilotgcs/src/plugins/uavobjectbrowser/treeitem.cpp

@@ -129,21 +129,23 @@ void TreeItem::appendChild(TreeItem *child)
     child->setParentTree(this);
 }
 
-void TreeItem::insert(int index, TreeItem *child)
+void TreeItem::insertChild(TreeItem *child)
 {
+    int index = nameIndex(child->data(0).toString());
     m_children.insert(index, child);
     child->setParentTree(this);
 }
 
-TreeItem *TreeItem::child(int row)
+TreeItem *TreeItem::getChild(int index)
 {
-    return m_children.value(row);
+    return m_children.value(index);
 }
 
 int TreeItem::childCount() const
 {
     return m_children.count();
 }
+
 int TreeItem::row() const
 {
     if (m_parent)
@@ -224,3 +226,8 @@ QTime TreeItem::getHiglightExpires()
 {
     return m_highlightExpires;
 }
+
+QList<MetaObjectTreeItem *> TopTreeItem::getMetaObjectItems()
+{
+    return m_metaObjectTreeItemsPerObjectIds.values();
+}

ground/openpilotgcs/src/plugins/uavobjectbrowser/treeitem.h

@@ -33,6 +33,7 @@
 #include "uavobjectfield.h"
 #include <QtCore/QList>
 #include <QtCore/QLinkedList>
+#include <QtCore/QMap>
 #include <QtCore/QVariant>
 #include <QtCore/QTime>
 #include <QtCore/QTimer>
@@ -94,9 +95,9 @@ public:
     virtual ~TreeItem();
 
     void appendChild(TreeItem *child);
-    void insert(int index, TreeItem *child);
+    void insertChild(TreeItem *child);
 
-    TreeItem *child(int row);
+    TreeItem *getChild(int index);
     inline QList<TreeItem*> treeChildren() const { return m_children; }
     int childCount() const;
     int columnCount() const;
@@ -131,6 +132,24 @@ public:
 
     virtual void removeHighlight();
 
+    int nameIndex(QString name) {
+        for (int i = 0; i < childCount(); ++i) {
+            if (name < getChild(i)->data(0).toString())
+                return i;
+        }
+        return childCount();
+    }
+
+    TreeItem* findChildByName(QString name)
+    {
+        foreach (TreeItem* child, m_children) {
+            if (name == child->data(0).toString()) {
+                return child;
+            }
+        }
+        return 0;
+    }
+
 signals:
     void updateHighlight(TreeItem*);
 
@@ -152,6 +171,9 @@ private:
     static int m_highlightTimeMs;
 };
 
+class DataObjectTreeItem;
+class MetaObjectTreeItem;
+
 class TopTreeItem : public TreeItem
 {
 Q_OBJECT
@@ -159,19 +181,27 @@ public:
     TopTreeItem(const QList<QVariant> &data, TreeItem *parent = 0) : TreeItem(data, parent) { }
     TopTreeItem(const QVariant &data, TreeItem *parent = 0) : TreeItem(data, parent) { }
 
-    QList<quint32> objIds() { return m_objIds; }
+    void addObjectTreeItem(quint32 objectId, DataObjectTreeItem* oti) {
-    void addObjId(quint32 objId) { m_objIds.append(objId); }
+        m_objectTreeItemsPerObjectIds[objectId] = oti;
-    void insertObjId(int index, quint32 objId) { m_objIds.insert(index, objId); }
-    int nameIndex(QString name) {
-        for (int i = 0; i < childCount(); ++i) {
-            if (name < child(i)->data(0).toString())
-                return i;
-        }
-        return childCount();
     }
 
+    DataObjectTreeItem* findDataObjectTreeItemByObjectId(quint32 objectId) {
+        return m_objectTreeItemsPerObjectIds.contains(objectId) ? m_objectTreeItemsPerObjectIds[objectId] : 0;
+    }
+
+    void addMetaObjectTreeItem(quint32 objectId, MetaObjectTreeItem* oti) {
+        m_metaObjectTreeItemsPerObjectIds[objectId] = oti;
+    }
+
+    MetaObjectTreeItem* findMetaObjectTreeItemByObjectId(quint32 objectId) {
+        return m_metaObjectTreeItemsPerObjectIds.contains(objectId) ? m_metaObjectTreeItemsPerObjectIds[objectId] : 0;
+    }
+
+    QList<MetaObjectTreeItem*> getMetaObjectItems();
+
 private:
-    QList<quint32> m_objIds;
+    QMap<quint32, DataObjectTreeItem*> m_objectTreeItemsPerObjectIds;
+    QMap<quint32, MetaObjectTreeItem*> m_metaObjectTreeItemsPerObjectIds;
 };
 
 class ObjectTreeItem : public TreeItem

ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowser.cpp

@@ -45,5 +45,6 @@ void UAVObjectBrowser::loadConfiguration(IUAVGadgetConfiguration* config)
     m_widget->setRecentlyUpdatedColor(m->recentlyUpdatedColor());
     m_widget->setManuallyChangedColor(m->manuallyChangedColor());
     m_widget->setRecentlyUpdatedTimeout(m->recentlyUpdatedTimeout());
+    m_widget->setOnlyHilightChangedValues(m->onlyHighlightChangedValues());
 }
 

ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowser.ui

@@ -202,6 +202,19 @@
        </property>
       </widget>
      </item>
+     <item>
+      <widget class="QCheckBox" name="categorizeCheckbox">
+       <property name="toolTip">
+        <string>Select to sort objects in to categories.</string>
+       </property>
+       <property name="text">
+        <string>Categorize Objects</string>
+       </property>
+       <property name="checked">
+        <bool>true</bool>
+       </property>
+      </widget>
+     </item>
      <item>
       <spacer name="horizontalSpacer_3">
        <property name="orientation">