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>

diff --git a/altoslib/AltosLink.java b/altoslib/AltosLink.java
index 639d2ac784a4f63642a5310ec1aef91658a38b36..95acfc44cd6a49bcf6400e517d43cc406ca199a3 100644 (file)
--- 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 fa1361454daa7f48795299353fa9f4814f2a1704..b3b97faea349f7212dd724d647fba46ff9cca142 100644 (file)
--- 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();