From 9cc48698ec14c34d437baad7b6540edc31e9741c Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 2 Apr 2010 22:47:40 -0700 Subject: [PATCH] Fix state updates --- ao-tools/altosui/AltosGPS.java | 1 + ao-tools/altosui/AltosState.java | 86 ++++++++++++++++------------ ao-tools/altosui/AltosTelemetry.java | 1 + ao-tools/altosui/AltosUI.java | 50 ++++++++-------- 4 files changed, 76 insertions(+), 62 deletions(-) diff --git a/ao-tools/altosui/AltosGPS.java b/ao-tools/altosui/AltosGPS.java index d242ad57..92a17018 100644 --- 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 b3054ce9..aacddfdf 100644 --- 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 34b4099f..e13b42e2 100644 --- 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 66c75487..1c6fd699 100644 --- 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; -- 2.30.2