diff --git a/androidgcs/src/org/openpilot/uavtalk/Telemetry.java b/androidgcs/src/org/openpilot/uavtalk/Telemetry.java index 4bed72fea..d2f82459b 100644 --- a/androidgcs/src/org/openpilot/uavtalk/Telemetry.java +++ b/androidgcs/src/org/openpilot/uavtalk/Telemetry.java @@ -1,5 +1,6 @@ package org.openpilot.uavtalk; +import java.util.ArrayList; import java.util.LinkedList; import java.util.List; import java.util.ListIterator; @@ -91,6 +92,15 @@ public class Telemetry { // 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) { transTimer = new Timer(); transTimerTask = new TimerTask() { @Override @@ -98,9 +108,10 @@ public class Telemetry { transactionTimeout(); } }; - // Setup and start the periodic timer - timeToNextUpdateMs = 0; - + transTimer.schedule(transTimerTask, periodMs, periodMs); + } + + synchronized void updateTimerSetPeriod(int periodMs) { updateTimer = new Timer(); updateTimerTask = new TimerTask() { @Override @@ -108,10 +119,8 @@ public class Telemetry { processPeriodicUpdates(); } }; - updateTimer.scheduleAtFixedRate(updateTimerTask, 1000, 1000); - // Setup and start the stats timer - txErrors = 0; - txRetries = 0; + updateTimer.schedule(updateTimerTask, periodMs, periodMs); + } /** @@ -341,7 +350,7 @@ public class Telemetry { // Start timer if a response is expected if ( transInfo.objRequest || transInfo.acked == Acked.TRUE ) { - transTimer.scheduleAtFixedRate(transTimerTask, REQ_TIMEOUT_MS, REQ_TIMEOUT_MS); + transTimerSetPeriod(REQ_TIMEOUT_MS); } else { @@ -433,6 +442,7 @@ public class Telemetry { // 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(); @@ -538,8 +548,7 @@ public class Telemetry { timeToNextUpdateMs = minDelay; // Restart timer - //updateTimer->start(timeToNextUpdateMs); - updateTimer.scheduleAtFixedRate(updateTimerTask, timeToNextUpdateMs, timeToNextUpdateMs); + updateTimerSetPeriod(timeToNextUpdateMs); } public TelemetryStats getStats() @@ -607,10 +616,10 @@ public class Telemetry { private UAVObjectManager objMngr; private UAVTalk utalk; private UAVObject gcsStatsObj; - private List objList; + private List objList = new ArrayList(); private Queue objQueue = new LinkedList(); private Queue objPriorityQueue = new LinkedList(); - private ObjectTransactionInfo transInfo; + private ObjectTransactionInfo transInfo = new ObjectTransactionInfo(); private boolean transPending; private Timer updateTimer; diff --git a/androidgcs/src/org/openpilot/uavtalk/TelemetryMonitor.java b/androidgcs/src/org/openpilot/uavtalk/TelemetryMonitor.java index dabbf1d4b..bc0bfd6eb 100644 --- a/androidgcs/src/org/openpilot/uavtalk/TelemetryMonitor.java +++ b/androidgcs/src/org/openpilot/uavtalk/TelemetryMonitor.java @@ -92,7 +92,7 @@ public class TelemetryMonitor { } } // Start retrieving - Log.d(TAG,"Starting to retrieve meta and settings objects from the autopilot (%1 objects)" + queue.size()) ; + System.out.println(TAG + "Starting to retrieve meta and settings objects from the autopilot (" + queue.size() + " objects)"); retrieveNextObject(); } @@ -120,7 +120,7 @@ public class TelemetryMonitor { // Get next object from the queue UAVObject obj = queue.remove(0); - Log.d(TAG, "Retrieving object: " + obj.getName()) ; +// Log.d(TAG, "Retrieving object: " + obj.getName()) ; // Connect to object obj.addTransactionCompleted(new Observer() { public void update(Observable observable, Object data) { @@ -161,6 +161,10 @@ public class TelemetryMonitor { public synchronized void flightStatsUpdated(UAVObject obj) { // Force update if not yet connected + gcsStatsObj = objMngr.getObject("GCSTelemetryStats"); + flightStatsObj = objMngr.getObject("FlightTelemetryStats"); + + System.out.println(flightStatsObj.toString()); if ( ((String) gcsStatsObj.getField("Status").getValue()).compareTo("Connected") != 0 || ((String) flightStatsObj.getField("Status").getValue()).compareTo("Connected") == 0 ) { @@ -204,8 +208,17 @@ public class TelemetryMonitor { } // Update connection state - UAVObjectField statusField = gcsStatsObj.getField("Connection"); + gcsStatsObj = objMngr.getObject("GCSTelemetryStats"); + flightStatsObj = objMngr.getObject("FlightTelemetryStats"); + if(gcsStatsObj == null) { + System.out.println("No GCS stats yet"); + return; + } + UAVObjectField statusField = gcsStatsObj.getField("Status"); String oldStatus = (String) statusField.getValue(); + + System.out.println("GCS: " + statusField.getValue() + " Flight: " + flightStatsObj.getField("Status").getValue()); + if ( oldStatus.compareTo("Disconnected") == 0 ) { // Request connection @@ -217,6 +230,7 @@ public class TelemetryMonitor { if ( ((String) flightStatsObj.getField("Status").getValue()).compareTo("HandshakeAck") == 0 ) { statusField.setValue("Connected"); + System.out.println("Connected" + statusField.toString()); } } else if ( oldStatus.compareTo("Connected") == 0 ) @@ -230,11 +244,18 @@ public class TelemetryMonitor { // Force telemetry update if not yet connected boolean gcsStatusChanged = !oldStatus.equals(statusField.getValue()); + + if(gcsStatusChanged) + System.out.println("GCS Status changed"); boolean gcsConnected = ((String) statusField.getValue()).compareTo("Connected") == 0; boolean gcsDisconnected = ((String) statusField.getValue()).compareTo("Disconnected") == 0; + + if(gcsConnected) + System.out.println("Detected here"); if ( gcsStatusChanged || ((String) flightStatsObj.getField("Status").getValue()).compareTo("Disconnected") != 0 ) { + System.out.println("Sending gcs status\n\n\n"); gcsStatsObj.updated(); } @@ -242,14 +263,15 @@ public class TelemetryMonitor { if (gcsConnected && gcsStatusChanged) { setPeriod(STATS_UPDATE_PERIOD_MS); - Log.d(TAG,"Connection with the autopilot established"); + System.out.println(TAG + " Connection with the autopilot established"); + //Log.d(TAG,"Connection with the autopilot established"); startRetrievingObjects(); } if (gcsDisconnected && gcsStatusChanged) { setPeriod(STATS_CONNECT_PERIOD_MS); - Log.d(TAG,"Connection with the autopilot lost"); - Log.d(TAG,"Trying to connect to the autopilot"); + System.out.println(TAG + " Connection with the autopilot lost"); + //Log.d(TAG,"Trying to connect to the autopilot"); //emit disconnected(); } } @@ -260,6 +282,7 @@ public class TelemetryMonitor { periodicTask.cancel(); currentPeriod = ms; + periodicTask = new Timer(); periodicTask.scheduleAtFixedRate(new TimerTask() { @Override public void run() { diff --git a/androidgcs/src/org/openpilot/uavtalk/UAVDataObject.java b/androidgcs/src/org/openpilot/uavtalk/UAVDataObject.java index 5694cc9d8..0acc02226 100644 --- a/androidgcs/src/org/openpilot/uavtalk/UAVDataObject.java +++ b/androidgcs/src/org/openpilot/uavtalk/UAVDataObject.java @@ -24,7 +24,7 @@ public abstract class UAVDataObject extends UAVObject { super.initialize(instID); } - public boolean isMetadata() { return true; }; + public boolean isMetadata() { return false; }; /** * Assign a metaobject */ diff --git a/androidgcs/src/org/openpilot/uavtalk/UAVObject.java b/androidgcs/src/org/openpilot/uavtalk/UAVObject.java index 9e4f69a39..fac67cabc 100644 --- a/androidgcs/src/org/openpilot/uavtalk/UAVObject.java +++ b/androidgcs/src/org/openpilot/uavtalk/UAVObject.java @@ -53,11 +53,14 @@ public abstract class UAVObject { updatedListeners.addObserver(o); } } - void updated() { + void updated(boolean manually) { synchronized(updatedListeners) { updatedListeners.event(); } + if(manually) + updatedManual(); } + void updated() { updated(true); }; private CallbackListener unpackedListeners = new CallbackListener(this); public void addUnpackedObserver(Observer o) { @@ -67,7 +70,6 @@ public abstract class UAVObject { } void unpacked() { synchronized(unpackedListeners) { - System.out.println("Unpacked!: " + unpackedListeners.countObservers() + " " + getName()); unpackedListeners.event(); } } @@ -387,7 +389,7 @@ public abstract class UAVObject { // Trigger all the listeners for the unpack event unpacked(); - updated(); + updated(false); return numBytes; } diff --git a/androidgcs/src/org/openpilot/uavtalk/UAVObjectField.java b/androidgcs/src/org/openpilot/uavtalk/UAVObjectField.java index 2fd7f7846..8d792bb39 100644 --- a/androidgcs/src/org/openpilot/uavtalk/UAVObjectField.java +++ b/androidgcs/src/org/openpilot/uavtalk/UAVObjectField.java @@ -131,7 +131,7 @@ public class UAVObjectField { case UINT32: // TODO: Deal properly with unsigned for (int index = 0; index < numElements; ++index) { - Integer val = (Integer) getValue(index); + Integer val = (int) ( ((Long) getValue(index)).longValue() & 0xffffffffL); dataOut.putInt(val); } break; @@ -364,7 +364,7 @@ public class UAVObjectField { public double getDouble() { return getDouble(0); }; public double getDouble(int index) { - return Double.valueOf((Double) getValue(index)); + return ((Number) getValue(index)).doubleValue(); } public void setDouble(double value) { setDouble(value, 0); }; diff --git a/androidgcs/src/org/openpilot/uavtalk/UAVTalk.java b/androidgcs/src/org/openpilot/uavtalk/UAVTalk.java index 847b7c425..8576be78c 100644 --- a/androidgcs/src/org/openpilot/uavtalk/UAVTalk.java +++ b/androidgcs/src/org/openpilot/uavtalk/UAVTalk.java @@ -200,9 +200,9 @@ public class UAVTalk extends Observable{ * \param[in] allInstances If set true then all instances will be updated * \return Success (true), Failure (false) */ - public boolean sendObject(UAVObject obj, boolean acked, boolean allInstances) + public synchronized boolean sendObject(UAVObject obj, boolean acked, boolean allInstances) { - //QMutexLocker locker(mutex); + System.out.println("Sending obj: " + obj.toString()); if (acked) { return objectTransaction(obj, TYPE_OBJ_ACK, allInstances); @@ -216,9 +216,8 @@ public class UAVTalk extends Observable{ /** * Cancel a pending transaction */ - public void cancelTransaction() + public synchronized void cancelTransaction() { - //QMutexLocker locker(mutex); respObj = null; } @@ -250,6 +249,7 @@ public class UAVTalk extends Observable{ } else if (type == TYPE_OBJ) { + System.out.println("Transmitting object: " + obj.toString()); return transmitObject(obj, TYPE_OBJ, allInstances); } else @@ -763,7 +763,12 @@ public class UAVTalk extends Observable{ bbuf.put((byte) (updateCRC(0, bbuf.array()) & 0xff)); try { - outStream.write(bbuf.array()); + int packlen = bbuf.position(); + bbuf.position(0); + byte [] dst = new byte[packlen]; + bbuf.get(dst,0,packlen); + System.out.println("Outputting: " + dst.length); + outStream.write(dst); } catch (IOException e) { // TODO Auto-generated catch block e.printStackTrace();