altoslib: Drop telemetry packets processed while monitoring is disabled

A lag between the thread queuing telemetry packets and one pulling
them out can result in stale telemetry data being returned to the
reader. Fix this by dropping telemetry read while monitoring is disabled.

Signed-off-by: Keith Packard <keithp@keithp.com>

2 files changed, 11 insertions, 4 deletions

diff --git a/altoslib/AltosLink.java b/altoslib/AltosLink.java
index 639d2ac7..95acfc44 100644
--- a/altoslib/AltosLink.java
+++ b/altoslib/AltosLink.java
@@ -386,7 +386,7 @@ public abstract class AltosLink implements Runnable {
 		flush_output();
 	}
 
-	public void set_monitor(boolean monitor) {
+	public synchronized void set_monitor(boolean monitor) {
 		monitor_mode = monitor;
 		if (monitor)
 			printf("m %x\n", telemetry_len());
@@ -395,6 +395,10 @@ public abstract class AltosLink implements Runnable {
 		flush_output();
 	}
 
+	public synchronized boolean get_monitor() {
+		return monitor_mode;
+	}
+
 	private void set_channel(int channel) {
 		if (monitor_mode)
 			printf("m 0\nc r %d\nm %x\n",
diff --git a/altoslib/AltosTelemetryReader.java b/altoslib/AltosTelemetryReader.java
index fa136145..b3b97fae 100644
--- a/altoslib/AltosTelemetryReader.java
+++ b/altoslib/AltosTelemetryReader.java
@@ -32,9 +32,12 @@ public class AltosTelemetryReader extends AltosFlightReader {
 	LinkedBlockingQueue<AltosLine> telem;
 
 	public AltosState read() throws InterruptedException, ParseException, AltosCRCException, IOException {
-		AltosLine l = telem.take();
-		if (l.line == null)
-			throw new IOException("IO error");
+		AltosLine l;
+		do {
+			l = telem.take();
+			if (l.line == null)
+				throw new IOException("IO error");
+		} while (!link.get_monitor());
 		AltosTelemetry	telem = AltosTelemetry.parse(l.line);
 		if (state == null)
 			state = new AltosState();