altosui: Add telemetry format menu and preferences

[fw/altos] / altosui / AltosSerial.java

diff --git a/altosui/AltosSerial.java b/altosui/AltosSerial.java
index 3ad16b2b955e0d0d7deccb4573d40ff980883d14..a8ba66bde009acc49f7c1c03d71741e7e7110fd2 100644 (file)
--- a/altosui/AltosSerial.java
+++ b/altosui/AltosSerial.java
@@ -47,7 +47,11 @@ public class AltosSerial implements Runnable {
        byte[] line_bytes;
        int line_count;
        boolean monitor_mode;
+       int telemetry;
+       int channel;
        static boolean debug;
+       boolean remote;
+       LinkedList<String> pending_output = new LinkedList<String>();
 
        static void set_debug(boolean new_debug) {
                debug = new_debug;
@@ -83,14 +87,14 @@ public class AltosSerial implements Runnable {
                                                                for (int i = 0; i < line_count; i++)
                                                                        line = line + line_bytes[i];
                                                        }
+                                                       if (debug)
+                                                               System.out.printf("\t\t\t\t\t%s\n", line);
                                                        if (line.startsWith("VERSION") || line.startsWith("CRC")) {
                                                                for (int e = 0; e < monitors.size(); e++) {
                                                                        LinkedBlockingQueue<AltosLine> q = monitors.get(e);
                                                                        q.put(new AltosLine (line));
                                                                }
                                                        } else {
-                                                               if (debug)
-                                                                       System.out.printf("\t\t\t\t\t%s\n", line);
                                                                reply_queue.put(new AltosLine (line));
                                                        }
                                                        line_count = 0;
@@ -114,16 +118,24 @@ public class AltosSerial implements Runnable {
        }
 
        public void flush_output() {
-               if (altos != null)
+               if (altos != null) {
+                       for (String s : pending_output)
+                               System.out.print(s);
+                       pending_output.clear();
                        libaltos.altos_flush(altos);
+               }
        }
 
        public void flush_input() {
                flush_output();
                boolean got_some;
+
+               int timeout = 100;
+               if (remote)
+                       timeout = 300;
                do {
                        try {
-                               Thread.sleep(100);
+                               Thread.sleep(timeout);
                        } catch (InterruptedException ie) {
                        }
                        got_some = !reply_queue.isEmpty();
@@ -180,6 +192,8 @@ public class AltosSerial implements Runnable {
                synchronized (devices_opened) {
                        devices_opened.remove(device.getPath());
                }
+               if (debug)
+                       System.out.printf("Closing %s\n", device.getPath());
        }
 
        private void putc(char c) {
@@ -189,7 +203,7 @@ public class AltosSerial implements Runnable {
 
        public void print(String data) {
                if (debug)
-                       System.out.printf("%s", data);
+                       pending_output.add(data);
                for (int i = 0; i < data.length(); i++)
                        putc(data.charAt(i));
        }
@@ -209,6 +223,8 @@ public class AltosSerial implements Runnable {
                        close();
                        throw new FileNotFoundException(device.toShortString());
                }
+               if (debug)
+                       System.out.printf("Open %s\n", device.getPath());
                input_thread = new Thread(this);
                input_thread.start();
                print("~\nE 0\n");
@@ -217,25 +233,37 @@ public class AltosSerial implements Runnable {
        }
 
        public void set_radio() {
-               set_channel(AltosPreferences.channel(device.getSerial()));
+               telemetry = AltosPreferences.telemetry(device.getSerial());
+               channel = AltosPreferences.channel(device.getSerial());
+               set_channel(channel);
                set_callsign(AltosPreferences.callsign());
        }
 
-       public void set_channel(int channel) {
+       public void set_channel(int in_channel) {
+               channel = in_channel;
                if (altos != null) {
                        if (monitor_mode)
-                               printf("m 0\nc r %d\nm 1\n", channel);
+                               printf("m 0\nc r %d\nm %d\n", channel, telemetry);
                        else
                                printf("c r %d\n", channel);
                        flush_output();
                }
        }
 
+       public void set_telemetry(int in_telemetry) {
+               telemetry = in_telemetry;
+               if (altos != null) {
+                       if (monitor_mode)
+                               printf("m 0\nm %d\n", telemetry);
+                       flush_output();
+               }
+       }
+
        void set_monitor(boolean monitor) {
                monitor_mode = monitor;
                if (altos != null) {
                        if (monitor)
-                               printf("m 1\n");
+                               printf("m %d\n", telemetry);
                        else
                                printf("m 0\n");
                        flush_output();
@@ -250,20 +278,28 @@ public class AltosSerial implements Runnable {
        }
 
        public void start_remote() {
+               if (debug)
+                       System.out.printf("start remote\n");
                set_radio();
                printf("p\nE 0\n");
                flush_input();
+               remote = true;
        }
 
        public void stop_remote() {
+               if (debug)
+                       System.out.printf("stop remote\n");
+               flush_input();
                printf ("~");
                flush_output();
+               remote = false;
        }
 
        public AltosSerial(AltosDevice in_device) throws FileNotFoundException, AltosSerialInUseException {
                device = in_device;
                line = "";
                monitor_mode = false;
+               telemetry = Altos.ao_telemetry_full;
                monitors = new LinkedList<LinkedBlockingQueue<AltosLine>> ();
                reply_queue = new LinkedBlockingQueue<AltosLine> ();
                open();