Merge branch 'android-telem' into android

androidgcs/AndroidManifest.xml

@@ -10,17 +10,26 @@
 	<uses-permission android:name="android.permission.BLUETOOTH_ADMIN" />
 	<uses-permission android:name="android.permission.ACCESS_FINE_LOCATION" />
 	<uses-permission android:name="android.permission.WRITE_EXTERNAL_STORAGE"/>  
-
+	<uses-feature android:name="android.hardware.usb.host" />
+	
 	<application android:icon="@drawable/ic_logo" android:label="@string/app_name" android:theme="@android:style/Theme.Holo">
 		<!-- for map overlay -->
 		<uses-library android:name="com.google.android.maps" />
 
 		<!-- Object browser - main activity at the moment -->
 		<activity android:name="HomePage" android:label="@string/app_name">
+		    
 			<intent-filter>
 				<action android:name="android.intent.action.MAIN" />
 				<category android:name="android.intent.category.LAUNCHER" />
 			</intent-filter>
+ 			
+<!--  			<intent-filter> -->
+<!--                 <action android:name="android.hardware.usb.action.USB_DEVICE_ATTACHED" /> -->
+<!--             </intent-filter> -->
+
+<!--             <meta-data android:name="android.hardware.usb.action.USB_DEVICE_ATTACHED" -->
+<!--                 android:resource="@xml/device_filter" /> -->
 		</activity>
 
 		<activity android:name="ObjectBrowser" android:label="@string/object_browser_name" />

androidgcs/res/values/arrays.xml

@@ -5,11 +5,13 @@
         <item>Fake</item>
         <item>Bluetooth</item>
         <item>Network</item>
+        <item>HID</item>
     </string-array>
     <string-array name="connectTypeValues">
         <item>0</item>
         <item>1</item>
         <item>2</item>
         <item>3</item>
-    </string-array>
+        <item>4</item>
+   </string-array>
 </resources>

androidgcs/res/xml/device_filter.xml

@@ -0,0 +1,9 @@
+<?xml version="1.0" encoding="utf-8"?>
+
+<resources>
+    <usb-device vendor-id="8352" product-id="16730" />  
+    <usb-device vendor-id="8352" product-id="16731" />  
+	<usb-device vendor-id="8352" product-id="16732" />  
+	<usb-device vendor-id="8352" product-id="16733" />  
+	<usb-device vendor-id="8352" product-id="16734" />  
+</resources>

androidgcs/src/org/openpilot/androidgcs/Controller.java

@@ -77,11 +77,11 @@ public class Controller extends ObjectManagerActivity {
 		public void update(Observable observable, Object data) {
 			// Once we have updated settings we can active the GCS receiver mode
 			Log.d(TAG,"Got update from settings");
-			activateGcsReceiver();
 			UAVDataObject manualControlSettings = (UAVDataObject) objMngr.getObject("ManualControlSettings");
 			if(manualControlSettings != null) {
 				manualControlSettings.removeUpdatedObserver(this);
 			}
+			activateGcsReceiver();
 		}
 	};
 

androidgcs/src/org/openpilot/androidgcs/telemetry/BluetoothUAVTalk.java

@@ -28,10 +28,6 @@ import java.io.IOException;
 import java.util.Set;
 import java.util.UUID;
 
-import org.openpilot.uavtalk.UAVObjectManager;
-import org.openpilot.uavtalk.UAVTalk;
-
-import android.annotation.TargetApi;
 import android.app.Activity;
 import android.bluetooth.BluetoothAdapter;
 import android.bluetooth.BluetoothDevice;
@@ -43,11 +39,13 @@ import android.content.SharedPreferences;
 import android.preference.PreferenceManager;
 import android.util.Log;
 
