X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=ao-tools%2Faltosui%2FAltosState.java;h=3ef00f35d7d87375cf4967553f5e28c3bdd6edd6;hp=da465c75d8a8b01dc993502747ece147c793796f;hb=63bd34cd1b5a411489e8c3ab377f0fe0eec11f67;hpb=65079f84ea64220fa928c3ad96652fed159bf74b diff --git a/ao-tools/altosui/AltosState.java b/ao-tools/altosui/AltosState.java index da465c75..3ef00f35 100644 --- a/ao-tools/altosui/AltosState.java +++ b/ao-tools/altosui/AltosState.java @@ -16,21 +16,23 @@ */ /* - * Track flight state from telemetry data stream + * Track flight state from telemetry or eeprom data stream */ package altosui; -import altosui.AltosTelemetry; +import altosui.AltosRecord; import altosui.AltosGPS; public class AltosState { - AltosTelemetry data; - AltosTelemetry prev_data; + AltosRecord data; /* derived data */ - double report_time; + long report_time; + + double time_change; + int tick; int state; boolean ascent; /* going up? */ @@ -50,82 +52,105 @@ public class AltosState { double max_speed; AltosGPS gps; - AltosGPSTracking gps_tracking; - boolean gps_valid; double pad_lat; double pad_lon; double pad_alt; - double pad_lat_total; - double pad_lon_total; - double pad_alt_total; + + static final int MIN_PAD_SAMPLES = 10; + int npad; - int prev_npad; + int gps_waiting; + boolean gps_ready; AltosGreatCircle from_pad; + double elevation; /* from pad */ + double range; /* total distance */ double gps_height; int speak_tick; double speak_altitude; - static double - aoview_time() - { - return System.currentTimeMillis() / 1000.0; - } - public AltosState (AltosTelemetry cur, AltosTelemetry prev, int prev_npad) { - int i; - double new_height; - double height_change; - double time_change; - double accel_counts_per_mss; - int tick_count; + void init (AltosRecord cur, AltosState prev_state) { + int i; + AltosRecord prev; 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; - - 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); - temperature = AltosConvert.cc_thermometer_to_temperature(data.temp); - 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); - state = data.state(); - if (state == AltosTelemetry.ao_flight_pad) { - if (data.gps.gps_locked && data.gps.nsat >= 4) { + + ground_altitude = data.ground_altitude(); + height = data.altitude() - ground_altitude; + + report_time = System.currentTimeMillis(); + + acceleration = data.acceleration(); + speed = data.accel_speed(); + temperature = data.temperature(); + drogue_sense = data.drogue_voltage(); + main_sense = data.main_voltage(); + battery = data.battery_voltage(); + 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; + max_height = prev_state.max_height; + max_acceleration = prev_state.max_acceleration; + max_speed = prev_state.max_speed; + + /* 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 == Altos.ao_flight_pad) { + if (data.gps != null && data.gps.locked) { 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; pad_alt = data.gps.alt; } + } else { + npad = 0; } } - ascent = (AltosTelemetry.ao_flight_boost <= state && - state <= AltosTelemetry.ao_flight_coast); + + gps_waiting = MIN_PAD_SAMPLES - npad; + if (gps_waiting < 0) + gps_waiting = 0; + + gps_ready = gps_waiting == 0; + + ascent = (Altos.ao_flight_boost <= state && + state <= Altos.ao_flight_coast); /* Only look at accelerometer data on the way up */ if (ascent && acceleration > max_acceleration) @@ -135,32 +160,31 @@ public class AltosState { if (height > max_height) max_height = height; - gps.gps_locked = data.gps.gps_locked; - gps.gps_connected = data.gps.gps_connected; - if (data.gps.gps_locked) { - gps = data.gps; - gps_valid = true; - if (npad > 0) + if (data.gps != null) { + if (gps == null || !gps.locked || data.gps.locked) + gps = data.gps; + if (npad > 0 && gps.locked) { from_pad = new AltosGreatCircle(pad_lat, pad_lon, gps.lat, gps.lon); + } } + elevation = 0; + range = -1; if (npad > 0) { gps_height = gps.alt - pad_alt; + if (from_pad != null) { + elevation = Math.atan2(height, from_pad.distance) * 180 / Math.PI; + range = Math.sqrt(height * height + from_pad.distance * from_pad.distance); + } } else { gps_height = 0; } } - public AltosState(AltosTelemetry cur) { - this(cur, cur, 0); + public AltosState(AltosRecord cur) { + init(cur, null); } - public AltosState (AltosTelemetry cur, AltosState prev) { - this(cur, prev.data, prev.npad); - if (gps == null) { - gps = prev.gps; - gps_valid = prev.gps_valid; - } - if (gps_tracking == null) - gps_tracking = prev.gps_tracking; + public AltosState (AltosRecord cur, AltosState prev) { + init(cur, prev); } }