From dc90210cfabb958452b69feaac9947fca94698ea Mon Sep 17 00:00:00 2001 From: James Cotton Date: Fri, 10 Aug 2012 23:19:47 -0500 Subject: [PATCH 01/32] AndroidGCS: Add support for HID to AndroidGCS --- androidgcs/AndroidManifest.xml | 11 +- androidgcs/res/values/arrays.xml | 4 +- androidgcs/res/xml/device_filter.xml | 9 + .../androidgcs/telemetry/HidUAVTalk.java | 622 ++++++++++++++++++ .../telemetry/OPTelemetryService.java | 124 +++- .../src/org/openpilot/uavtalk/UAVTalk.java | 2 +- 6 files changed, 766 insertions(+), 6 deletions(-) create mode 100644 androidgcs/res/xml/device_filter.xml create mode 100644 androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java 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/telemetry/HidUAVTalk.java b/androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java new file mode 100644 index 000000000..879c02ba2 --- /dev/null +++ b/androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java @@ -0,0 +1,622 @@ +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 org.openpilot.uavtalk.UAVObjectManager; +import org.openpilot.uavtalk.UAVTalk; + +import android.app.PendingIntent; +import android.app.Service; +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 { + + private static final String TAG = HidUAVTalk.class.getSimpleName(); + public static int LOGLEVEL = 2; + public static boolean DEBUG = LOGLEVEL > 1; + public static boolean WARN = LOGLEVEL > 0; + + + Service hostDisplayActivity; + + private boolean connected; + + private UAVTalk uavTalk; + + private UAVObjectManager objMngr; + + //! USB constants + 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; + + public HidUAVTalk(Service service) { + hostDisplayActivity = service; + } + + 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 + */ + public boolean openTelemetryHid(UAVObjectManager objMngr) { + uavTalk = new UAVTalk(inStream, outStream, objMngr); + this.objMngr = objMngr; + return true; + } + + public void disconnect() { + CleanUpAndClose(); + //hostDisplayActivity.unregisterReceiver(usbReceiver); + //hostDisplayActivity.unregisterReceiver(usbPermissionReceiver); + inStream.stop(); + outStream.stop(); + } + + public boolean connect(UAVObjectManager objMngr) { + if (DEBUG) Log.d(TAG, "connect()"); + + // Register to get permission requested dialog + usbManager = (UsbManager) hostDisplayActivity.getSystemService(Context.USB_SERVICE); + permissionIntent = PendingIntent.getBroadcast(hostDisplayActivity, 0, new Intent(ACTION_USB_PERMISSION), 0); + permissionFilter = new IntentFilter(ACTION_USB_PERMISSION); + hostDisplayActivity.registerReceiver(usbPermissionReceiver, permissionFilter); + + // Register to get notified on device attached + /*deviceAttachedFilter = new IntentFilter(); + deviceAttachedFilter.addAction(UsbManager.ACTION_USB_DEVICE_DETACHED); + deviceAttachedFilter.addAction(UsbManager.ACTION_USB_DEVICE_ATTACHED); + hostDisplayActivity.registerReceiver(usbReceiver, deviceAttachedFilter);*/ + + 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); + //ConnectToDeviceInterface(dev); + } + + if (DEBUG) Log.d(TAG, "Registered the deviceAttachedFilter"); + + try { + Thread.sleep(1000); + } catch (InterruptedException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } + + if( !openTelemetryHid(objMngr) ) + return false; + + return connected; + + } + + public HidUAVTalk(OPTelemetryService opTelemetryService) { + this.hostDisplayActivity = opTelemetryService; + } + + private static final String ACTION_USB_PERMISSION = "com.access.device.USB_PERMISSION"; + + UsbDevice currentDevice; + + /* + * 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"); + } + sendEnabledMessage(); + // TODO: Create a listener to receive messages from the host + + } + } + 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)) + { + // call your method that cleans up and closes communication with the device + CleanUpAndClose(); + } + } + } + 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); + } + } + } + + private void sendEnabledMessage() { + // TODO Auto-generated method stub + + }; + }; + private UsbEndpoint usbEndpointRead; + + private UsbEndpoint usbEndpointWrite; + + private UsbManager usbManager; + + private PendingIntent permissionIntent; + + private UsbDeviceConnection connectionRead; + + private UsbDeviceConnection connectionWrite; + + private IntentFilter deviceAttachedFilter; + + private IntentFilter permissionFilter; + + protected void sendEnabledMessage() { + // TODO Auto-generated method stub + + } + + protected void CleanUpAndClose() { + if (UsingSingleInterface) { + if(connectionRead != null && usbInterfaceRead != null) + connectionRead.releaseInterface(usbInterfaceRead); + usbInterfaceRead = null; + } + else { + if(connectionRead != null && usbInterfaceRead != null) + connectionRead.releaseInterface(usbInterfaceRead); + if(connectionWrite != null && usbInterfaceWrite != null) + connectionWrite.releaseInterface(usbInterfaceWrite); + usbInterfaceWrite = null; + usbInterfaceRead = 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; + } + + private UsbInterface usbInterfaceRead = null; + private UsbInterface usbInterfaceWrite = null; + private final boolean UsingSingleInterface = true; + + private UsbDevice connectedDevice; + + 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; + + + if (UsingSingleInterface) + { + // Using the same interface for reading and writing + usbInterfaceRead = connectDevice.getInterface(0x2); + usbInterfaceWrite = usbInterfaceRead; + if (usbInterfaceRead.getEndpointCount() == 2) + { + ep1 = usbInterfaceRead.getEndpoint(0); + ep2 = usbInterfaceRead.getEndpoint(1); + } + } + else // if (!UsingSingleInterface) + { + usbInterfaceRead = connectDevice.getInterface(0x01); + usbInterfaceWrite = connectDevice.getInterface(0x02); + if ((usbInterfaceRead.getEndpointCount() == 1) && (usbInterfaceWrite.getEndpointCount() == 1)) + { + ep1 = usbInterfaceRead.getEndpoint(0); + ep2 = usbInterfaceWrite.getEndpoint(0); + } + } + + + if ((ep1 == null) || (ep2 == null)) + { + if (DEBUG) Log.d(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 (DEBUG) Log.d(TAG, "Endpoints wrong way around"); + return false; + } + connectionRead = usbManager.openDevice(connectDevice); + connectionRead.claimInterface(usbInterfaceRead, true); + + + if (UsingSingleInterface) + { + connectionWrite = connectionRead; + } + else // if (!UsingSingleInterface) + { + connectionWrite = usbManager.openDevice(connectDevice); + connectionWrite.claimInterface(usbInterfaceWrite, true); + } + + connectedDevice = connectDevice; + connected = true; + if (DEBUG) Log.d(TAG, "Opened endpoints"); + + return true; + } + + private class TalkInputStream extends InputStream { + ByteBuffer data = null; + boolean stopped = false; + private static final int SIZE = 1024; + int readPosition = 0; + TalkInputStream() { + data = ByteBuffer.allocate(SIZE); + data.limit(SIZE); + data.clear(); + } + + @Override + public int read() throws IOException { + while(!stopped) { + synchronized(data) { + if(data.capacity() > readPosition) + return data.get(readPosition++); + else + try { + data.wait(50); + } catch (InterruptedException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } + } + } + throw new IOException(); + } + + void compress() { + if (readPosition > (data.capacity() * 3.0 / 4.0)) { + synchronized(data) { + data.position(readPosition); + data.compact(); + readPosition = 0; + if (DEBUG) Log.d(TAG, "Compact() Capacity: " + data.capacity() + " Position: " + data.position() + " Available: " + data.remaining() + " Limit: " + data.limit()); + } + } + } + + public void stop() { + stopped = true; + } + + public void put(byte b) { + synchronized(data) { + if(data.hasRemaining()) { + data.compact(); + data.put(b); + data.notify(); + } + } + } + + public void write(byte[] b) { + synchronized(data) { + int available = data.remaining(); + if(available >= b.length) { + if (DEBUG) Log.d(TAG, "Size: " + b.length + " Capacity: " + data.capacity() + " Position: " + data.position() + " Available: " + available + " Limit: " + data.limit()); + //data.compact(); + data.put(b); + data.notify(); + } + } + } + }; + private final TalkInputStream inStream = new TalkInputStream(); + + /** + * Gets a report from HID, extract the meaningful data and push + * it to the input stream + */ + public int readData() { + int bufferDataLength = usbEndpointRead.getMaxPacketSize(); + ByteBuffer buffer = ByteBuffer.allocate(bufferDataLength + 1); + UsbRequest request = new UsbRequest(); + request.initialize(connectionRead, usbEndpointRead); + + + // queue a request on the interrupt endpoint + if(!request.queue(buffer, bufferDataLength)) { + if (DEBUG) Log.d(TAG, "Failed to queue request"); + return 0; + } + + if (DEBUG) Log.d(TAG, "Request queued"); + + int dataSize; + // wait for status event + if (connectionRead.requestWait() == request) { + // Packet format: + // 0: Report ID (1) + // 1: Number of valid bytes + // 2:63: Data + + dataSize = buffer.get(1); // Data size + //Assert.assertEquals(1, buffer.get()); // Report ID + //Assert.assertTrue(dataSize < buffer.capacity()); + + if (buffer.get(0) != 1 || buffer.get(1) < 0 || buffer.get(2) > (buffer.capacity() - 2)) { + if (DEBUG) Log.d(TAG, "Badly formatted HID packet"); + } else { + byte[] dst = new byte[dataSize]; + buffer.position(2); + buffer.get(dst, 0, dataSize); + inStream.write(dst); + if (DEBUG) Log.d(TAG, "Got read: " + dataSize + " bytes"); + } + } else + return 0; + + return dataSize; + } + + private class TalkOutputStream extends OutputStream { + ByteBuffer data = ByteBuffer.allocate(128); + boolean stopped = false; + + public int read() throws IOException { + if (stopped) + while(!stopped) { + synchronized(data) { + if(data.hasRemaining()) + return data.get(); + else + try { + data.wait(); + } catch (InterruptedException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } + } + throw new IOException(); + } + return 0; + } + + public void stop() { + stopped = true; + } + + @Override + public void write(int oneByte) throws IOException { + if (stopped) + throw new IOException(); + synchronized(data) { + data.put((byte) oneByte); + data.notify(); + } + } + + @Override + public void write(byte[] b) throws IOException { + if (stopped) + throw new IOException(); + + synchronized(data) { + //data.put(b); + data.notify(); + } + } + }; + private final TalkOutputStream outStream = new TalkOutputStream(); + + boolean WriteToDevice(ByteBuffer DataToSend) { + + //The report must be formatted correctly for the device being connected to. On some devices, this requires that a specific value must be the first byte in the report. This can be followed by the length of the data in the report. This format is determined by the device, and isn't specified here. + + int bufferDataLength = usbEndpointWrite.getMaxPacketSize(); + ByteBuffer buffer = ByteBuffer.allocate(bufferDataLength + 1); + UsbRequest request = new UsbRequest(); + + buffer.put(DataToSend); + + request.initialize(connectionWrite, usbEndpointWrite); + request.queue(buffer, bufferDataLength); + try + { + if (request.equals(connectionWrite.requestWait())) + { + return true; + } + } + catch (Exception ex) + { + // An exception has occured + return false; + } + return false; + } + + + //Reading from the Device - As USB devices work with reports of a specific length. The data received from the device will always be the size specified by the report length. Even if there isn't enough data to fill the report. Some devices require the controlTransfer method for reading data. I don't cover this command in this blog. + void ReadFromDevice() { + //If you are expecting unsolicited data from the device, then a read thread should be started so that the data can be processed as soon as it arrives. + + int bufferDataLength = usbEndpointRead.getMaxPacketSize(); + ByteBuffer buffer = ByteBuffer.allocate(bufferDataLength + 1); + UsbRequest requestQueued = null; + UsbRequest request = new UsbRequest(); + request.initialize(connectionRead, usbEndpointRead); + + try + { + while (!getStopping()) + { + request.queue(buffer, bufferDataLength); + requestQueued = connectionRead.requestWait(); + if (request.equals(requestQueued)) + { + byte[] byteBuffer = new byte[bufferDataLength + 1]; + buffer.get(byteBuffer, 0, bufferDataLength); + + // Handle data received + + buffer.clear(); + } + else + { + Thread.sleep(20); + } + } + } + catch (Exception ex) + { + // An exception has occured + } + try + { + request.cancel(); + request.close(); + } + catch (Exception ex) + { + // An exception has occured + } + } + + private boolean getStopping() { + // TODO Auto-generated method stub + return false; + } +} diff --git a/androidgcs/src/org/openpilot/androidgcs/telemetry/OPTelemetryService.java b/androidgcs/src/org/openpilot/androidgcs/telemetry/OPTelemetryService.java index 2e518b7e0..7406909da 100644 --- a/androidgcs/src/org/openpilot/androidgcs/telemetry/OPTelemetryService.java +++ b/androidgcs/src/org/openpilot/androidgcs/telemetry/OPTelemetryService.java @@ -56,9 +56,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"; @@ -132,6 +132,10 @@ public class OPTelemetryService extends Service { Toast.makeText(getApplicationContext(), "Attempting TCP connection", Toast.LENGTH_SHORT).show(); activeTelem = new TcpTelemetryThread(); break; + case 4: + Toast.makeText(getApplicationContext(), "Attempting USB HID connection", Toast.LENGTH_SHORT).show(); + activeTelem = new HidTelemetryThread(); + break; default: throw new Error("Unsupported"); } @@ -482,4 +486,118 @@ public class OPTelemetryService extends Service { } }; + + private class HidTelemetryThread extends Thread implements TelemTask { + + private final UAVObjectManager objMngr; + private UAVTalk uavTalk; + private Telemetry tel; + private TelemetryMonitor mon; + + @Override + public UAVObjectManager getObjectManager() { return objMngr; }; + + HidTelemetryThread() { + objMngr = new UAVObjectManager(); + UAVObjectsInitialize.register(objMngr); + } + + @Override + public void run() { + if (DEBUG) Log.d(TAG, "HID Telemetry Thread started"); + + Looper.prepare(); + + final HidUAVTalk hid = new HidUAVTalk(OPTelemetryService.this); + hid.connect(objMngr); + + try { + Thread.sleep(200); + } catch (InterruptedException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } + + if( !hid.getConnected() ) { + toastMessage("HID connection failed"); + return; + } + + hid.readData(); + hid.readData(); + hid.readData(); + hid.readData(); + + uavTalk = hid.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); + } + } + }); + + // Read data from HID and push it ont the UAVTalk input stream + Thread t = new Thread(this) { + @Override + public void run() { + while(!terminate) { + hid.readData(); + } + } + }; + t.start(); + + // Process any bytes that have been pushed onto the UAVTalk stream + 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; + } + } + + try { + Thread.sleep(5000); + } catch (InterruptedException e1) { + // TODO Auto-generated catch block + e1.printStackTrace(); + } + + Looper.myLooper().quit(); + + // Stop the HID reading loop + t.interrupt(); + try { + t.join(); + } catch (InterruptedException e) { + e.printStackTrace(); + } + + hid.disconnect(); + + // Shut down all the attached + mon.stopMonitor(); + mon = null; + tel.stopTelemetry(); + tel = null; + + // Finally close the stream if it is still open + hid.disconnect(); + + if (DEBUG) Log.d(TAG, "UAVTalk stream disconnected"); + toastMessage("TCP Thread finished"); + } + + }; } diff --git a/androidgcs/src/org/openpilot/uavtalk/UAVTalk.java b/androidgcs/src/org/openpilot/uavtalk/UAVTalk.java index 1c4d25db6..c2a0a577b 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 = 4; public static boolean VERBOSE = LOGLEVEL > 3; public static boolean WARN = LOGLEVEL > 2; public static boolean DEBUG = LOGLEVEL > 1; From 4eb846ba93b8d5cba4a1184fbd1e0f710f58d4b9 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sat, 11 Aug 2012 02:29:26 -0500 Subject: [PATCH 02/32] AndroidGCS: Get HID working properly for PipX --- .../androidgcs/telemetry/HidUAVTalk.java | 217 ++++++++++++------ .../telemetry/OPTelemetryService.java | 26 ++- .../src/org/openpilot/uavtalk/UAVTalk.java | 6 +- 3 files changed, 178 insertions(+), 71 deletions(-) diff --git a/androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java b/androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java index 879c02ba2..81e6ca7d7 100644 --- a/androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java +++ b/androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java @@ -10,6 +10,8 @@ import java.nio.ByteBuffer; import java.util.HashMap; import java.util.Iterator; +import junit.framework.Assert; + import org.openpilot.uavtalk.UAVObjectManager; import org.openpilot.uavtalk.UAVTalk; @@ -31,10 +33,9 @@ import android.util.Log; public class HidUAVTalk { private static final String TAG = HidUAVTalk.class.getSimpleName(); - public static int LOGLEVEL = 2; - public static boolean DEBUG = LOGLEVEL > 1; - public static boolean WARN = LOGLEVEL > 0; - + public static int LOGLEVEL = 0; + public static boolean WARN = LOGLEVEL > 1; + public static boolean DEBUG = LOGLEVEL > 0; Service hostDisplayActivity; @@ -377,44 +378,24 @@ public class HidUAVTalk { return true; } + private int byteToInt(byte b) { return b & 0x000000ff; } + private class TalkInputStream extends InputStream { - ByteBuffer data = null; + + ByteFifo data = new ByteFifo(); boolean stopped = false; - private static final int SIZE = 1024; - int readPosition = 0; + TalkInputStream() { - data = ByteBuffer.allocate(SIZE); - data.limit(SIZE); - data.clear(); } @Override - public int read() throws IOException { - while(!stopped) { - synchronized(data) { - if(data.capacity() > readPosition) - return data.get(readPosition++); - else - try { - data.wait(50); - } catch (InterruptedException e) { - // TODO Auto-generated catch block - e.printStackTrace(); - } - } - } - throw new IOException(); - } - - void compress() { - if (readPosition > (data.capacity() * 3.0 / 4.0)) { - synchronized(data) { - data.position(readPosition); - data.compact(); - readPosition = 0; - if (DEBUG) Log.d(TAG, "Compact() Capacity: " + data.capacity() + " Position: " + data.position() + " Available: " + data.remaining() + " Limit: " + data.limit()); - } + public int read() { + try { + return data.getByteBlocking(); + } catch (InterruptedException e) { + e.printStackTrace(); } + return -1; } public void stop() { @@ -422,42 +403,89 @@ public class HidUAVTalk { } public void put(byte b) { - synchronized(data) { - if(data.hasRemaining()) { - data.compact(); - data.put(b); - data.notify(); - } - } } public void write(byte[] b) { - synchronized(data) { - int available = data.remaining(); - if(available >= b.length) { - if (DEBUG) Log.d(TAG, "Size: " + b.length + " Capacity: " + data.capacity() + " Position: " + data.position() + " Available: " + available + " Limit: " + data.limit()); - //data.compact(); - data.put(b); - data.notify(); - } - } + data.put(b); } }; private final TalkInputStream inStream = new TalkInputStream(); + private class ByteFifo { + + //! The maximum size of the fifo + private final int MAX_SIZE = 1024; + //! 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; + } + + public boolean put(byte[] dat) { + if ((size + dat.length) > MAX_SIZE) + 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 byte[] get(int size) throws InterruptedException { + size = Math.min(size, this.size); + if (size > 0) { + synchronized(buf) { + byte[] dst = new byte[size]; + buf.position(0); + buf.get(dst,0,size); + buf.compact(); + this.size = this.size - size; + Assert.assertEquals(this.size, buf.position()); + + buf.wait(); + } + } + return new byte[0]; + } + + 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; + } + } + } + /** * Gets a report from HID, extract the meaningful data and push * it to the input stream */ + UsbRequest readRequest = null; public int readData() { int bufferDataLength = usbEndpointRead.getMaxPacketSize(); ByteBuffer buffer = ByteBuffer.allocate(bufferDataLength + 1); - UsbRequest request = new UsbRequest(); - request.initialize(connectionRead, usbEndpointRead); + if(readRequest == null) { + readRequest = new UsbRequest(); + readRequest.initialize(connectionRead, usbEndpointRead); + } // queue a request on the interrupt endpoint - if(!request.queue(buffer, bufferDataLength)) { + if(!readRequest.queue(buffer, bufferDataLength)) { if (DEBUG) Log.d(TAG, "Failed to queue request"); return 0; } @@ -466,7 +494,7 @@ public class HidUAVTalk { int dataSize; // wait for status event - if (connectionRead.requestWait() == request) { + if (connectionRead.requestWait() == readRequest) { // Packet format: // 0: Report ID (1) // 1: Number of valid bytes @@ -482,21 +510,30 @@ public class HidUAVTalk { byte[] dst = new byte[dataSize]; buffer.position(2); buffer.get(dst, 0, dataSize); + if (DEBUG) Log.d(TAG, "Entered read"); inStream.write(dst); if (DEBUG) Log.d(TAG, "Got read: " + dataSize + " bytes"); } } else return 0; + if(false) { + readRequest.cancel(); + readRequest.close(); + } + return dataSize; } private class TalkOutputStream extends OutputStream { - ByteBuffer data = ByteBuffer.allocate(128); + ByteBuffer data = ByteBuffer.allocate(1024); boolean stopped = false; + int writePosition = 0; public int read() throws IOException { - if (stopped) + if (!stopped) + + while(!stopped) { synchronized(data) { if(data.hasRemaining()) @@ -523,6 +560,7 @@ public class HidUAVTalk { if (stopped) throw new IOException(); synchronized(data) { + data.put((byte) oneByte); data.notify(); } @@ -534,37 +572,88 @@ public class HidUAVTalk { throw new IOException(); synchronized(data) { - //data.put(b); + // Move the cursor to the end of the byte array to append + data.position(writePosition); + if (b.length < data.remaining()) { + data.put(b); + writePosition = data.position(); + } data.notify(); } } + + public void packetizeData() { + ByteBuffer packet; + synchronized(data) { + // Determine how much data to put in the packet + int size = Math.min(writePosition, MAX_HID_PACKET_SIZE - 2); + if (size <= 0) + return; + + // 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 bytes of data + data.position(0); + data.get(packet.array(), 2, size); // Copy data into the other array + + // Remove that data from the write buffer + data.compact(); + writePosition -= size; + } + WriteToDevice(packet); + } + }; private final TalkOutputStream outStream = new TalkOutputStream(); + private static final int MAX_HID_PACKET_SIZE = 64; + /** + * Send a packet or wait for data to be available + */ + public void send() { + synchronized(outStream.data){ + if (outStream.writePosition > 0) + outStream.packetizeData(); + else { + outStream.data.notify(); + outStream.packetizeData(); + } + } + } + + UsbRequest writeRequest = null; boolean WriteToDevice(ByteBuffer DataToSend) { //The report must be formatted correctly for the device being connected to. On some devices, this requires that a specific value must be the first byte in the report. This can be followed by the length of the data in the report. This format is determined by the device, and isn't specified here. int bufferDataLength = usbEndpointWrite.getMaxPacketSize(); ByteBuffer buffer = ByteBuffer.allocate(bufferDataLength + 1); - UsbRequest request = new UsbRequest(); + + if(writeRequest == null) { + writeRequest = new UsbRequest(); + writeRequest.initialize(connectionWrite, usbEndpointWrite); + } buffer.put(DataToSend); - request.initialize(connectionWrite, usbEndpointWrite); - request.queue(buffer, bufferDataLength); + writeRequest.queue(buffer, bufferDataLength); try { - if (request.equals(connectionWrite.requestWait())) - { + if (writeRequest.equals(connectionWrite.requestWait())) return true; - } } catch (Exception ex) { // An exception has occured return false; } + + if (false) { + writeRequest.cancel(); + writeRequest.close(); + } + return false; } diff --git a/androidgcs/src/org/openpilot/androidgcs/telemetry/OPTelemetryService.java b/androidgcs/src/org/openpilot/androidgcs/telemetry/OPTelemetryService.java index 7406909da..38a9a4087 100644 --- a/androidgcs/src/org/openpilot/androidgcs/telemetry/OPTelemetryService.java +++ b/androidgcs/src/org/openpilot/androidgcs/telemetry/OPTelemetryService.java @@ -523,11 +523,6 @@ public class OPTelemetryService extends Service { return; } - hid.readData(); - hid.readData(); - hid.readData(); - hid.readData(); - uavTalk = hid.getUavtalk(); tel = new Telemetry(uavTalk, objMngr); mon = new TelemetryMonitor(objMngr,tel); @@ -549,11 +544,30 @@ public class OPTelemetryService extends Service { public void run() { while(!terminate) { hid.readData(); + hid.send(); } + Log.e(TAG, "TERMINATED"); } }; t.start(); + // Read data from HID and push it ont the UAVTalk input stream + Thread t2 = new Thread(this) { + @Override + public void run() { + while(!terminate) { + hid.send(); + try { + sleep(100); + } catch (InterruptedException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } + } + } + }; + //t2.start(); + // Process any bytes that have been pushed onto the UAVTalk stream if (DEBUG) Log.d(TAG, "Entering UAVTalk processing loop"); while( !terminate ) { @@ -584,6 +598,8 @@ public class OPTelemetryService extends Service { e.printStackTrace(); } + + hid.disconnect(); // Shut down all the attached diff --git a/androidgcs/src/org/openpilot/uavtalk/UAVTalk.java b/androidgcs/src/org/openpilot/uavtalk/UAVTalk.java index c2a0a577b..f7618bfeb 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 = 4; + 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; } From 90779506be663e6fd006e2fb83028ed97ed80ee0 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sat, 11 Aug 2012 14:49:35 -0500 Subject: [PATCH 03/32] AndroidGCS: Refactor TCP and HID onto a common TelemTask class which handles most of the logic of setting up the UAVTalk, ObjectMangager, Telemetry and TelemetryMonitor classes. Provides a cleaner shutdown too. --- .../androidgcs/telemetry/HidUAVTalk.java | 196 +++++++------ .../telemetry/OPTelemetryService.java | 257 +++--------------- .../androidgcs/telemetry/TcpUAVTalk.java | 96 +++---- .../androidgcs/telemetry/TelemetryTask.java | 235 ++++++++++++++++ 4 files changed, 405 insertions(+), 379 deletions(-) create mode 100644 androidgcs/src/org/openpilot/androidgcs/telemetry/TelemetryTask.java diff --git a/androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java b/androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java index 81e6ca7d7..35c7b6cce 100644 --- a/androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java +++ b/androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java @@ -11,12 +11,7 @@ import java.util.HashMap; import java.util.Iterator; import junit.framework.Assert; - -import org.openpilot.uavtalk.UAVObjectManager; -import org.openpilot.uavtalk.UAVTalk; - import android.app.PendingIntent; -import android.app.Service; import android.content.BroadcastReceiver; import android.content.Context; import android.content.Intent; @@ -30,21 +25,13 @@ import android.hardware.usb.UsbManager; import android.hardware.usb.UsbRequest; import android.util.Log; -public class HidUAVTalk { +public class HidUAVTalk extends TelemetryTask { private static final String TAG = HidUAVTalk.class.getSimpleName(); public static int LOGLEVEL = 0; public static boolean WARN = LOGLEVEL > 1; public static boolean DEBUG = LOGLEVEL > 0; - Service hostDisplayActivity; - - private boolean connected; - - private UAVTalk uavTalk; - - private UAVObjectManager objMngr; - //! USB constants static final int OPENPILOT_VENDOR_ID = 0x20A0; @@ -56,51 +43,57 @@ public class HidUAVTalk { static final int USB_PRODUCT_ID_OSD = 0x4194; static final int USB_PRODUCT_ID_SPARE = 0x4195; - public HidUAVTalk(Service service) { - hostDisplayActivity = service; - } - - 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 - */ - public boolean openTelemetryHid(UAVObjectManager objMngr) { - uavTalk = new UAVTalk(inStream, outStream, objMngr); - this.objMngr = objMngr; - return true; + private static final String ACTION_USB_PERMISSION = "com.access.device.USB_PERMISSION"; + + UsbDevice currentDevice; + + public HidUAVTalk(OPTelemetryService service) { + super(service); } + @Override public void disconnect() { + CleanUpAndClose(); //hostDisplayActivity.unregisterReceiver(usbReceiver); - //hostDisplayActivity.unregisterReceiver(usbPermissionReceiver); - inStream.stop(); - outStream.stop(); + telemService.unregisterReceiver(usbPermissionReceiver); + ((TalkInputStream)inStream).stop(); + ((TalkOutputStream)outStream).stop(); + + super.disconnect(); + + try { + readThread.join(); + writeThread.join(); + } catch (InterruptedException e) { + e.printStackTrace(); + } + + if (readRequest != null) { + readRequest.cancel(); + readRequest.close(); + readRequest = null; + } + + if (writeRequest != null) { + writeRequest.cancel(); + writeRequest.close(); + writeRequest = null; + } + } - public boolean connect(UAVObjectManager objMngr) { + @Override + boolean attemptConnection() { if (DEBUG) Log.d(TAG, "connect()"); // Register to get permission requested dialog - usbManager = (UsbManager) hostDisplayActivity.getSystemService(Context.USB_SERVICE); - permissionIntent = PendingIntent.getBroadcast(hostDisplayActivity, 0, new Intent(ACTION_USB_PERMISSION), 0); + usbManager = (UsbManager) telemService.getSystemService(Context.USB_SERVICE); + permissionIntent = PendingIntent.getBroadcast(telemService, 0, new Intent(ACTION_USB_PERMISSION), 0); permissionFilter = new IntentFilter(ACTION_USB_PERMISSION); - hostDisplayActivity.registerReceiver(usbPermissionReceiver, permissionFilter); - - // Register to get notified on device attached - /*deviceAttachedFilter = new IntentFilter(); - deviceAttachedFilter.addAction(UsbManager.ACTION_USB_DEVICE_DETACHED); - deviceAttachedFilter.addAction(UsbManager.ACTION_USB_DEVICE_ATTACHED); - hostDisplayActivity.registerReceiver(usbReceiver, deviceAttachedFilter);*/ + telemService.registerReceiver(usbPermissionReceiver, permissionFilter); + // 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(); @@ -108,33 +101,13 @@ public class HidUAVTalk { UsbDevice dev = deviceIterator.next(); if (DEBUG) Log.d(TAG, "Testing device: " + dev); usbManager.requestPermission(dev, permissionIntent); - //ConnectToDeviceInterface(dev); } if (DEBUG) Log.d(TAG, "Registered the deviceAttachedFilter"); - try { - Thread.sleep(1000); - } catch (InterruptedException e) { - // TODO Auto-generated catch block - e.printStackTrace(); - } - - if( !openTelemetryHid(objMngr) ) - return false; - - return connected; - + return deviceList.size() > 0; } - public HidUAVTalk(OPTelemetryService opTelemetryService) { - this.hostDisplayActivity = opTelemetryService; - } - - private static final String ACTION_USB_PERMISSION = "com.access.device.USB_PERMISSION"; - - UsbDevice currentDevice; - /* * Receives a requested broadcast from the operating system. * In this case the following actions are handled: @@ -172,9 +145,6 @@ public class HidUAVTalk { { if (DEBUG) Log.d(TAG, "Unable to connect to device"); } - sendEnabledMessage(); - // TODO: Create a listener to receive messages from the host - } } else @@ -224,12 +194,8 @@ public class HidUAVTalk { } } } - - private void sendEnabledMessage() { - // TODO Auto-generated method stub - - }; }; + private UsbEndpoint usbEndpointRead; private UsbEndpoint usbEndpointWrite; @@ -246,11 +212,6 @@ public class HidUAVTalk { private IntentFilter permissionFilter; - protected void sendEnabledMessage() { - // TODO Auto-generated method stub - - } - protected void CleanUpAndClose() { if (UsingSingleInterface) { if(connectionRead != null && usbInterfaceRead != null) @@ -372,12 +333,53 @@ public class HidUAVTalk { } connectedDevice = connectDevice; - connected = true; if (DEBUG) Log.d(TAG, "Opened endpoints"); + // Create the USB requests + readRequest = new UsbRequest(); + readRequest.initialize(connectionRead, usbEndpointRead); + + writeRequest = new UsbRequest(); + writeRequest.initialize(connectionWrite, usbEndpointWrite); + + + handler.post(new Runnable() { + @Override + public void run() { + inStream = new TalkInputStream(); + outStream = new TalkOutputStream(); + attemptSucceeded(); + } + }); + + readThread = new Thread(new Runnable() { + @Override + public void run() { + while (!shutdown) { + readData(); + sendData(); + } + } + + }); + readThread.start(); + + writeThread = new Thread(new Runnable() { + @Override + public void run() { + while (!shutdown) { + sendData(); + } + } + + }); + //writeThread.start(); + return true; } + Thread readThread; + Thread writeThread; private int byteToInt(byte b) { return b & 0x000000ff; } private class TalkInputStream extends InputStream { @@ -409,7 +411,6 @@ public class HidUAVTalk { data.put(b); } }; - private final TalkInputStream inStream = new TalkInputStream(); private class ByteFifo { @@ -479,11 +480,6 @@ public class HidUAVTalk { int bufferDataLength = usbEndpointRead.getMaxPacketSize(); ByteBuffer buffer = ByteBuffer.allocate(bufferDataLength + 1); - if(readRequest == null) { - readRequest = new UsbRequest(); - readRequest.initialize(connectionRead, usbEndpointRead); - } - // queue a request on the interrupt endpoint if(!readRequest.queue(buffer, bufferDataLength)) { if (DEBUG) Log.d(TAG, "Failed to queue request"); @@ -511,17 +507,12 @@ public class HidUAVTalk { buffer.position(2); buffer.get(dst, 0, dataSize); if (DEBUG) Log.d(TAG, "Entered read"); - inStream.write(dst); + ((TalkInputStream)inStream).write(dst); if (DEBUG) Log.d(TAG, "Got read: " + dataSize + " bytes"); } } else return 0; - if(false) { - readRequest.cancel(); - readRequest.close(); - } - return dataSize; } @@ -600,30 +591,32 @@ public class HidUAVTalk { // Remove that data from the write buffer data.compact(); writePosition -= size; + if (DEBUG) Log.d(TAG, "packetizeData(): size="+size); } WriteToDevice(packet); } }; - private final TalkOutputStream outStream = new TalkOutputStream(); private static final int MAX_HID_PACKET_SIZE = 64; /** * Send a packet or wait for data to be available */ - public void send() { - synchronized(outStream.data){ - if (outStream.writePosition > 0) - outStream.packetizeData(); + public void sendData() { + TalkOutputStream o = (TalkOutputStream) outStream; + synchronized(o.data){ + if (o.writePosition > 0) + o.packetizeData(); else { - outStream.data.notify(); - outStream.packetizeData(); + o.data.notify(); + o.packetizeData(); } } } UsbRequest writeRequest = null; boolean WriteToDevice(ByteBuffer DataToSend) { + if (DEBUG) Log.d(TAG, "Writing to device()"); //The report must be formatted correctly for the device being connected to. On some devices, this requires that a specific value must be the first byte in the report. This can be followed by the length of the data in the report. This format is determined by the device, and isn't specified here. @@ -708,4 +701,5 @@ public class HidUAVTalk { // TODO Auto-generated method stub return false; } + } diff --git a/androidgcs/src/org/openpilot/androidgcs/telemetry/OPTelemetryService.java b/androidgcs/src/org/openpilot/androidgcs/telemetry/OPTelemetryService.java index 38a9a4087..0ce1ab166 100644 --- a/androidgcs/src/org/openpilot/androidgcs/telemetry/OPTelemetryService.java +++ b/androidgcs/src/org/openpilot/androidgcs/telemetry/OPTelemetryService.java @@ -81,6 +81,7 @@ public class OPTelemetryService extends Service { private boolean terminate = false; private Thread activeTelem; + private TelemetryTask telemTask; private final IBinder mBinder = new LocalBinder(); @@ -130,11 +131,13 @@ public class OPTelemetryService extends Service { break; case 3: Toast.makeText(getApplicationContext(), "Attempting TCP connection", Toast.LENGTH_SHORT).show(); - activeTelem = new TcpTelemetryThread(); + telemTask = new TcpUAVTalk(this); + activeTelem = new Thread(telemTask); break; case 4: Toast.makeText(getApplicationContext(), "Attempting USB HID connection", Toast.LENGTH_SHORT).show(); - activeTelem = new HidTelemetryThread(); + telemTask = new HidUAVTalk(this); + activeTelem = new Thread(telemTask); break; default: throw new Error("Unsupported"); @@ -143,8 +146,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(); @@ -153,6 +167,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); @@ -215,12 +230,25 @@ public class OPTelemetryService extends Service { @Override public void onDestroy() { + + if (telemTask != null) { + Log.d(TAG, "onDestory() 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; + return telemTask.getTelemTaskIface(); } public void openConnection() { Toast.makeText(getApplicationContext(), "Requested open connection", Toast.LENGTH_SHORT).show(); @@ -395,225 +423,4 @@ public class OPTelemetryService extends Service { } }; - - 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"); - } - - }; - - private class HidTelemetryThread extends Thread implements TelemTask { - - private final UAVObjectManager objMngr; - private UAVTalk uavTalk; - private Telemetry tel; - private TelemetryMonitor mon; - - @Override - public UAVObjectManager getObjectManager() { return objMngr; }; - - HidTelemetryThread() { - objMngr = new UAVObjectManager(); - UAVObjectsInitialize.register(objMngr); - } - - @Override - public void run() { - if (DEBUG) Log.d(TAG, "HID Telemetry Thread started"); - - Looper.prepare(); - - final HidUAVTalk hid = new HidUAVTalk(OPTelemetryService.this); - hid.connect(objMngr); - - try { - Thread.sleep(200); - } catch (InterruptedException e) { - // TODO Auto-generated catch block - e.printStackTrace(); - } - - if( !hid.getConnected() ) { - toastMessage("HID connection failed"); - return; - } - - uavTalk = hid.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); - } - } - }); - - // Read data from HID and push it ont the UAVTalk input stream - Thread t = new Thread(this) { - @Override - public void run() { - while(!terminate) { - hid.readData(); - hid.send(); - } - Log.e(TAG, "TERMINATED"); - } - }; - t.start(); - - // Read data from HID and push it ont the UAVTalk input stream - Thread t2 = new Thread(this) { - @Override - public void run() { - while(!terminate) { - hid.send(); - try { - sleep(100); - } catch (InterruptedException e) { - // TODO Auto-generated catch block - e.printStackTrace(); - } - } - } - }; - //t2.start(); - - // Process any bytes that have been pushed onto the UAVTalk stream - 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; - } - } - - try { - Thread.sleep(5000); - } catch (InterruptedException e1) { - // TODO Auto-generated catch block - e1.printStackTrace(); - } - - Looper.myLooper().quit(); - - // Stop the HID reading loop - t.interrupt(); - try { - t.join(); - } catch (InterruptedException e) { - e.printStackTrace(); - } - - - - hid.disconnect(); - - // Shut down all the attached - mon.stopMonitor(); - mon = null; - tel.stopTelemetry(); - tel = null; - - // Finally close the stream if it is still open - hid.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..3210ebebf --- /dev/null +++ b/androidgcs/src/org/openpilot/androidgcs/telemetry/TelemetryTask.java @@ -0,0 +1,235 @@ +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; + + //! 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); + mon = new TelemetryMonitor(objMngr,tel); + + // 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(); + } + }); + + // 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() { + new Thread(new processUavTalk()).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; + } + }; + } + +} From 5fa36ddd5c71ada4144ca3fc886823cf4fa35cc4 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sun, 12 Aug 2012 00:29:13 -0500 Subject: [PATCH 04/32] AndroidGCS: Make sure not to try and get interface to expired telemetry --- .../openpilot/androidgcs/telemetry/OPTelemetryService.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/androidgcs/src/org/openpilot/androidgcs/telemetry/OPTelemetryService.java b/androidgcs/src/org/openpilot/androidgcs/telemetry/OPTelemetryService.java index 0ce1ab166..33e2be728 100644 --- a/androidgcs/src/org/openpilot/androidgcs/telemetry/OPTelemetryService.java +++ b/androidgcs/src/org/openpilot/androidgcs/telemetry/OPTelemetryService.java @@ -248,7 +248,9 @@ public class OPTelemetryService extends Service { public class LocalBinder extends Binder { public TelemTask getTelemTask(int id) { - return telemTask.getTelemTaskIface(); + if (telemTask != null) + return telemTask.getTelemTaskIface(); + return null; } public void openConnection() { Toast.makeText(getApplicationContext(), "Requested open connection", Toast.LENGTH_SHORT).show(); From 3d34a5d28010bdf73a8598cecc590c248afabaf4 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sun, 12 Aug 2012 00:30:19 -0500 Subject: [PATCH 05/32] AndrodGCS: Correctly check HID packet format --- .../src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java b/androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java index 35c7b6cce..b9cab9854 100644 --- a/androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java +++ b/androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java @@ -500,7 +500,7 @@ public class HidUAVTalk extends TelemetryTask { //Assert.assertEquals(1, buffer.get()); // Report ID //Assert.assertTrue(dataSize < buffer.capacity()); - if (buffer.get(0) != 1 || buffer.get(1) < 0 || buffer.get(2) > (buffer.capacity() - 2)) { + if (buffer.get(0) != 1 || buffer.get(1) < 0 || buffer.get(1) > (buffer.capacity() - 2)) { if (DEBUG) Log.d(TAG, "Badly formatted HID packet"); } else { byte[] dst = new byte[dataSize]; From 405c5a60f0e4aee5dbac484ac01f8a6fdc8ae0de Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sun, 12 Aug 2012 01:14:21 -0500 Subject: [PATCH 06/32] AndroidGCS: Cleanup of the HID telemetry code --- .../androidgcs/telemetry/HidUAVTalk.java | 437 +++++++----------- .../telemetry/OPTelemetryService.java | 4 +- .../androidgcs/telemetry/TelemetryTask.java | 2 +- .../src/org/openpilot/uavtalk/UAVTalk.java | 3 - 4 files changed, 164 insertions(+), 282 deletions(-) diff --git a/androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java b/androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java index b9cab9854..991e83871 100644 --- a/androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java +++ b/androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java @@ -33,6 +33,7 @@ public class HidUAVTalk extends TelemetryTask { public static boolean DEBUG = 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; @@ -57,14 +58,14 @@ public class HidUAVTalk extends TelemetryTask { CleanUpAndClose(); //hostDisplayActivity.unregisterReceiver(usbReceiver); telemService.unregisterReceiver(usbPermissionReceiver); - ((TalkInputStream)inStream).stop(); - ((TalkOutputStream)outStream).stop(); super.disconnect(); try { - readThread.join(); - writeThread.join(); + if(readWriteThread != null) { + readWriteThread.join(); + readWriteThread = null; + } } catch (InterruptedException e) { e.printStackTrace(); } @@ -158,6 +159,8 @@ public class HidUAVTalk extends TelemetryTask { } } }; + + /* TODO: Detect dettached events and close the connection private final BroadcastReceiver usbReceiver = new BroadcastReceiver() { @Override @@ -194,7 +197,7 @@ public class HidUAVTalk extends TelemetryTask { } } } - }; + }; */ private UsbEndpoint usbEndpointRead; @@ -208,8 +211,6 @@ public class HidUAVTalk extends TelemetryTask { private UsbDeviceConnection connectionWrite; - private IntentFilter deviceAttachedFilter; - private IntentFilter permissionFilter; protected void CleanUpAndClose() { @@ -247,7 +248,9 @@ public class HidUAVTalk extends TelemetryTask { private UsbInterface usbInterfaceWrite = null; private final boolean UsingSingleInterface = true; - private UsbDevice connectedDevice; + private TalkInputStream inTalkStream; + private TalkOutputStream outTalkStream; + UsbRequest writeRequest = null; boolean ConnectToDeviceInterface(UsbDevice connectDevice) { // Connecting to the Device - If you are reading and writing, then the device @@ -332,7 +335,6 @@ public class HidUAVTalk extends TelemetryTask { connectionWrite.claimInterface(usbInterfaceWrite, true); } - connectedDevice = connectDevice; if (DEBUG) Log.d(TAG, "Opened endpoints"); // Create the USB requests @@ -342,17 +344,18 @@ public class HidUAVTalk extends TelemetryTask { writeRequest = new UsbRequest(); writeRequest.initialize(connectionWrite, usbEndpointWrite); - + inTalkStream = new TalkInputStream(); + outTalkStream = new TalkOutputStream(); + inStream = inTalkStream; + outStream = outTalkStream; handler.post(new Runnable() { @Override public void run() { - inStream = new TalkInputStream(); - outStream = new TalkOutputStream(); attemptSucceeded(); } }); - readThread = new Thread(new Runnable() { + readWriteThread = new Thread(new Runnable() { @Override public void run() { while (!shutdown) { @@ -361,116 +364,22 @@ public class HidUAVTalk extends TelemetryTask { } } - }); - readThread.start(); - - writeThread = new Thread(new Runnable() { - @Override - public void run() { - while (!shutdown) { - sendData(); - } - } - - }); - //writeThread.start(); + }, "HID Read Write"); + readWriteThread.start(); return true; } - Thread readThread; - Thread writeThread; - private int byteToInt(byte b) { return b & 0x000000ff; } + Thread readWriteThread; - private class TalkInputStream extends InputStream { - - ByteFifo data = new ByteFifo(); - boolean stopped = false; - - TalkInputStream() { - } - - @Override - public int read() { - try { - return data.getByteBlocking(); - } catch (InterruptedException e) { - e.printStackTrace(); - } - return -1; - } - - public void stop() { - stopped = true; - } - - public void put(byte b) { - } - - public void write(byte[] b) { - data.put(b); - } - }; - - private class ByteFifo { - - //! The maximum size of the fifo - private final int MAX_SIZE = 1024; - //! 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; - } - - public boolean put(byte[] dat) { - if ((size + dat.length) > MAX_SIZE) - 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 byte[] get(int size) throws InterruptedException { - size = Math.min(size, this.size); - if (size > 0) { - synchronized(buf) { - byte[] dst = new byte[size]; - buf.position(0); - buf.get(dst,0,size); - buf.compact(); - this.size = this.size - size; - Assert.assertEquals(this.size, buf.position()); - - buf.wait(); - } - } - return new byte[0]; - } - - 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; - } + 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 @@ -507,7 +416,7 @@ public class HidUAVTalk extends TelemetryTask { buffer.position(2); buffer.get(dst, 0, dataSize); if (DEBUG) Log.d(TAG, "Entered read"); - ((TalkInputStream)inStream).write(dst); + inTalkStream.write(dst); if (DEBUG) Log.d(TAG, "Got read: " + dataSize + " bytes"); } } else @@ -516,42 +425,64 @@ public class HidUAVTalk extends TelemetryTask { return dataSize; } - private class TalkOutputStream extends OutputStream { - ByteBuffer data = ByteBuffer.allocate(1024); - boolean stopped = false; - int writePosition = 0; + /** + * Send a packet if data is available + */ + public void sendData() { + ByteBuffer packet = null; + do { // Send all the data available to prevent sending backlog + packet = outTalkStream.getHIDpacket(); + if (packet != null) { + if (DEBUG) Log.d(TAG, "Writing to device()"); - public int read() throws IOException { - if (!stopped) + int bufferDataLength = usbEndpointWrite.getMaxPacketSize(); + Assert.assertTrue(packet.capacity() <= bufferDataLength); - - while(!stopped) { - synchronized(data) { - if(data.hasRemaining()) - return data.get(); - else - try { - data.wait(); - } catch (InterruptedException e) { - // TODO Auto-generated catch block - e.printStackTrace(); - } - } - throw new IOException(); + writeRequest.queue(packet, bufferDataLength); + try + { + if (!writeRequest.equals(connectionWrite.requestWait())) + Log.e(TAG, "writeRequest failed"); } - return 0; - } + catch (Exception ex) + { + } + } + } while (packet != null); + } - public void stop() { - stopped = true; - } + /*********** Helper classes for telemetry streams ************/ + class TalkOutputStream extends OutputStream { + // Uses ByteFifo.getByteBlocking() + // and ByteFifo.put(byte []) + ByteFifo data = new ByteFifo(); + + 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 { - if (stopped) + // Throw exception when try and read after shutdown + if (shutdown) throw new IOException(); - synchronized(data) { + synchronized(data) { data.put((byte) oneByte); data.notify(); } @@ -559,147 +490,101 @@ public class HidUAVTalk extends TelemetryTask { @Override public void write(byte[] b) throws IOException { - if (stopped) + if (shutdown) throw new IOException(); synchronized(data) { - // Move the cursor to the end of the byte array to append - data.position(writePosition); - if (b.length < data.remaining()) { - data.put(b); - writePosition = data.position(); - } + data.put(b); data.notify(); } } - public void packetizeData() { - ByteBuffer packet; - synchronized(data) { - // Determine how much data to put in the packet - int size = Math.min(writePosition, MAX_HID_PACKET_SIZE - 2); - if (size <= 0) - return; - - // 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 bytes of data - data.position(0); - data.get(packet.array(), 2, size); // Copy data into the other array - - // Remove that data from the write buffer - data.compact(); - writePosition -= size; - if (DEBUG) Log.d(TAG, "packetizeData(): size="+size); - } - WriteToDevice(packet); - } - }; - private static final int MAX_HID_PACKET_SIZE = 64; - /** - * Send a packet or wait for data to be available - */ - public void sendData() { - TalkOutputStream o = (TalkOutputStream) outStream; - synchronized(o.data){ - if (o.writePosition > 0) - o.packetizeData(); - else { - o.data.notify(); - o.packetizeData(); + 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; } } } - - UsbRequest writeRequest = null; - boolean WriteToDevice(ByteBuffer DataToSend) { - if (DEBUG) Log.d(TAG, "Writing to device()"); - - //The report must be formatted correctly for the device being connected to. On some devices, this requires that a specific value must be the first byte in the report. This can be followed by the length of the data in the report. This format is determined by the device, and isn't specified here. - - int bufferDataLength = usbEndpointWrite.getMaxPacketSize(); - ByteBuffer buffer = ByteBuffer.allocate(bufferDataLength + 1); - - if(writeRequest == null) { - writeRequest = new UsbRequest(); - writeRequest.initialize(connectionWrite, usbEndpointWrite); - } - - buffer.put(DataToSend); - - writeRequest.queue(buffer, bufferDataLength); - try - { - if (writeRequest.equals(connectionWrite.requestWait())) - return true; - } - catch (Exception ex) - { - // An exception has occured - return false; - } - - if (false) { - writeRequest.cancel(); - writeRequest.close(); - } - - return false; - } - - - //Reading from the Device - As USB devices work with reports of a specific length. The data received from the device will always be the size specified by the report length. Even if there isn't enough data to fill the report. Some devices require the controlTransfer method for reading data. I don't cover this command in this blog. - void ReadFromDevice() { - //If you are expecting unsolicited data from the device, then a read thread should be started so that the data can be processed as soon as it arrives. - - int bufferDataLength = usbEndpointRead.getMaxPacketSize(); - ByteBuffer buffer = ByteBuffer.allocate(bufferDataLength + 1); - UsbRequest requestQueued = null; - UsbRequest request = new UsbRequest(); - request.initialize(connectionRead, usbEndpointRead); - - try - { - while (!getStopping()) - { - request.queue(buffer, bufferDataLength); - requestQueued = connectionRead.requestWait(); - if (request.equals(requestQueued)) - { - byte[] byteBuffer = new byte[bufferDataLength + 1]; - buffer.get(byteBuffer, 0, bufferDataLength); - - // Handle data received - - buffer.clear(); - } - else - { - Thread.sleep(20); - } - } - } - catch (Exception ex) - { - // An exception has occured - } - try - { - request.cancel(); - request.close(); - } - catch (Exception ex) - { - // An exception has occured - } - } - - private boolean getStopping() { - // TODO Auto-generated method stub - return false; - } - -} +} \ 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 33e2be728..4eeb57abe 100644 --- a/androidgcs/src/org/openpilot/androidgcs/telemetry/OPTelemetryService.java +++ b/androidgcs/src/org/openpilot/androidgcs/telemetry/OPTelemetryService.java @@ -132,12 +132,12 @@ public class OPTelemetryService extends Service { case 3: Toast.makeText(getApplicationContext(), "Attempting TCP connection", Toast.LENGTH_SHORT).show(); telemTask = new TcpUAVTalk(this); - activeTelem = new Thread(telemTask); + 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); + activeTelem = new Thread(telemTask, "Hid telemetry thread"); break; default: throw new Error("Unsupported"); diff --git a/androidgcs/src/org/openpilot/androidgcs/telemetry/TelemetryTask.java b/androidgcs/src/org/openpilot/androidgcs/telemetry/TelemetryTask.java index 3210ebebf..87196bc1e 100644 --- a/androidgcs/src/org/openpilot/androidgcs/telemetry/TelemetryTask.java +++ b/androidgcs/src/org/openpilot/androidgcs/telemetry/TelemetryTask.java @@ -157,7 +157,7 @@ public abstract class TelemetryTask implements Runnable { * to read from the input stream. */ private void startInputProcessing() { - new Thread(new processUavTalk()).start(); + new Thread(new processUavTalk(), "Process UAV talk").start(); } //! Runnable to process input stream diff --git a/androidgcs/src/org/openpilot/uavtalk/UAVTalk.java b/androidgcs/src/org/openpilot/uavtalk/UAVTalk.java index f7618bfeb..c996da845 100644 --- a/androidgcs/src/org/openpilot/uavtalk/UAVTalk.java +++ b/androidgcs/src/org/openpilot/uavtalk/UAVTalk.java @@ -686,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; From f8ba049d565765d26d99a619091a8c75ac43b61f Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sun, 12 Aug 2012 13:16:49 -0500 Subject: [PATCH 07/32] AndroidGCS: In the telemetry class use a thread safe queue for transations and removed the synhronized blocks to prevent deadlocks. --- androidgcs/src/org/openpilot/uavtalk/Telemetry.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/androidgcs/src/org/openpilot/uavtalk/Telemetry.java b/androidgcs/src/org/openpilot/uavtalk/Telemetry.java index 2e0b57f41..21525ea14 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,6 +33,7 @@ import java.util.Observer; import java.util.Queue; import java.util.Timer; import java.util.TimerTask; +import java.util.concurrent.ConcurrentLinkedQueue; import android.util.Log; @@ -485,7 +485,7 @@ public class Telemetry { * 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 + private 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); @@ -538,7 +538,7 @@ public class Telemetry { * Process events from the object queue * @throws IOException */ - private synchronized void processObjectQueue() throws IOException + private void processObjectQueue() throws IOException { if (DEBUG) Log.d(TAG, "Process object queue - Depth " + objQueue.size() + " priority " + objPriorityQueue.size()); @@ -744,8 +744,8 @@ public class Telemetry { 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 Queue objQueue = new ConcurrentLinkedQueue(); + private final Queue objPriorityQueue = new ConcurrentLinkedQueue(); private final ObjectTransactionInfo transInfo = new ObjectTransactionInfo(); private boolean transPending; From 050ec8096ba89cdc769179f3c30ee4e883299c92 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sun, 12 Aug 2012 13:28:38 -0500 Subject: [PATCH 08/32] HID: move all the variables to the top --- .../androidgcs/telemetry/HidUAVTalk.java | 44 ++++++++----------- 1 file changed, 19 insertions(+), 25 deletions(-) diff --git a/androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java b/androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java index 991e83871..6ec0a950a 100644 --- a/androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java +++ b/androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java @@ -28,7 +28,7 @@ import android.util.Log; public class HidUAVTalk extends TelemetryTask { private static final String TAG = HidUAVTalk.class.getSimpleName(); - public static int LOGLEVEL = 0; + public static int LOGLEVEL = 2; public static boolean WARN = LOGLEVEL > 1; public static boolean DEBUG = LOGLEVEL > 0; @@ -46,7 +46,24 @@ public class HidUAVTalk extends TelemetryTask { private static final String ACTION_USB_PERMISSION = "com.access.device.USB_PERMISSION"; - UsbDevice currentDevice; + //! Define whether to use a single interface for reading and writing + private final boolean UsingSingleInterface = true; + + private UsbDevice currentDevice; + private UsbEndpoint usbEndpointRead; + private UsbEndpoint usbEndpointWrite; + private UsbManager usbManager; + private PendingIntent permissionIntent; + private UsbDeviceConnection connectionRead; + private UsbDeviceConnection connectionWrite; + private IntentFilter permissionFilter; + private UsbInterface usbInterfaceRead = null; + private UsbInterface usbInterfaceWrite = null; + private TalkInputStream inTalkStream; + private TalkOutputStream outTalkStream; + private UsbRequest writeRequest = null; + private UsbRequest readRequest = null; + private Thread readWriteThread; public HidUAVTalk(OPTelemetryService service) { super(service); @@ -199,19 +216,6 @@ public class HidUAVTalk extends TelemetryTask { } }; */ - private UsbEndpoint usbEndpointRead; - - private UsbEndpoint usbEndpointWrite; - - private UsbManager usbManager; - - private PendingIntent permissionIntent; - - private UsbDeviceConnection connectionRead; - - private UsbDeviceConnection connectionWrite; - - private IntentFilter permissionFilter; protected void CleanUpAndClose() { if (UsingSingleInterface) { @@ -244,14 +248,6 @@ public class HidUAVTalk extends TelemetryTask { return false; } - private UsbInterface usbInterfaceRead = null; - private UsbInterface usbInterfaceWrite = null; - private final boolean UsingSingleInterface = true; - - private TalkInputStream inTalkStream; - private TalkOutputStream outTalkStream; - UsbRequest writeRequest = null; - 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 @@ -370,7 +366,6 @@ public class HidUAVTalk extends TelemetryTask { return true; } - Thread readWriteThread; void displayBuffer(String msg, byte[] buf) { msg += " ("; @@ -384,7 +379,6 @@ public class HidUAVTalk extends TelemetryTask { * Gets a report from HID, extract the meaningful data and push * it to the input stream */ - UsbRequest readRequest = null; public int readData() { int bufferDataLength = usbEndpointRead.getMaxPacketSize(); ByteBuffer buffer = ByteBuffer.allocate(bufferDataLength + 1); From 6b4a14fa87c0869ddf893050a1f2c883089f9d3e Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sun, 12 Aug 2012 13:41:15 -0500 Subject: [PATCH 09/32] Remove legacy code for supporting two USB interfaces --- .../androidgcs/telemetry/HidUAVTalk.java | 93 +++++-------------- 1 file changed, 23 insertions(+), 70 deletions(-) diff --git a/androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java b/androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java index 6ec0a950a..4f3c64a5a 100644 --- a/androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java +++ b/androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java @@ -28,9 +28,10 @@ import android.util.Log; public class HidUAVTalk extends TelemetryTask { private static final String TAG = HidUAVTalk.class.getSimpleName(); - public static int LOGLEVEL = 2; - public static boolean WARN = LOGLEVEL > 1; - public static boolean DEBUG = LOGLEVEL > 0; + public static final int LOGLEVEL = 2; + 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; @@ -46,19 +47,14 @@ public class HidUAVTalk extends TelemetryTask { private static final String ACTION_USB_PERMISSION = "com.access.device.USB_PERMISSION"; - //! Define whether to use a single interface for reading and writing - private final boolean UsingSingleInterface = true; - private UsbDevice currentDevice; private UsbEndpoint usbEndpointRead; private UsbEndpoint usbEndpointWrite; private UsbManager usbManager; private PendingIntent permissionIntent; - private UsbDeviceConnection connectionRead; - private UsbDeviceConnection connectionWrite; + private UsbDeviceConnection usbDeviceConnection; private IntentFilter permissionFilter; - private UsbInterface usbInterfaceRead = null; - private UsbInterface usbInterfaceWrite = null; + private UsbInterface usbInterface = null; private TalkInputStream inTalkStream; private TalkOutputStream outTalkStream; private UsbRequest writeRequest = null; @@ -218,19 +214,9 @@ public class HidUAVTalk extends TelemetryTask { protected void CleanUpAndClose() { - if (UsingSingleInterface) { - if(connectionRead != null && usbInterfaceRead != null) - connectionRead.releaseInterface(usbInterfaceRead); - usbInterfaceRead = null; - } - else { - if(connectionRead != null && usbInterfaceRead != null) - connectionRead.releaseInterface(usbInterfaceRead); - if(connectionWrite != null && usbInterfaceWrite != null) - connectionWrite.releaseInterface(usbInterfaceWrite); - usbInterfaceWrite = null; - usbInterfaceRead = null; - } + 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. @@ -258,87 +244,54 @@ public class HidUAVTalk extends TelemetryTask { UsbEndpoint ep1 = null; UsbEndpoint ep2 = null; - - if (UsingSingleInterface) + // Using the same interface for reading and writing + usbInterface = connectDevice.getInterface(0x2); + if (usbInterface.getEndpointCount() == 2) { - // Using the same interface for reading and writing - usbInterfaceRead = connectDevice.getInterface(0x2); - usbInterfaceWrite = usbInterfaceRead; - if (usbInterfaceRead.getEndpointCount() == 2) - { - ep1 = usbInterfaceRead.getEndpoint(0); - ep2 = usbInterfaceRead.getEndpoint(1); - } + ep1 = usbInterface.getEndpoint(0); + ep2 = usbInterface.getEndpoint(1); } - else // if (!UsingSingleInterface) - { - usbInterfaceRead = connectDevice.getInterface(0x01); - usbInterfaceWrite = connectDevice.getInterface(0x02); - if ((usbInterfaceRead.getEndpointCount() == 1) && (usbInterfaceWrite.getEndpointCount() == 1)) - { - ep1 = usbInterfaceRead.getEndpoint(0); - ep2 = usbInterfaceWrite.getEndpoint(0); - } - } - if ((ep1 == null) || (ep2 == null)) { - if (DEBUG) Log.d(TAG, "Null endpoints"); + 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 (DEBUG) Log.d(TAG, "Endpoints wrong way around"); + if (ERROR) Log.e(TAG, "Could not find write and read endpoint"); return false; } - connectionRead = usbManager.openDevice(connectDevice); - connectionRead.claimInterface(usbInterfaceRead, true); + // Claim the interface + usbDeviceConnection = usbManager.openDevice(connectDevice); + usbDeviceConnection.claimInterface(usbInterface, true); - if (UsingSingleInterface) - { - connectionWrite = connectionRead; - } - else // if (!UsingSingleInterface) - { - connectionWrite = usbManager.openDevice(connectDevice); - connectionWrite.claimInterface(usbInterfaceWrite, true); - } if (DEBUG) Log.d(TAG, "Opened endpoints"); // Create the USB requests readRequest = new UsbRequest(); - readRequest.initialize(connectionRead, usbEndpointRead); + readRequest.initialize(usbDeviceConnection, usbEndpointRead); writeRequest = new UsbRequest(); - writeRequest.initialize(connectionWrite, usbEndpointWrite); + writeRequest.initialize(usbDeviceConnection, usbEndpointWrite); inTalkStream = new TalkInputStream(); outTalkStream = new TalkOutputStream(); @@ -393,7 +346,7 @@ public class HidUAVTalk extends TelemetryTask { int dataSize; // wait for status event - if (connectionRead.requestWait() == readRequest) { + if (usbDeviceConnection.requestWait() == readRequest) { // Packet format: // 0: Report ID (1) // 1: Number of valid bytes @@ -435,7 +388,7 @@ public class HidUAVTalk extends TelemetryTask { writeRequest.queue(packet, bufferDataLength); try { - if (!writeRequest.equals(connectionWrite.requestWait())) + if (!writeRequest.equals(usbDeviceConnection.requestWait())) Log.e(TAG, "writeRequest failed"); } catch (Exception ex) From 69fbefee85088ef82cd3a5bcd9ed026b1b19310d Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sun, 12 Aug 2012 14:00:50 -0500 Subject: [PATCH 10/32] HID: Since there is only one usb device connection queue read and write events onto that single connection and wait for either. --- .../androidgcs/telemetry/HidUAVTalk.java | 99 +++++++++++-------- 1 file changed, 56 insertions(+), 43 deletions(-) diff --git a/androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java b/androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java index 4f3c64a5a..5abb4dd09 100644 --- a/androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java +++ b/androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java @@ -28,7 +28,7 @@ import android.util.Log; public class HidUAVTalk extends TelemetryTask { private static final String TAG = HidUAVTalk.class.getSimpleName(); - public static final int LOGLEVEL = 2; + public static final int LOGLEVEL = 0; public static final boolean DEBUG = LOGLEVEL > 2; public static final boolean WARN = LOGLEVEL > 1; public static final boolean ERROR = LOGLEVEL > 0; @@ -61,6 +61,9 @@ public class HidUAVTalk extends TelemetryTask { private UsbRequest readRequest = null; private Thread readWriteThread; + private boolean readPending = false; + private boolean writePending = false; + public HidUAVTalk(OPTelemetryService service) { super(service); } @@ -307,8 +310,23 @@ public class HidUAVTalk extends TelemetryTask { readWriteThread = new Thread(new Runnable() { @Override public void run() { + + // Enqueue the first read + readData(); + while (!shutdown) { - readData(); + // If there are no request + + // Enqueue requests appropriately first + UsbRequest returned = usbDeviceConnection.requestWait(); + if (returned == readRequest) { + if (DEBUG) Log.d(TAG, "Received read request"); + readData(); + } else if (returned == writeRequest) { + if (DEBUG) Log.d(TAG, "Received write completed request"); + writePending = false; + } + sendData(); } } @@ -332,70 +350,65 @@ public class HidUAVTalk extends TelemetryTask { * Gets a report from HID, extract the meaningful data and push * it to the input stream */ - public int readData() { - int bufferDataLength = usbEndpointRead.getMaxPacketSize(); - ByteBuffer buffer = ByteBuffer.allocate(bufferDataLength + 1); + ByteBuffer readBuffer = ByteBuffer.allocate(MAX_HID_PACKET_SIZE); + private void queueRead() { + if(!readRequest.queue(readBuffer, MAX_HID_PACKET_SIZE)) { + if (ERROR) Log.e(TAG, "Failed to queue request"); + } else + readPending = true; + } + public void readData() { - // queue a request on the interrupt endpoint - if(!readRequest.queue(buffer, bufferDataLength)) { - if (DEBUG) Log.d(TAG, "Failed to queue request"); - return 0; - } - - if (DEBUG) Log.d(TAG, "Request queued"); - - int dataSize; - // wait for status event - if (usbDeviceConnection.requestWait() == readRequest) { - // Packet format: + if (!readPending) { + queueRead(); + } else { // We just received a read + readPending = false; + // Packet format: // 0: Report ID (1) // 1: Number of valid bytes // 2:63: Data - dataSize = buffer.get(1); // Data size + int dataSize = readBuffer.get(1); // Data size //Assert.assertEquals(1, buffer.get()); // Report ID //Assert.assertTrue(dataSize < buffer.capacity()); - if (buffer.get(0) != 1 || buffer.get(1) < 0 || buffer.get(1) > (buffer.capacity() - 2)) { - if (DEBUG) Log.d(TAG, "Badly formatted HID packet"); + 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]; - buffer.position(2); - buffer.get(dst, 0, 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"); } - } else - return 0; - return dataSize; + // Queue another read + queueRead(); + } } /** * Send a packet if data is available */ public void sendData() { - ByteBuffer packet = null; - do { // Send all the data available to prevent sending backlog - packet = outTalkStream.getHIDpacket(); - if (packet != null) { - if (DEBUG) Log.d(TAG, "Writing to device()"); - int bufferDataLength = usbEndpointWrite.getMaxPacketSize(); - Assert.assertTrue(packet.capacity() <= bufferDataLength); + // Don't try and send data till previous request completes + if (writePending) + return; - writeRequest.queue(packet, bufferDataLength); - try - { - if (!writeRequest.equals(usbDeviceConnection.requestWait())) - Log.e(TAG, "writeRequest failed"); - } - catch (Exception ex) - { - } - } - } while (packet != null); + 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"); + } } /*********** Helper classes for telemetry streams ************/ From 75ce520503ca120f5dabf482333211b08032671e Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sun, 12 Aug 2012 14:12:35 -0500 Subject: [PATCH 11/32] Make sure the USB scheduling is thread safe. --- .../androidgcs/telemetry/HidUAVTalk.java | 110 +++++++++++------- 1 file changed, 65 insertions(+), 45 deletions(-) diff --git a/androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java b/androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java index 5abb4dd09..a074e79ca 100644 --- a/androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java +++ b/androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java @@ -312,7 +312,7 @@ public class HidUAVTalk extends TelemetryTask { public void run() { // Enqueue the first read - readData(); + queueRead(); while (!shutdown) { // If there are no request @@ -325,9 +325,8 @@ public class HidUAVTalk extends TelemetryTask { } else if (returned == writeRequest) { if (DEBUG) Log.d(TAG, "Received write completed request"); writePending = false; + sendData(); } - - sendData(); } } @@ -351,63 +350,79 @@ public class HidUAVTalk extends TelemetryTask { * it to the input stream */ ByteBuffer readBuffer = ByteBuffer.allocate(MAX_HID_PACKET_SIZE); - private void queueRead() { - if(!readRequest.queue(readBuffer, MAX_HID_PACKET_SIZE)) { - if (ERROR) Log.e(TAG, "Failed to queue request"); - } else - readPending = true; - } - public void readData() { - if (!readPending) { - queueRead(); - } else { // We just received a read + /** + * 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 + // 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()); + 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"); - } + 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(); - } + // 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; - // 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()"); - ByteBuffer packet = outTalkStream.getHIDpacket(); - if (packet != null) { - if (DEBUG) Log.d(TAG, "Writing to device()"); + int bufferDataLength = usbEndpointWrite.getMaxPacketSize(); + Assert.assertTrue(packet.capacity() <= bufferDataLength); - 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"); + if(writeRequest.queue(packet, bufferDataLength)) + writePending = true; + else if (ERROR) + Log.e(TAG, "Write queuing failed"); + } } } @@ -446,6 +461,9 @@ public class HidUAVTalk extends TelemetryTask { data.put((byte) oneByte); data.notify(); } + + // If there is not a write request queued, add one + sendData(); } @Override @@ -457,6 +475,8 @@ public class HidUAVTalk extends TelemetryTask { data.put(b); data.notify(); } + + sendData(); } }; From 335ccc54a3bbe5cca7014b6fe0ace0d6f3bfc05c Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sun, 12 Aug 2012 17:21:13 -0500 Subject: [PATCH 12/32] AndroidGCS Controller: Make sure to remove the callback on manual control settings before updating it. --- androidgcs/src/org/openpilot/androidgcs/Controller.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/androidgcs/src/org/openpilot/androidgcs/Controller.java b/androidgcs/src/org/openpilot/androidgcs/Controller.java index 3ef291d4d..0d3a3c911 100644 --- a/androidgcs/src/org/openpilot/androidgcs/Controller.java +++ b/androidgcs/src/org/openpilot/androidgcs/Controller.java @@ -78,11 +78,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(); } }; From 785cda805236bf7ab9cda098a62cf7f631125651 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sun, 12 Aug 2012 17:21:40 -0500 Subject: [PATCH 13/32] AndroidGCS: Add back a few synchronize blocks in telemetry to avoid removing elements twice. --- .../src/org/openpilot/uavtalk/Telemetry.java | 27 +++++++++++-------- 1 file changed, 16 insertions(+), 11 deletions(-) diff --git a/androidgcs/src/org/openpilot/uavtalk/Telemetry.java b/androidgcs/src/org/openpilot/uavtalk/Telemetry.java index 21525ea14..f130c33eb 100644 --- a/androidgcs/src/org/openpilot/uavtalk/Telemetry.java +++ b/androidgcs/src/org/openpilot/uavtalk/Telemetry.java @@ -551,17 +551,22 @@ public class Telemetry { // 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; + synchronized (objPriorityQueue) { + if ( !objPriorityQueue.isEmpty() ) + { + objInfo = objPriorityQueue.remove(); + } else { + synchronized (objQueue) { + if ( !objQueue.isEmpty() ) + { + objInfo = objQueue.remove(); + } + else + { + return; + } + } + } } // Check if a connection has been established, only process GCSTelemetryStats updates From 5e163861a45a75995c5fefa95c84f06224e77a50 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Mon, 13 Aug 2012 01:07:09 -0500 Subject: [PATCH 14/32] AndroidGCS HID: Go back to a read and write thread but now use synchronous bultTransfer for write which gets rid of the segfaults with running two asynchronous transfers. --- .../androidgcs/telemetry/HidUAVTalk.java | 98 ++++++++++++------- 1 file changed, 64 insertions(+), 34 deletions(-) diff --git a/androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java b/androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java index a074e79ca..9cb2d8be7 100644 --- a/androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java +++ b/androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java @@ -28,7 +28,7 @@ import android.util.Log; public class HidUAVTalk extends TelemetryTask { private static final String TAG = HidUAVTalk.class.getSimpleName(); - public static final int LOGLEVEL = 0; + 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; @@ -57,9 +57,10 @@ public class HidUAVTalk extends TelemetryTask { private UsbInterface usbInterface = null; private TalkInputStream inTalkStream; private TalkOutputStream outTalkStream; - private UsbRequest writeRequest = null; + private final UsbRequest writeRequest = null; private UsbRequest readRequest = null; - private Thread readWriteThread; + private Thread readThread; + private Thread writeThread; private boolean readPending = false; private boolean writePending = false; @@ -78,9 +79,15 @@ public class HidUAVTalk extends TelemetryTask { super.disconnect(); try { - if(readWriteThread != null) { - readWriteThread.join(); - readWriteThread = null; + 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(); @@ -91,13 +98,6 @@ public class HidUAVTalk extends TelemetryTask { readRequest.close(); readRequest = null; } - - if (writeRequest != null) { - writeRequest.cancel(); - writeRequest.close(); - writeRequest = null; - } - } @Override @@ -293,9 +293,6 @@ public class HidUAVTalk extends TelemetryTask { readRequest = new UsbRequest(); readRequest.initialize(usbDeviceConnection, usbEndpointRead); - writeRequest = new UsbRequest(); - writeRequest.initialize(usbDeviceConnection, usbEndpointWrite); - inTalkStream = new TalkInputStream(); outTalkStream = new TalkOutputStream(); inStream = inTalkStream; @@ -307,31 +304,39 @@ public class HidUAVTalk extends TelemetryTask { } }); - readWriteThread = new Thread(new Runnable() { + readThread = new Thread(new Runnable() { @Override public void run() { - // Enqueue the first read queueRead(); - while (!shutdown) { - // If there are no request - - // Enqueue requests appropriately first UsbRequest returned = usbDeviceConnection.requestWait(); if (returned == readRequest) { if (DEBUG) Log.d(TAG, "Received read request"); readData(); - } else if (returned == writeRequest) { - if (DEBUG) Log.d(TAG, "Received write completed request"); - writePending = false; - sendData(); - } + } else + Log.e(TAG, "Received unknown USB response"); } } - }, "HID Read Write"); - readWriteThread.start(); + }, "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 { + sendDataSynchronous(); + } catch (InterruptedException e) { + break; + } + } + if (DEBUG) Log.d(TAG, "Ending HID write thread"); + } + }, "HID Write"); + writeThread.start(); return true; } @@ -402,6 +407,7 @@ public class HidUAVTalk extends TelemetryTask { } } + /** * Send a packet if data is available */ @@ -426,6 +432,21 @@ public class HidUAVTalk extends TelemetryTask { } } + /** + * Send a packet if data is available + * @throws InterruptedException + */ + public void 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 bult write"); + } + } + /*********** Helper classes for telemetry streams ************/ class TalkOutputStream extends OutputStream { @@ -433,6 +454,20 @@ public class HidUAVTalk extends TelemetryTask { // 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) { @@ -461,9 +496,6 @@ public class HidUAVTalk extends TelemetryTask { data.put((byte) oneByte); data.notify(); } - - // If there is not a write request queued, add one - sendData(); } @Override @@ -475,8 +507,6 @@ public class HidUAVTalk extends TelemetryTask { data.put(b); data.notify(); } - - sendData(); } }; From 53d4d8b5079fef5f4142268de78ce46bbd8cb2ad Mon Sep 17 00:00:00 2001 From: James Cotton Date: Mon, 13 Aug 2012 01:25:08 -0500 Subject: [PATCH 15/32] AndroidGCS HID: Use the dettached message to shut down HID telemetry properly --- .../androidgcs/telemetry/HidUAVTalk.java | 31 +++++++++++++------ 1 file changed, 22 insertions(+), 9 deletions(-) diff --git a/androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java b/androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java index 9cb2d8be7..c1c848a0a 100644 --- a/androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java +++ b/androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java @@ -64,6 +64,7 @@ public class HidUAVTalk extends TelemetryTask { private boolean readPending = false; private boolean writePending = false; + private IntentFilter deviceAttachedFilter; public HidUAVTalk(OPTelemetryService service) { super(service); @@ -73,7 +74,7 @@ public class HidUAVTalk extends TelemetryTask { public void disconnect() { CleanUpAndClose(); - //hostDisplayActivity.unregisterReceiver(usbReceiver); + telemService.unregisterReceiver(usbReceiver); telemService.unregisterReceiver(usbPermissionReceiver); super.disconnect(); @@ -110,6 +111,11 @@ public class HidUAVTalk extends TelemetryTask { 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"); @@ -176,7 +182,6 @@ public class HidUAVTalk extends TelemetryTask { } }; - /* TODO: Detect dettached events and close the connection private final BroadcastReceiver usbReceiver = new BroadcastReceiver() { @Override @@ -195,8 +200,10 @@ public class HidUAVTalk extends TelemetryTask { { if (device.equals(currentDevice)) { + if (DEBUG) Log.d(TAG, "Matching device disconnected"); + // call your method that cleans up and closes communication with the device - CleanUpAndClose(); + disconnect(); } } } @@ -213,7 +220,7 @@ public class HidUAVTalk extends TelemetryTask { } } } - }; */ + }; protected void CleanUpAndClose() { @@ -314,8 +321,10 @@ public class HidUAVTalk extends TelemetryTask { if (returned == readRequest) { if (DEBUG) Log.d(TAG, "Received read request"); readData(); - } else + } else { Log.e(TAG, "Received unknown USB response"); + break; + } } } @@ -328,7 +337,8 @@ public class HidUAVTalk extends TelemetryTask { if (DEBUG) Log.d(TAG, "Starting HID write thread"); while(!shutdown) { try { - sendDataSynchronous(); + if (sendDataSynchronous() == false) + break; } catch (InterruptedException e) { break; } @@ -436,15 +446,18 @@ public class HidUAVTalk extends TelemetryTask { * Send a packet if data is available * @throws InterruptedException */ - public void sendDataSynchronous() 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 bult write"); + 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 ************/ From f96b419a85b34f5d04efc95a3314d04c6d1f3dd5 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Mon, 13 Aug 2012 01:37:15 -0500 Subject: [PATCH 16/32] AndroidGCS: Add a toast message when connected (all objects downloaded) --- .../openpilot/androidgcs/telemetry/HidUAVTalk.java | 6 ++++-- .../androidgcs/telemetry/TelemetryTask.java | 2 +- .../src/org/openpilot/uavtalk/TelemetryMonitor.java | 12 +++++++++++- 3 files changed, 16 insertions(+), 4 deletions(-) diff --git a/androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java b/androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java index c1c848a0a..ab170ff48 100644 --- a/androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java +++ b/androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java @@ -112,7 +112,7 @@ public class HidUAVTalk extends TelemetryTask { telemService.registerReceiver(usbPermissionReceiver, permissionFilter); deviceAttachedFilter = new IntentFilter(); - //deviceAttachedFilter.addAction(UsbManager.ACTION_USB_DEVICE_ATTACHED); + deviceAttachedFilter.addAction(UsbManager.ACTION_USB_DEVICE_ATTACHED); deviceAttachedFilter.addAction(UsbManager.ACTION_USB_DEVICE_DETACHED); telemService.registerReceiver(usbReceiver, deviceAttachedFilter); @@ -200,8 +200,8 @@ public class HidUAVTalk extends TelemetryTask { { 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(); } @@ -348,6 +348,8 @@ public class HidUAVTalk extends TelemetryTask { }, "HID Write"); writeThread.start(); + telemService.toastMessage("HID Device Opened"); + return true; } diff --git a/androidgcs/src/org/openpilot/androidgcs/telemetry/TelemetryTask.java b/androidgcs/src/org/openpilot/androidgcs/telemetry/TelemetryTask.java index 87196bc1e..f09cc7236 100644 --- a/androidgcs/src/org/openpilot/androidgcs/telemetry/TelemetryTask.java +++ b/androidgcs/src/org/openpilot/androidgcs/telemetry/TelemetryTask.java @@ -106,7 +106,7 @@ public abstract class TelemetryTask implements Runnable { // data stream uavTalk = new UAVTalk(inStream, outStream, objMngr); tel = new Telemetry(uavTalk, objMngr); - mon = new TelemetryMonitor(objMngr,tel); + mon = new TelemetryMonitor(objMngr,tel, telemService); // Create an observer to notify system of connection mon.addObserver(connectionObserver); 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(); From d5c1e3578e3dc02d3eaca79e67bb14fb677aca86 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Mon, 13 Aug 2012 02:00:54 -0500 Subject: [PATCH 17/32] AndroidGCS HID: Remove more locks to try and prevent HID deadlocking --- .../src/org/openpilot/uavtalk/Telemetry.java | 72 ++++++++++--------- 1 file changed, 40 insertions(+), 32 deletions(-) diff --git a/androidgcs/src/org/openpilot/uavtalk/Telemetry.java b/androidgcs/src/org/openpilot/uavtalk/Telemetry.java index f130c33eb..8e6161074 100644 --- a/androidgcs/src/org/openpilot/uavtalk/Telemetry.java +++ b/androidgcs/src/org/openpilot/uavtalk/Telemetry.java @@ -343,7 +343,7 @@ public class Telemetry { /** * Update an object based on its metadata properties */ - private synchronized void updateObject(UAVObject obj) + private void updateObject(UAVObject obj) { // Get metadata UAVObject.Metadata metadata = obj.getMetadata(); @@ -393,7 +393,7 @@ public class Telemetry { * Called when a transaction is successfully completed (uavtalk event) * @throws IOException */ - private synchronized void transactionCompleted(UAVObject obj, boolean result) throws IOException + private 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 @@ -401,8 +401,11 @@ public class Telemetry { { if (DEBUG) Log.d(TAG,"Telemetry: transaction completed for " + obj.getName()); // Complete transaction - transTimer.cancel(); - transPending = false; + + synchronized(transTimer) { + transTimer.cancel(); + transPending = false; + } //Send signal obj.transactionCompleted(result); @@ -419,39 +422,41 @@ public class Telemetry { * Called when a transaction is not completed within the timeout period (timer event) * @throws IOException */ - private synchronized void transactionTimeout() throws IOException + private 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()); + synchronized(transTimer) { + 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 - // 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; - } - } + // 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; + } + } + } } /** * Start an object transaction with UAVTalk, all information is stored in transInfo * @throws IOException */ - private synchronized void processObjectTransaction() throws IOException + private void processObjectTransaction() throws IOException { if (transPending) { @@ -472,8 +477,10 @@ public class Telemetry { } else { - transTimer.cancel(); - transPending = false; + synchronized(transTimer) { + transTimer.cancel(); + transPending = false; + } } } else { @@ -628,11 +635,12 @@ public class Telemetry { * TODO: Clean-up * @throws IOException */ - private synchronized void processPeriodicUpdates() throws IOException + private 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. @@ -705,7 +713,7 @@ public class Telemetry { return stats; } - public synchronized void resetStats() + public void resetStats() { utalk.resetStats(); txErrors = 0; From 319baa9e6f1c833559b0071e2bd072f6383e26fa Mon Sep 17 00:00:00 2001 From: James Cotton Date: Mon, 13 Aug 2012 15:02:15 -0500 Subject: [PATCH 18/32] AndroidGCS: Start moving the telemetry object queue to a handler and a looper --- .../telemetry/OPTelemetryService.java | 15 +- .../androidgcs/telemetry/TelemetryTask.java | 2 +- .../src/org/openpilot/uavtalk/Telemetry.java | 238 ++++++++++++------ .../uavtalk/TelemetryMonitorTest.java | 20 +- 4 files changed, 176 insertions(+), 99 deletions(-) diff --git a/androidgcs/src/org/openpilot/androidgcs/telemetry/OPTelemetryService.java b/androidgcs/src/org/openpilot/androidgcs/telemetry/OPTelemetryService.java index 4eeb57abe..4698900ef 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; @@ -127,7 +121,7 @@ public class OPTelemetryService extends Service { break; case 2: Toast.makeText(getApplicationContext(), "Attempting BT connection", Toast.LENGTH_SHORT).show(); - activeTelem = new BTTelemetryThread(); + //activeTelem = new BTTelemetryThread(); break; case 3: Toast.makeText(getApplicationContext(), "Attempting TCP connection", Toast.LENGTH_SHORT).show(); @@ -347,6 +341,8 @@ public class OPTelemetryService extends Service { } } } + + /* private class BTTelemetryThread extends Thread implements TelemTask { private final UAVObjectManager objMngr; @@ -401,7 +397,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); @@ -423,6 +419,5 @@ public class OPTelemetryService extends Service { } if (DEBUG) Log.d(TAG, "UAVTalk stream disconnected"); } - - }; + };*/ } diff --git a/androidgcs/src/org/openpilot/androidgcs/telemetry/TelemetryTask.java b/androidgcs/src/org/openpilot/androidgcs/telemetry/TelemetryTask.java index f09cc7236..3d16d94f9 100644 --- a/androidgcs/src/org/openpilot/androidgcs/telemetry/TelemetryTask.java +++ b/androidgcs/src/org/openpilot/androidgcs/telemetry/TelemetryTask.java @@ -105,7 +105,7 @@ public abstract class TelemetryTask implements Runnable { // Create the required telemetry objects attached to this // data stream uavTalk = new UAVTalk(inStream, outStream, objMngr); - tel = new Telemetry(uavTalk, objMngr); + tel = new Telemetry(uavTalk, objMngr, Looper.myLooper()); mon = new TelemetryMonitor(objMngr,tel, telemService); // Create an observer to notify system of connection diff --git a/androidgcs/src/org/openpilot/uavtalk/Telemetry.java b/androidgcs/src/org/openpilot/uavtalk/Telemetry.java index 8e6161074..ac814e76b 100644 --- a/androidgcs/src/org/openpilot/uavtalk/Telemetry.java +++ b/androidgcs/src/org/openpilot/uavtalk/Telemetry.java @@ -35,9 +35,17 @@ 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; @@ -98,11 +106,14 @@ public class Telemetry { /** * Constructor */ - public Telemetry(UAVTalk utalkIn, UAVObjectManager objMngr) + 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 > objs = objMngr.getObjects(); ListIterator> li = objs.listIterator(); @@ -265,48 +276,28 @@ public class Telemetry { 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); } }; @@ -488,59 +479,6 @@ public class Telemetry { } } - /** - * 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 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 - if (!transPending) - { - processObjectQueue(); - } - } - /** * Process events from the object queue * @throws IOException @@ -666,7 +604,8 @@ public class Telemetry { objinfo.timeToNextUpdateMs = objinfo.updatePeriodMs - offset; // Send object startTime = System.currentTimeMillis(); - enqueueObjectUpdates(objinfo.obj, EV_UPDATED_MANUAL, true, false); + 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; @@ -780,6 +719,147 @@ public class Telemetry { private static final int MIN_UPDATE_PERIOD_MS = 1; private static final int MAX_QUEUE_SIZE = 20; + private final ObjectUpdateHandler handler; + public class ObjectUpdateHandler extends Handler { + //! This can only be created while attaching to a particular looper + ObjectUpdateHandler(Looper l) { + super(l); + } + + //! 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; + + post(new ObjectRunnable(objInfo)); + } + + //! 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_UPDATE_REQ, false, true); + } + + //! Enqueue an update requested event + void updateRequested(UAVObject obj) { + enqueueObjectUpdates(obj, EV_UPDATE_REQ, false, true); + } + + } + + class ObjectRunnable implements Runnable { + + //! Transaction information to perform + private final ObjectQueueInfo objInfo; +// private final ObjectTransactionInfo transInfo = new ObjectTransactionInfo(); + + ObjectRunnable(ObjectQueueInfo info) { + Assert.assertNotNull(info); + objInfo = info; + } + + //! Perform the transaction on the looper thread + @Override + public void run () { + Log.d(TAG,"object transaction running"); + // 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; + } + } + + Log.e(TAG, "A"); + // 2. Setup transaction (skip if unpack event) + if ( objInfo.event != EV_UNPACKED ) + { + Log.e(TAG, "A1"); + 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; + } + Log.e(TAG, "B"); + // 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() ); + } + Log.e(TAG, "C"); + // 3. Execute transaction + if (transPending) + { + Log.e(TAG, "D"); + try { + if (DEBUG || true) Log.d(TAG, "Process Object transaction for " + transInfo.obj.getName()); + // Initiate transaction + if (transInfo.objRequest) + { + utalk.sendObjectRequest(transInfo.obj, transInfo.allInstances); + } + else + { + Log.d(TAG, "Sending object"); + utalk.sendObject(transInfo.obj, transInfo.acked, transInfo.allInstances); + } + + // TODO: Block if request expected (??) + if ( transInfo.objRequest || transInfo.acked ) + { + transTimerSetPeriod(REQ_TIMEOUT_MS); + } + else + { + synchronized(transTimer) { + transTimer.cancel(); + transPending = false; + } + } + } catch (IOException e) { + // TODO Auto-generated catch block + Log.e(TAG, "E"); + e.printStackTrace(); + } + } + } + } } 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"); } From a78fd852b166362b57d4011c9f79d7ea70a9a475 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Mon, 13 Aug 2012 23:47:37 -0500 Subject: [PATCH 19/32] AndroidGCS Telemetry: Finish moving telemetry into a runnable. --- .../src/org/openpilot/uavtalk/Telemetry.java | 1357 +++++++++-------- 1 file changed, 679 insertions(+), 678 deletions(-) diff --git a/androidgcs/src/org/openpilot/uavtalk/Telemetry.java b/androidgcs/src/org/openpilot/uavtalk/Telemetry.java index ac814e76b..3e4007777 100644 --- a/androidgcs/src/org/openpilot/uavtalk/Telemetry.java +++ b/androidgcs/src/org/openpilot/uavtalk/Telemetry.java @@ -42,147 +42,158 @@ 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. + * 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 > 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, 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 > 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 { + try { transactionCompleted(data, true); } catch (IOException e) { // Disconnect when stream fails utalk.setOnTransactionCompletedListener(null); } - } + } + @Override void TransactionFailed(UAVObject data) { - try { - if (DEBUG) Log.d(TAG, "TransactionFailed(" + data.getName() + ")"); + try { + 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"); + // 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; - } + // 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(); + synchronized void transTimerSetPeriod(int periodMs) { + if (transTimerTask != null) + transTimerTask.cancel(); - if(transTimer != null) - transTimer.purge(); + if (transTimer != null) + transTimer.purge(); - transTimer = new Timer(); + transTimer = new Timer(); - transTimerTask = new TimerTask() { + transTimerTask = new TimerTask() { @Override public void run() { try { @@ -191,21 +202,21 @@ public class Telemetry { cancel(); } } - }; - transTimer.schedule(transTimerTask, periodMs, periodMs); - } + }; + 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() { + 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 { @@ -215,651 +226,641 @@ 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) { handler.unpacked((UAVObject) data); - } + } }; final Observer updatedAutoObserver = new Observer() { @Override public void update(Observable observable, Object data) { handler.updatedAuto((UAVObject) data); - } + } }; final Observer updatedManualObserver = new Observer() { @Override public void update(Observable observable, Object data) { handler.updatedManual((UAVObject) data); - } + } }; final Observer updatedRequestedObserver = new Observer() { @Override public void update(Observable observable, Object data) { 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 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 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 + /** + * Called when a transaction is successfully completed (uavtalk event) + * + * @throws IOException + */ + private 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()); - synchronized(transTimer) { - transTimer.cancel(); - transPending = false; - } + // Complete transaction + synchronized(transTimer) { + transTimer.cancel(); + transPending = false; + } - //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; - } - } + //Send signal + obj.transactionCompleted(result); + } else { + if (ERROR) + Log.e(TAG, + "Error: received a transaction completed when did not expect it."); + transPending = false; + } + } - /** - * Called when a transaction is not completed within the timeout period (timer event) - * @throws IOException - */ - private void transactionTimeout() throws IOException - { - if (DEBUG) Log.d(TAG,"Telemetry: transaction timeout."); - synchronized(transTimer) { - 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()); + /** + * Called when a transaction is not completed within the timeout period + * (timer event) + * + * @throws IOException + */ + private void transactionTimeout() throws IOException { + if (DEBUG) + Log.d(TAG, "Telemetry: transaction timeout."); + synchronized (transTimer) { + 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 - // 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; - } - } - } - } + // 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; + } + } + } + } - /** - * Start an object transaction with UAVTalk, all information is stored in transInfo - * @throws IOException - */ - private 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 - { - synchronized(transTimer) { - transTimer.cancel(); - transPending = false; - } - } - } else - { - if (ERROR) Log.e(TAG,"Error: inside of processObjectTransaction with no transPending"); - } - } + /** + * Start an object transaction with UAVTalk, all information is stored in + * transInfo + * + * @throws IOException + */ + private 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 { + synchronized (transTimer) { + transTimer.cancel(); + transPending = false; + } + } + } else { + if (ERROR) + Log.e(TAG, + "Error: inside of processObjectTransaction with no transPending"); + } + } - /** - * Process events from the object queue - * @throws IOException - */ - private void processObjectQueue() throws IOException - { - if (DEBUG) Log.d(TAG, "Process object queue - Depth " + objQueue.size() + " priority " + objPriorityQueue.size()); + /** + * Process events from the object queue + * + * @throws IOException + */ + private 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) - if (transPending) - { - if (WARN) Log.e(TAG,"Dequeue while a transaction pending"); - return; - } + // 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; + } - // Get object information from queue (first the priority and then the regular queue) - ObjectQueueInfo objInfo; - synchronized (objPriorityQueue) { - if ( !objPriorityQueue.isEmpty() ) - { - objInfo = objPriorityQueue.remove(); - } else { - synchronized (objQueue) { - if ( !objQueue.isEmpty() ) - { - objInfo = objQueue.remove(); - } - else - { - return; - } - } - } - } + // Get object information from queue (first the priority and then the + // regular queue) + ObjectQueueInfo objInfo; + synchronized (objPriorityQueue) { + if (!objPriorityQueue.isEmpty()) { + objInfo = objPriorityQueue.remove(); + } else { + synchronized (objQueue) { + if (!objQueue.isEmpty()) { + objInfo = objQueue.remove(); + } else { + return; + } + } + } + } - // 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; - } - } + // 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; + } + } - // 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()); - } + // 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()); + } - // If this is a metaobject then make necessary telemetry updates - if (objInfo.obj.isMetadata()) - { - UAVMetaObject metaobj = (UAVMetaObject) objInfo.obj; - updateObject( metaobj.getParentObject() ); - } + // If this is a metaobject then make necessary telemetry updates + if (objInfo.obj.isMetadata()) { + UAVMetaObject metaobj = (UAVMetaObject) objInfo.obj; + updateObject(metaobj.getParentObject()); + } - // 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(); + // 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(); - } + } - /** - * Check is any objects are pending for periodic updates - * TODO: Clean-up - * @throws IOException - */ - private void processPeriodicUpdates() throws IOException - { + /** + * Check is any objects are pending for periodic updates TODO: Clean-up + * + * @throws IOException + */ + private void processPeriodicUpdates() throws IOException { - if (DEBUG) Log.d(TAG, "processPeriodicUpdates()"); - // Stop timer + if (DEBUG) + Log.d(TAG, "processPeriodicUpdates()"); + // Stop timer - updateTimer.cancel(); + 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(); - 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; - } - } - } + // 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(); + 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; + } + } + } - // Check if delay for the next update is too short - if (minDelay < MIN_UPDATE_PERIOD_MS) - { - minDelay = MIN_UPDATE_PERIOD_MS; - } + // Check if delay for the next update is too short + if (minDelay < MIN_UPDATE_PERIOD_MS) { + minDelay = MIN_UPDATE_PERIOD_MS; + } - // Done - timeToNextUpdateMs = minDelay; + // Done + timeToNextUpdateMs = minDelay; - // Restart timer - updateTimerSetPeriod(timeToNextUpdateMs); - } + // Restart timer + updateTimerSetPeriod(timeToNextUpdateMs); + } - public TelemetryStats getStats() - { - // Get UAVTalk stats - UAVTalk.ComStats utalkStats = utalk.getStats(); + 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; + // 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; - } + // Done + return stats; + } - public void resetStats() - { - utalk.resetStats(); - txErrors = 0; - txRetries = 0; - } + public void resetStats() { + utalk.resetStats(); + txErrors = 0; + txRetries = 0; + } + private void newObject(UAVObject obj) { + registerObject(obj); + } - private void newObject(UAVObject obj) - { - registerObject(obj); - } + private synchronized void newInstance(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; + if (transTimerTask != null) + transTimerTask.cancel(); + transTimerTask = null; + if (transTimer != null) + transTimer.cancel(); + transTimer = 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 ConcurrentLinkedQueue(); - private final Queue objPriorityQueue = new ConcurrentLinkedQueue(); - 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 final Queue objQueue = new ConcurrentLinkedQueue(); + private final Queue objPriorityQueue = new ConcurrentLinkedQueue(); + private final 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 Timer transTimer; + private TimerTask transTimerTask; - 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; + private static final int MAX_QUEUE_SIZE = 20; - private final ObjectUpdateHandler handler; + private final ObjectUpdateHandler handler; - public class ObjectUpdateHandler extends Handler { + public class ObjectUpdateHandler extends Handler { - //! This can only be created while attaching to a particular looper - ObjectUpdateHandler(Looper l) { - super(l); - } - - //! 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; - - post(new ObjectRunnable(objInfo)); - } - - //! Enqueue an unpacked event - void unpacked(UAVObject obj) { - enqueueObjectUpdates(obj, EV_UNPACKED, false, true); + // ! This can only be created while attaching to a particular looper + ObjectUpdateHandler(Looper l) { + super(l); } - //! Enqueue an updated auto event - void updatedAuto(UAVObject obj) { - enqueueObjectUpdates(obj,EV_UPDATED, false, true); - } + // ! Generic enqueue + void enqueueObjectUpdates(UAVObject obj, int event, + boolean allInstances, boolean priority) { - //! Enqueue an updated manual event - void updatedManual(UAVObject obj) { - enqueueObjectUpdates(obj, EV_UPDATE_REQ, false, true); - } + if (DEBUG) + Log.d(TAG, "Enqueing update " + obj.getName() + " event " + + event); - //! Enqueue an update requested event - void updateRequested(UAVObject obj) { - enqueueObjectUpdates(obj, EV_UPDATE_REQ, false, true); - } + ObjectQueueInfo objInfo = new ObjectQueueInfo(); + objInfo.obj = obj; + objInfo.event = event; + objInfo.allInstances = allInstances; - } + post(new ObjectRunnable(objInfo)); + } - class ObjectRunnable implements Runnable { + // ! Enqueue an unpacked event + void unpacked(UAVObject obj) { + enqueueObjectUpdates(obj, EV_UNPACKED, false, true); + } - //! Transaction information to perform - private final ObjectQueueInfo objInfo; -// private final ObjectTransactionInfo transInfo = new ObjectTransactionInfo(); + // ! Enqueue an updated auto event + void updatedAuto(UAVObject obj) { + enqueueObjectUpdates(obj, EV_UPDATED, false, true); + } - ObjectRunnable(ObjectQueueInfo info) { - Assert.assertNotNull(info); - objInfo = info; - } + // ! Enqueue an updated manual event + void updatedManual(UAVObject obj) { + enqueueObjectUpdates(obj, EV_UPDATED_MANUAL, false, true); + } - //! Perform the transaction on the looper thread - @Override - public void run () { - Log.d(TAG,"object transaction running"); - // 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 + // ! Enqueue an update requested event + void updateRequested(UAVObject obj) { + enqueueObjectUpdates(obj, EV_UPDATE_REQ, false, true); + } - // 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; - } - } + } - Log.e(TAG, "A"); - // 2. Setup transaction (skip if unpack event) - if ( objInfo.event != EV_UNPACKED ) - { - Log.e(TAG, "A1"); - 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; - } - Log.e(TAG, "B"); - // 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() ); - } - Log.e(TAG, "C"); - // 3. Execute transaction - if (transPending) - { - Log.e(TAG, "D"); - try { - if (DEBUG || true) Log.d(TAG, "Process Object transaction for " + transInfo.obj.getName()); - // Initiate transaction - if (transInfo.objRequest) - { - utalk.sendObjectRequest(transInfo.obj, transInfo.allInstances); - } - else - { - Log.d(TAG, "Sending object"); - utalk.sendObject(transInfo.obj, transInfo.acked, transInfo.allInstances); - } + class ObjectRunnable implements Runnable { - // TODO: Block if request expected (??) - if ( transInfo.objRequest || transInfo.acked ) - { - transTimerSetPeriod(REQ_TIMEOUT_MS); - } - else - { - synchronized(transTimer) { - transTimer.cancel(); - transPending = false; - } - } - } catch (IOException e) { - // TODO Auto-generated catch block - Log.e(TAG, "E"); - e.printStackTrace(); - } - } - } - } + // ! Transaction information to perform + private final ObjectQueueInfo objInfo; + + // private final ObjectTransactionInfo transInfo = new + // ObjectTransactionInfo(); + + 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; + } + } + + // 2. 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; + } + + // 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()); + } + + // 3. Execute transaction + if (transPending) { + try { + if (DEBUG) Log.d(TAG, "Process Object transaction for " + transInfo.obj.getName()); + + // Initiate transaction + if (transInfo.objRequest) { + if (DEBUG) Log.d(TAG, "Sending object request"); + utalk.sendObjectRequest(transInfo.obj, transInfo.allInstances); + } else { + if (DEBUG) Log.d(TAG, "Sending object " + transInfo.obj.getName() + " " + transInfo.obj.toStringData()); + utalk.sendObject(transInfo.obj, transInfo.acked, transInfo.allInstances); + } + + + // TODO: Block if request expected (??) + if (transInfo.objRequest || transInfo.acked ) { + transTimerSetPeriod(REQ_TIMEOUT_MS); + } else { + synchronized(transTimer) { + transTimer.cancel(); + transPending = false; + } + } + + } catch (IOException e) { + // TODO Auto-generated catch block + Log.e(TAG, "E"); + e.printStackTrace(); + } + } + } + } } From 9f326f28d8cac9195514ef1dd7d43af8c1958aa7 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Wed, 15 Aug 2012 00:01:14 -0500 Subject: [PATCH 20/32] AndroidGCS: Handler based telemetry. Now reschedule transactions if one is pending. --- .../src/org/openpilot/uavtalk/Telemetry.java | 238 ++++++------------ 1 file changed, 72 insertions(+), 166 deletions(-) diff --git a/androidgcs/src/org/openpilot/uavtalk/Telemetry.java b/androidgcs/src/org/openpilot/uavtalk/Telemetry.java index 3e4007777..e514e61c4 100644 --- a/androidgcs/src/org/openpilot/uavtalk/Telemetry.java +++ b/androidgcs/src/org/openpilot/uavtalk/Telemetry.java @@ -30,10 +30,8 @@ import java.util.List; import java.util.ListIterator; import java.util.Observable; 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; @@ -49,8 +47,8 @@ public class Telemetry { private final String TAG = "Telemetry"; public static int LOGLEVEL = 0; - public static boolean WARN = LOGLEVEL > 2; - public static boolean DEBUG = LOGLEVEL > 1; + public static boolean DEBUG = LOGLEVEL > 2; + public static boolean WARN = LOGLEVEL > 1; public static boolean ERROR = LOGLEVEL > 0; public class TelemetryStats { @@ -391,8 +389,8 @@ public class Telemetry { */ private void transactionCompleted(UAVObject obj, boolean result) throws IOException { - if (DEBUG) - Log.d(TAG, "UAVTalk transactionCompleted"); + 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()); @@ -406,9 +404,7 @@ public class Telemetry { //Send signal obj.transactionCompleted(result); } else { - if (ERROR) - Log.e(TAG, - "Error: received a transaction completed when did not expect it."); + if (ERROR) Log.e(TAG, "Error: received a transaction completed when did not expect it."); transPending = false; } } @@ -433,16 +429,11 @@ public class Telemetry { ++txRetries; } else { if (ERROR) - Log.e(TAG, - "Transaction failed for: " - + transInfo.obj.getName()); + 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 + // 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; @@ -460,9 +451,8 @@ public class Telemetry { private void processObjectTransaction() throws IOException { if (transPending) { if (DEBUG) - Log.d(TAG, - "Process Object transaction for " - + transInfo.obj.getName()); + Log.d(TAG, "Process Object transaction for " + transInfo.obj.getName()); + // Initiate transaction if (transInfo.objRequest) { utalk.sendObjectRequest(transInfo.obj, transInfo.allInstances); @@ -470,6 +460,7 @@ public class Telemetry { utalk.sendObject(transInfo.obj, transInfo.acked, transInfo.allInstances); } + // Start timer if a response is expected if (transInfo.objRequest || transInfo.acked) { transTimerSetPeriod(REQ_TIMEOUT_MS); @@ -481,99 +472,10 @@ public class Telemetry { } } else { if (ERROR) - Log.e(TAG, - "Error: inside of processObjectTransaction with no transPending"); + Log.e(TAG, "Error: inside of processObjectTransaction with no transPending"); } } - /** - * Process events from the object queue - * - * @throws IOException - */ - private 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) - if (transPending) { - if (WARN) - Log.e(TAG, "Dequeue while a transaction pending"); - return; - } - - // Get object information from queue (first the priority and then the - // regular queue) - ObjectQueueInfo objInfo; - synchronized (objPriorityQueue) { - if (!objPriorityQueue.isEmpty()) { - objInfo = objPriorityQueue.remove(); - } else { - synchronized (objQueue) { - if (!objQueue.isEmpty()) { - objInfo = objQueue.remove(); - } else { - return; - } - } - } - } - - // 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; - } - } - - // 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()); - } - - // If this is a metaobject then make necessary telemetry updates - if (objInfo.obj.isMetadata()) { - UAVMetaObject metaobj = (UAVMetaObject) objInfo.obj; - updateObject(metaobj.getParentObject()); - } - - // 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(); - - } - /** * Check is any objects are pending for periodic updates TODO: Clean-up * @@ -697,9 +599,7 @@ public class Telemetry { private final UAVTalk utalk; private UAVObject gcsStatsObj; private final List objList = new ArrayList(); - private final Queue objQueue = new ConcurrentLinkedQueue(); - private final Queue objPriorityQueue = new ConcurrentLinkedQueue(); - private final ObjectTransactionInfo transInfo = new ObjectTransactionInfo(); + private ObjectTransactionInfo transInfo = new ObjectTransactionInfo(); private boolean transPending; private Timer updateTimer; @@ -718,7 +618,6 @@ public class Telemetry { 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 final ObjectUpdateHandler handler; @@ -733,9 +632,7 @@ public class Telemetry { void enqueueObjectUpdates(UAVObject obj, int event, boolean allInstances, boolean priority) { - if (DEBUG) - Log.d(TAG, "Enqueing update " + obj.getName() + " event " - + event); + if (DEBUG) Log.d(TAG, "Enqueing update " + obj.getName() + " event " + event); ObjectQueueInfo objInfo = new ObjectQueueInfo(); objInfo.obj = obj; @@ -794,35 +691,15 @@ public class Telemetry { // 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 (((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."); + Log.d(TAG, "transactionCompleted(false) due to receiving object not GCSTelemetryStats while not connected."); objInfo.obj.transactionCompleted(false); return; } } - // 2. 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; - } - // If this is a metaobject then make necessary telemetry updates // (this is why we catch unpack) if (objInfo.obj.isMetadata()) { @@ -830,35 +707,64 @@ public class Telemetry { updateObject(metaobj.getParentObject()); } - // 3. Execute transaction - if (transPending) { - try { + // 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); + + // 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()); + handler.postDelayed(this, 100); + return; + } + + synchronized (transInfo) { + transPending = newTransactionPending; + transInfo = newTrans; + if (DEBUG) Log.d(TAG, "Process Object transaction for " + transInfo.obj.getName()); - // Initiate transaction - if (transInfo.objRequest) { - if (DEBUG) Log.d(TAG, "Sending object request"); - utalk.sendObjectRequest(transInfo.obj, transInfo.allInstances); - } else { - if (DEBUG) Log.d(TAG, "Sending object " + transInfo.obj.getName() + " " + transInfo.obj.toStringData()); - utalk.sendObject(transInfo.obj, transInfo.acked, transInfo.allInstances); + // 3. Execute transaction + try { + + // Initiate transaction + if (transInfo.objRequest) { + if (DEBUG) Log.d(TAG, "Sending object request"); + utalk.sendObjectRequest(transInfo.obj, transInfo.allInstances); + } else { + if (DEBUG) Log.d(TAG, "Sending object " + transInfo.obj.getName() + " " + transInfo.obj.toStringData()); + utalk.sendObject(transInfo.obj, transInfo.acked, transInfo.allInstances); + } + + + // TODO: Block if request expected (??) + if (transPending) { + transTimerSetPeriod(REQ_TIMEOUT_MS); + } else if (transTimer != null) { + synchronized(transTimer) { + transTimer.cancel(); + transPending = false; + } + } + } catch (IOException e) { + // TODO Auto-generated catch block + Log.e(TAG, "E"); + e.printStackTrace(); } - - - // TODO: Block if request expected (??) - if (transInfo.objRequest || transInfo.acked ) { - transTimerSetPeriod(REQ_TIMEOUT_MS); - } else { - synchronized(transTimer) { - transTimer.cancel(); - transPending = false; - } - } - - } catch (IOException e) { - // TODO Auto-generated catch block - Log.e(TAG, "E"); - e.printStackTrace(); } } } From b614368359ee2a8583463f4c5e7e5c173e74e65c Mon Sep 17 00:00:00 2001 From: James Cotton Date: Wed, 15 Aug 2012 01:14:57 -0500 Subject: [PATCH 21/32] AndroidGCS Telemetry: Use a runnable for the transaction timeout. Now all of telemetry is using handlers nicely, but we still can have multiple transactions queued for the same object. --- .../src/org/openpilot/uavtalk/Telemetry.java | 286 ++++++++---------- 1 file changed, 118 insertions(+), 168 deletions(-) diff --git a/androidgcs/src/org/openpilot/uavtalk/Telemetry.java b/androidgcs/src/org/openpilot/uavtalk/Telemetry.java index e514e61c4..10885f746 100644 --- a/androidgcs/src/org/openpilot/uavtalk/Telemetry.java +++ b/androidgcs/src/org/openpilot/uavtalk/Telemetry.java @@ -126,7 +126,7 @@ public class Telemetry { ListIterator> li = objs.listIterator(); while (li.hasNext()) registerObject(li.next().get(0)); // we only need to register one - // instance per object type + // instance per object type // Listen to new object creations objMngr.addNewInstanceObserver(new Observer() { @@ -146,25 +146,15 @@ public class Telemetry { 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); - } + transactionCompleted(data, false); } }); @@ -182,28 +172,6 @@ public class Telemetry { 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(); @@ -354,7 +322,7 @@ public class Telemetry { eventMask = EV_UPDATED_MANUAL | EV_UPDATE_REQ; if (obj.isMetadata()) eventMask |= EV_UNPACKED; // we also need to act on remote - // updates (unpack events) + // updates (unpack events) connectToObjectInstances(obj, eventMask); } else if (metadata.GetGcsTelemetryUpdateMode() == UAVObject.UpdateMode.UPDATEMODE_ONCHANGE) { @@ -364,7 +332,7 @@ public class Telemetry { 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) + // updates (unpack events) connectToObjectInstances(obj, eventMask); } else if (metadata.GetGcsTelemetryUpdateMode() == UAVObject.UpdateMode.UPDATEMODE_THROTTLED) { @@ -376,106 +344,12 @@ public class Telemetry { eventMask = EV_UPDATED_MANUAL | EV_UPDATE_REQ; if (obj.isMetadata()) eventMask |= EV_UNPACKED; // we also need to act on remote - // updates (unpack events) + // updates (unpack events) connectToObjectInstances(obj, eventMask); } } - /** - * Called when a transaction is successfully completed (uavtalk event) - * - * @throws IOException - */ - private 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 - synchronized(transTimer) { - transTimer.cancel(); - 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; - } - } - - /** - * Called when a transaction is not completed within the timeout period - * (timer event) - * - * @throws IOException - */ - private void transactionTimeout() throws IOException { - if (DEBUG) - Log.d(TAG, "Telemetry: transaction timeout."); - synchronized (transTimer) { - 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 - // 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; - } - } - } - } - - /** - * Start an object transaction with UAVTalk, all information is stored in - * transInfo - * - * @throws IOException - */ - private 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 { - synchronized (transTimer) { - transTimer.cancel(); - transPending = false; - } - } - } else { - if (ERROR) - Log.e(TAG, "Error: inside of processObjectTransaction with no transPending"); - } - } - /** * Check is any objects are pending for periodic updates TODO: Clean-up * @@ -513,6 +387,8 @@ public class Telemetry { - offset; // Send object startTime = System.currentTimeMillis(); + + if (DEBUG) Log.d(TAG, "Manual update: " + objinfo.obj.getName()); handler.updatedManual(objinfo.obj); // enqueueObjectUpdates(objinfo.obj, EV_UPDATED_MANUAL, // true, false); @@ -584,12 +460,6 @@ public class Telemetry { if (updateTimer != null) updateTimer.cancel(); updateTimer = null; - if (transTimerTask != null) - transTimerTask.cancel(); - transTimerTask = null; - if (transTimer != null) - transTimer.cancel(); - transTimer = null; } /** @@ -604,8 +474,6 @@ public class Telemetry { private Timer updateTimer; private TimerTask updateTimerTask; - private Timer transTimer; - private TimerTask transTimerTask; private int timeToNextUpdateMs; private int txErrors; @@ -664,14 +532,15 @@ public class Telemetry { } + /** + * 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; - // private final ObjectTransactionInfo transInfo = new - // ObjectTransactionInfo(); - ObjectRunnable(ObjectQueueInfo info) { Assert.assertNotNull(info); objInfo = info; @@ -725,48 +594,129 @@ public class Telemetry { // Determine if this will schedule a new transaction newTransactionPending = !(newTrans.objRequest || newTrans.acked); - // 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()); - handler.postDelayed(this, 100); - return; - } - 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()); + 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()); - // 3. Execute transaction try { - // Initiate transaction + // 3. Execute transaction by sending the appropriate UAVTalk command if (transInfo.objRequest) { - if (DEBUG) Log.d(TAG, "Sending object request"); + 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() + " " + transInfo.obj.toStringData()); + if (DEBUG) Log.d(TAG, "Sending object " + transInfo.obj.getName()); utalk.sendObject(transInfo.obj, transInfo.acked, transInfo.allInstances); } - - // TODO: Block if request expected (??) - if (transPending) { - transTimerSetPeriod(REQ_TIMEOUT_MS); - } else if (transTimer != null) { - synchronized(transTimer) { - transTimer.cancel(); - transPending = false; - } - } } catch (IOException e) { - // TODO Auto-generated catch block - Log.e(TAG, "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; + } + } + } + } + From caff64ed7e75783f066f81d175b37df45155d36a Mon Sep 17 00:00:00 2001 From: James Cotton Date: Wed, 15 Aug 2012 02:02:05 -0500 Subject: [PATCH 22/32] AndroidGCS Telemetry: Fix the determination of whether a transaction is pending --- .../src/org/openpilot/uavtalk/Telemetry.java | 42 ++++++++++++++++--- 1 file changed, 37 insertions(+), 5 deletions(-) diff --git a/androidgcs/src/org/openpilot/uavtalk/Telemetry.java b/androidgcs/src/org/openpilot/uavtalk/Telemetry.java index 10885f746..6fcbb2491 100644 --- a/androidgcs/src/org/openpilot/uavtalk/Telemetry.java +++ b/androidgcs/src/org/openpilot/uavtalk/Telemetry.java @@ -30,8 +30,10 @@ import java.util.List; import java.util.ListIterator; import java.util.Observable; 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; @@ -487,8 +489,14 @@ public class Telemetry { private static final int MAX_UPDATE_PERIOD_MS = 1000; private static final int MIN_UPDATE_PERIOD_MS = 1; - private final ObjectUpdateHandler handler; + 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 @@ -496,6 +504,8 @@ public class Telemetry { super(l); } + Queue objQueue = new ConcurrentLinkedQueue(); + // ! Generic enqueue void enqueueObjectUpdates(UAVObject obj, int event, boolean allInstances, boolean priority) { @@ -507,7 +517,26 @@ public class Telemetry { objInfo.event = event; objInfo.allInstances = allInstances; - post(new ObjectRunnable(objInfo)); + // 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 @@ -592,13 +621,13 @@ public class Telemetry { } // Determine if this will schedule a new transaction - newTransactionPending = !(newTrans.objRequest || newTrans.acked); + 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()); + if (WARN) Log.w(TAG, "Postponing transaction for" + newTrans.obj.getName() + " existing transaction for " + transInfo.obj.getName()); handler.postDelayed(this, 100); return; } @@ -609,11 +638,14 @@ public class Telemetry { 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()); + 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()); From 34b21bec3ca41951aab017bff788e003e39e4f28 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Mon, 20 Aug 2012 22:56:21 -0500 Subject: [PATCH 23/32] AndroidGCS: OSG import basic OSG code --- androidgcs/AndroidManifest.xml | 3 +- androidgcs/jni/Android.mk | 67 +++ androidgcs/jni/Application.mk | 11 + androidgcs/jni/OsgAndroidNotifyHandler.cpp | 38 ++ androidgcs/jni/OsgAndroidNotifyHandler.hpp | 26 ++ androidgcs/jni/OsgMainApp.cpp | 217 +++++++++ androidgcs/jni/OsgMainApp.hpp | 182 ++++++++ androidgcs/jni/osgNativeLib.cpp | 113 +++++ .../org/openpilot/osg/ColorPickerDialog.java | 251 +++++++++++ androidgcs/src/org/openpilot/osg/EGLview.java | 333 ++++++++++++++ .../src/org/openpilot/osg/osgNativeLib.java | 28 ++ .../src/org/openpilot/osg/osgViewer.java | 414 ++++++++++++++++++ 12 files changed, 1682 insertions(+), 1 deletion(-) create mode 100644 androidgcs/jni/Android.mk create mode 100644 androidgcs/jni/Application.mk create mode 100644 androidgcs/jni/OsgAndroidNotifyHandler.cpp create mode 100644 androidgcs/jni/OsgAndroidNotifyHandler.hpp create mode 100644 androidgcs/jni/OsgMainApp.cpp create mode 100644 androidgcs/jni/OsgMainApp.hpp create mode 100644 androidgcs/jni/osgNativeLib.cpp create mode 100644 androidgcs/src/org/openpilot/osg/ColorPickerDialog.java create mode 100644 androidgcs/src/org/openpilot/osg/EGLview.java create mode 100644 androidgcs/src/org/openpilot/osg/osgNativeLib.java create mode 100644 androidgcs/src/org/openpilot/osg/osgViewer.java diff --git a/androidgcs/AndroidManifest.xml b/androidgcs/AndroidManifest.xml index 3dc9bc3e3..ad63dc791 100644 --- a/androidgcs/AndroidManifest.xml +++ b/androidgcs/AndroidManifest.xml @@ -3,6 +3,7 @@ package="org.openpilot.androidgcs" android:versionCode="1" android:versionName="1.0"> + @@ -57,4 +58,4 @@ - \ No newline at end of file + diff --git a/androidgcs/jni/Android.mk b/androidgcs/jni/Android.mk new file mode 100644 index 000000000..2fbd46e5f --- /dev/null +++ b/androidgcs/jni/Android.mk @@ -0,0 +1,67 @@ +LOCAL_PATH := $(call my-dir) + +include $(CLEAR_VARS) + +LOCAL_MODULE := osgNativeLib +### Main Install dir +OSG_ANDROID_DIR := ../../osg_android/osg +LIBDIR := $(OSG_ANDROID_DIR)/obj/local/armeabi + +ifeq ($(TARGET_ARCH_ABI),armeabi-v7a) + LOCAL_ARM_NEON := true + LIBDIR := $(OSG_ANDROID_DIR)/obj/local/armeabi-v7a +endif + +### Add all source file names to be included in lib separated by a whitespace + +LOCAL_C_INCLUDES:= $(OSG_ANDROID_DIR)/include +LOCAL_CFLAGS := -Werror -fno-short-enums +LOCAL_CPPFLAGS := -DOSG_LIBRARY_STATIC + +LOCAL_LDLIBS := -llog -lGLESv2 -ldl -lz -lgnustl_static +LOCAL_SRC_FILES := osgNativeLib.cpp OsgMainApp.cpp OsgAndroidNotifyHandler.cpp +LOCAL_LDFLAGS := -L $(LIBDIR) \ +-losgdb_dds \ +-losgdb_openflight \ +-losgdb_tga \ +-losgdb_rgb \ +-losgdb_osgterrain \ +-losgdb_osg \ +-losgdb_ive \ +-losgdb_deprecated_osgviewer \ +-losgdb_deprecated_osgvolume \ +-losgdb_deprecated_osgtext \ +-losgdb_deprecated_osgterrain \ +-losgdb_deprecated_osgsim \ +-losgdb_deprecated_osgshadow \ +-losgdb_deprecated_osgparticle \ +-losgdb_deprecated_osgfx \ +-losgdb_deprecated_osganimation \ +-losgdb_deprecated_osg \ +-losgdb_serializers_osgvolume \ +-losgdb_serializers_osgtext \ +-losgdb_serializers_osgterrain \ +-losgdb_serializers_osgsim \ +-losgdb_serializers_osgshadow \ +-losgdb_serializers_osgparticle \ +-losgdb_serializers_osgmanipulator \ +-losgdb_serializers_osgfx \ +-losgdb_serializers_osganimation \ +-losgdb_serializers_osg \ +-losgViewer \ +-losgVolume \ +-losgTerrain \ +-losgText \ +-losgShadow \ +-losgSim \ +-losgParticle \ +-losgManipulator \ +-losgGA \ +-losgFX \ +-losgDB \ +-losgAnimation \ +-losgUtil \ +-losg \ +-lOpenThreads + +include $(BUILD_SHARED_LIBRARY) diff --git a/androidgcs/jni/Application.mk b/androidgcs/jni/Application.mk new file mode 100644 index 000000000..0387c4daa --- /dev/null +++ b/androidgcs/jni/Application.mk @@ -0,0 +1,11 @@ +#ANDROID APPLICATION MAKEFILE +APP_BUILD_SCRIPT := $(call my-dir)/Android.mk +#APP_PROJECT_PATH := $(call my-dir) + +APP_OPTIM := release + +APP_PLATFORM := android-7 +APP_STL := gnustl_static +APP_CPPFLAGS := -fexceptions -frtti +APP_ABI := armeabi armeabi-v7a +APP_MODULES := osgNativeLib diff --git a/androidgcs/jni/OsgAndroidNotifyHandler.cpp b/androidgcs/jni/OsgAndroidNotifyHandler.cpp new file mode 100644 index 000000000..2e4f38c15 --- /dev/null +++ b/androidgcs/jni/OsgAndroidNotifyHandler.cpp @@ -0,0 +1,38 @@ +/* + * OsgAndroidNotifyHandler.cpp + * + * Created on: 31/05/2011 + * Author: Jorge Izquierdo Ciges + */ + +#include "OsgAndroidNotifyHandler.hpp" +#include + +void OsgAndroidNotifyHandler::setTag(std::string tag){ + _tag = tag; +} +void OsgAndroidNotifyHandler::notify(osg::NotifySeverity severity, const char *message){ + + switch ( severity ) { + case osg::DEBUG_FP: + __android_log_write(ANDROID_LOG_VERBOSE,_tag.c_str(),message); + break; + case osg::DEBUG_INFO: + __android_log_write(ANDROID_LOG_DEBUG,_tag.c_str(),message); + break; + case osg::NOTICE: + case osg::INFO: + __android_log_write(ANDROID_LOG_INFO,_tag.c_str(),message); + break; + case osg::WARN: + __android_log_write(ANDROID_LOG_WARN,_tag.c_str(),message); + break; + case osg::FATAL: + case osg::ALWAYS: + __android_log_write(ANDROID_LOG_ERROR,_tag.c_str(),message); + break; + default: + __android_log_write(ANDROID_LOG_DEBUG,_tag.c_str(),message); + break; + } +} diff --git a/androidgcs/jni/OsgAndroidNotifyHandler.hpp b/androidgcs/jni/OsgAndroidNotifyHandler.hpp new file mode 100644 index 000000000..114a369aa --- /dev/null +++ b/androidgcs/jni/OsgAndroidNotifyHandler.hpp @@ -0,0 +1,26 @@ +/* + * OsgAndroidNotifyHandler.hpp + * + * Created on: 31/05/2011 + * Author: Jorge Izquierdo Ciges + */ + +#ifndef OSGANDROIDNOTIFYHANDLER_HPP_ +#define OSGANDROIDNOTIFYHANDLER_HPP_ + +#include + +#include + +#include + +class OSG_EXPORT OsgAndroidNotifyHandler : public osg::NotifyHandler +{ +private: + std::string _tag; +public: + void setTag(std::string tag); + void notify(osg::NotifySeverity severity, const char *message); +}; + +#endif /* OSGANDROIDNOTIFYHANDLER_HPP_ */ diff --git a/androidgcs/jni/OsgMainApp.cpp b/androidgcs/jni/OsgMainApp.cpp new file mode 100644 index 000000000..45bdec4d6 --- /dev/null +++ b/androidgcs/jni/OsgMainApp.cpp @@ -0,0 +1,217 @@ +#include "OsgMainApp.hpp" + + +OsgMainApp::OsgMainApp(){ + + _lodScale = 1.0f; + _prevFrame = 0; + + _initialized = false; + _clean_scene = false; + +} +OsgMainApp::~OsgMainApp(){ + +} +void OsgMainApp::loadModels(){ + if(_vModelsToLoad.size()==0) return; + + osg::notify(osg::ALWAYS)<<"There are "<<_vModelsToLoad.size()<<" models to load"< loadedModel = osgDB::readNodeFile(newModel.filename); + if (loadedModel == 0) { + osg::notify(osg::ALWAYS)<<"Model not loaded"<setName(newModel.name); + + osg::Shader * vshader = new osg::Shader(osg::Shader::VERTEX, gVertexShader ); + osg::Shader * fshader = new osg::Shader(osg::Shader::FRAGMENT, gFragmentShader ); + + osg::Program * prog = new osg::Program; + prog->addShader ( vshader ); + prog->addShader ( fshader ); + + loadedModel->getOrCreateStateSet()->setAttribute ( prog ); + + _root->addChild(loadedModel); + } + } + + osgViewer::Viewer::Windows windows; + _viewer->getWindows(windows); + for(osgViewer::Viewer::Windows::iterator itr = windows.begin();itr != windows.end();++itr) + { + (*itr)->getState()->setUseModelViewAndProjectionUniforms(true); + (*itr)->getState()->setUseVertexAttributeAliasing(true); + } + + _viewer->setSceneData(NULL); + _viewer->setSceneData(_root.get()); + _manipulator->getNode(); + _viewer->home(); + + _viewer->getDatabasePager()->clear(); + _viewer->getDatabasePager()->registerPagedLODs(_root.get()); + _viewer->getDatabasePager()->setUpThreads(3, 1); + _viewer->getDatabasePager()->setTargetMaximumNumberOfPageLOD(2); + _viewer->getDatabasePager()->setUnrefImageDataAfterApplyPolicy(true, true); + + _vModelsToLoad.clear(); + +} +void OsgMainApp::deleteModels(){ + if(_vModelsToDelete.size()==0) return; + + osg::notify(osg::ALWAYS)<<"There are "<<_vModelsToDelete.size()<<" models to delete"<getNumChildren(); j>0; j--){ + osg::ref_ptr children = _root->getChild(j-1); + if(children->getName() == modelToDelete.name){ + _root->removeChild(children); + } + } + + } + + _vModelsToDelete.clear(); + osg::notify(osg::ALWAYS)<<"finished"<setTag("Osg Viewer"); + osg::setNotifyHandler(_notifyHandler); + + osg::notify(osg::ALWAYS)<<"Testing"<setUpViewerAsEmbeddedInWindow(x, y, width, height); + _viewer->setThreadingModel(osgViewer::ViewerBase::SingleThreaded); + + _root = new osg::Group(); + + _viewer->realize(); + + _viewer->addEventHandler(new osgViewer::StatsHandler); + _viewer->addEventHandler(new osgGA::StateSetManipulator(_viewer->getCamera()->getOrCreateStateSet())); + _viewer->addEventHandler(new osgViewer::ThreadingHandler); + _viewer->addEventHandler(new osgViewer::LODScaleHandler); + + _manipulator = new osgGA::KeySwitchMatrixManipulator; + + _manipulator->addMatrixManipulator( '1', "Trackball", new osgGA::TrackballManipulator() ); + _manipulator->addMatrixManipulator( '2', "Flight", new osgGA::FlightManipulator() ); + _manipulator->addMatrixManipulator( '3', "Drive", new osgGA::DriveManipulator() ); + _manipulator->addMatrixManipulator( '4', "Terrain", new osgGA::TerrainManipulator() ); + _manipulator->addMatrixManipulator( '5', "Orbit", new osgGA::OrbitManipulator() ); + _manipulator->addMatrixManipulator( '6', "FirstPerson", new osgGA::FirstPersonManipulator() ); + _manipulator->addMatrixManipulator( '7', "Spherical", new osgGA::SphericalManipulator() ); + + _viewer->setCameraManipulator( _manipulator.get() ); + + _viewer->getViewerStats()->collectStats("scene", true); + + _initialized = true; + +} +//Draw +void OsgMainApp::draw(){ + //Every load o remove has to be done before any drawing + loadModels(); + deleteModels(); + + _viewer->frame(); +} +//Events +void OsgMainApp::mouseButtonPressEvent(float x,float y,int button){ + _viewer->getEventQueue()->mouseButtonPress(x, y, button); +} +void OsgMainApp::mouseButtonReleaseEvent(float x,float y,int button){ + _viewer->getEventQueue()->mouseButtonRelease(x, y, button); +} +void OsgMainApp::mouseMoveEvent(float x,float y){ + _viewer->getEventQueue()->mouseMotion(x, y); +} +void OsgMainApp::keyboardDown(int key){ + _viewer->getEventQueue()->keyPress(key); +} +void OsgMainApp::keyboardUp(int key){ + _viewer->getEventQueue()->keyRelease(key); +} +//Loading and unloading +void OsgMainApp::loadObject(std::string filePath){ + Model newModel; + newModel.filename = filePath; + newModel.name = filePath; + + int num = 0; + for(unsigned int i=0;i<_vModels.size();i++){ + if(_vModels[i].name==newModel.name) + return; + } + + _vModelsToLoad.push_back(newModel); + +} +void OsgMainApp::loadObject(std::string name,std::string filePath){ + + Model newModel; + newModel.filename = filePath; + newModel.name = name; + + for(unsigned int i=0;i<_vModels.size();i++){ + if(_vModels[i].name==newModel.name){ + osg::notify(osg::ALWAYS)<<"Name already used"<getCamera()->setClearColor(color); +} +osg::Vec4f OsgMainApp::getClearColor(){ + osg::notify(osg::ALWAYS)<<"Getting Clear Color"<getCamera()->getClearColor(); +} diff --git a/androidgcs/jni/OsgMainApp.hpp b/androidgcs/jni/OsgMainApp.hpp new file mode 100644 index 000000000..cc3b02b89 --- /dev/null +++ b/androidgcs/jni/OsgMainApp.hpp @@ -0,0 +1,182 @@ +/* + * OsgMainApp.hpp + * + * Created on: 29/05/2011 + * Author: Jorge Izquierdo Ciges + */ + +#ifndef OSGMAINAPP_HPP_ +#define OSGMAINAPP_HPP_ + +//Android log +#include +#include +#include +#include + +//Standard libraries +#include + +//osg +#include +#include +#include +#include +#include +#include +#include +//osgText +#include +//osgDB +#include +#include +#include +#include +//osg_viewer +#include +#include +#include +//osgGA +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +//Self headers +#include "OsgAndroidNotifyHandler.hpp" + +//Static plugins Macro +USE_OSGPLUGIN(ive) +USE_OSGPLUGIN(osg) +USE_OSGPLUGIN(osg2) +USE_OSGPLUGIN(terrain) +USE_OSGPLUGIN(rgb) +USE_OSGPLUGIN(OpenFlight) +USE_OSGPLUGIN(dds) +//Static DOTOSG +USE_DOTOSGWRAPPER_LIBRARY(osg) +USE_DOTOSGWRAPPER_LIBRARY(osgFX) +USE_DOTOSGWRAPPER_LIBRARY(osgParticle) +USE_DOTOSGWRAPPER_LIBRARY(osgTerrain) +USE_DOTOSGWRAPPER_LIBRARY(osgText) +USE_DOTOSGWRAPPER_LIBRARY(osgViewer) +USE_DOTOSGWRAPPER_LIBRARY(osgVolume) +//Static serializer +USE_SERIALIZER_WRAPPER_LIBRARY(osg) +USE_SERIALIZER_WRAPPER_LIBRARY(osgAnimation) +USE_SERIALIZER_WRAPPER_LIBRARY(osgFX) +USE_SERIALIZER_WRAPPER_LIBRARY(osgManipulator) +USE_SERIALIZER_WRAPPER_LIBRARY(osgParticle) +USE_SERIALIZER_WRAPPER_LIBRARY(osgTerrain) +USE_SERIALIZER_WRAPPER_LIBRARY(osgText) +USE_SERIALIZER_WRAPPER_LIBRARY(osgVolume) + +#define LOG_TAG "osgNativeLib" +#define LOGI(...) __android_log_print(ANDROID_LOG_INFO,LOG_TAG,__VA_ARGS__) +#define LOGE(...) __android_log_print(ANDROID_LOG_ERROR,LOG_TAG,__VA_ARGS__) + +struct Model{ + std::string filename; + std::string name; +}; + +static const char gVertexShader[] = + "varying vec4 color; \n" + "const vec3 lightPos =vec3(0.0, 0.0, 10.0); \n" + "const vec4 cessnaColor =vec4(0.8, 0.8, 0.8, 1.0); \n" + "const vec4 lightAmbient =vec4(0.1, 0.1, 0.1, 1.0); \n" + "const vec4 lightDiffuse =vec4(0.4, 0.4, 0.4, 1.0); \n" + "const vec4 lightSpecular =vec4(0.8, 0.8, 0.8, 1.0); \n" + "void DirectionalLight(in vec3 normal, \n" + " in vec3 ecPos, \n" + " inout vec4 ambient, \n" + " inout vec4 diffuse, \n" + " inout vec4 specular) \n" + "{ \n" + " float nDotVP; \n" + " vec3 L = normalize(gl_ModelViewMatrix*vec4(lightPos, 0.0)).xyz; \n" + " nDotVP = max(0.0, dot(normal, L)); \n" + " \n" + " if (nDotVP > 0.0) { \n" + " vec3 E = normalize(-ecPos); \n" + " vec3 R = normalize(reflect( L, normal )); \n" + " specular = pow(max(dot(R, E), 0.0), 16.0) * lightSpecular; \n" + " } \n" + " ambient = lightAmbient; \n" + " diffuse = lightDiffuse * nDotVP; \n" + "} \n" + "void main() { \n" + " vec4 ambiCol = vec4(0.0); \n" + " vec4 diffCol = vec4(0.0); \n" + " vec4 specCol = vec4(0.0); \n" + " gl_Position = gl_ModelViewProjectionMatrix * gl_Vertex; \n" + " vec3 normal = normalize(gl_NormalMatrix * gl_Normal); \n" + " vec4 ecPos = gl_ModelViewMatrix * gl_Vertex; \n" + " DirectionalLight(normal, ecPos.xyz, ambiCol, diffCol, specCol); \n" + " color = cessnaColor * (ambiCol + diffCol + specCol); \n" + "} \n"; + +static const char gFragmentShader[] = + "precision mediump float; \n" + "varying mediump vec4 color; \n" + "void main() { \n" + " gl_FragColor = color; \n" + "} \n"; + + +class OsgMainApp{ +private: + osg::ref_ptr _viewer; + osg::ref_ptr _root; + osg::ref_ptr _state; + osg::ref_ptr _manipulator; + + float _lodScale; + unsigned int _prevFrame; + + bool _initialized; + bool _clean_scene; + + OsgAndroidNotifyHandler *_notifyHandler; + + std::vector _vModels; + std::vector _vModelsToLoad; + std::vector _vModelsToDelete; + + void loadModels(); + void deleteModels(); + +public: + OsgMainApp(); + ~OsgMainApp(); + + //Initialization function + void initOsgWindow(int x,int y,int width,int height); + //Draw + void draw(); + //Events + void mouseButtonPressEvent(float x,float y,int button); + void mouseButtonReleaseEvent(float x,float y,int button); + void mouseMoveEvent(float x,float y); + void keyboardDown(int key); + void keyboardUp(int key); + //Loading and unloading + void loadObject(std::string filePath); + void loadObject(std::string name,std::string filePath); + void unLoadObject(int number); + void clearScene(); + //Other functions + int getNumberObjects(); + std::string getObjectName(int nunmber); + + void setClearColor(osg::Vec4f color); + osg::Vec4f getClearColor(); +}; + + +#endif /* OSGMAINAPP_HPP_ */ diff --git a/androidgcs/jni/osgNativeLib.cpp b/androidgcs/jni/osgNativeLib.cpp new file mode 100644 index 000000000..4c8efc3f2 --- /dev/null +++ b/androidgcs/jni/osgNativeLib.cpp @@ -0,0 +1,113 @@ +#include +#include +#include + +#include + +#include "OsgMainApp.hpp" + +OsgMainApp mainApp; + +extern "C" { + JNIEXPORT void JNICALL Java_osg_AndroidExample_osgNativeLib_init(JNIEnv * env, jobject obj, jint width, jint height); + JNIEXPORT void JNICALL Java_osg_AndroidExample_osgNativeLib_step(JNIEnv * env, jobject obj); + JNIEXPORT void JNICALL Java_osg_AndroidExample_osgNativeLib_clearContents(JNIEnv * env, jobject obj); + JNIEXPORT void JNICALL Java_osg_AndroidExample_osgNativeLib_mouseButtonPressEvent(JNIEnv * env, jobject obj, jfloat x, jfloat y, jint button); + JNIEXPORT void JNICALL Java_osg_AndroidExample_osgNativeLib_mouseButtonReleaseEvent(JNIEnv * env, jobject obj, jfloat x, jfloat y, jint button); + JNIEXPORT void JNICALL Java_osg_AndroidExample_osgNativeLib_mouseMoveEvent(JNIEnv * env, jobject obj, jfloat x, jfloat y); + JNIEXPORT void JNICALL Java_osg_AndroidExample_osgNativeLib_keyboardDown(JNIEnv * env, jobject obj, jint key); + JNIEXPORT void JNICALL Java_osg_AndroidExample_osgNativeLib_keyboardUp(JNIEnv * env, jobject obj, jint key); + JNIEXPORT jintArray JNICALL Java_osg_AndroidExample_osgNativeLib_getClearColor(JNIEnv * env, jobject obj); + JNIEXPORT void JNICALL Java_osg_AndroidExample_osgNativeLib_setClearColor(JNIEnv * env, jobject obj, jint red, jint green, jint blue); + JNIEXPORT void JNICALL Java_osg_AndroidExample_osgNativeLib_loadObject(JNIEnv * env, jobject obj, jstring address); + JNIEXPORT void JNICALL Java_osg_AndroidExample_osgNativeLib_unLoadObject(JNIEnv * env, jobject obj, jint number); + JNIEXPORT jobjectArray JNICALL Java_osg_AndroidExample_osgNativeLib_getObjectNames(JNIEnv * env, jobject obj); +}; + +JNIEXPORT void JNICALL Java_osg_AndroidExample_osgNativeLib_init(JNIEnv * env, jobject obj, jint width, jint height){ + mainApp.initOsgWindow(0,0,width,height); +} +JNIEXPORT void JNICALL Java_osg_AndroidExample_osgNativeLib_step(JNIEnv * env, jobject obj){ + mainApp.draw(); +} +JNIEXPORT void JNICALL Java_osg_AndroidExample_osgNativeLib_clearContents(JNIEnv * env, jobject obj){ + mainApp.clearScene(); +} +JNIEXPORT void JNICALL Java_osg_AndroidExample_osgNativeLib_mouseButtonPressEvent(JNIEnv * env, jobject obj, jfloat x, jfloat y, jint button){ + mainApp.mouseButtonPressEvent(x,y,button); +} +JNIEXPORT void JNICALL Java_osg_AndroidExample_osgNativeLib_mouseButtonReleaseEvent(JNIEnv * env, jobject obj, jfloat x, jfloat y, jint button){ + mainApp.mouseButtonReleaseEvent(x,y,button); +} +JNIEXPORT void JNICALL Java_osg_AndroidExample_osgNativeLib_mouseMoveEvent(JNIEnv * env, jobject obj, jfloat x, jfloat y){ + mainApp.mouseMoveEvent(x,y); +} +JNIEXPORT void JNICALL Java_osg_AndroidExample_osgNativeLib_keyboardDown(JNIEnv * env, jobject obj, jint key){ + mainApp.keyboardDown(key); +} +JNIEXPORT void JNICALL Java_osg_AndroidExample_osgNativeLib_keyboardUp(JNIEnv * env, jobject obj, jint key){ + mainApp.keyboardUp(key); +} +JNIEXPORT jintArray JNICALL Java_osg_AndroidExample_osgNativeLib_getClearColor(JNIEnv * env, jobject obj){ + + jintArray color; + color = env->NewIntArray(3); + if (color == NULL) { + return NULL; + } + osg::Vec4 vTemp1 = mainApp.getClearColor(); + + jint vTemp2[3]; + + vTemp2[0] = (int) (vTemp1.r() * 255); + vTemp2[1] = (int) (vTemp1.g() * 255); + vTemp2[2] = (int) (vTemp1.b() * 255); + + std::cout<SetIntArrayRegion(color, 0, 3, vTemp2); + + return color; +} +JNIEXPORT void JNICALL Java_osg_AndroidExample_osgNativeLib_setClearColor(JNIEnv * env, jobject obj, jint red, jint green, jint blue){ + osg::Vec4 tVec((float) red / 255.0f, (float) green / 255.0f, (float) blue / 255.0f, 0.0f); + mainApp.setClearColor(tVec); +} +JNIEXPORT void JNICALL Java_osg_AndroidExample_osgNativeLib_loadObject(JNIEnv * env, jobject obj, jstring address){ + //Import Strings from JNI + const char *nativeAddress = env->GetStringUTFChars(address, false); + + mainApp.loadObject(std::string(nativeAddress)); + + //Release Strings to JNI + env->ReleaseStringUTFChars(address, nativeAddress); +} +JNIEXPORT void JNICALL Java_osg_AndroidExample_osgNativeLib_loadObject(JNIEnv * env, jobject obj, jstring address, jstring name){ + //Import Strings from JNI + const char *nativeAddress = env->GetStringUTFChars(address, false); + const char *nativeName = env->GetStringUTFChars(name, false); + + mainApp.loadObject(std::string(nativeName),std::string(nativeAddress)); + + //Release Strings to JNI + env->ReleaseStringUTFChars(address, nativeAddress); + env->ReleaseStringUTFChars(address, nativeName); +} +JNIEXPORT void JNICALL Java_osg_AndroidExample_osgNativeLib_unLoadObject(JNIEnv * env, jobject obj, jint number){ + + mainApp.unLoadObject(number); + +} +JNIEXPORT jobjectArray JNICALL Java_osg_AndroidExample_osgNativeLib_getObjectNames(JNIEnv * env, jobject obj){ + + jobjectArray fileNames; + unsigned int numModels = mainApp.getNumberObjects(); + fileNames = (jobjectArray)env->NewObjectArray(numModels,env->FindClass("java/lang/String"),env->NewStringUTF("")); + + for(unsigned int i=0;i < numModels;i++){ + std::string name = mainApp.getObjectName(i); + env->SetObjectArrayElement(fileNames,i,env->NewStringUTF(name.c_str())); + } + + return fileNames; +} diff --git a/androidgcs/src/org/openpilot/osg/ColorPickerDialog.java b/androidgcs/src/org/openpilot/osg/ColorPickerDialog.java new file mode 100644 index 000000000..4e130f361 --- /dev/null +++ b/androidgcs/src/org/openpilot/osg/ColorPickerDialog.java @@ -0,0 +1,251 @@ +package org.openpilot.osg; + +import android.app.Dialog; +import android.content.Context; +import android.graphics.Canvas; +import android.graphics.Color; +import android.graphics.LinearGradient; +import android.graphics.Paint; +import android.graphics.Shader; +import android.os.Bundle; +import android.view.MotionEvent; +import android.view.View; + +public class ColorPickerDialog extends Dialog{ + + public interface OnColorChangeListener{ + void colorChange(int color); + } + + private final OnColorChangeListener tListener; + private final int tInitialColor; + + private static class ColorPickerView extends View{ + + private final Paint tPaint; + private float tCurrentHue = 0; + private int tCurrentX = 0; + private int tCurrentY = 0; + private int tCurrentColor; + private final int[] tHueGradientColors = new int [258]; + private final int [] tGradientColors = new int[65536]; //256X256 colors + private final OnColorChangeListener tListener; + private boolean tQSelected = false; + + public ColorPickerView(Context context, OnColorChangeListener listener, int color) { + super(context); + // TODO Auto-generated constructor stub + tListener = listener; + + //Get Hue from tCurrentColor and update the Gradient of Color + float[] newHSV = new float[3]; + Color.colorToHSV(color, newHSV); + tCurrentHue = newHSV[0]; + updateGradientColors(); + tCurrentColor = color; + + //Initialize of colors in Hue slider + + int index = 0; + for(float i=0; i<256; i += 256/42 , index++){ + tHueGradientColors[index] = Color.rgb(255, 0, (int)i); + } + for(float i=0; i<256; i += 256/42 , index++){ + tHueGradientColors[index] = Color.rgb(255-(int)i, 0, 255); + } + for(float i=0; i<256; i += 256/42 , index++){ + tHueGradientColors[index] = Color.rgb(0, (int) i, 255); + } + for(float i=0; i<256; i += 256/42 , index++){ + tHueGradientColors[index] = Color.rgb(0, 255, 255-(int)i); + } + for(float i=0; i<256; i += 256/42 , index++){ + tHueGradientColors[index] = Color.rgb((int)i, 255, 0); + } + for(float i=0; i<256; i += 256/42 , index++){ + tHueGradientColors[index] = Color.rgb(255, 255-(int)i, 0); + } + + // Paint initialized + tPaint = new Paint(Paint.ANTI_ALIAS_FLAG); + tPaint.setTextAlign(Paint.Align.CENTER); + tPaint.setTextSize(12); + } + + // Get the Color from Hue Bar + private int getCurrentGradientColor(){ + + int currentHue = 255 - (int)(tCurrentHue*255/360); + int index = 0; + for (float i=0; i<256; i += 256/42, index++){ + if (index == currentHue) return Color.rgb(255, 0, (int) i ); + } + for (float i=0; i<256; i += 256/42, index++){ + if (index == currentHue) return Color.rgb(255-(int)i, 0, 255 ); + } + for (float i=0; i<256; i += 256/42, index++){ + if (index == currentHue) return Color.rgb(0, (int) i, 255 ); + } + for (float i=0; i<256; i += 256/42, index++){ + if (index == currentHue) return Color.rgb(0, 255, 255-(int) i ); + } + for (float i=0; i<256; i += 256/42, index++){ + if (index == currentHue) return Color.rgb((int) i, 255, 0 ); + } + for (float i=0; i<256; i += 256/42, index++){ + if (index == currentHue) return Color.rgb(255, 255-(int) i, 0); + } + return Color.RED; + } + + private void updateGradientColors(){ + + int actualColor = getCurrentGradientColor(); + int index = 0; + int[] colColors = new int[256]; + for(int y=0; y<256; y++){ + for(int x=0; x<256; x++ , index++){ + if(y==0){ + tGradientColors[index] = Color.rgb(255-(255-Color.red(actualColor))*x/255, 255-(255-Color.green(actualColor))*x/255, 255-(255-Color.blue(actualColor))*x/255); + colColors[x] = tGradientColors[index]; + } + else{ + tGradientColors[index] = Color.rgb((255-y)*Color.red(colColors[x])/255, (255-y)*Color.green(colColors[x])/255, (255-y)*Color.blue(colColors[x])/255); + } + } + } + } + + @Override + protected void onDraw(Canvas canvas){ + int translatedHue = 255 - (int)(tCurrentHue*255/360); + //Display HUE with bar lines + + for(int x=0; x<256; x++){ + //We display the color or a big white bar + if(translatedHue != x){ + tPaint.setColor(tHueGradientColors[x]); + tPaint.setStrokeWidth(1); + } + else{ + tPaint.setColor(Color.WHITE); + tPaint.setStrokeWidth(3); + } + canvas.drawLine(x+10, 0, x+10, 40, tPaint); + } + + // Display Gradient Box + for(int x=0; x<256;x++){ + int[] colors = new int[2]; + colors[0] = tGradientColors[x]; + colors[1] = Color.BLACK; + Shader shader = new LinearGradient(0,50,0,306,colors,null, Shader.TileMode.REPEAT); + tPaint.setShader(shader); + canvas.drawLine(x+10, 50, x+10, 306, tPaint); + } + tPaint.setShader(null); + + //Display the circle around the currently selected color in the main field + if(tCurrentX !=0 && tCurrentY != 0){ + tPaint.setStyle(Paint.Style.STROKE); + tPaint.setColor(Color.BLACK); + canvas.drawCircle(tCurrentX, tCurrentY, 10, tPaint); + } + + //Draw a button + + tPaint.setStyle(Paint.Style.FILL); + if(tQSelected){ + tPaint.setColor(Color.WHITE); + canvas.drawCircle(138, 336, 30, tPaint); + } + tPaint.setColor(tCurrentColor); + canvas.drawCircle(138, 336, 20, tPaint); + + } + + @Override + protected void onMeasure(int width,int height){ + setMeasuredDimension(276, 366); + } + + @Override + public boolean onTouchEvent(MotionEvent event){ + if(event.getAction() != MotionEvent.ACTION_DOWN && event.getAction() != MotionEvent.ACTION_MOVE && event.getAction() != MotionEvent.ACTION_UP) return true; + float x = event.getX(); + float y = event.getY(); + + tQSelected=false; + + // if in Hue Bar + if(x >10 && x <266 && y>0 && y<40){ + //Update gradient + tCurrentHue = (255-x)*360/255; + updateGradientColors(); + + //Update current Selected Color + int nX = tCurrentX-10; + int nY = tCurrentY-60; + int index = 256 * (nY-1)+nX; + + if(index>0 && index < tGradientColors.length) + tCurrentColor = tGradientColors[256*(nY-1)+nX]; + + invalidate(); //By invalidating we are forcing a redraw; + } + + // If Main gradient + + if ( x >10 && x< 266 && y>50 && y <306){ + tCurrentX = (int) x; + tCurrentY = (int) y; + int nX = tCurrentX - 10; + int nY = tCurrentY - 60; + int index = 256*(nY-1)+nX; + if (index >0 && index < tGradientColors.length){ + tCurrentColor = tGradientColors[index]; + + invalidate(); //By invalidating we are forcing a redraw; + } + } + if( x>118 && x<158 && y > 316 && y <356){ + if(event.getAction() == MotionEvent.ACTION_DOWN || event.getAction() == MotionEvent.ACTION_MOVE){ + tQSelected=true; + } + if(event.getAction() == MotionEvent.ACTION_UP){ + tQSelected=false; + tListener.colorChange(tCurrentColor); + } + invalidate(); + } + + + + return true; + } + + + } + + public ColorPickerDialog(Context context, OnColorChangeListener listener, int initialColor){ + super(context); + + tListener = listener; + tInitialColor = initialColor; + } + + @Override + protected void onCreate( Bundle savedInstanceState){ + super.onCreate(savedInstanceState); + OnColorChangeListener l = new OnColorChangeListener(){ + @Override + public void colorChange(int color){ + tListener.colorChange(color); + dismiss(); + } + }; + + setContentView(new ColorPickerView(getContext(),l,tInitialColor)); + } + +} diff --git a/androidgcs/src/org/openpilot/osg/EGLview.java b/androidgcs/src/org/openpilot/osg/EGLview.java new file mode 100644 index 000000000..86397e7fd --- /dev/null +++ b/androidgcs/src/org/openpilot/osg/EGLview.java @@ -0,0 +1,333 @@ + +package org.openpilot.osg; +/* + * Copyright (C) 2008 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +import javax.microedition.khronos.egl.EGL10; +import javax.microedition.khronos.egl.EGLConfig; +import javax.microedition.khronos.egl.EGLContext; +import javax.microedition.khronos.egl.EGLDisplay; +import javax.microedition.khronos.opengles.GL10; + +import android.content.Context; +import android.graphics.PixelFormat; +import android.opengl.GLSurfaceView; +import android.util.AttributeSet; +import android.util.Log; +/** + * A simple GLSurfaceView sub-class that demonstrate how to perform + * OpenGL ES 1.0 rendering into a GL Surface. Note the following important + * details: + * + * - The class must use a custom context factory to enable 1.0 rendering. + * See ContextFactory class definition below. + * + * - The class must use a custom EGLConfigChooser to be able to select + * an EGLConfig that supports 1.0. This is done by providing a config + * specification to eglChooseConfig() that has the attribute + * EGL10.ELG_RENDERABLE_TYPE containing the EGL_OPENGL_ES_BIT flag + * set. See ConfigChooser class definition below. + * + * - The class must select the surface's format, then choose an EGLConfig + * that matches it exactly (with regards to red/green/blue/alpha channels + * bit depths). Failure to do so would result in an EGL_BAD_MATCH error. + */ +public class EGLview extends GLSurfaceView { + private static String TAG = "EGLview"; + private static final boolean DEBUG = false; + + public EGLview(Context context) { + super(context); + init(false, 16, 8); + } + public EGLview(Context context, AttributeSet attrs) { + super(context,attrs); + init(false, 16, 8); + } + + + public EGLview(Context context, boolean translucent, int depth, int stencil) { + super(context); + init(translucent, depth, stencil); + } + + private void init(boolean translucent, int depth, int stencil) { + + /* By default, GLSurfaceView() creates a RGB_565 opaque surface. + * If we want a translucent one, we should change the surface's + * format here, using PixelFormat.TRANSLUCENT for GL Surfaces + * is interpreted as any 32-bit surface with alpha by SurfaceFlinger. + */ + if (translucent) { + this.getHolder().setFormat(PixelFormat.TRANSLUCENT); + } + + /* Setup the context factory for 2.0 rendering. + * See ContextFactory class definition below + */ + setEGLContextFactory(new ContextFactory()); + + /* We need to choose an EGLConfig that matches the format of + * our surface exactly. This is going to be done in our + * custom config chooser. See ConfigChooser class definition + * below. + */ + setEGLConfigChooser( translucent ? + new ConfigChooser(8, 8, 8, 8, depth, stencil) : + new ConfigChooser(5, 6, 5, 0, depth, stencil) ); + + /* Set the renderer responsible for frame rendering */ + setRenderer(new Renderer()); + } + + private static class ContextFactory implements GLSurfaceView.EGLContextFactory { + private static int EGL_CONTEXT_CLIENT_VERSION = 0x3098; + @Override + public EGLContext createContext(EGL10 egl, EGLDisplay display, EGLConfig eglConfig) { + Log.w(TAG, "creating OpenGL ES 2.0 context"); + checkEglError("Before eglCreateContext", egl); + int[] attrib_list = {EGL_CONTEXT_CLIENT_VERSION, 2, EGL10.EGL_NONE }; + EGLContext context = egl.eglCreateContext(display, eglConfig, EGL10.EGL_NO_CONTEXT, attrib_list); + checkEglError("After eglCreateContext", egl); + return context; + } + + @Override + public void destroyContext(EGL10 egl, EGLDisplay display, EGLContext context) { + egl.eglDestroyContext(display, context); + } + } + + private static void checkEglError(String prompt, EGL10 egl) { + int error; + while ((error = egl.eglGetError()) != EGL10.EGL_SUCCESS) { + Log.e(TAG, String.format("%s: EGL error: 0x%x", prompt, error)); + } + } + + private static class ConfigChooser implements GLSurfaceView.EGLConfigChooser { + + public ConfigChooser(int r, int g, int b, int a, int depth, int stencil) { + mRedSize = r; + mGreenSize = g; + mBlueSize = b; + mAlphaSize = a; + mDepthSize = depth; + mStencilSize = stencil; + } + + /* This EGL config specification is used to specify 1.x rendering. + * We use a minimum size of 4 bits for red/green/blue, but will + * perform actual matching in chooseConfig() below. + */ + private static int EGL_OPENGL_ES_BIT = 4; + private static int[] s_configAttribs2 = + { + EGL10.EGL_RED_SIZE, 4, + EGL10.EGL_GREEN_SIZE, 4, + EGL10.EGL_BLUE_SIZE, 4, + EGL10.EGL_RENDERABLE_TYPE, EGL_OPENGL_ES_BIT, + EGL10.EGL_NONE + }; + + @Override + public EGLConfig chooseConfig(EGL10 egl, EGLDisplay display) { + + /* Get the number of minimally matching EGL configurations + */ + int[] num_config = new int[1]; + egl.eglChooseConfig(display, s_configAttribs2, null, 0, num_config); + + int numConfigs = num_config[0]; + + if (numConfigs <= 0) { + throw new IllegalArgumentException("No configs match configSpec"); + } + + /* Allocate then read the array of minimally matching EGL configs + */ + EGLConfig[] configs = new EGLConfig[numConfigs]; + egl.eglChooseConfig(display, s_configAttribs2, configs, numConfigs, num_config); + + if (DEBUG) { + printConfigs(egl, display, configs); + } + /* Now return the "best" one + */ + return chooseConfig(egl, display, configs); + } + + public EGLConfig chooseConfig(EGL10 egl, EGLDisplay display, + EGLConfig[] configs) { + for(EGLConfig config : configs) { + int d = findConfigAttrib(egl, display, config, + EGL10.EGL_DEPTH_SIZE, 0); + int s = findConfigAttrib(egl, display, config, + EGL10.EGL_STENCIL_SIZE, 0); + + // We need at least mDepthSize and mStencilSize bits + if (d < mDepthSize || s < mStencilSize) + continue; + + // We want an *exact* match for red/green/blue/alpha + int r = findConfigAttrib(egl, display, config, + EGL10.EGL_RED_SIZE, 0); + int g = findConfigAttrib(egl, display, config, + EGL10.EGL_GREEN_SIZE, 0); + int b = findConfigAttrib(egl, display, config, + EGL10.EGL_BLUE_SIZE, 0); + int a = findConfigAttrib(egl, display, config, + EGL10.EGL_ALPHA_SIZE, 0); + + if (r == mRedSize && g == mGreenSize && b == mBlueSize && a == mAlphaSize) + return config; + } + return null; + } + + private int findConfigAttrib(EGL10 egl, EGLDisplay display, + EGLConfig config, int attribute, int defaultValue) { + + if (egl.eglGetConfigAttrib(display, config, attribute, mValue)) { + return mValue[0]; + } + return defaultValue; + } + + private void printConfigs(EGL10 egl, EGLDisplay display, + EGLConfig[] configs) { + int numConfigs = configs.length; + Log.w(TAG, String.format("%d configurations", numConfigs)); + for (int i = 0; i < numConfigs; i++) { + Log.w(TAG, String.format("Configuration %d:\n", i)); + printConfig(egl, display, configs[i]); + } + } + + private void printConfig(EGL10 egl, EGLDisplay display, + EGLConfig config) { + int[] attributes = { + EGL10.EGL_BUFFER_SIZE, + EGL10.EGL_ALPHA_SIZE, + EGL10.EGL_BLUE_SIZE, + EGL10.EGL_GREEN_SIZE, + EGL10.EGL_RED_SIZE, + EGL10.EGL_DEPTH_SIZE, + EGL10.EGL_STENCIL_SIZE, + EGL10.EGL_CONFIG_CAVEAT, + EGL10.EGL_CONFIG_ID, + EGL10.EGL_LEVEL, + EGL10.EGL_MAX_PBUFFER_HEIGHT, + EGL10.EGL_MAX_PBUFFER_PIXELS, + EGL10.EGL_MAX_PBUFFER_WIDTH, + EGL10.EGL_NATIVE_RENDERABLE, + EGL10.EGL_NATIVE_VISUAL_ID, + EGL10.EGL_NATIVE_VISUAL_TYPE, + 0x3030, // EGL10.EGL_PRESERVED_RESOURCES, + EGL10.EGL_SAMPLES, + EGL10.EGL_SAMPLE_BUFFERS, + EGL10.EGL_SURFACE_TYPE, + EGL10.EGL_TRANSPARENT_TYPE, + EGL10.EGL_TRANSPARENT_RED_VALUE, + EGL10.EGL_TRANSPARENT_GREEN_VALUE, + EGL10.EGL_TRANSPARENT_BLUE_VALUE, + 0x3039, // EGL10.EGL_BIND_TO_TEXTURE_RGB, + 0x303A, // EGL10.EGL_BIND_TO_TEXTURE_RGBA, + 0x303B, // EGL10.EGL_MIN_SWAP_INTERVAL, + 0x303C, // EGL10.EGL_MAX_SWAP_INTERVAL, + EGL10.EGL_LUMINANCE_SIZE, + EGL10.EGL_ALPHA_MASK_SIZE, + EGL10.EGL_COLOR_BUFFER_TYPE, + EGL10.EGL_RENDERABLE_TYPE, + 0x3042 // EGL10.EGL_CONFORMANT + }; + String[] names = { + "EGL_BUFFER_SIZE", + "EGL_ALPHA_SIZE", + "EGL_BLUE_SIZE", + "EGL_GREEN_SIZE", + "EGL_RED_SIZE", + "EGL_DEPTH_SIZE", + "EGL_STENCIL_SIZE", + "EGL_CONFIG_CAVEAT", + "EGL_CONFIG_ID", + "EGL_LEVEL", + "EGL_MAX_PBUFFER_HEIGHT", + "EGL_MAX_PBUFFER_PIXELS", + "EGL_MAX_PBUFFER_WIDTH", + "EGL_NATIVE_RENDERABLE", + "EGL_NATIVE_VISUAL_ID", + "EGL_NATIVE_VISUAL_TYPE", + "EGL_PRESERVED_RESOURCES", + "EGL_SAMPLES", + "EGL_SAMPLE_BUFFERS", + "EGL_SURFACE_TYPE", + "EGL_TRANSPARENT_TYPE", + "EGL_TRANSPARENT_RED_VALUE", + "EGL_TRANSPARENT_GREEN_VALUE", + "EGL_TRANSPARENT_BLUE_VALUE", + "EGL_BIND_TO_TEXTURE_RGB", + "EGL_BIND_TO_TEXTURE_RGBA", + "EGL_MIN_SWAP_INTERVAL", + "EGL_MAX_SWAP_INTERVAL", + "EGL_LUMINANCE_SIZE", + "EGL_ALPHA_MASK_SIZE", + "EGL_COLOR_BUFFER_TYPE", + "EGL_RENDERABLE_TYPE", + "EGL_CONFORMANT" + }; + int[] value = new int[1]; + for (int i = 0; i < attributes.length; i++) { + int attribute = attributes[i]; + String name = names[i]; + if ( egl.eglGetConfigAttrib(display, config, attribute, value)) { + Log.w(TAG, String.format(" %s: %d\n", name, value[0])); + } else { + // Log.w(TAG, String.format(" %s: failed\n", name)); + while (egl.eglGetError() != EGL10.EGL_SUCCESS); + } + } + } + + // Subclasses can adjust these values: + protected int mRedSize; + protected int mGreenSize; + protected int mBlueSize; + protected int mAlphaSize; + protected int mDepthSize; + protected int mStencilSize; + private final int[] mValue = new int[1]; + } + + private static class Renderer implements GLSurfaceView.Renderer { + @Override + public void onDrawFrame(GL10 gl) { + osgNativeLib.step(); + } + + @Override + public void onSurfaceChanged(GL10 gl, int width, int height) { + osgNativeLib.init(width, height); + } + + @Override + public void onSurfaceCreated(GL10 gl, EGLConfig config) { + // Do nothing + gl.glEnable(GL10.GL_DEPTH_TEST); + } + } + +} + diff --git a/androidgcs/src/org/openpilot/osg/osgNativeLib.java b/androidgcs/src/org/openpilot/osg/osgNativeLib.java new file mode 100644 index 000000000..a98e7534f --- /dev/null +++ b/androidgcs/src/org/openpilot/osg/osgNativeLib.java @@ -0,0 +1,28 @@ +package org.openpilot.osg; + +public class osgNativeLib { + + static { + System.loadLibrary("osgNativeLib"); + } + + /** + * @param width the current view width + * @param height the current view height + */ + public static native void init(int width, int height); + public static native void step(); + public static native void clearContents(); + public static native void mouseButtonPressEvent(float x,float y, int button); + public static native void mouseButtonReleaseEvent(float x,float y, int button); + public static native void mouseMoveEvent(float x,float y); + public static native void keyboardDown(int key); + public static native void keyboardUp(int key); + public static native void setClearColor(int red,int green, int blue); + public static native int[] getClearColor(); + public static native void loadObject(String address); + public static native void loadObject(String address,String name); + public static native void unLoadObject(int number); + public static native String[] getObjectNames(); + +} diff --git a/androidgcs/src/org/openpilot/osg/osgViewer.java b/androidgcs/src/org/openpilot/osg/osgViewer.java new file mode 100644 index 000000000..39032822d --- /dev/null +++ b/androidgcs/src/org/openpilot/osg/osgViewer.java @@ -0,0 +1,414 @@ +package org.openpilot.osg; + +import android.app.Activity; +import android.app.AlertDialog; +import android.content.Context; +import android.content.DialogInterface; +import android.graphics.Color; +import android.graphics.PointF; +import android.os.Bundle; +import android.util.Log; +import android.view.KeyEvent; +import android.view.LayoutInflater; +import android.view.Menu; +import android.view.MenuInflater; +import android.view.MenuItem; +import android.view.MotionEvent; +import android.view.View; +import android.view.View.OnClickListener; +import android.view.inputmethod.InputMethodManager; +import android.widget.Button; +import android.widget.EditText; +import android.widget.ImageButton; +import android.widget.Toast; + +public class osgViewer extends Activity implements View.OnTouchListener, View.OnKeyListener, ColorPickerDialog.OnColorChangeListener { + enum moveTypes { NONE , DRAG, MDRAG, ZOOM ,ACTUALIZE} + enum navType { PRINCIPAL , SECONDARY } + enum lightType { ON , OFF } + + moveTypes mode=moveTypes.NONE; + navType navMode = navType.PRINCIPAL; + lightType lightMode = lightType.ON; + + PointF oneFingerOrigin = new PointF(0,0); + long timeOneFinger=0; + PointF twoFingerOrigin = new PointF(0,0); + long timeTwoFinger=0; + float distanceOrigin; + + int backgroundColor; + + private static final String TAG = "OSG Activity"; + //Ui elements + EGLview mView; + Button uiCenterViewButton; + Button uiNavigationChangeButton; + ImageButton uiNavigationLeft; + ImageButton uiNavigationRight; + Button uiLightChangeButton; + + //Toasts + Toast msgUiNavPrincipal; + Toast msgUiNavSecondary; + Toast msgUiLightOn; + Toast msgUiLightOff; + + //Dialogs + AlertDialog removeLayerDialog; + AlertDialog loadLayerAddress; + + //Main Android Activity life cycle + @Override protected void onCreate(Bundle icicle) { + super.onCreate(icicle); + setContentView(R.layout.ui_layout_gles); + //Obtain every Ui element + mView= (EGLview) findViewById(R.id.surfaceGLES); + mView.setOnTouchListener(this); + mView.setOnKeyListener(this); + + uiCenterViewButton = (Button) findViewById(R.id.uiButtonCenter); + uiCenterViewButton.setOnClickListener(uiListenerCenterView); + uiNavigationChangeButton = (Button) findViewById(R.id.uiButtonChangeNavigation); + uiNavigationChangeButton.setOnClickListener(uiListenerChangeNavigation); + uiLightChangeButton = (Button) findViewById(R.id.uiButtonLight); + uiLightChangeButton.setOnClickListener(uiListenerChangeLight); + + //Creating Toasts + msgUiNavPrincipal = Toast.makeText(getApplicationContext(), R.string.uiToastNavPrincipal, Toast.LENGTH_SHORT); + msgUiNavSecondary = Toast.makeText(getApplicationContext(), R.string.uiToastNavSecond, Toast.LENGTH_SHORT); + msgUiLightOn = Toast.makeText(getApplicationContext(), R.string.uiToastLightOn, Toast.LENGTH_SHORT); + msgUiLightOff = Toast.makeText(getApplicationContext(), R.string.uiToastLightOff, Toast.LENGTH_SHORT); + + //Creating Dialogs + + LayoutInflater factory = LayoutInflater.from(getApplicationContext()); + final View textEntryView = factory.inflate(R.layout.dialog_text_entry, null); + AlertDialog.Builder loadLayerDialogBuilder = new AlertDialog.Builder(this); + loadLayerDialogBuilder.setIcon(R.drawable.web_browser); + loadLayerDialogBuilder.setTitle(R.string.uiDialogTextAddress); + loadLayerDialogBuilder.setView(textEntryView); + loadLayerDialogBuilder.setPositiveButton(R.string.uiDialogOk, new DialogInterface.OnClickListener() { + + @Override + public void onClick(DialogInterface dialog, int which) { + // TODO Auto-generated method stub + EditText address; + address = (EditText) textEntryView.findViewById(R.id.uiEditTextInput); + osgNativeLib.loadObject(address.getText().toString()); + } + }); + loadLayerDialogBuilder.setNegativeButton(R.string.uiDialogCancel, new DialogInterface.OnClickListener() { + + @Override + public void onClick(DialogInterface dialog, int which) { + // TODO Auto-generated method stub + + } + }); + + loadLayerAddress = loadLayerDialogBuilder.create(); + } + @Override protected void onPause() { + super.onPause(); + mView.onPause(); + } + @Override protected void onResume() { + super.onResume(); + mView.onResume(); + } + + //Main view event processing + @Override + public boolean onKey(View v, int keyCode, KeyEvent event) { + + return true; + } + @Override + public boolean onKeyDown(int keyCode, KeyEvent event){ + //DO NOTHING this will render useless every menu key except Home + int keyChar= event.getUnicodeChar(); + osgNativeLib.keyboardDown(keyChar); + return true; + } + @Override + public boolean onKeyUp(int keyCode, KeyEvent event){ + switch (keyCode){ + case KeyEvent.KEYCODE_BACK: + super.onDestroy(); + this.finish(); + break; + case KeyEvent.KEYCODE_SEARCH: + break; + case KeyEvent.KEYCODE_MENU: + this.openOptionsMenu(); + break; + default: + int keyChar= event.getUnicodeChar(); + osgNativeLib.keyboardUp(keyChar); + } + + return true; + } + @Override + public boolean onTouch(View v, MotionEvent event) { + + //dumpEvent(event); + + long time_arrival = event.getEventTime(); + int n_points = event.getPointerCount(); + int action = event.getAction() & MotionEvent.ACTION_MASK; + + switch(n_points){ + case 1: + switch(action){ + case MotionEvent.ACTION_DOWN: + mode = moveTypes.DRAG; + + osgNativeLib.mouseMoveEvent(event.getX(0), event.getY(0)); + if(navMode==navType.PRINCIPAL) + osgNativeLib.mouseButtonPressEvent(event.getX(0), event.getY(0), 2); + else + osgNativeLib.mouseButtonPressEvent(event.getX(0), event.getY(0), 1); + + oneFingerOrigin.x=event.getX(0); + oneFingerOrigin.y=event.getY(0); + break; + case MotionEvent.ACTION_CANCEL: + switch(mode){ + case DRAG: + osgNativeLib.mouseMoveEvent(event.getX(0), event.getY(0)); + if(navMode==navType.PRINCIPAL) + osgNativeLib.mouseButtonReleaseEvent(event.getX(0), event.getY(0), 2); + else + osgNativeLib.mouseButtonReleaseEvent(event.getX(0), event.getY(0), 1); + break; + default : + Log.e(TAG,"There has been an anomaly in touch input 1point/action"); + } + mode = moveTypes.NONE; + break; + case MotionEvent.ACTION_MOVE: + + osgNativeLib.mouseMoveEvent(event.getX(0), event.getY(0)); + + oneFingerOrigin.x=event.getX(0); + oneFingerOrigin.y=event.getY(0); + + break; + case MotionEvent.ACTION_UP: + switch(mode){ + case DRAG: + if(navMode==navType.PRINCIPAL) + osgNativeLib.mouseButtonReleaseEvent(event.getX(0), event.getY(0), 2); + else + osgNativeLib.mouseButtonReleaseEvent(event.getX(0), event.getY(0), 1); + break; + default : + Log.e(TAG,"There has been an anomaly in touch input 1 point/action"); + } + mode = moveTypes.NONE; + break; + default : + Log.e(TAG,"1 point Action not captured"); + } + break; + case 2: + switch (action){ + case MotionEvent.ACTION_POINTER_DOWN: + //Free previous Action + switch(mode){ + case DRAG: + if(navMode==navType.PRINCIPAL) + osgNativeLib.mouseButtonReleaseEvent(event.getX(0), event.getY(0), 2); + else + osgNativeLib.mouseButtonReleaseEvent(event.getX(0), event.getY(0), 1); + break; + } + mode = moveTypes.ZOOM; + distanceOrigin = sqrDistance(event); + twoFingerOrigin.x=event.getX(1); + twoFingerOrigin.y=event.getY(1); + oneFingerOrigin.x=event.getX(0); + oneFingerOrigin.y=event.getY(0); + + osgNativeLib.mouseMoveEvent(oneFingerOrigin.x,oneFingerOrigin.y); + osgNativeLib.mouseButtonPressEvent(oneFingerOrigin.x,oneFingerOrigin.y, 3); + osgNativeLib.mouseMoveEvent(oneFingerOrigin.x,oneFingerOrigin.y); + + case MotionEvent.ACTION_MOVE: + float distance = sqrDistance(event); + float result = distance-distanceOrigin; + distanceOrigin=distance; + + if(result>1||result<-1){ + oneFingerOrigin.y=oneFingerOrigin.y+result; + osgNativeLib.mouseMoveEvent(oneFingerOrigin.x,oneFingerOrigin.y); + } + + break; + case MotionEvent.ACTION_POINTER_UP: + mode =moveTypes.NONE; + osgNativeLib.mouseButtonReleaseEvent(oneFingerOrigin.x,oneFingerOrigin.y, 3); + break; + case MotionEvent.ACTION_UP: + mode =moveTypes.NONE; + osgNativeLib.mouseButtonReleaseEvent(oneFingerOrigin.x,oneFingerOrigin.y, 3); + break; + default : + Log.e(TAG,"2 point Action not captured"); + } + break; + } + + return true; + } + + //Ui Listeners + OnClickListener uiListenerCenterView = new OnClickListener() { + @Override + public void onClick(View v) { + //Log.d(TAG, "Center View"); + osgNativeLib.keyboardDown(32); + osgNativeLib.keyboardUp(32); + } + }; + OnClickListener uiListenerChangeNavigation = new OnClickListener() { + @Override + public void onClick(View v) { + //Log.d(TAG, "Change Navigation"); + if(navMode==navType.PRINCIPAL){ + msgUiNavSecondary.show(); + navMode=navType.SECONDARY; + } + else{ + msgUiNavPrincipal.show(); + navMode=navType.PRINCIPAL; + } + } + }; + OnClickListener uiListenerChangeLight = new OnClickListener() { + @Override + public void onClick(View v) { + //Log.d(TAG, "Change Light"); + if(lightMode==lightType.ON){ + msgUiLightOff.show(); + lightMode=lightType.OFF; + osgNativeLib.keyboardDown(108); + osgNativeLib.keyboardUp(108); + } + else{ + msgUiLightOn.show(); + lightMode=lightType.ON; + osgNativeLib.keyboardDown(108); + osgNativeLib.keyboardUp(108); + } + } + }; + + //Menu + + @Override + public void colorChange(int color) { + // TODO Auto-generated method stub + // Do nothing + int red = Color.red(color); + int green = Color.green(color); + int blue = Color.blue(color); + //Log.d(TAG,"BACK color "+red+" "+green+" "+blue+" "); + osgNativeLib.setClearColor(red,green,blue); + } + + //Android Life Cycle Menu + @Override + public boolean onCreateOptionsMenu(Menu menu) { + MenuInflater inflater = getMenuInflater(); + inflater.inflate(R.menu.appmenu, menu); + return super.onCreateOptionsMenu(menu); + } + @Override + public boolean onOptionsItemSelected(MenuItem item) { + // Handle item selection + switch (item.getItemId()) { + case R.id.menuLoadObject: + Log.d(TAG,"Load Object"); + loadLayerAddress.show(); + return true; + case R.id.menuCleanScene: + Log.d(TAG,"Clean Scene"); + osgNativeLib.clearContents(); + return true; + case R.id.menuDeleteObject: + Log.d(TAG,"Delete a object"); + String vNames[] = osgNativeLib.getObjectNames(); + + //Remove Layer Dialog + AlertDialog.Builder removeLayerDialogBuilder = new AlertDialog.Builder(this); + removeLayerDialogBuilder.setTitle(R.string.uiDialogTextChoseRemove); + removeLayerDialogBuilder.setItems(vNames, new DialogInterface.OnClickListener() { + + @Override + public void onClick(DialogInterface dialog, int witch) { + // TODO Auto-generated method stub + osgNativeLib.unLoadObject(witch); + } + }); + removeLayerDialog = removeLayerDialogBuilder.create(); + + if(vNames.length > 0) + removeLayerDialog.show(); + + return true; + case R.id.menuChangeBackground: + Log.d(TAG,"Change background color"); + int[] test = new int [3]; + test = osgNativeLib.getClearColor(); + backgroundColor = Color.rgb(test[0], test[1], test[2]); + + ColorPickerDialog colorDialog; + new ColorPickerDialog(this, this, backgroundColor).show(); + return true; + case R.id.menuShowKeyboard: + Log.d(TAG,"Keyboard"); + InputMethodManager mgr= (InputMethodManager)this.getSystemService(Context.INPUT_METHOD_SERVICE); + mgr.toggleSoftInput(InputMethodManager.SHOW_IMPLICIT, 0); + return true; + default: + return super.onOptionsItemSelected(item); + } + } + + //Utilities + /** Show an event in the LogCat view, for debugging */ + private void dumpEvent(MotionEvent event) { + String names[] = { "DOWN", "UP", "MOVE", "CANCEL", "OUTSIDE", + "POINTER_DOWN", "POINTER_UP", "7?", "8?", "9?" }; + StringBuilder sb = new StringBuilder(); + int action = event.getAction(); + int actionCode = action & MotionEvent.ACTION_MASK; + sb.append("event ACTION_").append(names[actionCode]); + if (actionCode == MotionEvent.ACTION_POINTER_DOWN + || actionCode == MotionEvent.ACTION_POINTER_UP) { + sb.append("(pid ").append( + action >> MotionEvent.ACTION_POINTER_ID_SHIFT); + sb.append(")"); + } + sb.append("["); + for (int i = 0; i < event.getPointerCount(); i++) { + sb.append("#").append(i); + sb.append("(pid ").append(event.getPointerId(i)); + sb.append(")=").append((int) event.getX(i)); + sb.append(",").append((int) event.getY(i)); + if (i + 1 < event.getPointerCount()) + sb.append(";"); + } + sb.append("]"); + //Log.d(TAG, sb.toString()); + } + private float sqrDistance(MotionEvent event) { + float x = event.getX(0) - event.getX(1); + float y = event.getY(0) - event.getY(1); + return (float)(Math.sqrt(x * x + y * y)); + } + +} \ No newline at end of file From 129edf4398a6adab4760c7f2615f05dc75010db8 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Tue, 21 Aug 2012 00:04:26 -0500 Subject: [PATCH 24/32] AndroidGCS OSG: Get context showing --- androidgcs/AndroidManifest.xml | 133 +++++++++++------- androidgcs/jni/osgNativeLib.cpp | 60 ++++---- androidgcs/res/layout/ui_layout_gles.xml | 44 ++++++ .../src/org/openpilot/osg/osgViewer.java | 90 ++---------- 4 files changed, 164 insertions(+), 163 deletions(-) create mode 100644 androidgcs/res/layout/ui_layout_gles.xml diff --git a/androidgcs/AndroidManifest.xml b/androidgcs/AndroidManifest.xml index ad63dc791..f697bc43a 100644 --- a/androidgcs/AndroidManifest.xml +++ b/androidgcs/AndroidManifest.xml @@ -1,61 +1,90 @@ - - + package="org.openpilot.androidgcs" + android:versionCode="1" + android:versionName="1.0" > - + - - - - - - - - - + - - - - - - - - - - - + + + + + - - - + - - - - - - - - + - - - - - - - - - - + + - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/androidgcs/jni/osgNativeLib.cpp b/androidgcs/jni/osgNativeLib.cpp index 4c8efc3f2..955717c0f 100644 --- a/androidgcs/jni/osgNativeLib.cpp +++ b/androidgcs/jni/osgNativeLib.cpp @@ -9,46 +9,46 @@ OsgMainApp mainApp; extern "C" { - JNIEXPORT void JNICALL Java_osg_AndroidExample_osgNativeLib_init(JNIEnv * env, jobject obj, jint width, jint height); - JNIEXPORT void JNICALL Java_osg_AndroidExample_osgNativeLib_step(JNIEnv * env, jobject obj); - JNIEXPORT void JNICALL Java_osg_AndroidExample_osgNativeLib_clearContents(JNIEnv * env, jobject obj); - JNIEXPORT void JNICALL Java_osg_AndroidExample_osgNativeLib_mouseButtonPressEvent(JNIEnv * env, jobject obj, jfloat x, jfloat y, jint button); - JNIEXPORT void JNICALL Java_osg_AndroidExample_osgNativeLib_mouseButtonReleaseEvent(JNIEnv * env, jobject obj, jfloat x, jfloat y, jint button); - JNIEXPORT void JNICALL Java_osg_AndroidExample_osgNativeLib_mouseMoveEvent(JNIEnv * env, jobject obj, jfloat x, jfloat y); - JNIEXPORT void JNICALL Java_osg_AndroidExample_osgNativeLib_keyboardDown(JNIEnv * env, jobject obj, jint key); - JNIEXPORT void JNICALL Java_osg_AndroidExample_osgNativeLib_keyboardUp(JNIEnv * env, jobject obj, jint key); - JNIEXPORT jintArray JNICALL Java_osg_AndroidExample_osgNativeLib_getClearColor(JNIEnv * env, jobject obj); - JNIEXPORT void JNICALL Java_osg_AndroidExample_osgNativeLib_setClearColor(JNIEnv * env, jobject obj, jint red, jint green, jint blue); - JNIEXPORT void JNICALL Java_osg_AndroidExample_osgNativeLib_loadObject(JNIEnv * env, jobject obj, jstring address); - JNIEXPORT void JNICALL Java_osg_AndroidExample_osgNativeLib_unLoadObject(JNIEnv * env, jobject obj, jint number); - JNIEXPORT jobjectArray JNICALL Java_osg_AndroidExample_osgNativeLib_getObjectNames(JNIEnv * env, jobject obj); + JNIEXPORT void JNICALL Java_org_openpilot_osg_osgNativeLib_init(JNIEnv * env, jobject obj, jint width, jint height); + JNIEXPORT void JNICALL Java_org_openpilot_osg_osgNativeLib_step(JNIEnv * env, jobject obj); + JNIEXPORT void JNICALL Java_org_openpilot_osg_osgNativeLib_clearContents(JNIEnv * env, jobject obj); + JNIEXPORT void JNICALL Java_org_openpilot_osg_osgNativeLib_mouseButtonPressEvent(JNIEnv * env, jobject obj, jfloat x, jfloat y, jint button); + JNIEXPORT void JNICALL Java_org_openpilot_osg_osgNativeLib_mouseButtonReleaseEvent(JNIEnv * env, jobject obj, jfloat x, jfloat y, jint button); + JNIEXPORT void JNICALL Java_org_openpilot_osg_osgNativeLib_mouseMoveEvent(JNIEnv * env, jobject obj, jfloat x, jfloat y); + JNIEXPORT void JNICALL Java_org_openpilot_osg_osgNativeLib_keyboardDown(JNIEnv * env, jobject obj, jint key); + JNIEXPORT void JNICALL Java_org_openpilot_osg_osgNativeLib_keyboardUp(JNIEnv * env, jobject obj, jint key); + JNIEXPORT jintArray JNICALL Java_org_openpilot_osg_osgNativeLib_getClearColor(JNIEnv * env, jobject obj); + JNIEXPORT void JNICALL Java_org_openpilot_osg_osgNativeLib_setClearColor(JNIEnv * env, jobject obj, jint red, jint green, jint blue); + JNIEXPORT void JNICALL Java_org_openpilot_osg_osgNativeLib_loadObject(JNIEnv * env, jobject obj, jstring address); + JNIEXPORT void JNICALL Java_org_openpilot_osg_osgNativeLib_unLoadObject(JNIEnv * env, jobject obj, jint number); + JNIEXPORT jobjectArray JNICALL Java_org_openpilot_osg_osgNativeLib_getObjectNames(JNIEnv * env, jobject obj); }; -JNIEXPORT void JNICALL Java_osg_AndroidExample_osgNativeLib_init(JNIEnv * env, jobject obj, jint width, jint height){ +JNIEXPORT void JNICALL Java_org_openpilot_osg_osgNativeLib_init(JNIEnv * env, jobject obj, jint width, jint height){ mainApp.initOsgWindow(0,0,width,height); } -JNIEXPORT void JNICALL Java_osg_AndroidExample_osgNativeLib_step(JNIEnv * env, jobject obj){ +JNIEXPORT void JNICALL Java_org_openpilot_osg_osgNativeLib_step(JNIEnv * env, jobject obj){ mainApp.draw(); } -JNIEXPORT void JNICALL Java_osg_AndroidExample_osgNativeLib_clearContents(JNIEnv * env, jobject obj){ +JNIEXPORT void JNICALL Java_org_openpilot_osg_osgNativeLib_clearContents(JNIEnv * env, jobject obj){ mainApp.clearScene(); } -JNIEXPORT void JNICALL Java_osg_AndroidExample_osgNativeLib_mouseButtonPressEvent(JNIEnv * env, jobject obj, jfloat x, jfloat y, jint button){ +JNIEXPORT void JNICALL Java_org_openpilot_osg_osgNativeLib_mouseButtonPressEvent(JNIEnv * env, jobject obj, jfloat x, jfloat y, jint button){ mainApp.mouseButtonPressEvent(x,y,button); } -JNIEXPORT void JNICALL Java_osg_AndroidExample_osgNativeLib_mouseButtonReleaseEvent(JNIEnv * env, jobject obj, jfloat x, jfloat y, jint button){ +JNIEXPORT void JNICALL Java_org_openpilot_osg_osgNativeLib_mouseButtonReleaseEvent(JNIEnv * env, jobject obj, jfloat x, jfloat y, jint button){ mainApp.mouseButtonReleaseEvent(x,y,button); } -JNIEXPORT void JNICALL Java_osg_AndroidExample_osgNativeLib_mouseMoveEvent(JNIEnv * env, jobject obj, jfloat x, jfloat y){ +JNIEXPORT void JNICALL Java_org_openpilot_osg_osgNativeLib_mouseMoveEvent(JNIEnv * env, jobject obj, jfloat x, jfloat y){ mainApp.mouseMoveEvent(x,y); } -JNIEXPORT void JNICALL Java_osg_AndroidExample_osgNativeLib_keyboardDown(JNIEnv * env, jobject obj, jint key){ +JNIEXPORT void JNICALL Java_org_openpilot_osg_osgNativeLib_keyboardDown(JNIEnv * env, jobject obj, jint key){ mainApp.keyboardDown(key); } -JNIEXPORT void JNICALL Java_osg_AndroidExample_osgNativeLib_keyboardUp(JNIEnv * env, jobject obj, jint key){ +JNIEXPORT void JNICALL Java_org_openpilot_osg_osgNativeLib_keyboardUp(JNIEnv * env, jobject obj, jint key){ mainApp.keyboardUp(key); } -JNIEXPORT jintArray JNICALL Java_osg_AndroidExample_osgNativeLib_getClearColor(JNIEnv * env, jobject obj){ +JNIEXPORT jintArray JNICALL Java_org_openpilot_osg_osgNativeLib_getClearColor(JNIEnv * env, jobject obj){ jintArray color; color = env->NewIntArray(3); @@ -69,23 +69,23 @@ JNIEXPORT jintArray JNICALL Java_osg_AndroidExample_osgNativeLib_getClearColor(J return color; } -JNIEXPORT void JNICALL Java_osg_AndroidExample_osgNativeLib_setClearColor(JNIEnv * env, jobject obj, jint red, jint green, jint blue){ +JNIEXPORT void JNICALL Java_org_openpilot_osg_osgNativeLib_setClearColor(JNIEnv * env, jobject obj, jint red, jint green, jint blue){ osg::Vec4 tVec((float) red / 255.0f, (float) green / 255.0f, (float) blue / 255.0f, 0.0f); mainApp.setClearColor(tVec); } -JNIEXPORT void JNICALL Java_osg_AndroidExample_osgNativeLib_loadObject(JNIEnv * env, jobject obj, jstring address){ +JNIEXPORT void JNICALL Java_org_openpilot_osg_osgNativeLib_loadObject(JNIEnv * env, jobject obj, jstring address){ //Import Strings from JNI - const char *nativeAddress = env->GetStringUTFChars(address, false); + const char *nativeAddress = env->GetStringUTFChars(address, JNI_FALSE); mainApp.loadObject(std::string(nativeAddress)); //Release Strings to JNI env->ReleaseStringUTFChars(address, nativeAddress); } -JNIEXPORT void JNICALL Java_osg_AndroidExample_osgNativeLib_loadObject(JNIEnv * env, jobject obj, jstring address, jstring name){ +JNIEXPORT void JNICALL Java_org_openpilot_osg_osgNativeLib_loadObject(JNIEnv * env, jobject obj, jstring address, jstring name){ //Import Strings from JNI - const char *nativeAddress = env->GetStringUTFChars(address, false); - const char *nativeName = env->GetStringUTFChars(name, false); + const char *nativeAddress = env->GetStringUTFChars(address, JNI_FALSE); + const char *nativeName = env->GetStringUTFChars(name, JNI_FALSE); mainApp.loadObject(std::string(nativeName),std::string(nativeAddress)); @@ -93,12 +93,12 @@ JNIEXPORT void JNICALL Java_osg_AndroidExample_osgNativeLib_loadObject(JNIEnv * env->ReleaseStringUTFChars(address, nativeAddress); env->ReleaseStringUTFChars(address, nativeName); } -JNIEXPORT void JNICALL Java_osg_AndroidExample_osgNativeLib_unLoadObject(JNIEnv * env, jobject obj, jint number){ +JNIEXPORT void JNICALL Java_org_openpilot_osg_osgNativeLib_unLoadObject(JNIEnv * env, jobject obj, jint number){ mainApp.unLoadObject(number); } -JNIEXPORT jobjectArray JNICALL Java_osg_AndroidExample_osgNativeLib_getObjectNames(JNIEnv * env, jobject obj){ +JNIEXPORT jobjectArray JNICALL Java_org_openpilot_osg_osgNativeLib_getObjectNames(JNIEnv * env, jobject obj){ jobjectArray fileNames; unsigned int numModels = mainApp.getNumberObjects(); diff --git a/androidgcs/res/layout/ui_layout_gles.xml b/androidgcs/res/layout/ui_layout_gles.xml new file mode 100644 index 000000000..d645559aa --- /dev/null +++ b/androidgcs/res/layout/ui_layout_gles.xml @@ -0,0 +1,44 @@ + + + + + + + + +