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>

1 files changed, 6 insertions, 3 deletions

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();