From a78fd852b166362b57d4011c9f79d7ea70a9a475 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Mon, 13 Aug 2012 23:47:37 -0500 Subject: [PATCH] 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(); + } + } + } + } }