diff options
| -rw-r--r-- | altoslib/AltosLink.java | 6 | ||||
| -rw-r--r-- | altoslib/AltosTelemetryReader.java | 9 |
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(); |