altoslib: Support multiple telemetry rates
[fw/altos] / altoslib / AltosTelemetryReader.java
index 589f57f25f6ac5e200d1f2cc81f892d33c18945e..5ed501348b07174bb0debee09d499ada26bffb56 100644 (file)
@@ -26,6 +26,7 @@ public class AltosTelemetryReader extends AltosFlightReader {
        AltosLog        log;
        double          frequency;
        int             telemetry;
+       int             telemetry_rate;
        AltosState      state = null;
 
        LinkedBlockingQueue<AltosLine> telem;
@@ -92,6 +93,23 @@ public class AltosTelemetryReader extends AltosFlightReader {
                }
        }
 
+       public boolean supports_telemetry_rate(int telemetry_rate) {
+               try {
+                       /* Version 1.4.1.1 supports all rates, older versions don't */
+                       if (link.config_data().compare_version("1.4.1.1") >= 0)
+                               return true;
+
+                       if (telemetry_rate == AltosLib.ao_telemetry_rate_38400)
+                               return true;
+                       else
+                               return false;
+               } catch (InterruptedException ie) {
+                       return false;
+               } catch (TimeoutException te) {
+                       return true;
+               }
+       }
+
        public void save_frequency() {
                AltosPreferences.set_frequency(link.serial, frequency);
        }
@@ -105,6 +123,15 @@ public class AltosTelemetryReader extends AltosFlightReader {
                AltosPreferences.set_telemetry(link.serial, telemetry);
        }
 
+       public void set_telemetry_rate(int in_telemetry_rate) {
+               telemetry_rate = in_telemetry_rate;
+               link.set_telemetry_rate(telemetry_rate);
+       }
+
+       public void save_telemetry_rate() {
+               AltosPreferences.set_telemetry_rate(link.serial, telemetry_rate);
+       }
+
        public void set_monitor(boolean monitor) {
                link.set_monitor(monitor);
        }
@@ -133,6 +160,8 @@ public class AltosTelemetryReader extends AltosFlightReader {
                        set_frequency(frequency);
                        telemetry = AltosPreferences.telemetry(link.serial);
                        set_telemetry(telemetry);
+                       telemetry_rate = AltosPreferences.telemetry_rate(link.serial);
+                       set_telemetry_rate(telemetry_rate);
                        link.add_monitor(telem);
                        success = true;
                } finally {