-@TargetApi(10) public class BluetoothUAVTalk {
+public class BluetoothUAVTalk extends TelemetryTask {
+
 	private final String TAG = "BluetoothUAVTalk";
-	public static int LOGLEVEL = 2;
+	public static final int LOGLEVEL = 4;
-	public static boolean WARN = LOGLEVEL > 1;
+	public static final boolean DEBUG = LOGLEVEL > 2;
-	public static boolean DEBUG = LOGLEVEL > 0;
+	public static final boolean WARN = LOGLEVEL > 1;
+	public static final boolean ERROR = LOGLEVEL > 0;
 
 	// Temporarily define fixed device name
 	private String device_name = "RN42-222D";
@@ -56,30 +54,35 @@ import android.util.Log;
 	private BluetoothAdapter mBluetoothAdapter;
 	private BluetoothSocket socket;
 	private BluetoothDevice device;
-	private UAVTalk uavTalk;
-	private boolean connected;
 
-	public BluetoothUAVTalk(Context caller) {
+	public BluetoothUAVTalk(OPTelemetryService caller) {
+		super(caller);
+	}
 
-		SharedPreferences prefs = PreferenceManager.getDefaultSharedPreferences(caller);
+	@Override
+	boolean attemptConnection() {
+
+		if( getConnected() )
+			return true;
+
+		SharedPreferences prefs = PreferenceManager.getDefaultSharedPreferences(telemService);
 		device_name = prefs.getString("bluetooth_mac","");
 
         if (DEBUG) Log.d(TAG, "Trying to open UAVTalk with " + device_name);
 
-        connected = false;
         device = null;
 
         mBluetoothAdapter = BluetoothAdapter.getDefaultAdapter();
         if (mBluetoothAdapter == null) {
             // Device does not support Bluetooth
         	Log.e(TAG, "Device does not support Bluetooth");
-        	return;
+        	return false;
         }
 
         if (!mBluetoothAdapter.isEnabled()) {
         	// Enable bluetooth if it isn't already
             Intent enableBtIntent = new Intent(BluetoothAdapter.ACTION_REQUEST_ENABLE);
-            caller.sendOrderedBroadcast(enableBtIntent, "android.permission.BLUETOOTH_ADMIN", new BroadcastReceiver() {
+            telemService.sendOrderedBroadcast(enableBtIntent, "android.permission.BLUETOOTH_ADMIN", new BroadcastReceiver() {
 				@Override
 				public void onReceive(Context context, Intent intent) {
 					Log.e(TAG,"Received " + context + intent);
@@ -90,60 +93,60 @@ import android.util.Log;
         } else {
         	queryDevices();
         }
-    }
 
-	public boolean connect(UAVObjectManager objMngr) {
-		if( getConnected() )
-			return true;
-		if( !getFoundDevice() )
-			return false;
-		if( !openTelemetryBluetooth(objMngr) )
-			return false;
 		return true;
 	}
 
-	public boolean getConnected() {
+	@Override
-		return connected;
+	public void disconnect() {
+		super.disconnect();
+		if(socket != null) {
+			try {
+				socket.close();
+			} catch (IOException e) {
+				if (ERROR) Log.e(TAG, "Unable to close BT socket");
+			}
+			socket = null;
+		}
 	}
 
 	public boolean getFoundDevice() {
 		return (device != null);
 	}
 
-	public UAVTalk getUavtalk() {
-		return uavTalk;
-	}
 
     private void queryDevices() {
-    	Log.d(TAG, "Searching for devices");
+    	if (DEBUG) Log.d(TAG, "Searching for devices matching the selected preference");
+
 		Set<BluetoothDevice> pairedDevices = mBluetoothAdapter.getBondedDevices();
 		// If there are paired devices
 		if (pairedDevices.size() > 0) {
 		    // Loop through paired devices
 		    for (BluetoothDevice device : pairedDevices) {
-		        // Add the name and address to an array adapter to show in a ListView
+
-		        //mArrayAdapter.add(device.getName() + "\n" + device.getAddress());
-		    	Log.d(TAG, "Paired device: " + device.getAddress() + " compared to " + device_name);
 		    	if(device.getAddress().compareTo(device_name) == 0) {
-		    		Log.d(TAG, "Found device: " + device.getName());
+		    		if (DEBUG) Log.d(TAG, "Found selected device: " + device.getName());
 		    		this.device = device;
+
+		    		openTelemetryBluetooth();
 		    		return;
 		    	}
 		    }
 		}
 
+		attemptedFailed();
     }
 
-	private boolean openTelemetryBluetooth(UAVObjectManager objMngr) {
+	private boolean openTelemetryBluetooth() {
-		Log.d(TAG, "Opening connection to " + device.getName());
+		if (DEBUG) Log.d(TAG, "Opening connection to " + device.getName());
+
 		socket = null;
-		connected = false;
+
 		try {
 			socket = device.createInsecureRfcommSocketToServiceRecord(MY_UUID);
 		} catch (IOException e) {
-			Log.e(TAG,"Unable to create Rfcomm socket");
+			if (ERROR) Log.e(TAG,"Unable to create Rfcomm socket");
 			return false;
-			//e.printStackTrace();
 		}
 
 		mBluetoothAdapter.cancelDiscovery();
@@ -152,26 +155,40 @@ import android.util.Log;
 			socket.connect();
 		}
 		catch (IOException e) {
-			Log.e(TAG,"Unable to connect to requested device", e);
+			if (ERROR) Log.e(TAG,"Unable to connect to requested device", e);
             try {
                 socket.close();
             } catch (IOException e2) {
-                Log.e(TAG, "unable to close() socket during connection failure", e2);
+                if (ERROR) Log.e(TAG, "unable to close() socket during connection failure", e2);
             }
+
+            attemptedFailed();
 			return false;
 		}
 
-		connected = true;
-
 		try {
-			uavTalk = new UAVTalk(socket.getInputStream(), socket.getOutputStream(), objMngr);
+			inStream = socket.getInputStream();
+			outStream = socket.getOutputStream();
 		} catch (IOException e) {
-			Log.e(TAG,"Error starting UAVTalk");
+			try {
-			// TODO Auto-generated catch block
+				socket.close();
-			//e.printStackTrace();
+			} catch (IOException e2) {
+
+			}
+            attemptedFailed();
 			return false;
 		}
 
+		telemService.toastMessage("Bluetooth device connected");
+
+		// Post message to call attempt succeeded on the parent class
+		handler.post(new Runnable() {
+			@Override
+			public void run() {
+				BluetoothUAVTalk.this.attemptSucceeded();
+			}
+		});
+
 		return true;
 	}
 

androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java

@@ -0,0 +1,615 @@
+package org.openpilot.androidgcs.telemetry;
+
+// Code based on notes from http://torvafirmus-android.blogspot.com/2011/09/implementing-usb-hid-interface-in.html
+// Taken 2012-08-10
+
+import java.io.IOException;
+import java.io.InputStream;
+import java.io.OutputStream;
+import java.nio.ByteBuffer;
+import java.util.HashMap;
+import java.util.Iterator;
+
+import junit.framework.Assert;
+import android.app.PendingIntent;
+import android.content.BroadcastReceiver;
+import android.content.Context;
+import android.content.Intent;
+import android.content.IntentFilter;
+import android.hardware.usb.UsbConstants;
+import android.hardware.usb.UsbDevice;
+import android.hardware.usb.UsbDeviceConnection;
+import android.hardware.usb.UsbEndpoint;
+import android.hardware.usb.UsbInterface;
+import android.hardware.usb.UsbManager;
+import android.hardware.usb.UsbRequest;
+import android.util.Log;
+
+public class HidUAVTalk extends TelemetryTask {
+
+	private static final String TAG = HidUAVTalk.class.getSimpleName();
+	public static final int LOGLEVEL = 1;
+	public static final boolean DEBUG = LOGLEVEL > 2;
+	public static final boolean WARN = LOGLEVEL > 1;
+	public static final boolean ERROR = LOGLEVEL > 0;
+
+	//! USB constants
+	private static final int MAX_HID_PACKET_SIZE = 64;
+	static final int OPENPILOT_VENDOR_ID = 0x20A0;
+
+	static final int USB_PRODUCT_ID_OPENPILOT_MAIN = 0x415A;
+	static final int USB_PRODUCT_ID_COPTERCONTROL  = 0x415B;
+	static final int USB_PRODUCT_ID_PIPXTREME      = 0x415C;
+	static final int USB_PRODUCT_ID_CC3D           = 0x415D;
+	static final int USB_PRODUCT_ID_REVOLUTION     = 0x415E;
+	static final int USB_PRODUCT_ID_OSD            = 0x4194;
+	static final int USB_PRODUCT_ID_SPARE          = 0x4195;
+
+	private static final String ACTION_USB_PERMISSION = "com.access.device.USB_PERMISSION";
+
+	private UsbDevice currentDevice;
+	private UsbEndpoint usbEndpointRead;
+	private UsbEndpoint usbEndpointWrite;
+	private UsbManager usbManager;
+	private PendingIntent permissionIntent;
+	private UsbDeviceConnection usbDeviceConnection;
+	private IntentFilter permissionFilter;
+	private UsbInterface usbInterface = null;
+	private TalkInputStream inTalkStream;
+	private TalkOutputStream outTalkStream;
+	private final UsbRequest writeRequest = null;
+	private UsbRequest readRequest = null;
+	private Thread readThread;
+	private Thread writeThread;
+
+	private boolean readPending = false;
+	private boolean writePending = false;
+	private IntentFilter deviceAttachedFilter;
+
+	public HidUAVTalk(OPTelemetryService service) {
+		super(service);
+	}
+
+	@Override
+	public void disconnect() {
+
+		CleanUpAndClose();
+		telemService.unregisterReceiver(usbReceiver);
+		telemService.unregisterReceiver(usbPermissionReceiver);
+
+		super.disconnect();
+
+		try {
+			if(readThread != null) {
+				readThread.interrupt(); // Make sure not blocking for data
+				readThread.join();
+				readThread = null;
+			}
+			if(writeThread != null) {
+				writeThread.interrupt();
+				writeThread.join();
+				writeThread = null;
+			}
+		} catch (InterruptedException e) {
+			e.printStackTrace();
+		}
+
+		if (readRequest != null) {
+			readRequest.cancel();
+			readRequest.close();
+			readRequest = null;
+		}
+	}
+
+	@Override
+	boolean attemptConnection() {
+		if (DEBUG) Log.d(TAG, "connect()");
+
+		// Register to get permission requested dialog
+		usbManager = (UsbManager) telemService.getSystemService(Context.USB_SERVICE);
+		permissionIntent = PendingIntent.getBroadcast(telemService, 0, new Intent(ACTION_USB_PERMISSION), 0);
+		permissionFilter = new IntentFilter(ACTION_USB_PERMISSION);
+		telemService.registerReceiver(usbPermissionReceiver, permissionFilter);
+
+		deviceAttachedFilter = new IntentFilter();
+		deviceAttachedFilter.addAction(UsbManager.ACTION_USB_DEVICE_ATTACHED);
+		deviceAttachedFilter.addAction(UsbManager.ACTION_USB_DEVICE_DETACHED);
+		telemService.registerReceiver(usbReceiver, deviceAttachedFilter);
+
+		// Go through all the devices plugged in
+		HashMap<String, UsbDevice> deviceList = usbManager.getDeviceList();
+		if (DEBUG) Log.d(TAG, "Found " + deviceList.size() + " devices");
+		Iterator<UsbDevice> deviceIterator = deviceList.values().iterator();
+		while(deviceIterator.hasNext()){
+			UsbDevice dev = deviceIterator.next();
+			if (DEBUG) Log.d(TAG, "Testing device: " + dev);
+			usbManager.requestPermission(dev, permissionIntent);
+		}
+
+		if (DEBUG) Log.d(TAG, "Registered the deviceAttachedFilter");
+
+		return deviceList.size() > 0;
+	}
+
+	/*
+	 * Receives a requested broadcast from the operating system.
+	 * In this case the following actions are handled:
+	 *   USB_PERMISSION
+	 *   UsbManager.ACTION_USB_DEVICE_DETACHED
+	 *   UsbManager.ACTION_USB_DEVICE_ATTACHED
+	 */
+	private final BroadcastReceiver usbPermissionReceiver = new BroadcastReceiver()
+	{
+		@Override
+		public void onReceive(Context context, Intent intent)
+		{
+			if (DEBUG) Log.d(TAG,"Broadcast receiver caught intent: " + intent);
+			String action = intent.getAction();
+			// Validate the action against the actions registered
+			if (ACTION_USB_PERMISSION.equals(action))
+			{
+				// A permission response has been received, validate if the user has
+				// GRANTED, or DENIED permission
+				synchronized (this)
+				{
+					UsbDevice deviceConnected = (UsbDevice)intent.getParcelableExtra(UsbManager.EXTRA_DEVICE);
+
+					if (DEBUG) Log.d(TAG, "Device Permission requested" + deviceConnected);
+					if (intent.getBooleanExtra(UsbManager.EXTRA_PERMISSION_GRANTED, false))
+					{
+						// Permission has been granted, so connect to the device
+						// If this fails, then keep looking
+						if (deviceConnected != null)
+						{
+							// call method to setup device communication
+							currentDevice = deviceConnected;
+							if (DEBUG) Log.d(TAG, "Device Permission Acquired" + currentDevice);
+							if (!ConnectToDeviceInterface(currentDevice))
+							{
+								if (DEBUG) Log.d(TAG, "Unable to connect to device");
+							}
+						}
+					}
+					else
+					{
+						// Permission has not been granted, so keep looking for another
+						// device to be attached....
+						if (DEBUG) Log.d(TAG, "Device Permission Denied" + deviceConnected);
+						currentDevice = null;
+					}
+				}
+			}
+		}
+	};
+
+	private final BroadcastReceiver usbReceiver = new BroadcastReceiver()
+	{
+		@Override
+		public void onReceive(Context context, Intent intent)
+		{
+			if (DEBUG) Log.d(TAG,"Broadcast receiver taught intent: " + intent);
+			String action = intent.getAction();
+			// Validate the action against the actions registered
+
+			if (UsbManager.ACTION_USB_DEVICE_DETACHED.equals(action))
+			{
+				// A device has been detached from the device, so close all the connections
+				// and restart the search for a new device being attached
+				UsbDevice device = (UsbDevice)intent.getParcelableExtra(UsbManager.EXTRA_DEVICE);
+				if ((device != null) && (currentDevice != null))
+				{
+					if (device.equals(currentDevice))
+					{
+						telemService.toastMessage("Device unplugged while in use");
+						if (DEBUG) Log.d(TAG, "Matching device disconnected");
+						// call your method that cleans up and closes communication with the device
+						disconnect();
+					}
+				}
+			}
+			else if (UsbManager.ACTION_USB_DEVICE_ATTACHED.equals(action))
+			{
+				// A device has been attached. If not already connected to a device,
+				// validate if this device is supported
+				UsbDevice searchDevice = (UsbDevice)intent.getParcelableExtra(UsbManager.EXTRA_DEVICE);
+				if (DEBUG) Log.d(TAG, "Device found" + searchDevice);
+				if ((searchDevice != null) && (currentDevice == null))
+				{
+					// call your method that cleans up and closes communication with the device
+					ValidateFoundDevice(searchDevice);
+				}
+			}
+		}
+	};
+
+
+	protected void CleanUpAndClose() {
+		if(usbDeviceConnection != null && usbInterface != null)
+			usbDeviceConnection.releaseInterface(usbInterface);
+		usbInterface = null;
+	}
+
+	//Validating the Connected Device - Before asking for permission to connect to the device, it is essential that you ensure that this is a device that you support or expect to connect to. This can be done by validating the devices Vendor ID and Product ID.
+	boolean ValidateFoundDevice(UsbDevice searchDevice) {
+		//A vendor id is a global identifier for the manufacturer. A product id refers to the product itself, and is unique to the manufacturer. The vendor id, product id combination refers to a particular product manufactured by a vendor.
+		if (DEBUG) Log.d(TAG, "ValidateFoundDevice: " + searchDevice );
+
+		if ( searchDevice.getVendorId() == OPENPILOT_VENDOR_ID ) {
+			//Requesting permission
+			if (DEBUG) Log.d(TAG, "Device: " + searchDevice );
+			usbManager.requestPermission(searchDevice, permissionIntent);
+			return true;
+		}
+		else
+			return false;
+	}
+
+	boolean ConnectToDeviceInterface(UsbDevice connectDevice) {
+		// Connecting to the Device - If you are reading and writing, then the device
+		// can either have two end points on a single interface, or two interfaces
+		// each with a single end point. Either way, it is best if you know which interface
+		// you need to use and which end points
+
+		if (DEBUG) Log.d(TAG, "ConnectToDeviceInterface:");
+		UsbEndpoint ep1 = null;
+		UsbEndpoint ep2 = null;
+
+		// Using the same interface for reading and writing
+		usbInterface = connectDevice.getInterface(0x2);
+		if (usbInterface.getEndpointCount() == 2)
+		{
+			ep1 = usbInterface.getEndpoint(0);
+			ep2 = usbInterface.getEndpoint(1);
+		}
+
+		if ((ep1 == null) || (ep2 == null))
+		{
+			if (ERROR) Log.e(TAG, "Null endpoints");
+			return false;
+		}
+
+		// Determine which endpoint is the read, and which is the write
+		if (ep1.getType() == UsbConstants.USB_ENDPOINT_XFER_INT)
+		{
+			if (ep1.getDirection() == UsbConstants.USB_DIR_IN)
+				usbEndpointRead = ep1;
+			else if (ep1.getDirection() == UsbConstants.USB_DIR_OUT)
+				usbEndpointWrite = ep1;
+		}
+		if (ep2.getType() == UsbConstants.USB_ENDPOINT_XFER_INT)
+		{
+			if (ep2.getDirection() == UsbConstants.USB_DIR_IN)
+				usbEndpointRead = ep2;
+			else if (ep2.getDirection() == UsbConstants.USB_DIR_OUT)
+				usbEndpointWrite = ep2;
+		}
+		if ((usbEndpointRead == null) || (usbEndpointWrite == null))
+		{
+			if (ERROR) Log.e(TAG, "Could not find write and read endpoint");
+			return false;
+		}
+
+		// Claim the interface
+		usbDeviceConnection = usbManager.openDevice(connectDevice);
+		usbDeviceConnection.claimInterface(usbInterface, true);
+
+
+		if (DEBUG) Log.d(TAG, "Opened endpoints");
+
+		// Create the USB requests
+		readRequest = new UsbRequest();
+		readRequest.initialize(usbDeviceConnection, usbEndpointRead);
+
+		inTalkStream = new TalkInputStream();
+		outTalkStream = new TalkOutputStream();
+		inStream = inTalkStream;
+		outStream = outTalkStream;
+		handler.post(new Runnable() {
+			@Override
+			public void run() {
+				attemptSucceeded();
+			}
+		});
+
+		readThread = new Thread(new Runnable() {
+			@Override
+			public void run() {
+				// Enqueue the first read
+				queueRead();
+				while (!shutdown) {
+					UsbRequest returned = usbDeviceConnection.requestWait();
+					if (returned == readRequest) {
+						if (DEBUG) Log.d(TAG, "Received read request");
+						readData();
+					} else {
+						Log.e(TAG, "Received unknown USB response");
+						break;
+					}
+				}
+			}
+
+		}, "HID Read");
+		readThread.start();
+
+		writeThread = new Thread(new Runnable() {
+			@Override
+			public void run() {
+				if (DEBUG) Log.d(TAG, "Starting HID write thread");
+				while(!shutdown) {
+					try {
+						if (sendDataSynchronous() == false)
+							break;
+					} catch (InterruptedException e) {
+						break;
+					}
+				}
+				if (DEBUG) Log.d(TAG, "Ending HID write thread");
+			}
+		}, "HID Write");
+		writeThread.start();
+
+		telemService.toastMessage("HID Device Opened");
+
+		return true;
+	}
+
+
+	void displayBuffer(String msg, byte[] buf) {
+		msg += " (";
+		for (int i = 0; i < buf.length; i++) {
+			msg += buf[i] + " ";
+		}
+		msg += ")";
+		Log.d(TAG, msg);
+	}
+	/**
+	 * Gets a report from HID, extract the meaningful data and push
+	 * it to the input stream
+	 */
+	ByteBuffer readBuffer = ByteBuffer.allocate(MAX_HID_PACKET_SIZE);
+
+	/**
+	 * Schedules a USB read
+	 */
+	private void queueRead() {
+		synchronized(readRequest) {
+			if(!readRequest.queue(readBuffer, MAX_HID_PACKET_SIZE)) {
+				if (ERROR) Log.e(TAG, "Failed to queue request");
+			} else
+				readPending = true;
+		}
+	}
+
+	/**
+	 * Reads data from the last USB transaction and schedules another read
+	 */
+	public void readData() {
+		synchronized(readRequest) {
+
+			if (!readPending) {
+				if (ERROR) Log.e(TAG, "Tried to read read while a transaction was not pending");
+				return;
+			}
+
+			// We just received a read
+			readPending = false;
+			// Packet format:
+			// 0: Report ID (1)
+			// 1: Number of valid bytes
+			// 2:63: Data
+
+			int dataSize = readBuffer.get(1);    // Data size
+			//Assert.assertEquals(1, buffer.get()); // Report ID
+			//Assert.assertTrue(dataSize < buffer.capacity());
+
+			if (readBuffer.get(0) != 1 || readBuffer.get(1) < 0 || readBuffer.get(1) > (readBuffer.capacity() - 2)) {
+				if (ERROR) Log.e(TAG, "Badly formatted HID packet");
+			} else {
+				byte[] dst = new byte[dataSize];
+				readBuffer.position(2);
+				readBuffer.get(dst, 0, dataSize);
+				if (DEBUG) Log.d(TAG, "Entered read");
+				inTalkStream.write(dst);
+				if (DEBUG) Log.d(TAG, "Got read: " + dataSize + " bytes");
+			}
+
+			// Queue another read
+			queueRead();
+
+		}
+	}
+
+
+	/**
+	 * Send a packet if data is available
+	 */
+	public void sendData() {
+		synchronized(writeRequest) {
+			// Don't try and send data till previous request completes
+			if (writePending)
+				return;
+
+			ByteBuffer packet = outTalkStream.getHIDpacket();
+			if (packet != null) {
+				if (DEBUG) Log.d(TAG, "Writing to device()");
+
+				int bufferDataLength = usbEndpointWrite.getMaxPacketSize();
+				Assert.assertTrue(packet.capacity() <= bufferDataLength);
+
+				if(writeRequest.queue(packet, bufferDataLength))
+					writePending = true;
+				else if (ERROR)
+					Log.e(TAG, "Write queuing failed");
+			}
+		}
+	}
+
+	/**
+	 * Send a packet if data is available
+	 * @throws InterruptedException
+	 */
+	public boolean sendDataSynchronous() throws InterruptedException {
+
+		ByteBuffer packet = outTalkStream.getHIDpacketBlocking();
+		if (packet != null) {
+			if (DEBUG) Log.d(TAG, "sendDataSynchronous() Writing to device()");
+
+			if (usbDeviceConnection.bulkTransfer(usbEndpointWrite, packet.array(), MAX_HID_PACKET_SIZE, 1000) < 0) {
+				Log.e(TAG, "Failed to perform bulk write");
+				return false;
+			}
+		}
+		return true;
+	}
+
+	/*********** Helper classes for telemetry streams ************/
+
+	class TalkOutputStream extends OutputStream {
+		// Uses ByteFifo.getByteBlocking()
+		// and  ByteFifo.put(byte [])
+		ByteFifo data = new ByteFifo();
+
+		/**
+		 * Blocks until data is available and then returns a properly formatted HID packet
+		 */
+		public ByteBuffer getHIDpacketBlocking() throws InterruptedException {
+			synchronized(data) {
+				if (data.remaining() == 0)
+					data.wait();
+				return getHIDpacket();
+			}
+		}
+
+		/**
+		 * Gets data from the ByteFifo in a properly formatted HID packet
+		 */
+		public ByteBuffer getHIDpacket() {
+			ByteBuffer packet = null;
+			synchronized(data) {
+				// Determine how much data to put in the packet
+				int size = Math.min(data.remaining(), MAX_HID_PACKET_SIZE - 2);
+				if (size <= 0)
+					return packet;
+
+				// Format into a HID packet
+				packet = ByteBuffer.allocate(MAX_HID_PACKET_SIZE);
+				packet.put(0,(byte) 2);             // Report ID
+				packet.put(1,(byte) size);          // The number of useful bytes
+				data.get(packet.array(), 2, size);
+
+				if (DEBUG) Log.d(TAG, "packetizeData(): size="+size);
+			}
+			return packet;
+		}
+		@Override
+		public void write(int oneByte) throws IOException {
+			// Throw exception when try and read after shutdown
+			if (shutdown)
+				throw new IOException();
+
+			synchronized(data) {
+				data.put((byte) oneByte);
+				data.notify();
+			}
+		}
+
+		@Override
+		public void write(byte[] b) throws IOException {
+			if (shutdown)
+				throw new IOException();
+
+			synchronized(data) {
+				data.put(b);
+				data.notify();
+			}
+		}
+
+	};
+
+	private class TalkInputStream extends InputStream {
+		// Uses ByteFifo.getByteBlocking()
+		// Uses ByteFifo.put(byte[])
+		ByteFifo data = new ByteFifo();
+
+		TalkInputStream() {
+		}
+
+		@Override
+		public int read() {
+			try {
+				return data.getByteBlocking();
+			} catch (InterruptedException e) {
+				Log.e(TAG, "Timed out");
+				e.printStackTrace();
+			}
+			return -1;
+		}
+
+		public void write(byte[] b) {
+			data.put(b);
+		}
+	};
+
+	private class ByteFifo {
+
+		//! The maximum size of the fifo
+		private final int MAX_SIZE = 256;
+		//! The number of bytes in the buffer
+		private int size = 0;
+		//! Internal buffer
+		private final ByteBuffer buf;
+
+		ByteFifo() {
+			buf = ByteBuffer.allocate(MAX_SIZE);
+			size = 0;
+		}
+
+		private int byteToInt(byte b) { return b & 0x000000ff; }
+
+		final int remaining() { return size; };
+
+		public boolean put(byte b) {
+			byte[] a = {b};
+			return put(a);
+		}
+
+		public boolean put(byte[] dat) {
+			if ((size + dat.length) > MAX_SIZE) {
+				Log.e(TAG, "Dropped data.  Size:" + size + " data length: " + dat.length);
+				return false;
+			}
+
+			// Place data at the end of the buffer
+			synchronized(buf) {
+				buf.position(size);
+				buf.put(dat);
+				size = size + dat.length;
+				buf.notify();
+			}
+			return true;
+		}
+
+		public ByteBuffer get(byte[] dst, int offset, int size) {
+			synchronized(buf) {
+				buf.flip();
+				buf.get(dst, offset, size);
+				buf.compact();
+				this.size -= size;
+			}
+			return buf;
+		}
+
+		public int getByteBlocking() throws InterruptedException {
+			synchronized(buf) {
+				if (size == 0) {
+					buf.wait();
+				}
+				int val = byteToInt(buf.get(0));
+				buf.position(1);
+				buf.compact();
+				size--;
+				return val;
+			}
+		}
+	}
+}

androidgcs/src/org/openpilot/androidgcs/telemetry/OPTelemetryService.java

@@ -26,16 +26,10 @@
  */
 package org.openpilot.androidgcs.telemetry;
 
-import java.io.IOException;
 import java.lang.ref.WeakReference;
-import java.util.Observable;
-import java.util.Observer;
 
-import org.openpilot.uavtalk.Telemetry;
-import org.openpilot.uavtalk.TelemetryMonitor;
 import org.openpilot.uavtalk.UAVDataObject;
 import org.openpilot.uavtalk.UAVObjectManager;
-import org.openpilot.uavtalk.UAVTalk;
 import org.openpilot.uavtalk.uavobjects.UAVObjectsInitialize;
 
 import android.app.Service;
@@ -56,9 +50,9 @@ public class OPTelemetryService extends Service {
 
 	// Logging settings
 	private final String TAG = OPTelemetryService.class.getSimpleName();
-	public static int LOGLEVEL = 0;
+	public static int LOGLEVEL = 2;
-	public static boolean WARN = LOGLEVEL > 1;
+	public static boolean DEBUG = LOGLEVEL > 1;
-	public static boolean DEBUG = LOGLEVEL > 0;
+	public static boolean WARN = LOGLEVEL > 0;
 
 	// Intent category
 	public final static String INTENT_CATEGORY_GCS        = "org.openpilot.intent.category.GCS";
@@ -81,6 +75,7 @@ public class OPTelemetryService extends Service {
 	private boolean terminate = false;
 
 	private Thread activeTelem;
+	private TelemetryTask telemTask;
 
 	private final IBinder mBinder = new LocalBinder();
 
@@ -126,11 +121,18 @@ public class OPTelemetryService extends Service {
 				break;
 			case 2:
 				Toast.makeText(getApplicationContext(), "Attempting BT connection", Toast.LENGTH_SHORT).show();
-				activeTelem = new BTTelemetryThread();
+				telemTask = new BluetoothUAVTalk(this);
+				activeTelem = new Thread(telemTask, "Bluetooth telemetry thread");
 				break;
 			case 3:
 				Toast.makeText(getApplicationContext(), "Attempting TCP connection", Toast.LENGTH_SHORT).show();
-				activeTelem = new TcpTelemetryThread();
+				telemTask = new TcpUAVTalk(this);
+				activeTelem = new Thread(telemTask, "Tcp telemetry thread");
+				break;
+			case 4:
+				Toast.makeText(getApplicationContext(), "Attempting USB HID connection", Toast.LENGTH_SHORT).show();
+				telemTask = new HidUAVTalk(this);
+				activeTelem = new Thread(telemTask, "Hid telemetry thread");
 				break;
 			default:
 				throw new Error("Unsupported");
@@ -139,8 +141,19 @@ public class OPTelemetryService extends Service {
 			break;
 		case MSG_DISCONNECT:
 			Toast.makeText(getApplicationContext(), "Disconnect requested", Toast.LENGTH_SHORT).show();
+			if (DEBUG) Log.d(TAG, "Calling disconnect");
 			terminate = true;
-			if (activeTelem != null) {
+			if (telemTask != null) {
+				telemTask.disconnect();
+				telemTask = null;
+
+				try {
+					activeTelem.join();
+				} catch (InterruptedException e) {
+					e.printStackTrace();
+				}
+			}
+			else  if (activeTelem != null) {
 				activeTelem.interrupt();
 				try {
 					activeTelem.join();
@@ -149,6 +162,7 @@ public class OPTelemetryService extends Service {
 				}
 				activeTelem = null;
 			}
+			if (DEBUG) Log.d(TAG, "Telemetry thread terminated");
 			Intent intent = new Intent();
 			intent.setAction(INTENT_ACTION_DISCONNECTED);
 			sendBroadcast(intent,null);
@@ -211,12 +225,27 @@ public class OPTelemetryService extends Service {
 
 	@Override
 	public void onDestroy() {
+
+		if (telemTask != null) {
+			Log.d(TAG, "onDestroy() shutting down telemetry task");
+			telemTask.disconnect();
+			telemTask = null;
+
+			try {
+				activeTelem.join();
+			} catch (InterruptedException e) {
+				e.printStackTrace();
+			}
+		}
+		Log.d(TAG, "onDestory() shut down telemetry task");
 		Toast.makeText(this, "Telemetry service done", Toast.LENGTH_SHORT).show();
 	}
 
 	public class LocalBinder extends Binder {
 		public TelemTask getTelemTask(int id) {
-			return (TelemTask) activeTelem;
+			if (telemTask != null)
+				return telemTask.getTelemTaskIface();
+			return null;
 		}
 		public void openConnection() {
 			Toast.makeText(getApplicationContext(), "Requested open connection", Toast.LENGTH_SHORT).show();
@@ -230,7 +259,7 @@ public class OPTelemetryService extends Service {
 			mServiceHandler.sendMessage(msg);
 		}
 		public boolean isConnected() {
-			return activeTelem != null;
+			return (activeTelem != null) && (telemTask != null) && (telemTask.getConnected());
 		}
 	};
 
@@ -313,6 +342,8 @@ public class OPTelemetryService extends Service {
 			}
 		}
 	}
+
+	/*
 	private class BTTelemetryThread extends Thread implements TelemTask {
 
 		private final UAVObjectManager objMngr;
@@ -367,7 +398,7 @@ public class OPTelemetryService extends Service {
 				@Override
 				public void update(Observable arg0, Object arg1) {
 					if (DEBUG) Log.d(TAG, "Mon updated. Connected: " + mon.getConnected() + " objects updated: " + mon.getObjectsUpdated());
-					if(mon.getConnected() /*&& mon.getObjectsUpdated()*/) {
+					if(mon.getConnected() ) {
 						Intent intent = new Intent();
 						intent.setAction(INTENT_ACTION_CONNECTED);
 						sendBroadcast(intent,null);
@@ -389,97 +420,5 @@ public class OPTelemetryService extends Service {
 			}
 			if (DEBUG) Log.d(TAG, "UAVTalk stream disconnected");
 		}
-
+	};*/
-	};
-
-	private class TcpTelemetryThread extends Thread implements TelemTask {
-
-		private final UAVObjectManager objMngr;
-		private UAVTalk uavTalk;
-		private Telemetry tel;
-		private TelemetryMonitor mon;
-
-		@Override
-		public UAVObjectManager getObjectManager() { return objMngr; };
-
-		TcpTelemetryThread() {
-			objMngr = new UAVObjectManager();
-			UAVObjectsInitialize.register(objMngr);
-		}
-
-		@Override
-		public void run() {
-			if (DEBUG) Log.d(TAG, "Telemetry Thread started");
-
-			Looper.prepare();
-
-			TcpUAVTalk tcp = new TcpUAVTalk(OPTelemetryService.this);
-			for( int i = 0; i < 10; i++ ) {
-				if (DEBUG) Log.d(TAG, "Attempting network Connection");
-
-				tcp.connect(objMngr);
-
-				if( tcp.getConnected() )
-
-					break;
-
-				try {
-					Thread.sleep(1000);
-				} catch (InterruptedException e) {
-					Log.e(TAG, "Thread interrupted while trying to connect");
-					e.printStackTrace();
-					return;
-				}
-			}
-			if( ! tcp.getConnected() || terminate ) {
-				toastMessage("TCP connection failed");
-				return;
-			}
-			toastMessage("TCP Connected");
-
-			if (DEBUG) Log.d(TAG, "Connected via network");
-
-			uavTalk = tcp.getUavtalk();
-			tel = new Telemetry(uavTalk, objMngr);
-			mon = new TelemetryMonitor(objMngr,tel);
-			mon.addObserver(new Observer() {
-				@Override
-				public void update(Observable arg0, Object arg1) {
-					if (DEBUG) Log.d(TAG, "Mon updated. Connected: " + mon.getConnected() + " objects updated: " + mon.getObjectsUpdated());
-					if(mon.getConnected()) {
-						Intent intent = new Intent();
-						intent.setAction(INTENT_ACTION_CONNECTED);
-						sendBroadcast(intent,null);
-					}
-				}
-			});
-
-
-			if (DEBUG) Log.d(TAG, "Entering UAVTalk processing loop");
-			while( !terminate ) {
-				try {
-					if( !uavTalk.processInputStream() )
-						break;
-				} catch (IOException e) {
-					e.printStackTrace();
-					toastMessage("TCP Stream interrupted");
-					break;
-				}
-			}
-			Looper.myLooper().quit();
-
-			// Shut down all the attached
-			mon.stopMonitor();
-			mon = null;
-			tel.stopTelemetry();
-			tel = null;
-
-			// Finally close the stream if it is still open
-			tcp.disconnect();
-
-			if (DEBUG) Log.d(TAG, "UAVTalk stream disconnected");
-			toastMessage("TCP Thread finished");
-		}
-
-	};
 }

androidgcs/src/org/openpilot/androidgcs/telemetry/TcpUAVTalk.java

@@ -28,15 +28,11 @@ import java.net.InetAddress;
 import java.net.Socket;
 import java.net.UnknownHostException;
 
-import org.openpilot.uavtalk.UAVObjectManager;
-import org.openpilot.uavtalk.UAVTalk;
-
-import android.content.Context;
 import android.content.SharedPreferences;
 import android.preference.PreferenceManager;
 import android.util.Log;
 
-public class TcpUAVTalk {
+public class TcpUAVTalk extends TelemetryTask {
 	private final String TAG = "TcpUAVTalk";
 	public static int LOGLEVEL = 2;
 	public static boolean WARN = LOGLEVEL > 1;
@@ -46,16 +42,23 @@ public class TcpUAVTalk {
 	private String ip_address = "1";
 	private int port = 9001;
 
-	private UAVTalk uavTalk;
-	private boolean connected;
 	private Socket socket;
 
 	/**
 	 * Construct a TcpUAVTalk object attached to the OPTelemetryService.  Gets the
 	 * connection settings from the preferences.
 	 */
-	public TcpUAVTalk(Context caller) {
+	public TcpUAVTalk(OPTelemetryService caller) {
-		SharedPreferences prefs = PreferenceManager.getDefaultSharedPreferences(caller);
+		super(caller);
+	}
+
+	@Override
+	boolean attemptConnection() {
+
+		if( getConnected() )
+			return true;
+
+		SharedPreferences prefs = PreferenceManager.getDefaultSharedPreferences(telemService);
 		ip_address = prefs.getString("ip_address","127.0.0.1");
 		try {
 			port = Integer.decode(prefs.getString("port", ""));
@@ -65,44 +68,6 @@ public class TcpUAVTalk {
 
 		if (DEBUG) Log.d(TAG, "Trying to open UAVTalk with " + ip_address);
 
-        connected = false;
-    }
-
-	/**
-	 * Connect a TCP object to an object manager.  Returns true if already
-	 * connected, otherwise returns true if managed a successful socket.
-	 */
-	public boolean connect(UAVObjectManager objMngr) {
-		if( getConnected() )
-			return true;
-		if( !openTelemetryTcp(objMngr) )
-			return false;
-		return true;
-	}
-
-	public void disconnect() {
-		try {
-			socket.close();
-		} catch (IOException e) {
-			// TODO Auto-generated catch block
-			e.printStackTrace();
-		}
-		socket = null;
-	}
-
-	public boolean getConnected() {
-		return connected;
-	}
-
-	public UAVTalk getUavtalk() {
-		return uavTalk;
-	}
-
-	/**
-	 * Opens a TCP socket to the address determined on construction.  If successful
-	 * creates a UAVTalk stream connection this socket to the passed in object manager
-	 */
-	private boolean openTelemetryTcp(UAVObjectManager objMngr) {
 		Log.d(TAG, "Opening connection to " + ip_address + " at address " + port);
 
 		InetAddress serverAddr = null;
@@ -121,18 +86,43 @@ public class TcpUAVTalk {
 			return false;
 		}
 
-		connected = true;
-
 		try {
-			uavTalk = new UAVTalk(socket.getInputStream(), socket.getOutputStream(), objMngr);
+			inStream = socket.getInputStream();
+			outStream = socket.getOutputStream();
 		} catch (IOException e) {
-			Log.e(TAG,"Error starting UAVTalk");
+			try {
-			// TODO Auto-generated catch block
+				socket.close();
-			//e.printStackTrace();
+			} catch (IOException e2) {
+
+			}
 			return false;
 		}
 
+
+		// Post message to call attempt succeeded on the parent class
+		handler.post(new Runnable() {
+			@Override
+			public void run() {
+				TcpUAVTalk.this.attemptSucceeded();
+			}
+		});
+
 		return true;
 	}
 
+
+	@Override
+	public void disconnect() {
+		super.disconnect();
+		if (socket != null) {
+			try {
+				socket.close();
+			} catch (IOException e) {
+				// TODO Auto-generated catch block
+				e.printStackTrace();
+			}
+			socket = null;
+		}
+	}
+
 }

androidgcs/src/org/openpilot/androidgcs/telemetry/TelemetryTask.java

@@ -0,0 +1,247 @@
+package org.openpilot.androidgcs.telemetry;
+
+import java.io.IOException;
+import java.io.InputStream;
+import java.io.OutputStream;
+import java.util.Observable;
+import java.util.Observer;
+
+import org.openpilot.uavtalk.Telemetry;
+import org.openpilot.uavtalk.TelemetryMonitor;
+import org.openpilot.uavtalk.UAVObjectManager;
+import org.openpilot.uavtalk.UAVTalk;
+import org.openpilot.uavtalk.uavobjects.UAVObjectsInitialize;
+
+import android.content.Intent;
+import android.os.Handler;
+import android.os.Looper;
+import android.util.Log;
+
+public abstract class TelemetryTask implements Runnable {
+
+	// Logging settings
+	private final String TAG = TelemetryTask.class.getSimpleName();
+	public static final int LOGLEVEL = 2;
+	public static final boolean WARN = LOGLEVEL > 1;
+	public static final boolean DEBUG = LOGLEVEL > 0;
+
+	/*
+	 * This is a self contained runnable that will establish (if possible)
+	 * a telemetry connection and provides a listener interface to be
+	 * notified of a set of events
+	 *
+	 * 1. attempt to establish connection
+	 * 2. callback when it succeeds (or fails) which notifies listener
+	 * 3. once physical connection is established instantiate uavtalk / objectmanager
+	 * 4. notify listener they are connected
+	 * 5. detect physical link failure and notify listener about that
+	 * 6. provide "close link" method
+	 *
+	 * There are essentially four tasks that need to occur here
+	 * 1. Transfer data from the outputStream to the physical link (some protocols do this automatically)
+	 * 2. Transfer data from the physical link to the inputStream (again some protocols do this automatically)
+	 * 3. Transfer data from the inputStream to UAVTalk (uavTalk.processInputByte)
+	 * 4. Transfer data from objects via UAVTalk to output stream (occurs from Telemetry object)
+	 */
+
+	//! Private variables
+	protected Handler handler;
+
+	//! Handle to the parent service
+	protected final OPTelemetryService telemService;
+
+	//! The object manager that will be used for this telemetry task
+	protected UAVObjectManager objMngr;
+
+	//! The UAVTalk connected to the below streams
+	private UAVTalk uavTalk;
+
+	//! The input stream for the telemetry channel
+	protected InputStream inStream;
+
+	//! The output stream for the telemetry channel
+	protected OutputStream outStream;
+
+	//! The telemetry object which takes care of higher level transactions
+	private Telemetry tel;
+
+	//! The telemetry monitor which takes care of high level connects / disconnects
+	private TelemetryMonitor mon;
+
+	//! Thread to process the input stream
+	Thread inputProcessThread;
+
+	//! Flag to indicate a shut down was requested.  Derived classes should take care to respect this.
+	boolean shutdown;
+
+	//! Indicate a physical connection is established
+	private boolean connected;
+
+	TelemetryTask(OPTelemetryService s) {
+		telemService = s;
+		shutdown = false;
+		connected = false;
+	}
+
+	/**
+	 * Attempt a connection.  This method may return before the results are
+	 * known.
+	 * @return False if the attempt failed and no connection will be established
+	 * @return True if the attempt succeeded but does not guarantee success
+	 */
+	abstract boolean attemptConnection();
+
+	/**
+	 * Called when a physical channel is opened
+	 *
+	 * When this method is called the derived class must have
+	 * created a valid inStream and outStream
+	 */
+	boolean attemptSucceeded() {
+		// Create a new object manager and register all objects
+		// in the future the particular register method should
+		// be dependent on what is connected (e.g. board and
+		// version number).
+		objMngr = new UAVObjectManager();
+		UAVObjectsInitialize.register(objMngr);
+
+		// Create the required telemetry objects attached to this
+		// data stream
+		uavTalk = new UAVTalk(inStream, outStream, objMngr);
+		tel = new Telemetry(uavTalk, objMngr, Looper.myLooper());
+		mon = new TelemetryMonitor(objMngr,tel, telemService);
+
+		// Create an observer to notify system of connection
+		mon.addObserver(connectionObserver);
+
+		// Create a new thread that processes the input bytes
+		startInputProcessing();
+
+		connected = true;
+		return connected;
+	}
+
+	boolean attemptedFailed() {
+		connected = false;
+		return connected;
+	}
+
+	void disconnect() {
+		// Make the default input procesing loop stop
+		shutdown = true;
+
+		// Shut down all the attached
+		if (mon != null) {
+			mon.stopMonitor();
+			mon.deleteObserver(connectionObserver);
+			mon = null;
+		}
+		if (tel != null) {
+			tel.stopTelemetry();
+			tel = null;
+		}
+
+		// Stop the master telemetry thread
+		handler.post(new Runnable() {
+			@Override
+			public void run() {
+				Looper.myLooper().quit();
+			}
+		});
+
+		if (inputProcessThread != null) {
+			inputProcessThread.interrupt();
+			try {
+				inputProcessThread.join();
+			} catch (InterruptedException e) {
+			}
+		}
+
+		// TODO: Make sure the input and output stream is closed
+
+		// TODO: Make sure any threads for input and output are closed
+	}
+
+	/**
+	 * Default implementation for processing input stream
+	 * which creates a new thread that keeps attempting
+	 * to read from the input stream.
+	 */
+	private void startInputProcessing() {
+		inputProcessThread = new Thread(new processUavTalk(), "Process UAV talk");
+		inputProcessThread.start();
+	}
+
+	//! Runnable to process input stream
+	class processUavTalk implements Runnable {
+		@Override
+		public void run() {
+			if (DEBUG) Log.d(TAG, "Entering UAVTalk processing loop");
+			while (!shutdown) {
+				try {
+					if( !uavTalk.processInputStream() )
+						break;
+				} catch (IOException e) {
+					e.printStackTrace();
+					telemService.toastMessage("Telemetry input stream interrupted");
+					break;
+				}
+			}
+			if (DEBUG) Log.d(TAG, "UAVTalk processing loop finished");
+		}
+	};
+
+	@Override
+	public void run() {
+		try {
+
+			Looper.prepare();
+			handler = new Handler();
+
+			if (DEBUG) Log.d(TAG, "Attempting connection");
+			if( attemptConnection() == false )
+				return; // Attempt failed
+
+			Looper.loop();
+
+			if (DEBUG) Log.d(TAG, "TelemetryTask runnable finished");
+
+		} catch (Throwable t) {
+			Log.e(TAG, "halted due to an error", t);
+		}
+
+		telemService.toastMessage("Telemetry Thread finished");
+	}
+
+	private final Observer connectionObserver = new Observer() {
+		@Override
+		public void update(Observable arg0, Object arg1) {
+			if (DEBUG) Log.d(TAG, "Mon updated. Connected: " + mon.getConnected() + " objects updated: " + mon.getObjectsUpdated());
+			if(mon.getConnected()) {
+				Intent intent = new Intent();
+				intent.setAction(OPTelemetryService.INTENT_ACTION_CONNECTED);
+				telemService.sendBroadcast(intent,null);
+			}
+		}
+	};
+
+	/**** General accessors ****/
+
+	public boolean getConnected() {
+		return connected;
+	}
+
+	public UAVTalk getUavtalk() {
+		return uavTalk;
+	}
+
+	public OPTelemetryService.TelemTask getTelemTaskIface() {
+		return new OPTelemetryService.TelemTask() {
+			@Override
+			public UAVObjectManager getObjectManager() {
+				return objMngr;
+			}
+		};
+	}
+
+}

androidgcs/src/org/openpilot/uavtalk/Telemetry.java

@@ -26,7 +26,6 @@ package org.openpilot.uavtalk;
 
 import java.io.IOException;
 import java.util.ArrayList;
-import java.util.LinkedList;
 import java.util.List;
 import java.util.ListIterator;
 import java.util.Observable;
@@ -34,167 +33,158 @@ import java.util.Observer;
 import java.util.Queue;
 import java.util.Timer;
 import java.util.TimerTask;
+import java.util.concurrent.ConcurrentLinkedQueue;
 
+import junit.framework.Assert;
+import android.os.Handler;
+import android.os.Looper;
 import android.util.Log;
 
 public class Telemetry {
+	/**
+	 * Telemetry provides a messaging handler to handle all the object updates
+	 * and transfer requests. This handler can either be attached to a new loop
+	 * attached to the thread started by the telemetry service.
+	 */
 
 	private final String TAG = "Telemetry";
-	public static int LOGLEVEL = 1;
+	public static int LOGLEVEL = 0;
-	public static boolean WARN = LOGLEVEL > 2;
+	public static boolean DEBUG = LOGLEVEL > 2;
-	public static boolean DEBUG = LOGLEVEL > 1;
+	public static boolean WARN  = LOGLEVEL > 1;
 	public static boolean ERROR = LOGLEVEL > 0;
-    public class TelemetryStats {
-        public int txBytes;
-        public int rxBytes;
-        public int txObjectBytes;
-        public int rxObjectBytes;
-        public int rxObjects;
-        public int txObjects;
-        public int txErrors;
-        public int rxErrors;
-        public int txRetries;
-    } ;
 
-    class ObjectTimeInfo {
+	public class TelemetryStats {
-        UAVObject obj;
+		public int txBytes;
-        int updatePeriodMs; /** Update period in ms or 0 if no periodic updates are needed */
+		public int rxBytes;
-        int timeToNextUpdateMs; /** Time delay to the next update */
+		public int txObjectBytes;
-    };
+		public int rxObjectBytes;
+		public int rxObjects;
+		public int txObjects;
+		public int txErrors;
+		public int rxErrors;
+		public int txRetries;
+	};
 
-    class ObjectQueueInfo {
+	class ObjectTimeInfo {
-        UAVObject obj;
+		UAVObject obj;
-        int event;
+		int updatePeriodMs;
-        boolean allInstances;
+		/** Update period in ms or 0 if no periodic updates are needed */
+		int timeToNextUpdateMs;
+		/** Time delay to the next update */
+	};
 
-        @Override
+	class ObjectQueueInfo {
+		UAVObject obj;
+		int event;
+		boolean allInstances;
+
+		@Override
 		public boolean equals(Object e) {
-        	try {
+			try {
-        		ObjectQueueInfo o = (ObjectQueueInfo) e;
+				ObjectQueueInfo o = (ObjectQueueInfo) e;
-        		return o.obj.getObjID() == obj.getObjID() &&  o.event == event && o.allInstances == allInstances;
+				return o.obj.getObjID() == obj.getObjID() && o.event == event
-        	} catch (Exception err) {
+						&& o.allInstances == allInstances;
+			} catch (Exception err) {
 
-        	};
-        	return false;
-        }
-    };
-
-    class ObjectTransactionInfo {
-        UAVObject obj;
-        boolean allInstances;
-        boolean objRequest;
-        int retriesRemaining;
-        boolean acked;
-    } ;
-
-    /**
-     * Events generated by objects.  Not enum because used in mask.
-     */
-    private static final int EV_UNPACKED = 0x01;       /** Object data updated by unpacking */
-    private static final int EV_UPDATED = 0x02;        /** Object data updated by changing the data structure */
-    private static final int EV_UPDATED_MANUAL = 0x04; /** Object update event manually generated */
-    private static final int EV_UPDATE_REQ = 0x08;     /** Request to update object data */
-
-    /**
-     * Constructor
-     */
-    public Telemetry(UAVTalk utalkIn, UAVObjectManager objMngr)
-    {
-        this.utalk = utalkIn;
-        this.objMngr = objMngr;
-
-        // Process all objects in the list
-        List< List<UAVObject> > objs = objMngr.getObjects();
-        ListIterator<List<UAVObject>> li = objs.listIterator();
-        while(li.hasNext())
-        	registerObject(li.next().get(0)); // we only need to register one instance per object type
-
-        // Listen to new object creations
-        objMngr.addNewInstanceObserver(new Observer() {
-			@Override
-			public void update(Observable observable, Object data) {
-        		newInstance((UAVObject) data);
 			}
-        });
+			;
-        objMngr.addNewObjectObserver(new Observer() {
+			return false;
+		}
+	};
+
+	class ObjectTransactionInfo {
+		UAVObject obj;
+		boolean allInstances;
+		boolean objRequest;
+		int retriesRemaining;
+		boolean acked;
+	};
+
+	/**
+	 * Events generated by objects. Not enum because used in mask.
+	 */
+	private static final int EV_UNPACKED = 0x01;
+	/** Object data updated by unpacking */
+	private static final int EV_UPDATED = 0x02;
+	/** Object data updated by changing the data structure */
+	private static final int EV_UPDATED_MANUAL = 0x04;
+	/** Object update event manually generated */
+	private static final int EV_UPDATE_REQ = 0x08;
+
+	/** Request to update object data */
+
+	/**
+	 * Constructor
+	 */
+	public Telemetry(UAVTalk utalkIn, UAVObjectManager objMngr, Looper l) {
+		this.utalk = utalkIn;
+		this.objMngr = objMngr;
+
+		// Create a handler for object messages
+		handler = new ObjectUpdateHandler(l);
+
+		// Process all objects in the list
+		List<List<UAVObject>> objs = objMngr.getObjects();
+		ListIterator<List<UAVObject>> li = objs.listIterator();
+		while (li.hasNext())
+			registerObject(li.next().get(0)); // we only need to register one
+		// instance per object type
+
+		// Listen to new object creations
+		objMngr.addNewInstanceObserver(new Observer() {
 			@Override
 			public void update(Observable observable, Object data) {
-        		newObject((UAVObject) data);
+				newInstance((UAVObject) data);
-        	}
+			}
-        });
+		});
+		objMngr.addNewObjectObserver(new Observer() {
+			@Override
+			public void update(Observable observable, Object data) {
+				newObject((UAVObject) data);
+			}
+		});
 
-        // Listen to transaction completions from uavtalk
+		// Listen to transaction completions from uavtalk
-        utalk.setOnTransactionCompletedListener(
+		utalk.setOnTransactionCompletedListener(utalk.new OnTransactionCompletedListener() {
-        		utalk.new OnTransactionCompletedListener() {
 			@Override
 			void TransactionSucceeded(UAVObject data) {
-	        	try {
+				transactionCompleted(data, true);
-					transactionCompleted(data, true);
+			}
-				} catch (IOException e) {
+
-					// Disconnect when stream fails
-					utalk.setOnTransactionCompletedListener(null);
-				}
-	        }
 			@Override
 			void TransactionFailed(UAVObject data) {
-	        	try {
+				if (DEBUG)
-	        		if (DEBUG) Log.d(TAG, "TransactionFailed(" + data.getName() + ")");
+					Log.d(TAG, "TransactionFailed(" + data.getName() + ")");
 
-					transactionCompleted(data, false);
+				transactionCompleted(data, false);
-				} catch (IOException e) {
-					// Disconnect when stream fails
-					utalk.setOnTransactionCompletedListener(null);
-				}
-	        }
-
-        });
-
-        // Get GCS stats object
-        gcsStatsObj = objMngr.getObject("GCSTelemetryStats");
-
-        // Setup transaction timer
-        transPending = false;
-        // Setup and start the periodic timer
-        timeToNextUpdateMs = 0;
-        updateTimerSetPeriod(1000);
-        // Setup and start the stats timer
-        txErrors = 0;
-        txRetries = 0;
-    }
-
-    synchronized void transTimerSetPeriod(int periodMs) {
-    	if(transTimerTask != null)
-    		transTimerTask.cancel();
-
-    	if(transTimer != null)
-    		transTimer.purge();
-
-   		transTimer = new Timer();
-
-        transTimerTask = new TimerTask() {
-			@Override
-			public void run() {
-				try {
-					transactionTimeout();
-				} catch (IOException e) {
-					cancel();
-				}
 			}
-        };
-        transTimer.schedule(transTimerTask, periodMs, periodMs);
-    }
 
-    synchronized void updateTimerSetPeriod(int periodMs) {
+		});
-    	if (updateTimer != null) {
+
-    		updateTimer.cancel();
+		// Get GCS stats object
-    		updateTimer = null;
+		gcsStatsObj = objMngr.getObject("GCSTelemetryStats");
-    	}
+
-    	if (updateTimerTask != null) {
+		// Setup transaction timer
-    		updateTimerTask.cancel();
+		transPending = false;
-    		updateTimerTask = null;
+		// Setup and start the periodic timer
-    	}
+		timeToNextUpdateMs = 0;
-        updateTimer = new Timer();
+		updateTimerSetPeriod(1000);
-        updateTimerTask = new TimerTask() {
+		// Setup and start the stats timer
+		txErrors = 0;
+		txRetries = 0;
+	}
+
+	synchronized void updateTimerSetPeriod(int periodMs) {
+		if (updateTimer != null) {
+			updateTimer.cancel();
+			updateTimer = null;
+		}
+		if (updateTimerTask != null) {
+			updateTimerTask.cancel();
+			updateTimerTask = null;
+		}
+		updateTimer = new Timer();
+		updateTimerTask = new TimerTask() {
 			@Override
 			public void run() {
 				try {
@@ -204,569 +194,561 @@ public class Telemetry {
 					updateTimer.cancel();
 				}
 			}
-        };
+		};
-        updateTimer.schedule(updateTimerTask, periodMs, periodMs);
+		updateTimer.schedule(updateTimerTask, periodMs, periodMs);
 
-    }
+	}
 
-    /**
+	/**
-     * Register a new object for periodic updates (if enabled)
+	 * Register a new object for periodic updates (if enabled)
-     */
+	 */
-    private synchronized void registerObject(UAVObject obj)
+	private synchronized void registerObject(UAVObject obj) {
-    {
+		// Setup object for periodic updates
-        // Setup object for periodic updates
+		addObject(obj);
-        addObject(obj);
 
-        // Setup object for telemetry updates
+		// Setup object for telemetry updates
-        updateObject(obj);
+		updateObject(obj);
-    }
+	}
 
-    /**
+	/**
-     * Add an object in the list used for periodic updates
+	 * Add an object in the list used for periodic updates
-     */
+	 */
-    private synchronized void addObject(UAVObject obj)
+	private synchronized void addObject(UAVObject obj) {
-    {
+		// Check if object type is already in the list
-        // Check if object type is already in the list
+		ListIterator<ObjectTimeInfo> li = objList.listIterator();
-    	ListIterator<ObjectTimeInfo> li = objList.listIterator();
+		while (li.hasNext()) {
-    	while(li.hasNext()) {
+			ObjectTimeInfo n = li.next();
-    		ObjectTimeInfo n = li.next();
+			if (n.obj.getObjID() == obj.getObjID()) {
-    		if( n.obj.getObjID() == obj.getObjID() )
+				// Object type (not instance!) is already in the list, do
-            {
+				// nothing
-                // Object type (not instance!) is already in the list, do nothing
+				return;
-                return;
+			}
-            }
+		}
-        }
 
-        // If this point is reached, then the object type is new, let's add it
+		// If this point is reached, then the object type is new, let's add it
-        ObjectTimeInfo timeInfo = new ObjectTimeInfo();
+		ObjectTimeInfo timeInfo = new ObjectTimeInfo();
-        timeInfo.obj = obj;
+		timeInfo.obj = obj;
-        timeInfo.timeToNextUpdateMs = 0;
+		timeInfo.timeToNextUpdateMs = 0;
-        timeInfo.updatePeriodMs = 0;
+		timeInfo.updatePeriodMs = 0;
-        objList.add(timeInfo);
+		objList.add(timeInfo);
-    }
+	}
 
-    /**
+	/**
-     * Update the object's timers
+	 * Update the object's timers
-     */
+	 */
-    private synchronized void setUpdatePeriod(UAVObject obj, int periodMs)
+	private synchronized void setUpdatePeriod(UAVObject obj, int periodMs) {
-    {
+		// Find object type (not instance!) and update its period
-        // Find object type (not instance!) and update its period
+		ListIterator<ObjectTimeInfo> li = objList.listIterator();
-    	ListIterator<ObjectTimeInfo> li = objList.listIterator();
+		while (li.hasNext()) {
-    	while(li.hasNext()) {
+			ObjectTimeInfo n = li.next();
-    		ObjectTimeInfo n = li.next();
+			if (n.obj.getObjID() == obj.getObjID()) {
-            if ( n.obj.getObjID() == obj.getObjID() )
+				n.updatePeriodMs = periodMs;
-            {
+				n.timeToNextUpdateMs = (int) (periodMs * (new java.util.Random())
-                n.updatePeriodMs = periodMs;
+						.nextDouble()); // avoid bunching of updates
-                n.timeToNextUpdateMs = (int) (periodMs * (new java.util.Random()).nextDouble()); // avoid bunching of updates
+			}
-            }
+		}
-        }
+	}
-    }
 
-    final Observer unpackedObserver = new Observer() {
+	final Observer unpackedObserver = new Observer() {
 		@Override
 		public void update(Observable observable, Object data) {
-    		try {
+			handler.unpacked((UAVObject) data);
-    			enqueueObjectUpdates((UAVObject) data, EV_UNPACKED, false, true);
+		}
-			} catch (IOException e) {
-				// TODO Auto-generated catch block
-				e.printStackTrace();
-			}
-    	}
 	};
 
 	final Observer updatedAutoObserver = new Observer() {
 		@Override
 		public void update(Observable observable, Object data) {
-			try {
+			handler.updatedAuto((UAVObject) data);
-				enqueueObjectUpdates((UAVObject) data, EV_UPDATED, false, true);
+		}
-			} catch (IOException e) {
-				// TODO Auto-generated catch block
-				e.printStackTrace();
-			}
-    	}
 	};
 
 	final Observer updatedManualObserver = new Observer() {
 		@Override
 		public void update(Observable observable, Object data) {
-			try {
+			handler.updatedManual((UAVObject) data);
-				enqueueObjectUpdates((UAVObject) data, EV_UPDATED_MANUAL, false, true);
+		}
-			} catch (IOException e) {
-				// TODO Auto-generated catch block
-				e.printStackTrace();
-			}
-    	}
 	};
 
 	final Observer updatedRequestedObserver = new Observer() {
 		@Override
 		public void update(Observable observable, Object data) {
-			try {
+			handler.updateRequested((UAVObject) data);
-				enqueueObjectUpdates((UAVObject) data, EV_UPDATE_REQ, false, true);
+		}
-			} catch (IOException e) {
-				// TODO Auto-generated catch block
-				e.printStackTrace();
-			}
-    	}
 	};
 
-    /**
+	/**
-     * Connect to all instances of an object depending on the event mask specified
+	 * Connect to all instances of an object depending on the event mask
-     */
+	 * specified
-    private synchronized void connectToObjectInstances(UAVObject obj, int eventMask)
+	 */
-    {
+	private synchronized void connectToObjectInstances(UAVObject obj,
-        List<UAVObject> objs = objMngr.getObjectInstances(obj.getObjID());
+			int eventMask) {
-        ListIterator<UAVObject> li = objs.listIterator();
+		List<UAVObject> objs = objMngr.getObjectInstances(obj.getObjID());
-        while(li.hasNext())
+		ListIterator<UAVObject> li = objs.listIterator();
-        {
+		while (li.hasNext()) {
-        	obj = li.next();
+			obj = li.next();
 
-            // Disconnect all previous observers from telemetry.  This is imortant as this can
+			// Disconnect all previous observers from telemetry. This is
-        	// be called multiple times
+			// imortant as this can
-        	obj.removeUnpackedObserver(unpackedObserver);
+			// be called multiple times
-        	obj.removeUpdatedAutoObserver(updatedAutoObserver);
+			obj.removeUnpackedObserver(unpackedObserver);
-        	obj.removeUpdatedManualObserver(updatedManualObserver);
+			obj.removeUpdatedAutoObserver(updatedAutoObserver);
-        	obj.removeUpdateRequestedObserver(updatedRequestedObserver);
+			obj.removeUpdatedManualObserver(updatedManualObserver);
+			obj.removeUpdateRequestedObserver(updatedRequestedObserver);
 
-            // Connect only the selected events
+			// Connect only the selected events
-            if ( (eventMask&EV_UNPACKED) != 0)
+			if ((eventMask & EV_UNPACKED) != 0)
-            	obj.addUnpackedObserver(unpackedObserver);
+				obj.addUnpackedObserver(unpackedObserver);
-            if ( (eventMask&EV_UPDATED) != 0)
+			if ((eventMask & EV_UPDATED) != 0)
-            	obj.addUpdatedAutoObserver(updatedAutoObserver);
+				obj.addUpdatedAutoObserver(updatedAutoObserver);
-            if ( (eventMask&EV_UPDATED_MANUAL) != 0)
+			if ((eventMask & EV_UPDATED_MANUAL) != 0)
-            	obj.addUpdatedManualObserver(updatedManualObserver);
+				obj.addUpdatedManualObserver(updatedManualObserver);
-            if ( (eventMask&EV_UPDATE_REQ) != 0)
+			if ((eventMask & EV_UPDATE_REQ) != 0)
-            	obj.addUpdateRequestedObserver(updatedRequestedObserver);
+				obj.addUpdateRequestedObserver(updatedRequestedObserver);
-        }
+		}
-    }
+	}
 
-    /**
+	/**
-     * Update an object based on its metadata properties
+	 * Update an object based on its metadata properties
-     */
+	 */
-    private synchronized void updateObject(UAVObject obj)
+	private void updateObject(UAVObject obj) {
-    {
+		// Get metadata
-        // Get metadata
+		UAVObject.Metadata metadata = obj.getMetadata();
-        UAVObject.Metadata metadata = obj.getMetadata();
 
-        // Setup object depending on update mode
+		// Setup object depending on update mode
-        int eventMask;
+		int eventMask;
-        if ( metadata.GetGcsTelemetryUpdateMode() == UAVObject.UpdateMode.UPDATEMODE_PERIODIC )
+		if (metadata.GetGcsTelemetryUpdateMode() == UAVObject.UpdateMode.UPDATEMODE_PERIODIC) {
-        {
+			// Set update period
-            // Set update period
+			setUpdatePeriod(obj, metadata.gcsTelemetryUpdatePeriod);
-            setUpdatePeriod(obj, metadata.gcsTelemetryUpdatePeriod);
+			// Connect signals for all instances
-            // Connect signals for all instances
+			eventMask = EV_UPDATED_MANUAL | EV_UPDATE_REQ;
-            eventMask = EV_UPDATED_MANUAL | EV_UPDATE_REQ;
+			if (obj.isMetadata())
-            if(obj.isMetadata())
+				eventMask |= EV_UNPACKED; // we also need to act on remote
-                eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events)
+			// updates (unpack events)
 
-            connectToObjectInstances(obj, eventMask);
+			connectToObjectInstances(obj, eventMask);
-        }
+		} else if (metadata.GetGcsTelemetryUpdateMode() == UAVObject.UpdateMode.UPDATEMODE_ONCHANGE) {
-        else if ( metadata.GetGcsTelemetryUpdateMode() == UAVObject.UpdateMode.UPDATEMODE_ONCHANGE )
+			// Set update period
-        {
+			setUpdatePeriod(obj, 0);
-            // Set update period
+			// Connect signals for all instances
-            setUpdatePeriod(obj, 0);
+			eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ;
-            // Connect signals for all instances
+			if (obj.isMetadata())
-            eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ;
+				eventMask |= EV_UNPACKED; // we also need to act on remote
-            if(obj.isMetadata())
+			// updates (unpack events)
-                eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events)
 
-            connectToObjectInstances(obj, eventMask);
+			connectToObjectInstances(obj, eventMask);
-        }
+		} else if (metadata.GetGcsTelemetryUpdateMode() == UAVObject.UpdateMode.UPDATEMODE_THROTTLED) {
-        else if ( metadata.GetGcsTelemetryUpdateMode() == UAVObject.UpdateMode.UPDATEMODE_THROTTLED )
+			// TODO
-        {
+		} else if (metadata.GetGcsTelemetryUpdateMode() == UAVObject.UpdateMode.UPDATEMODE_MANUAL) {
-        	// TODO
+			// Set update period
-        }
+			setUpdatePeriod(obj, 0);
-        else if ( metadata.GetGcsTelemetryUpdateMode() == UAVObject.UpdateMode.UPDATEMODE_MANUAL )
+			// Connect signals for all instances
-        {
+			eventMask = EV_UPDATED_MANUAL | EV_UPDATE_REQ;
-            // Set update period
+			if (obj.isMetadata())
-            setUpdatePeriod(obj, 0);
+				eventMask |= EV_UNPACKED; // we also need to act on remote
-            // Connect signals for all instances
+			// updates (unpack events)
-            eventMask = EV_UPDATED_MANUAL | EV_UPDATE_REQ;
-            if(obj.isMetadata())
-                eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events)
 
-            connectToObjectInstances(obj, eventMask);
+			connectToObjectInstances(obj, eventMask);
-        }
+		}
-    }
+	}
 
-    /**
+	/**
-     * Called when a transaction is successfully completed (uavtalk event)
+	 * Check is any objects are pending for periodic updates TODO: Clean-up
-     * @throws IOException
+	 *
-     */
+	 * @throws IOException
-    private synchronized void transactionCompleted(UAVObject obj, boolean result) throws IOException
+	 */
-    {
+	private void processPeriodicUpdates() throws IOException {
-    	if (DEBUG) Log.d(TAG,"UAVTalk transactionCompleted");
-        // Check if there is a pending transaction and the objects match
-        if ( transPending && transInfo.obj.getObjID() == obj.getObjID() )
-        {
-        	if (DEBUG) Log.d(TAG,"Telemetry: transaction completed for " + obj.getName());
-            // Complete transaction
-        	transTimer.cancel();
-            transPending = false;
 
-            //Send signal
+		if (DEBUG)
-            obj.transactionCompleted(result);
+			Log.d(TAG, "processPeriodicUpdates()");
-            // Process new object updates from queue
+		// Stop timer
-            processObjectQueue();
-        } else
-        {
-        	if (ERROR) Log.e(TAG,"Error: received a transaction completed when did not expect it.");
-        	transPending = false;
-        }
-    }
 
-    /**
+		updateTimer.cancel();
-     * Called when a transaction is not completed within the timeout period (timer event)
-     * @throws IOException
-     */
-    private synchronized void transactionTimeout() throws IOException
-    {
-    	if (DEBUG) Log.d(TAG,"Telemetry: transaction timeout.");
-        transTimer.cancel();
-        // Proceed only if there is a pending transaction
-        if ( transPending )
-        {
-            // Check if more retries are pending
-            if (transInfo.retriesRemaining > 0)
-            {
-                --transInfo.retriesRemaining;
-                processObjectTransaction();
-                ++txRetries;
-            }
-            else
-            {
-            	if (ERROR) Log.e(TAG, "Transaction failed for: " + transInfo.obj.getName());
 
-                // Terminate transaction.  This triggers UAVTalk to send a transaction
+		// Iterate through each object and update its timer, if zero then
-            	// failed signal which will make the next queue entry be processed
+		// transmit object.
-            	// Note this is UAVTalk listener TransactionFailed function and not the
+		// Also calculate smallest delay to next update (will be used for
-            	// object specific transaction failed.
+		// setting timeToNextUpdateMs)
-                utalk.cancelPendingTransaction(transInfo.obj);
+		int minDelay = MAX_UPDATE_PERIOD_MS;
-                ++txErrors;
+		ObjectTimeInfo objinfo;
-            }
+		int elapsedMs = 0;
-        }
+		long startTime;
-    }
+		int offset;
+		ListIterator<ObjectTimeInfo> li = objList.listIterator();
+		while (li.hasNext()) {
+			objinfo = li.next();
+			// If object is configured for periodic updates
+			if (objinfo.updatePeriodMs > 0) {
+				objinfo.timeToNextUpdateMs -= timeToNextUpdateMs;
+				// Check if time for the next update
+				if (objinfo.timeToNextUpdateMs <= 0) {
+					// Reset timer
+					offset = (-objinfo.timeToNextUpdateMs)
+							% objinfo.updatePeriodMs;
+					objinfo.timeToNextUpdateMs = objinfo.updatePeriodMs
+							- offset;
+					// Send object
+					startTime = System.currentTimeMillis();
 
-    /**
+					if (DEBUG) Log.d(TAG, "Manual update: " + objinfo.obj.getName());
-     * Start an object transaction with UAVTalk, all information is stored in transInfo
+					handler.updatedManual(objinfo.obj);
-     * @throws IOException
+					// enqueueObjectUpdates(objinfo.obj, EV_UPDATED_MANUAL,
-     */
+					// true, false);
-    private synchronized void processObjectTransaction() throws IOException
+					elapsedMs = (int) (System.currentTimeMillis() - startTime);
-    {
+					// Update timeToNextUpdateMs with the elapsed delay of
-        if (transPending)
+					// sending the object;
-        {
+					timeToNextUpdateMs += elapsedMs;
-        	if (DEBUG) Log.d(TAG, "Process Object transaction for " + transInfo.obj.getName());
+				}
-            // Initiate transaction
+				// Update minimum delay
-            if (transInfo.objRequest)
+				if (objinfo.timeToNextUpdateMs < minDelay) {
-            {
+					minDelay = objinfo.timeToNextUpdateMs;
-                utalk.sendObjectRequest(transInfo.obj, transInfo.allInstances);
+				}
-            }
+			}
-            else
+		}
-            {
-                utalk.sendObject(transInfo.obj, transInfo.acked, transInfo.allInstances);
-            }
-            // Start timer if a response is expected
-            if ( transInfo.objRequest || transInfo.acked )
-            {
-            	transTimerSetPeriod(REQ_TIMEOUT_MS);
-            }
-            else
-            {
-            	transTimer.cancel();
-                transPending = false;
-            }
-        } else
-        {
-        	if (ERROR) Log.e(TAG,"Error: inside of processObjectTransaction with no transPending");
-        }
-    }
 
-    /**
+		// Check if delay for the next update is too short
-     * Enqueue the event received from an object.  This is the main method that handles all the callbacks
+		if (minDelay < MIN_UPDATE_PERIOD_MS) {
-     * from UAVObjects (due to updates, or update requests)
+			minDelay = MIN_UPDATE_PERIOD_MS;
-     */
+		}
-    private synchronized void enqueueObjectUpdates(UAVObject obj, int event, boolean allInstances, boolean priority) throws IOException
-    {
-        // Push event into queue
-    	if (DEBUG) Log.d(TAG, "Push event into queue for obj " + obj.getName() + " event " + event);
-    	if(event == 8 && obj.getName().compareTo("GCSTelemetryStats") == 0)
-    		Thread.dumpStack();
-        ObjectQueueInfo objInfo = new ObjectQueueInfo();
-        objInfo.obj = obj;
-        objInfo.event = event;
-        objInfo.allInstances = allInstances;
-        if (priority)
-        {
-        	// Only enqueue if an identical transaction does not already exist
-        	if(!objPriorityQueue.contains(objInfo)) {
-        		if ( objPriorityQueue.size() < MAX_QUEUE_SIZE )
-        		{
-        			objPriorityQueue.add(objInfo);
-        		}
-        		else
-        		{
-        			++txErrors;
-        			obj.transactionCompleted(false);
-        			Log.w(TAG,"Telemetry: priority event queue is full, event lost " + obj.getName());
-        		}
-        	}
-        }
-        else
-        {
-        	// Only enqueue if an identical transaction does not already exist
-        	if(!objQueue.contains(objInfo)) {
-        		if ( objQueue.size() < MAX_QUEUE_SIZE )
-        		{
-        			objQueue.add(objInfo);
-        		}
-        		else
-        		{
-        			++txErrors;
-        			obj.transactionCompleted(false);
-        		}
-        	}
-        }
 
-        // If there is no transaction in progress then process event
+		// Done
-        if (!transPending)
+		timeToNextUpdateMs = minDelay;
-        {
-            processObjectQueue();
-        }
-    }
 
-    /**
+		// Restart timer
-     * Process events from the object queue
+		updateTimerSetPeriod(timeToNextUpdateMs);
-     * @throws IOException
+	}
-     */
-    private synchronized void processObjectQueue() throws IOException
-    {
-      	if (DEBUG) Log.d(TAG, "Process object queue - Depth " + objQueue.size() + " priority " + objPriorityQueue.size());
 
-        // Don nothing if a transaction is already in progress (should not happen)
+	public TelemetryStats getStats() {
-        if (transPending)
+		// Get UAVTalk stats
-        {
+		UAVTalk.ComStats utalkStats = utalk.getStats();
-        	if (WARN) Log.e(TAG,"Dequeue while a transaction pending");
-            return;
-        }
 
-        // Get object information from queue (first the priority and then the regular queue)
+		// Update stats
-        ObjectQueueInfo objInfo;
+		TelemetryStats stats = new TelemetryStats();
-        if ( !objPriorityQueue.isEmpty() )
+		stats.txBytes = utalkStats.txBytes;
-        {
+		stats.rxBytes = utalkStats.rxBytes;
-            objInfo = objPriorityQueue.remove();
+		stats.txObjectBytes = utalkStats.txObjectBytes;
-        }
+		stats.rxObjectBytes = utalkStats.rxObjectBytes;
-        else if ( !objQueue.isEmpty() )
+		stats.rxObjects = utalkStats.rxObjects;
-        {
+		stats.txObjects = utalkStats.txObjects;
-            objInfo = objQueue.remove();
+		stats.txErrors = utalkStats.txErrors + txErrors;
-        }
+		stats.rxErrors = utalkStats.rxErrors;
-        else
+		stats.txRetries = txRetries;
-        {
-            return;
-        }
 
-        // Check if a connection has been established, only process GCSTelemetryStats updates
+		// Done
-        // (used to establish the connection)
+		return stats;
-        gcsStatsObj = objMngr.getObject("GCSTelemetryStats");
+	}
-        if ( ((String) gcsStatsObj.getField("Status").getValue()).compareTo("Connected") != 0 )
-        {
-            objQueue.clear();
-            if ( objInfo.obj.getObjID() != objMngr.getObject("GCSTelemetryStats").getObjID() )
-            {
-            	if (DEBUG) Log.d(TAG,"transactionCompleted(false) due to receiving object not GCSTelemetryStats while not connected.");
-                objInfo.obj.transactionCompleted(false);
-                return;
-            }
-        }
 
-        // Setup transaction (skip if unpack event)
+	public void resetStats() {
-        if ( objInfo.event != EV_UNPACKED )
+		utalk.resetStats();
-        {
+		txErrors = 0;
-            UAVObject.Metadata metadata = objInfo.obj.getMetadata();
+		txRetries = 0;
-            transInfo.obj = objInfo.obj;
+	}
-            transInfo.allInstances = objInfo.allInstances;
-            transInfo.retriesRemaining = MAX_RETRIES;
-            transInfo.acked = metadata.GetGcsTelemetryAcked();
-            if ( objInfo.event == EV_UPDATED || objInfo.event == EV_UPDATED_MANUAL )
-            {
-                transInfo.objRequest = false;
-            }
-            else if ( objInfo.event == EV_UPDATE_REQ )
-            {
-                transInfo.objRequest = true;
-            }
-            // Start transaction
-            transPending = true;
-            processObjectTransaction();
-        } else
-        {
-//            qDebug() << QString("Process object queue: this is an unpack event for %1").arg(objInfo.obj->getName());
-        }
 
-        // If this is a metaobject then make necessary telemetry updates
+	private void newObject(UAVObject obj) {
-        if (objInfo.obj.isMetadata())
+		registerObject(obj);
-        {
+	}
-        	UAVMetaObject metaobj = (UAVMetaObject) objInfo.obj;
-            updateObject( metaobj.getParentObject() );
-        }
 
-        // The fact we received an unpacked event does not mean that
+	private synchronized void newInstance(UAVObject obj) {
-        // we do not have additional objects still in the queue,
+		registerObject(obj);
-        // so we have to reschedule queue processing to make sure they are not
+	}
-        // stuck:
-        if ( objInfo.event == EV_UNPACKED && !transPending)
-            processObjectQueue();
 
-    }
+	/**
-
+	 * Stop all the telemetry timers
-    /**
+	 */
-     * Check is any objects are pending for periodic updates
+	public void stopTelemetry() {
-     * TODO: Clean-up
+		if (updateTimerTask != null)
-     * @throws IOException
+			updateTimerTask.cancel();
-     */
+		updateTimerTask = null;
-    private synchronized void processPeriodicUpdates() throws IOException
+		if (updateTimer != null)
-    {
+			updateTimer.cancel();
-
+		updateTimer = null;
-    	if (DEBUG) Log.d(TAG, "processPeriodicUpdates()");
+	}
-        // Stop timer
-    	updateTimer.cancel();
-
-        // Iterate through each object and update its timer, if zero then transmit object.
-        // Also calculate smallest delay to next update (will be used for setting timeToNextUpdateMs)
-        int minDelay = MAX_UPDATE_PERIOD_MS;
-        ObjectTimeInfo objinfo;
-        int elapsedMs = 0;
-        long startTime;
-        int offset;
-        ListIterator<ObjectTimeInfo> li = objList.listIterator();
-        while(li.hasNext())
-        {
-            objinfo = li.next();
-            // If object is configured for periodic updates
-            if (objinfo.updatePeriodMs > 0)
-            {
-                objinfo.timeToNextUpdateMs -= timeToNextUpdateMs;
-                // Check if time for the next update
-                if (objinfo.timeToNextUpdateMs <= 0)
-                {
-                    // Reset timer
-                    offset = (-objinfo.timeToNextUpdateMs) % objinfo.updatePeriodMs;
-                    objinfo.timeToNextUpdateMs = objinfo.updatePeriodMs - offset;
-                    // Send object
-                    startTime = System.currentTimeMillis();
-                    enqueueObjectUpdates(objinfo.obj, EV_UPDATED_MANUAL, true, false);
-                    elapsedMs = (int) (System.currentTimeMillis() - startTime);
-                    // Update timeToNextUpdateMs with the elapsed delay of sending the object;
-                    timeToNextUpdateMs += elapsedMs;
-                }
-                // Update minimum delay
-                if (objinfo.timeToNextUpdateMs < minDelay)
-                {
-                    minDelay = objinfo.timeToNextUpdateMs;
-                }
-            }
-        }
-
-        // Check if delay for the next update is too short
-        if (minDelay < MIN_UPDATE_PERIOD_MS)
-        {
-            minDelay = MIN_UPDATE_PERIOD_MS;
-        }
-
-        // Done
-        timeToNextUpdateMs = minDelay;
-
-        // Restart timer
-        updateTimerSetPeriod(timeToNextUpdateMs);
-    }
-
-    public TelemetryStats getStats()
-    {
-        // Get UAVTalk stats
-        UAVTalk.ComStats utalkStats = utalk.getStats();
-
-        // Update stats
-        TelemetryStats stats = new TelemetryStats();
-        stats.txBytes = utalkStats.txBytes;
-        stats.rxBytes = utalkStats.rxBytes;
-        stats.txObjectBytes = utalkStats.txObjectBytes;
-        stats.rxObjectBytes = utalkStats.rxObjectBytes;
-        stats.rxObjects = utalkStats.rxObjects;
-        stats.txObjects = utalkStats.txObjects;
-        stats.txErrors = utalkStats.txErrors + txErrors;
-        stats.rxErrors = utalkStats.rxErrors;
-        stats.txRetries = txRetries;
-
-        // Done
-        return stats;
-    }
-
-    public synchronized void resetStats()
-    {
-        utalk.resetStats();
-        txErrors = 0;
-        txRetries = 0;
-    }
-
-
-    private void newObject(UAVObject obj)
-    {
-        registerObject(obj);
-    }
-
-    private synchronized void newInstance(UAVObject obj)
-    {
-        registerObject(obj);
-    }
-
-    /**
-     * Stop all the telemetry timers
-     */
-    public void stopTelemetry()
-    {
-    	if (updateTimerTask != null)
-    		updateTimerTask.cancel();
-    	updateTimerTask = null;
-    	if (updateTimer != null)
-    		updateTimer.cancel();
-    	updateTimer = null;
-    	if (transTimerTask != null)
-    		transTimerTask.cancel();
-    	transTimerTask = null;
-    	if (transTimer != null)
-    		transTimer.cancel();
-    	transTimer = null;
-    }
 
 	/**
 	 * Private variables
 	 */
-    private final UAVObjectManager objMngr;
+	private final UAVObjectManager objMngr;
-    private final UAVTalk utalk;
+	private final UAVTalk utalk;
-    private UAVObject gcsStatsObj;
+	private UAVObject gcsStatsObj;
-    private final List<ObjectTimeInfo> objList = new ArrayList<ObjectTimeInfo>();
+	private final List<ObjectTimeInfo> objList = new ArrayList<ObjectTimeInfo>();
-    private final Queue<ObjectQueueInfo> objQueue = new LinkedList<ObjectQueueInfo>();
+	private ObjectTransactionInfo transInfo = new ObjectTransactionInfo();
-    private final Queue<ObjectQueueInfo> objPriorityQueue = new LinkedList<ObjectQueueInfo>();
+	private boolean transPending;
-    private final ObjectTransactionInfo transInfo = new ObjectTransactionInfo();
-    private boolean transPending;
 
-    private Timer updateTimer;
+	private Timer updateTimer;
-    private TimerTask updateTimerTask;
+	private TimerTask updateTimerTask;
-    private Timer transTimer;
-    private TimerTask transTimerTask;
 
-    private int timeToNextUpdateMs;
+	private int timeToNextUpdateMs;
-    private int txErrors;
+	private int txErrors;
-    private int txRetries;
+	private int txRetries;
 
-    /**
+	/**
-     * Private constants
+	 * Private constants
-     */
+	 */
-    private static final int REQ_TIMEOUT_MS = 250;
+	private static final int REQ_TIMEOUT_MS = 250;
-    private static final int MAX_RETRIES = 2;
+	private static final int MAX_RETRIES = 2;
-    private static final int MAX_UPDATE_PERIOD_MS = 1000;
+	private static final int MAX_UPDATE_PERIOD_MS = 1000;
-    private static final int MIN_UPDATE_PERIOD_MS = 1;
+	private static final int MIN_UPDATE_PERIOD_MS = 1;
-    private static final int MAX_QUEUE_SIZE = 20;
+
+	static private ObjectUpdateHandler handler;
+
+	//! Accessor for the object updated handler
+	ObjectUpdateHandler getHandler() { return handler; }
+
+	/**
+	 * Handler which posts all the messages for individual object updates
+	 */
+	public class ObjectUpdateHandler extends Handler {
+
+		// ! This can only be created while attaching to a particular looper
+		ObjectUpdateHandler(Looper l) {
+			super(l);
+		}
+
+		Queue<ObjectQueueInfo> objQueue = new ConcurrentLinkedQueue<ObjectQueueInfo>();
+
+		// ! Generic enqueue
+		void enqueueObjectUpdates(UAVObject obj, int event,
+				boolean allInstances, boolean priority) {
+
+			if (DEBUG) Log.d(TAG, "Enqueing update " + obj.getName() + " event " + event);
+
+			ObjectQueueInfo objInfo = new ObjectQueueInfo();
+			objInfo.obj = obj;
+			objInfo.event = event;
+			objInfo.allInstances = allInstances;
+
+			// For now maintain a list of objects in the queue so we don't add duplicates
+			// later we should make the runnables static to each class so we can use removeCallback
+			synchronized(objQueue) {
+				if (objQueue.contains(objInfo)) {
+					if (WARN) Log.w(TAG, "Found previously scheduled queue element: " + objInfo.obj.getName());
+				} else {
+					objQueue.add(objInfo);
+					post(new ObjectRunnable(objInfo));
+				}
+			}
+		}
+
+		public boolean removeActivatedQueue(ObjectQueueInfo objInfo) {
+			synchronized(objQueue) {
+				if (objQueue.remove(objInfo)) {
+					if (WARN) Log.w(TAG, "Unable to find queue element to remove");
+					return false;
+				}
+			}
+			return true;
+		}
+
+		// ! Enqueue an unpacked event
+		void unpacked(UAVObject obj) {
+			enqueueObjectUpdates(obj, EV_UNPACKED, false, true);
+		}
+
+		// ! Enqueue an updated auto event
+		void updatedAuto(UAVObject obj) {
+			enqueueObjectUpdates(obj, EV_UPDATED, false, true);
+		}
+
+		// ! Enqueue an updated manual event
+		void updatedManual(UAVObject obj) {
+			enqueueObjectUpdates(obj, EV_UPDATED_MANUAL, false, true);
+		}
+
+		// ! Enqueue an update requested event
+		void updateRequested(UAVObject obj) {
+			enqueueObjectUpdates(obj, EV_UPDATE_REQ, false, true);
+		}
+
+	}
+
+	/**
+	 * Perform an update on an object where on an event based on the contents provided
+	 * to the constructors.  This update will also set a timeout for transaction failure.
+	 */
+	class ObjectRunnable implements Runnable {
+
+		// ! Transaction information to perform
+		private final ObjectQueueInfo objInfo;
+
+		ObjectRunnable(ObjectQueueInfo info) {
+			Assert.assertNotNull(info);
+			objInfo = info;
+		}
+
+		// ! Perform the transaction on the looper thread
+		@Override
+		public void run() {
+			if (DEBUG) Log.d(TAG, "Object transaction running.  Event:" + objInfo.event);
+			// 1. Check GCS is connected, throw this out if not
+			// 2. Set up a transaction which includes multiple retries, whether
+			// to wait for ack etc
+			// 3. Send UAVTalk message
+			// 4. Based on transaction type either wait for update or end
+
+			// 1. Check if a connection has been established, only process
+			// GCSTelemetryStats updates
+			// (used to establish the connection)
+			gcsStatsObj = objMngr.getObject("GCSTelemetryStats");
+			if (((String) gcsStatsObj.getField("Status").getValue()).compareTo("Connected") != 0) {
+				if (objInfo.obj.getObjID() != objMngr.getObject("GCSTelemetryStats").getObjID()) {
+					if (DEBUG)
+						Log.d(TAG, "transactionCompleted(false) due to receiving object not GCSTelemetryStats while not connected.");
+					objInfo.obj.transactionCompleted(false);
+					return;
+				}
+			}
+
+			// If this is a metaobject then make necessary telemetry updates
+			// (this is why we catch unpack)
+			if (objInfo.obj.isMetadata()) {
+				UAVMetaObject metaobj = (UAVMetaObject) objInfo.obj;
+				updateObject(metaobj.getParentObject());
+			}
+
+			// 2. Setup transaction (skip if unpack event)
+			ObjectTransactionInfo newTrans = new ObjectTransactionInfo();
+			boolean newTransactionPending = false;
+			if (objInfo.event != EV_UNPACKED) {
+				UAVObject.Metadata metadata = objInfo.obj.getMetadata();
+				newTrans.obj = objInfo.obj;
+				newTrans.allInstances = objInfo.allInstances;
+				newTrans.retriesRemaining = MAX_RETRIES;
+				newTrans.acked = metadata.GetGcsTelemetryAcked();
+				if (objInfo.event == EV_UPDATED || objInfo.event == EV_UPDATED_MANUAL) {
+					newTrans.objRequest = false;
+				} else if (objInfo.event == EV_UPDATE_REQ) {
+					newTrans.objRequest = true;
+				}
+
+				// Determine if this will schedule a new transaction
+				newTransactionPending = (newTrans.objRequest || newTrans.acked);
+
+				synchronized (transInfo) {
+
+					// If there is a transaction pending and this would set up a new one reschedule it
+					if (transPending && newTransactionPending) {
+						if (WARN) Log.w(TAG, "Postponing transaction for" + newTrans.obj.getName() + " existing transaction for " + transInfo.obj.getName());
+						handler.postDelayed(this, 100);
+						return;
+					}
+
+					// Store this as the active transaction
+					transPending = newTransactionPending;
+					transInfo = newTrans;
+
+					if (DEBUG) Log.d(TAG, "Process Object transaction for " + transInfo.obj.getName());
+
+					// Remove this one from the list of pending transactions
+					handler.removeActivatedQueue(objInfo);
+
+					try {
+
+						// 3. Execute transaction by sending the appropriate UAVTalk command
+						if (transInfo.objRequest) {
+							if (DEBUG) Log.d(TAG, "Sending object request " + transInfo.obj.getName());
+							utalk.sendObjectRequest(transInfo.obj, transInfo.allInstances);
+						} else {
+							if (DEBUG) Log.d(TAG, "Sending object " + transInfo.obj.getName());
+							utalk.sendObject(transInfo.obj, transInfo.acked, transInfo.allInstances);
+						}
+
+					} catch (IOException e) {
+						if (ERROR) Log.e(TAG, "Unable to send UAVTalk message");
+						e.printStackTrace();
+					}
+
+					// Post a timeout timer if a response is epxected
+					if (transPending)
+						handler.postDelayed(transactionTimeout, REQ_TIMEOUT_MS);
+				}
+			}
+		}
+	}
 
 
+	/**
+	 * Runnable posted to handle a timeout of a transaction.  Tracks the number of retry attempts
+	 * retries that many, and finally sends a transaction failed signal.
+	 */
+	final Runnable transactionTimeout = new Runnable() {
+		@Override
+		public void run() {
+			// Lock on the transaction
+			synchronized (transInfo) {
+
+				// Proceed only if there is a pending transaction
+				if (!transPending) {
+					if (WARN) Log.w(TAG,"Transaction completed but timeout still called.  Probable race condition");
+					return;
+				}
+
+				if (DEBUG) Log.d(TAG, "Telemetry: transaction timeout.");
+
+				// Check if more retries are pending
+				if (transInfo.retriesRemaining > 0) {
+					--transInfo.retriesRemaining;
+
+					// Repeat whatever is required for this transaction type
+					// (transInfo.objRequest) {
+					if (DEBUG) Log.d(TAG, "Sending object request");
+
+					try {
+						// Execute transaction by sending the appropriate UAVTalk command
+						if (transInfo.objRequest) {
+							if (DEBUG) Log.d(TAG, "Sending object request" + transInfo.obj.getName());
+							utalk.sendObjectRequest(transInfo.obj, transInfo.allInstances);
+						} else {
+							if (DEBUG) Log.d(TAG, "Sending object " + transInfo.obj.getName());
+							utalk.sendObject(transInfo.obj, transInfo.acked, transInfo.allInstances);
+						}
+					} catch (IOException e) {
+						if (ERROR) Log.e(TAG, "Unable to send UAVTalk message");
+						e.printStackTrace();
+					}
+
+					handler.postDelayed(transactionTimeout, REQ_TIMEOUT_MS);
+
+					++txRetries;
+				} else {
+					if (ERROR) Log.e(TAG, "Transaction failed for: " + transInfo.obj.getName());
+
+					// Terminate transaction. This triggers UAVTalk to send a transaction
+					// failed signal which will make the next queue entry be processed
+					// Note this is UAVTalk listener TransactionFailed function
+					// object specific transaction failed.
+					utalk.cancelPendingTransaction(transInfo.obj);
+					++txErrors;
+				}
+			}
+		}
+	};
+
+
+	/**
+	 * Called when a transaction is successfully completed (UAVTalk event) and maps that to
+	 * the appropriate object event as well as canceling the pending transaction and timeout
+	 */
+	private void transactionCompleted(UAVObject obj, boolean result) {
+
+		if (DEBUG) Log.d(TAG, "UAVTalk transactionCompleted");
+
+		// Check if there is a pending transaction and the objects match
+		synchronized(transInfo) {
+			if (transPending && transInfo.obj.getObjID() == obj.getObjID()) {
+				if (DEBUG) Log.d(TAG, "Telemetry: transaction completed for " + obj.getName());
+
+				// Cancel timeout and complete transaction
+				handler.removeCallbacks(transactionTimeout);
+				transPending = false;
+
+				//Send signal
+				obj.transactionCompleted(result);
+			} else {
+				if (ERROR) Log.e(TAG, "Error: received a transaction completed when did not expect it.");
+				transPending = false;
+			}
+		}
+	}
 
 }
+

androidgcs/src/org/openpilot/uavtalk/TelemetryMonitor.java

@@ -34,6 +34,8 @@ import java.util.Observer;
 import java.util.Timer;
 import java.util.TimerTask;
 
+import org.openpilot.androidgcs.telemetry.OPTelemetryService;
+
 import android.util.Log;
 
 public class TelemetryMonitor extends Observable {
@@ -60,6 +62,7 @@ public class TelemetryMonitor extends Observable {
 	private long lastUpdateTime;
 	private final List<UAVObject> queue;
 
+	private OPTelemetryService telemService;
 	private boolean connected = false;
 	private boolean objects_updated = false;
 
@@ -71,6 +74,11 @@ public class TelemetryMonitor extends Observable {
 		return objects_updated;
 	};
 
+	public TelemetryMonitor(UAVObjectManager objMngr, Telemetry tel, OPTelemetryService s) {
+		this(objMngr, tel);
+		telemService = s;
+	}
+
 	public TelemetryMonitor(UAVObjectManager objMngr, Telemetry tel) {
 		this.objMngr = objMngr;
 		this.tel = tel;
@@ -171,7 +179,9 @@ public class TelemetryMonitor extends Observable {
 	public synchronized void retrieveNextObject() throws IOException {
 		// If queue is empty return
 		if (queue.isEmpty()) {
-			if (DEBUG || true) Log.d(TAG, "All objects retrieved: Connected Successfully");
+			if (telemService != null)
+				telemService.toastMessage("Connected");
+			if (DEBUG) Log.d(TAG, "All objects retrieved: Connected Successfully");
 			objects_updated = true;
 			if (!HANDSHAKE_IS_CONNECTED) {
 				setChanged();

androidgcs/src/org/openpilot/uavtalk/UAVTalk.java

@@ -38,7 +38,7 @@ import android.util.Log;
 public class UAVTalk {
 
 	static final String TAG = "UAVTalk";
-	public static int LOGLEVEL = 1;
+	public static int LOGLEVEL = 0;
 	public static boolean VERBOSE = LOGLEVEL > 3;
 	public static boolean WARN = LOGLEVEL > 2;
 	public static boolean DEBUG = LOGLEVEL > 1;
@@ -227,6 +227,8 @@ public class UAVTalk {
 		//inStream.wait();
 		val = inStream.read();
 
+		if (VERBOSE) Log.v(TAG, "Read: " + val);
+
 		if (val == -1) {
 			return false;
 		}
@@ -357,7 +359,7 @@ public class UAVTalk {
 				rxCS = updateCRC(rxCS, rxbyte);
 
 				if ((rxbyte & TYPE_MASK) != TYPE_VER) {
-					Log.e(TAG, "Unknown UAVTalk type:" + rxbyte);
+					if (ERROR) Log.e(TAG, "Unknown UAVTalk type:" + rxbyte);
 					rxState = RxStateType.STATE_SYNC;
 					break;
 				}
@@ -684,14 +686,11 @@ public class UAVTalk {
 				// TODO Auto-generated catch block
 				e.printStackTrace();
 			}
-			// System.out.println("Unpacking new object");
 			if (DEBUG) Log.d(TAG, "Unpacking new object");
 			instobj.unpack(data);
 			return instobj;
 		} else {
 			// Unpack data into object instance
-			// System.out.println("Unpacking existing object: " +
-			// data.position() + " / " + data.capacity() );
 			if (DEBUG) Log.d(TAG, "Unpacking existing object: " + obj.getName());
 			obj.unpack(data);
 			return obj;

androidgcs/tests/org/openpilot/uavtalk/TelemetryMonitorTest.java

@@ -1,6 +1,6 @@
 package org.openpilot.uavtalk;
 
-import static org.junit.Assert.*;
+import static org.junit.Assert.fail;
 
 import java.io.IOException;
 import java.net.InetAddress;
@@ -8,11 +8,12 @@ import java.net.Socket;
 
 import org.junit.Test;
 import org.openpilot.uavtalk.uavobjects.UAVObjectsInitialize;
-import org.openpilot.uavtalk.UAVTalk;
+
+import android.os.Looper;
 
 
 public class TelemetryMonitorTest {
-	
+
 	static UAVObjectManager objMngr;
 	static UAVTalk talk;
 	static final String IP_ADDRDESS = new String("127.0.0.1");
@@ -32,7 +33,7 @@ public class TelemetryMonitorTest {
 			e.printStackTrace();
 			fail("Couldn't connect to test platform");
 		}
-		
+
 		try {
 			talk = new UAVTalk(connection.getInputStream(), connection.getOutputStream(), objMngr);
 		} catch (IOException e) {
@@ -40,16 +41,17 @@ public class TelemetryMonitorTest {
 			e.printStackTrace();
 			fail("Couldn't construct UAVTalk object");
 		}
-		
+
 		Thread inputStream = talk.getInputProcessThread();
 		inputStream.start();
-		
+
-		Telemetry tel = new Telemetry(talk, objMngr);
+		Looper.prepare();
+		Telemetry tel = new Telemetry(talk, objMngr, Looper.myLooper());
 		@SuppressWarnings("unused")
 		TelemetryMonitor mon = new TelemetryMonitor(objMngr,tel);
-		
+
 		Thread.sleep(10000);
-		
+
 		System.out.println("Done");
 	}