diff --git a/androidgcs/AndroidManifest.xml b/androidgcs/AndroidManifest.xml index 1fbb16f55..3dc9bc3e3 100644 --- a/androidgcs/AndroidManifest.xml +++ b/androidgcs/AndroidManifest.xml @@ -10,17 +10,26 @@ - + + + + + + + + + + diff --git a/androidgcs/res/values/arrays.xml b/androidgcs/res/values/arrays.xml index 18ee52298..4461c31d8 100644 --- a/androidgcs/res/values/arrays.xml +++ b/androidgcs/res/values/arrays.xml @@ -5,11 +5,13 @@ Fake Bluetooth Network + HID 0 1 2 3 - + 4 + \ No newline at end of file diff --git a/androidgcs/res/xml/device_filter.xml b/androidgcs/res/xml/device_filter.xml new file mode 100644 index 000000000..8347ef270 --- /dev/null +++ b/androidgcs/res/xml/device_filter.xml @@ -0,0 +1,9 @@ + + + + + + + + + \ No newline at end of file diff --git a/androidgcs/src/org/openpilot/androidgcs/Controller.java b/androidgcs/src/org/openpilot/androidgcs/Controller.java index 62b7d0c31..39a86639c 100644 --- a/androidgcs/src/org/openpilot/androidgcs/Controller.java +++ b/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(); } }; diff --git a/androidgcs/src/org/openpilot/androidgcs/telemetry/BluetoothUAVTalk.java b/androidgcs/src/org/openpilot/androidgcs/telemetry/BluetoothUAVTalk.java index db3d44134..5c1028cb3 100644 --- a/androidgcs/src/org/openpilot/androidgcs/telemetry/BluetoothUAVTalk.java +++ b/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 boolean WARN = LOGLEVEL > 1; - public static boolean DEBUG = LOGLEVEL > 0; + public static final int LOGLEVEL = 4; + public static final boolean DEBUG = LOGLEVEL > 2; + 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() { - return connected; + @Override + 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 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) { - Log.d(TAG, "Opening connection to " + device.getName()); + private boolean openTelemetryBluetooth() { + 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"); - // TODO Auto-generated catch block - //e.printStackTrace(); + try { + socket.close(); + } 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; } diff --git a/androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java b/androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java new file mode 100644 index 000000000..ab170ff48 --- /dev/null +++ b/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 deviceList = usbManager.getDeviceList(); + if (DEBUG) Log.d(TAG, "Found " + deviceList.size() + " devices"); + Iterator 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; + } + } + } +} \ No newline at end of file diff --git a/androidgcs/src/org/openpilot/androidgcs/telemetry/OPTelemetryService.java b/androidgcs/src/org/openpilot/androidgcs/telemetry/OPTelemetryService.java index 2e518b7e0..69ab6e0d8 100644 --- a/androidgcs/src/org/openpilot/androidgcs/telemetry/OPTelemetryService.java +++ b/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 boolean WARN = LOGLEVEL > 1; - public static boolean DEBUG = LOGLEVEL > 0; + public static int LOGLEVEL = 2; + public static boolean DEBUG = LOGLEVEL > 1; + 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"); - } - - }; + };*/ } diff --git a/androidgcs/src/org/openpilot/androidgcs/telemetry/TcpUAVTalk.java b/androidgcs/src/org/openpilot/androidgcs/telemetry/TcpUAVTalk.java index 439069d29..b51ee8b13 100644 --- a/androidgcs/src/org/openpilot/androidgcs/telemetry/TcpUAVTalk.java +++ b/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) { - SharedPreferences prefs = PreferenceManager.getDefaultSharedPreferences(caller); + public TcpUAVTalk(OPTelemetryService 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"); - // TODO Auto-generated catch block - //e.printStackTrace(); + try { + socket.close(); + } 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; + } + } + } diff --git a/androidgcs/src/org/openpilot/androidgcs/telemetry/TelemetryTask.java b/androidgcs/src/org/openpilot/androidgcs/telemetry/TelemetryTask.java new file mode 100644 index 000000000..c9a44d6e1 --- /dev/null +++ b/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; + } + }; + } + +} diff --git a/androidgcs/src/org/openpilot/uavtalk/Telemetry.java b/androidgcs/src/org/openpilot/uavtalk/Telemetry.java index 2e0b57f41..6fcbb2491 100644 --- a/androidgcs/src/org/openpilot/uavtalk/Telemetry.java +++ b/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 boolean WARN = LOGLEVEL > 2; - public static boolean DEBUG = LOGLEVEL > 1; + public static int LOGLEVEL = 0; + public static boolean DEBUG = LOGLEVEL > 2; + 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 { - UAVObject obj; - int updatePeriodMs; /** Update period in ms or 0 if no periodic updates are needed */ - int timeToNextUpdateMs; /** Time delay to the next update */ - }; + 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 ObjectQueueInfo { - UAVObject obj; - int event; - boolean allInstances; + class ObjectTimeInfo { + UAVObject obj; + int updatePeriodMs; + /** 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 { - ObjectQueueInfo o = (ObjectQueueInfo) e; - return o.obj.getObjID() == obj.getObjID() && o.event == event && o.allInstances == allInstances; - } catch (Exception err) { + try { + ObjectQueueInfo o = (ObjectQueueInfo) e; + return o.obj.getObjID() == obj.getObjID() && o.event == event + && 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 > objs = objMngr.getObjects(); - ListIterator> 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> objs = objMngr.getObjects(); + ListIterator> 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 - utalk.setOnTransactionCompletedListener( - utalk.new OnTransactionCompletedListener() { + // Listen to transaction completions from uavtalk + utalk.setOnTransactionCompletedListener(utalk.new OnTransactionCompletedListener() { @Override void TransactionSucceeded(UAVObject data) { - try { - transactionCompleted(data, true); - } catch (IOException e) { - // Disconnect when stream fails - utalk.setOnTransactionCompletedListener(null); - } - } + transactionCompleted(data, true); + } + @Override void TransactionFailed(UAVObject data) { - try { - if (DEBUG) Log.d(TAG, "TransactionFailed(" + data.getName() + ")"); + if (DEBUG) + Log.d(TAG, "TransactionFailed(" + data.getName() + ")"); - 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(); - } + transactionCompleted(data, false); } - }; - transTimer.schedule(transTimerTask, periodMs, periodMs); - } - 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() { + }); + + // 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 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) - */ - private synchronized void registerObject(UAVObject obj) - { - // Setup object for periodic updates - addObject(obj); + /** + * Register a new object for periodic updates (if enabled) + */ + private synchronized void registerObject(UAVObject obj) { + // Setup object for periodic updates + addObject(obj); - // Setup object for telemetry updates - updateObject(obj); - } + // Setup object for telemetry updates + updateObject(obj); + } - /** - * Add an object in the list used for periodic updates - */ - private synchronized void addObject(UAVObject obj) - { - // Check if object type is already in the list - ListIterator li = objList.listIterator(); - while(li.hasNext()) { - ObjectTimeInfo n = li.next(); - if( n.obj.getObjID() == obj.getObjID() ) - { - // Object type (not instance!) is already in the list, do nothing - return; - } - } + /** + * Add an object in the list used for periodic updates + */ + private synchronized void addObject(UAVObject obj) { + // Check if object type is already in the list + ListIterator li = objList.listIterator(); + while (li.hasNext()) { + ObjectTimeInfo n = li.next(); + if (n.obj.getObjID() == obj.getObjID()) { + // Object type (not instance!) is already in the list, do + // nothing + return; + } + } - // If this point is reached, then the object type is new, let's add it - ObjectTimeInfo timeInfo = new ObjectTimeInfo(); - timeInfo.obj = obj; - timeInfo.timeToNextUpdateMs = 0; - timeInfo.updatePeriodMs = 0; - objList.add(timeInfo); - } + // If this point is reached, then the object type is new, let's add it + ObjectTimeInfo timeInfo = new ObjectTimeInfo(); + timeInfo.obj = obj; + timeInfo.timeToNextUpdateMs = 0; + timeInfo.updatePeriodMs = 0; + objList.add(timeInfo); + } - /** - * Update the object's timers - */ - private synchronized void setUpdatePeriod(UAVObject obj, int periodMs) - { - // Find object type (not instance!) and update its period - ListIterator li = objList.listIterator(); - while(li.hasNext()) { - ObjectTimeInfo n = li.next(); - if ( n.obj.getObjID() == obj.getObjID() ) - { - n.updatePeriodMs = periodMs; - n.timeToNextUpdateMs = (int) (periodMs * (new java.util.Random()).nextDouble()); // avoid bunching of updates - } - } - } + /** + * Update the object's timers + */ + private synchronized void setUpdatePeriod(UAVObject obj, int periodMs) { + // Find object type (not instance!) and update its period + ListIterator li = objList.listIterator(); + while (li.hasNext()) { + ObjectTimeInfo n = li.next(); + if (n.obj.getObjID() == obj.getObjID()) { + n.updatePeriodMs = periodMs; + 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 { - enqueueObjectUpdates((UAVObject) data, EV_UNPACKED, false, true); - } catch (IOException e) { - // TODO Auto-generated catch block - e.printStackTrace(); - } - } + handler.unpacked((UAVObject) data); + } }; final Observer updatedAutoObserver = new Observer() { @Override public void update(Observable observable, Object data) { - try { - enqueueObjectUpdates((UAVObject) data, EV_UPDATED, false, true); - } catch (IOException e) { - // TODO Auto-generated catch block - e.printStackTrace(); - } - } + handler.updatedAuto((UAVObject) data); + } }; final Observer updatedManualObserver = new Observer() { @Override public void update(Observable observable, Object data) { - try { - enqueueObjectUpdates((UAVObject) data, EV_UPDATED_MANUAL, false, true); - } catch (IOException e) { - // TODO Auto-generated catch block - e.printStackTrace(); - } - } + handler.updatedManual((UAVObject) data); + } }; final Observer updatedRequestedObserver = new Observer() { @Override public void update(Observable observable, Object data) { - try { - enqueueObjectUpdates((UAVObject) data, EV_UPDATE_REQ, false, true); - } catch (IOException e) { - // TODO Auto-generated catch block - e.printStackTrace(); - } - } + handler.updateRequested((UAVObject) data); + } }; - /** - * Connect to all instances of an object depending on the event mask specified - */ - private synchronized void connectToObjectInstances(UAVObject obj, int eventMask) - { - List objs = objMngr.getObjectInstances(obj.getObjID()); - ListIterator li = objs.listIterator(); - while(li.hasNext()) - { - obj = li.next(); + /** + * Connect to all instances of an object depending on the event mask + * specified + */ + private synchronized void connectToObjectInstances(UAVObject obj, + int eventMask) { + List objs = objMngr.getObjectInstances(obj.getObjID()); + ListIterator li = objs.listIterator(); + while (li.hasNext()) { + obj = li.next(); - // Disconnect all previous observers from telemetry. This is imortant as this can - // be called multiple times - obj.removeUnpackedObserver(unpackedObserver); - obj.removeUpdatedAutoObserver(updatedAutoObserver); - obj.removeUpdatedManualObserver(updatedManualObserver); - obj.removeUpdateRequestedObserver(updatedRequestedObserver); + // Disconnect all previous observers from telemetry. This is + // imortant as this can + // be called multiple times + obj.removeUnpackedObserver(unpackedObserver); + obj.removeUpdatedAutoObserver(updatedAutoObserver); + obj.removeUpdatedManualObserver(updatedManualObserver); + obj.removeUpdateRequestedObserver(updatedRequestedObserver); - // Connect only the selected events - if ( (eventMask&EV_UNPACKED) != 0) - obj.addUnpackedObserver(unpackedObserver); - if ( (eventMask&EV_UPDATED) != 0) - obj.addUpdatedAutoObserver(updatedAutoObserver); - if ( (eventMask&EV_UPDATED_MANUAL) != 0) - obj.addUpdatedManualObserver(updatedManualObserver); - if ( (eventMask&EV_UPDATE_REQ) != 0) - obj.addUpdateRequestedObserver(updatedRequestedObserver); - } - } + // Connect only the selected events + if ((eventMask & EV_UNPACKED) != 0) + obj.addUnpackedObserver(unpackedObserver); + if ((eventMask & EV_UPDATED) != 0) + obj.addUpdatedAutoObserver(updatedAutoObserver); + if ((eventMask & EV_UPDATED_MANUAL) != 0) + obj.addUpdatedManualObserver(updatedManualObserver); + if ((eventMask & EV_UPDATE_REQ) != 0) + obj.addUpdateRequestedObserver(updatedRequestedObserver); + } + } - /** - * Update an object based on its metadata properties - */ - private synchronized void updateObject(UAVObject obj) - { - // Get metadata - UAVObject.Metadata metadata = obj.getMetadata(); + /** + * Update an object based on its metadata properties + */ + private void updateObject(UAVObject obj) { + // Get metadata + UAVObject.Metadata metadata = obj.getMetadata(); - // Setup object depending on update mode - int eventMask; - if ( metadata.GetGcsTelemetryUpdateMode() == UAVObject.UpdateMode.UPDATEMODE_PERIODIC ) - { - // Set update period - setUpdatePeriod(obj, metadata.gcsTelemetryUpdatePeriod); - // Connect signals for all instances - eventMask = EV_UPDATED_MANUAL | EV_UPDATE_REQ; - if(obj.isMetadata()) - eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events) + // Setup object depending on update mode + int eventMask; + if (metadata.GetGcsTelemetryUpdateMode() == UAVObject.UpdateMode.UPDATEMODE_PERIODIC) { + // Set update period + setUpdatePeriod(obj, metadata.gcsTelemetryUpdatePeriod); + // Connect signals for all instances + 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); - } - else if ( metadata.GetGcsTelemetryUpdateMode() == UAVObject.UpdateMode.UPDATEMODE_ONCHANGE ) - { - // Set update period - setUpdatePeriod(obj, 0); - // Connect signals for all instances - eventMask = EV_UPDATED | 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); + } else if (metadata.GetGcsTelemetryUpdateMode() == UAVObject.UpdateMode.UPDATEMODE_ONCHANGE) { + // Set update period + setUpdatePeriod(obj, 0); + // Connect signals for all instances + eventMask = EV_UPDATED | 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); - } - else if ( metadata.GetGcsTelemetryUpdateMode() == UAVObject.UpdateMode.UPDATEMODE_THROTTLED ) - { - // TODO - } - else if ( metadata.GetGcsTelemetryUpdateMode() == UAVObject.UpdateMode.UPDATEMODE_MANUAL ) - { - // Set update period - setUpdatePeriod(obj, 0); - // Connect signals for all instances - 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); + } else if (metadata.GetGcsTelemetryUpdateMode() == UAVObject.UpdateMode.UPDATEMODE_THROTTLED) { + // TODO + } else if (metadata.GetGcsTelemetryUpdateMode() == UAVObject.UpdateMode.UPDATEMODE_MANUAL) { + // Set update period + setUpdatePeriod(obj, 0); + // Connect signals for all instances + 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) - * @throws IOException - */ - private synchronized void transactionCompleted(UAVObject obj, boolean result) 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; + /** + * Check is any objects are pending for periodic updates TODO: Clean-up + * + * @throws IOException + */ + private void processPeriodicUpdates() throws IOException { - //Send signal - obj.transactionCompleted(result); - // Process new object updates from queue - processObjectQueue(); - } else - { - if (ERROR) Log.e(TAG,"Error: received a transaction completed when did not expect it."); - transPending = false; - } - } + if (DEBUG) + Log.d(TAG, "processPeriodicUpdates()"); + // Stop timer - /** - * 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()); + updateTimer.cancel(); - // 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 and not the - // object specific transaction failed. - utalk.cancelPendingTransaction(transInfo.obj); - ++txErrors; - } - } - } + // 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 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(); - /** - * Start an object transaction with UAVTalk, all information is stored in transInfo - * @throws IOException - */ - private synchronized void processObjectTransaction() throws IOException - { - if (transPending) - { - if (DEBUG) Log.d(TAG, "Process Object transaction for " + transInfo.obj.getName()); - // Initiate transaction - if (transInfo.objRequest) - { - 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"); - } - } + if (DEBUG) Log.d(TAG, "Manual update: " + objinfo.obj.getName()); + handler.updatedManual(objinfo.obj); + // 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; + } + } + } - /** - * Enqueue the event received from an object. This is the main method that handles all the callbacks - * from UAVObjects (due to updates, or update requests) - */ - 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); - } - } - } + // Check if delay for the next update is too short + if (minDelay < MIN_UPDATE_PERIOD_MS) { + minDelay = MIN_UPDATE_PERIOD_MS; + } - // If there is no transaction in progress then process event - if (!transPending) - { - processObjectQueue(); - } - } + // Done + timeToNextUpdateMs = minDelay; - /** - * Process events from the object queue - * @throws IOException - */ - private synchronized void processObjectQueue() throws IOException - { - if (DEBUG) Log.d(TAG, "Process object queue - Depth " + objQueue.size() + " priority " + objPriorityQueue.size()); + // Restart timer + updateTimerSetPeriod(timeToNextUpdateMs); + } - // Don nothing if a transaction is already in progress (should not happen) - if (transPending) - { - if (WARN) Log.e(TAG,"Dequeue while a transaction pending"); - return; - } + public TelemetryStats getStats() { + // Get UAVTalk stats + UAVTalk.ComStats utalkStats = utalk.getStats(); - // Get object information from queue (first the priority and then the regular queue) - ObjectQueueInfo objInfo; - if ( !objPriorityQueue.isEmpty() ) - { - objInfo = objPriorityQueue.remove(); - } - else if ( !objQueue.isEmpty() ) - { - objInfo = objQueue.remove(); - } - else - { - return; - } + // 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; - // 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 ) - { - 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; - } - } + // Done + return stats; + } - // Setup transaction (skip if unpack event) - if ( objInfo.event != EV_UNPACKED ) - { - UAVObject.Metadata metadata = objInfo.obj.getMetadata(); - 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()); - } + public void resetStats() { + utalk.resetStats(); + txErrors = 0; + txRetries = 0; + } - // If this is a metaobject then make necessary telemetry updates - if (objInfo.obj.isMetadata()) - { - UAVMetaObject metaobj = (UAVMetaObject) objInfo.obj; - updateObject( metaobj.getParentObject() ); - } + private void newObject(UAVObject obj) { + registerObject(obj); + } - // The fact we received an unpacked event does not mean that - // we do not have additional objects still in the queue, - // so we have to reschedule queue processing to make sure they are not - // stuck: - if ( objInfo.event == EV_UNPACKED && !transPending) - processObjectQueue(); + private synchronized void newInstance(UAVObject obj) { + registerObject(obj); + } - } - - /** - * Check is any objects are pending for periodic updates - * TODO: Clean-up - * @throws IOException - */ - private synchronized void processPeriodicUpdates() throws IOException - { - - 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 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; - } + /** + * Stop all the telemetry timers + */ + public void stopTelemetry() { + if (updateTimerTask != null) + updateTimerTask.cancel(); + updateTimerTask = null; + if (updateTimer != null) + updateTimer.cancel(); + updateTimer = null; + } /** * Private variables */ - private final UAVObjectManager objMngr; - private final UAVTalk utalk; - private UAVObject gcsStatsObj; - private final List objList = new ArrayList(); - private final Queue objQueue = new LinkedList(); - private final Queue objPriorityQueue = new LinkedList(); - private final ObjectTransactionInfo transInfo = new ObjectTransactionInfo(); - private boolean transPending; + private final UAVObjectManager objMngr; + private final UAVTalk utalk; + private UAVObject gcsStatsObj; + private final List objList = new ArrayList(); + private ObjectTransactionInfo transInfo = new ObjectTransactionInfo(); + private boolean transPending; - private Timer updateTimer; - private TimerTask updateTimerTask; - private Timer transTimer; - private TimerTask transTimerTask; + private Timer updateTimer; + private TimerTask updateTimerTask; - private int timeToNextUpdateMs; - private int txErrors; - private int txRetries; + private int timeToNextUpdateMs; + private int txErrors; + private int txRetries; - /** - * Private constants - */ - private static final int REQ_TIMEOUT_MS = 250; - private static final int MAX_RETRIES = 2; - private static final int MAX_UPDATE_PERIOD_MS = 1000; - private static final int MIN_UPDATE_PERIOD_MS = 1; - private static final int MAX_QUEUE_SIZE = 20; + /** + * Private constants + */ + private static final int REQ_TIMEOUT_MS = 250; + private static final int MAX_RETRIES = 2; + private static final int MAX_UPDATE_PERIOD_MS = 1000; + private static final int MIN_UPDATE_PERIOD_MS = 1; + + 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 objQueue = new ConcurrentLinkedQueue(); + + // ! 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; + } + } + } } + diff --git a/androidgcs/src/org/openpilot/uavtalk/TelemetryMonitor.java b/androidgcs/src/org/openpilot/uavtalk/TelemetryMonitor.java index a1e341197..121a5ebcc 100644 --- a/androidgcs/src/org/openpilot/uavtalk/TelemetryMonitor.java +++ b/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 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(); diff --git a/androidgcs/src/org/openpilot/uavtalk/UAVTalk.java b/androidgcs/src/org/openpilot/uavtalk/UAVTalk.java index 1c4d25db6..c996da845 100644 --- a/androidgcs/src/org/openpilot/uavtalk/UAVTalk.java +++ b/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; diff --git a/androidgcs/tests/org/openpilot/uavtalk/TelemetryMonitorTest.java b/androidgcs/tests/org/openpilot/uavtalk/TelemetryMonitorTest.java index 0b0cc1d73..6ddcfe03e 100644 --- a/androidgcs/tests/org/openpilot/uavtalk/TelemetryMonitorTest.java +++ b/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"); }