rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-29 14:52:12 +01:00
Code Issues Releases Activity

Merge branch 'android-telem' into android

Browse Source
This commit is contained in:
James Cotton 2012-08-28 11:32:08 -05:00
commit 150af9eb60
13 changed files with 1692 additions and 871 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
androidgcs/AndroidManifest.xml
View File

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

4
androidgcs/res/values/arrays.xml
View File

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

9
androidgcs/res/xml/device_filter.xml Normal file
View File

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

2
androidgcs/src/org/openpilot/androidgcs/Controller.java
View File

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

109
androidgcs/src/org/openpilot/androidgcs/telemetry/BluetoothUAVTalk.java
View File

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

615
androidgcs/src/org/openpilot/androidgcs/telemetry/HidUAVTalk.java Normal file
View File

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

155
androidgcs/src/org/openpilot/androidgcs/telemetry/OPTelemetryService.java
View File

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

96
androidgcs/src/org/openpilot/androidgcs/telemetry/TcpUAVTalk.java
View File

@ -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;
}
}
}

247
androidgcs/src/org/openpilot/androidgcs/telemetry/TelemetryTask.java Normal file
View File

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

1274
androidgcs/src/org/openpilot/uavtalk/Telemetry.java
View File

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

12
androidgcs/src/org/openpilot/uavtalk/TelemetryMonitor.java
View File

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

9
androidgcs/src/org/openpilot/uavtalk/UAVTalk.java
View File

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

20
androidgcs/tests/org/openpilot/uavtalk/TelemetryMonitorTest.java
View File

@ -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");
}