Fix state updates

diff --git a/ao-tools/altosui/AltosGPS.java b/ao-tools/altosui/AltosGPS.java
index d242ad57e4c5c03a3355d1059c7dc247254639c6..92a17018a6775566269e4c1c1ccd26c02f61b098 100644 (file)
--- a/ao-tools/altosui/AltosGPS.java
+++ b/ao-tools/altosui/AltosGPS.java
@@ -111,6 +111,7 @@ public class AltosGPS {
                int tracking_channels = AltosParse.parse_int(words[i++]);
                cc_gps_sat = new AltosGPS.AltosGPSSat[tracking_channels];
                for (int chan = 0; chan < tracking_channels; chan++) {
+                       cc_gps_sat[chan] = new AltosGPS.AltosGPSSat();
                        cc_gps_sat[chan].svid = AltosParse.parse_int(words[i++]);
                        cc_gps_sat[chan].c_n0 = AltosParse.parse_int(words[i++]);
                }

diff --git a/ao-tools/altosui/AltosState.java b/ao-tools/altosui/AltosState.java
index b3054ce99e389c69a20245d1bb91b5787d565ed0..aacddfdf5ee3f80e66f13e6be85d8f1b11fe071a 100644 (file)
--- a/ao-tools/altosui/AltosState.java
+++ b/ao-tools/altosui/AltosState.java
@@ -26,17 +26,17 @@ import altosui.AltosGPS;
 
 public class AltosState {
        AltosTelemetry data;
-       AltosTelemetry prev_data;
 
        /* derived data */
 
        double  report_time;
 
+       double  time_change;
+       int     tick;
+
        int     state;
        boolean ascent; /* going up? */
 
-       double  time_change;
-
        double  ground_altitude;
        double  height;
        double  speed;
@@ -56,9 +56,6 @@ public class AltosState {
        double  pad_lat;
        double  pad_lon;
        double  pad_alt;
-       double  pad_lat_total;
-       double  pad_lon_total;
-       double  pad_alt_total;
        int     npad;
        int     prev_npad;
 
@@ -75,29 +72,18 @@ public class AltosState {
                return System.currentTimeMillis() / 1000.0;
        }
 
-       void init (AltosTelemetry cur, AltosTelemetry prev, int prev_npad) {
-               int     i;
-               double  new_height;
-               double  height_change;
+       void init (AltosTelemetry cur, AltosState prev_state) {
+               int             i;
+               AltosTelemetry prev;
                double  accel_counts_per_mss;
-               int     tick_count;
 
                data = cur;
-               prev_data = prev;
-               npad = prev_npad;
-               tick_count = data.tick;
-               if (tick_count < prev_data.tick)
-                       tick_count += 65536;
-               time_change = (tick_count - prev_data.tick) / 100.0;
+
+               ground_altitude = AltosConvert.cc_barometer_to_altitude(data.ground_pres);
+               height = AltosConvert.cc_barometer_to_altitude(data.flight_pres) - ground_altitude;
 
                report_time = aoview_time();
 
-               ground_altitude = AltosConvert.cc_pressure_to_altitude(data.ground_pres);
-               new_height = AltosConvert.cc_pressure_to_altitude(data.flight_pres) - ground_altitude;
-               height_change = new_height - height;
-               height = new_height;
-               if (time_change > 0)
-                       baro_speed = (baro_speed * 3 + (height_change / time_change)) / 4.0;
                accel_counts_per_mss = ((data.accel_minus_g - data.accel_plus_g) / 2.0) / 9.80665;
                acceleration = (data.ground_accel - data.flight_accel) / accel_counts_per_mss;
                speed = data.flight_vel / (accel_counts_per_mss * 100.0);
@@ -105,17 +91,46 @@ public class AltosState {
                drogue_sense = AltosConvert.cc_ignitor_to_voltage(data.drogue);
                main_sense = AltosConvert.cc_ignitor_to_voltage(data.main);
                battery = AltosConvert.cc_battery_to_voltage(data.batt);
+               tick = data.tick;
                state = data.state();
+
+               if (prev_state != null) {
+
+                       /* Preserve any existing gps data */
+                       npad = prev_state.npad;
+                       gps = prev_state.gps;
+                       pad_lat = prev_state.pad_lat;
+                       pad_lon = prev_state.pad_lon;
+                       pad_alt = prev_state.pad_alt;
+
+                       /* make sure the clock is monotonic */
+                       while (tick < prev_state.tick)
+                               tick += 65536;
+
+                       time_change = (tick - prev_state.tick) / 100.0;
+
+                       /* compute barometric speed */
+
+                       double height_change = height - prev_state.height;
+                       if (time_change > 0)
+                               baro_speed = (prev_state.baro_speed * 3 + (height_change / time_change)) / 4.0;
+                       else
+                               baro_speed = prev_state.baro_speed;
+               } else {
+                       npad = 0;
+                       gps = null;
+                       baro_speed = 0;
+                       time_change = 0;
+               }
+
                if (state == AltosTelemetry.ao_flight_pad) {
                        if (data.gps != null && data.gps.gps_locked && data.gps.nsat >= 4) {
                                npad++;
-                               pad_lat_total += data.gps.lat;
-                               pad_lon_total += data.gps.lon;
-                               pad_alt_total += data.gps.alt;
                                if (npad > 1) {
-                                       pad_lat = (pad_lat * 31 + data.gps.lat) / 32.0;
-                                       pad_lon = (pad_lon * 31 + data.gps.lon) / 32.0;
-                                       pad_alt = (pad_alt * 31 + data.gps.alt) / 32.0;
+                                       /* filter pad position */
+                                       pad_lat = (pad_lat * 31.0 + data.gps.lat) / 32.0;
+                                       pad_lon = (pad_lon * 31.0 + data.gps.lon) / 32.0;
+                                       pad_alt = (pad_alt * 31.0 + data.gps.alt) / 32.0;
                                } else {
                                        pad_lat = data.gps.lat;
                                        pad_lon = data.gps.lon;
@@ -135,7 +150,8 @@ public class AltosState {
                if (height > max_height)
                        max_height = height;
                if (data.gps != null) {
-                       gps = data.gps;
+                       if (gps == null || !gps.gps_locked || data.gps.gps_locked)
+                               gps = data.gps;
                        if (npad > 0 && gps.gps_locked)
                                from_pad = new AltosGreatCircle(pad_lat, pad_lon, gps.lat, gps.lon);
                }
@@ -147,16 +163,10 @@ public class AltosState {
        }
 
        public AltosState(AltosTelemetry cur) {
-               init(cur, cur, 0);
+               init(cur, null);
        }
 
        public AltosState (AltosTelemetry cur, AltosState prev) {
-               if (prev == null)
-                       init(cur, cur, 0);
-               else {
-                       init(cur, prev.data, prev.npad);
-                       if (gps == null)
-                               gps = prev.gps;
-               }
+               init(cur, prev);
        }
 }

diff --git a/ao-tools/altosui/AltosTelemetry.java b/ao-tools/altosui/AltosTelemetry.java
index 34b4099f77fc9ad0cad1dd35da9820058940ff6f..e13b42e2df811465faf2c038a828f2d18e5adada 100644 (file)
--- a/ao-tools/altosui/AltosTelemetry.java
+++ b/ao-tools/altosui/AltosTelemetry.java
@@ -181,5 +181,6 @@ public class AltosTelemetry {
                AltosParse.word(words[i++], "a-:");
                accel_minus_g = AltosParse.parse_int(words[i++]);
 
+               gps = new AltosGPS(words, i);
        }
 }

diff --git a/ao-tools/altosui/AltosUI.java b/ao-tools/altosui/AltosUI.java
index 66c75487638cbe2065df73bc2ea3a7b386c32d6f..1c6fd69944ca4e579f785c21cc96302aec2c594f 100644 (file)
--- a/ao-tools/altosui/AltosUI.java
+++ b/ao-tools/altosui/AltosUI.java
@@ -214,7 +214,7 @@ public class AltosUI extends JFrame {
                serialLine = new AltosSerial();
                serialLine.monitor(new AltosUIMonitor());
                int dpi = Toolkit.getDefaultToolkit().getScreenResolution();
-               this.setSize(new Dimension (infoValueMetrics.charWidth('0') * 6 * 15,
+               this.setSize(new Dimension (infoValueMetrics.charWidth('0') * 6 * 20,
                                            statusHeight * 4 + infoHeight * 17));
                this.validate();
                setDefaultCloseOperation(JFrame.DO_NOTHING_ON_CLOSE);
@@ -292,22 +292,20 @@ public class AltosUI extends JFrame {
                info_add_row(0, "Drogue", "%9.2f V", state.drogue_sense);
                info_add_row(0, "Main", "%9.2f V", state.main_sense);
                info_add_row(0, "Pad altitude", "%6.0f    m", state.ground_altitude);
-               if (state.gps != null)
-                       info_add_row(1, "Satellites", "%6d", state.gps.nsat);
-               else
-                       info_add_row(1, "Satellites", "%6d", 0);
-               if (state.gps != null && state.gps.gps_locked) {
-                       info_add_row(1, "GPS", "locked");
-               } else if (state.gps != null && state.gps.gps_connected) {
-                       info_add_row(1, "GPS", "unlocked");
-               } else {
+               if (state.gps == null) {
                        info_add_row(1, "GPS", "not available");
-               }
-               if (state.gps != null) {
+               } else {
+                       if (state.gps.gps_locked)
+                               info_add_row(1, "GPS", "locked");
+                       else if (state.gps.gps_connected)
+                               info_add_row(1, "GPS", "unlocked");
+                       else
+                               info_add_row(1, "GPS", "missing");
+                       info_add_row(1, "Satellites", "%6d", state.gps.nsat);
                        info_add_deg(1, "Latitude", state.gps.lat, 'N', 'S');
                        info_add_deg(1, "Longitude", state.gps.lon, 'E', 'W');
                        info_add_row(1, "GPS altitude", "%d", state.gps.alt);
-                       info_add_row(1, "GPS height", "%d", state.gps_height);
+                       info_add_row(1, "GPS height", "%6.0f", state.gps_height);
                        info_add_row(1, "GPS date", "%04d-%02d-%02d",
                                       state.gps.gps_time.year,
                                       state.gps.gps_time.month,
@@ -321,17 +319,21 @@ public class AltosUI extends JFrame {
                                       state.gps.course);
                        info_add_row(1, "GPS climb rate", "%7.1fm/s",
                                     state.gps.climb_rate);
-                       info_add_row(1, "GPS precision", "%4.1f(hdop) %3dm(h) %3dm(v)",
-                                    state.gps.hdop, state.gps.h_error, state.gps.v_error);
-               }
-               if (state.npad > 0) {
-                       info_add_row(1, "Distance from pad", "%5.0fm", state.from_pad.distance);
-                       info_add_row(1, "Direction from pad", "%4.0f°", state.from_pad.bearing);
-                       info_add_deg(1, "Pad latitude", state.pad_lat, 'N', 'S');
-                       info_add_deg(1, "Pad longitude", state.pad_lon, 'E', 'W');
-                       info_add_row(1, "Pad GPS alt", "%gm", state.pad_alt);
-               }
-               if (state.gps != null && state.gps.gps_connected) {
+                       info_add_row(1, "GPS hdop", "%4.1f", state.gps.hdop);
+                       info_add_row(1, "GPS error", "%3dm(h) %3dm(v)",
+                                    state.gps.h_error, state.gps.v_error);
+                       if (state.npad > 0) {
+                               if (state.from_pad != null) {
+                                       info_add_row(1, "Distance from pad", "%5.0fm", state.from_pad.distance);
+                                       info_add_row(1, "Direction from pad", "%4.0f°", state.from_pad.bearing);
+                               } else {
+                                       info_add_row(1, "Distance from pad", "unknown");
+                                       info_add_row(1, "Direction from pad", "unknown");
+                               }
+                               info_add_deg(1, "Pad latitude", state.pad_lat, 'N', 'S');
+                               info_add_deg(1, "Pad longitude", state.pad_lon, 'E', 'W');
+                               info_add_row(1, "Pad GPS alt", "%gm", state.pad_alt);
+                       }
                        int     nsat_vis = 0;
                        int     c;