altosui: Deal with telem data that goes backwards in time

[fw/altos] / altosui / AltosTelemetryIterable.java

diff --git a/altosui/AltosTelemetryIterable.java b/altosui/AltosTelemetryIterable.java
index a71ab8726d0dce64bf1b746e0e13e61e444c84b3..278cbfb75b9e2e28ebceff7aa5f0621f84c60cb2 100644 (file)
--- a/altosui/AltosTelemetryIterable.java
+++ b/altosui/AltosTelemetryIterable.java
@@ -22,18 +22,26 @@ import java.util.*;
 import java.text.*;
 
 public class AltosTelemetryIterable extends AltosRecordIterable {
-       LinkedList<AltosRecord> records;
+       TreeSet<AltosRecord>    records;
 
        public Iterator<AltosRecord> iterator () {
                return records.iterator();
        }
 
+       boolean has_gps = false;
+       boolean has_accel = false;
+       boolean has_ignite = false;
+       public boolean has_gps() { return has_gps; }
+       public boolean has_accel() { return has_accel; }
+       public boolean has_ignite() { return has_ignite; };
+
        public AltosTelemetryIterable (FileInputStream input) {
                boolean saw_boost = false;
                int     current_tick = 0;
                int     boost_tick = 0;
 
-               records = new LinkedList<AltosRecord> ();
+               AltosRecord     previous = null;
+               records = new TreeSet<AltosRecord> ();
 
                try {
                        for (;;) {
@@ -42,14 +50,14 @@ public class AltosTelemetryIterable extends AltosRecordIterable {
                                        break;
                                }
                                try {
-                                       AltosTelemetry record = new AltosTelemetry(line);
+                                       AltosRecord record = AltosTelemetry.parse(line, previous);
                                        if (record == null)
                                                break;
                                        if (records.isEmpty()) {
                                                current_tick = record.tick;
                                        } else {
-                                               int tick = record.tick | (current_tick & ~ 0xffff);
-                                               if (tick < current_tick - 0x1000)
+                                               int tick = record.tick;
+                                               while (tick < current_tick - 0x1000)
                                                        tick += 0x10000;
                                                current_tick = tick;
                                                record.tick = current_tick;
@@ -59,17 +67,27 @@ public class AltosTelemetryIterable extends AltosRecordIterable {
                                                saw_boost = true;
                                                boost_tick = record.tick;
                                        }
-                                       records.add(record);
+                                       if (record.accel != AltosRecord.MISSING)
+                                               has_accel = true;
+                                       if (record.gps != null)
+                                               has_gps = true;
+                                       if (record.main != AltosRecord.MISSING)
+                                               has_ignite = true;
+                                       if (previous != null && previous.tick != record.tick)
+                                               records.add(previous);
+                                       previous = record;
                                } catch (ParseException pe) {
                                        System.out.printf("parse exception %s\n", pe.getMessage());
                                } catch (AltosCRCException ce) {
-                                       System.out.printf("crc error\n");
                                }
                        }
                } catch (IOException io) {
                        System.out.printf("io exception\n");
                }
 
+               if (previous != null)
+                       records.add(previous);
+
                /* adjust all tick counts to be relative to boost time */
                for (AltosRecord r : this)
                        r.time = (r.tick - boost_tick) / 100.0;