From 528e2e41112cad8a81bccbb89c3bd202b818a506 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 2 Sep 2013 23:10:23 -0600 Subject: [PATCH] altoslib: More AltosState hacking EasyMini graphs are looking good now. Signed-off-by: Keith Packard --- altoslib/AltosEeprom.java | 7 +- altoslib/AltosEepromGPS.java | 6 +- altoslib/AltosEepromMega.java | 6 +- altoslib/AltosEepromMetrum2.java | 11 +- altoslib/AltosEepromMini.java | 2 + altoslib/AltosEepromTM.java | 12 +- altoslib/AltosState.java | 675 ++++++++++++++++---------- altoslib/AltosTelemetry.java | 2 + altoslib/AltosTelemetryFile.java | 5 +- altoslib/AltosTelemetryLocation.java | 2 +- altoslib/AltosTelemetrySatellite.java | 2 +- altosui/AltosAscent.java | 6 +- altosui/AltosCSV.java | 12 +- altosui/AltosDescent.java | 4 +- altosui/AltosDisplayThread.java | 10 +- altosui/AltosFlightStats.java | 18 +- altosui/AltosFlightStatsTable.java | 6 +- altosui/AltosGraphDataPoint.java | 6 +- altosui/AltosInfoTable.java | 28 +- altosui/AltosKML.java | 4 +- altosui/AltosLanded.java | 4 +- altosui/AltosUI.java | 2 +- 22 files changed, 514 insertions(+), 316 deletions(-) diff --git a/altoslib/AltosEeprom.java b/altoslib/AltosEeprom.java index 081b3be1..3a996ae0 100644 --- a/altoslib/AltosEeprom.java +++ b/altoslib/AltosEeprom.java @@ -47,7 +47,12 @@ public abstract class AltosEeprom implements AltosStateUpdate { public abstract int record_length(); - public abstract void update_state(AltosState state); + public void update_state(AltosState state) { + if (cmd == AltosLib.AO_LOG_FLIGHT) + state.set_boost_tick(tick); + else + state.set_tick(tick); + } public void write(PrintStream out) { out.printf("%c %04x", cmd, tick); diff --git a/altoslib/AltosEepromGPS.java b/altoslib/AltosEepromGPS.java index d8e47a6e..f97fbbf9 100644 --- a/altoslib/AltosEepromGPS.java +++ b/altoslib/AltosEepromGPS.java @@ -71,6 +71,8 @@ public class AltosEepromGPS extends AltosEeprom { } public void update_state(AltosState state) { + super.update_state(state); + AltosGPS gps; /* Flush any pending GPS changes */ @@ -89,11 +91,8 @@ public class AltosEepromGPS extends AltosEeprom { } } - if (cmd != AltosLib.AO_LOG_FLIGHT) - state.set_tick(tick); switch (cmd) { case AltosLib.AO_LOG_FLIGHT: - state.set_boost_tick(tick); state.set_flight(flight()); state.set_ground_accel(ground_accel()); state.set_ground_pressure(ground_pres()); @@ -139,7 +138,6 @@ public class AltosEepromGPS extends AltosEeprom { gps.day = day(); break; case AltosLib.AO_LOG_GPS_SAT: - state.set_tick(tick); gps = state.make_temp_gps(); int n = nsat(); diff --git a/altoslib/AltosEepromMega.java b/altoslib/AltosEepromMega.java index e8f9b1fc..bccfc621 100644 --- a/altoslib/AltosEepromMega.java +++ b/altoslib/AltosEepromMega.java @@ -79,6 +79,8 @@ public class AltosEepromMega extends AltosEeprom { } public void update_state(AltosState state) { + super.update_state(state); + AltosGPS gps; /* Flush any pending GPS changes */ @@ -149,7 +151,7 @@ public class AltosEepromMega extends AltosEeprom { break; case AltosLib.AO_LOG_GPS_TIME: state.set_tick(tick); - gps = state.make_temp_gps(); + gps = state.make_temp_gps(false); gps.lat = latitude() / 1e7; gps.lon = longitude() / 1e7; gps.alt = altitude(); @@ -171,7 +173,7 @@ public class AltosEepromMega extends AltosEeprom { break; case AltosLib.AO_LOG_GPS_SAT: state.set_tick(tick); - gps = state.make_temp_gps(); + gps = state.make_temp_gps(true); int n = nsat(); for (int i = 0; i < n; i++) diff --git a/altoslib/AltosEepromMetrum2.java b/altoslib/AltosEepromMetrum2.java index 5a616e6c..3b494839 100644 --- a/altoslib/AltosEepromMetrum2.java +++ b/altoslib/AltosEepromMetrum2.java @@ -71,6 +71,8 @@ public class AltosEepromMetrum2 extends AltosEeprom { } public void update_state(AltosState state) { + super.update_state(state); + AltosGPS gps; /* Flush any pending GPS changes */ @@ -89,11 +91,8 @@ public class AltosEepromMetrum2 extends AltosEeprom { } } - if (cmd != AltosLib.AO_LOG_FLIGHT) - state.set_tick(tick); switch (cmd) { case AltosLib.AO_LOG_FLIGHT: - state.set_boost_tick(tick); state.set_flight(flight()); state.set_ground_accel(ground_accel()); state.set_ground_pressure(ground_pres()); @@ -115,13 +114,13 @@ public class AltosEepromMetrum2 extends AltosEeprom { break; case AltosLib.AO_LOG_GPS_POS: - gps = state.make_temp_gps(); + gps = state.make_temp_gps(false); gps.lat = latitude() / 1e7; gps.lon = longitude() / 1e7; gps.alt = altitude(); break; case AltosLib.AO_LOG_GPS_TIME: - gps = state.make_temp_gps(); + gps = state.make_temp_gps(false); gps.hour = hour(); gps.minute = minute(); @@ -140,7 +139,7 @@ public class AltosEepromMetrum2 extends AltosEeprom { break; case AltosLib.AO_LOG_GPS_SAT: state.set_tick(tick); - gps = state.make_temp_gps(); + gps = state.make_temp_gps(true); int n = nsat(); for (int i = 0; i < n; i++) diff --git a/altoslib/AltosEepromMini.java b/altoslib/AltosEepromMini.java index 15ec1929..e0eedb73 100644 --- a/altoslib/AltosEepromMini.java +++ b/altoslib/AltosEepromMini.java @@ -52,6 +52,8 @@ public class AltosEepromMini extends AltosEeprom { } public void update_state(AltosState state) { + super.update_state(state); + switch (cmd) { case AltosLib.AO_LOG_FLIGHT: state.set_flight(flight()); diff --git a/altoslib/AltosEepromTM.java b/altoslib/AltosEepromTM.java index 461a7a9c..08f9af5a 100644 --- a/altoslib/AltosEepromTM.java +++ b/altoslib/AltosEepromTM.java @@ -90,7 +90,7 @@ public class AltosEepromTM extends AltosEeprom { state.set_state(a); break; case AltosLib.AO_LOG_GPS_TIME: - gps = state.make_temp_gps(); + gps = state.make_temp_gps(false); gps.hour = (a & 0xff); gps.minute = (a >> 8); @@ -104,29 +104,29 @@ public class AltosEepromTM extends AltosEeprom { AltosLib.AO_GPS_NUM_SAT_SHIFT; break; case AltosLib.AO_LOG_GPS_LAT: - gps = state.make_temp_gps(); + gps = state.make_temp_gps(false); int lat32 = a | (b << 16); gps.lat = (double) lat32 / 1e7; break; case AltosLib.AO_LOG_GPS_LON: - gps = state.make_temp_gps(); + gps = state.make_temp_gps(false); int lon32 = a | (b << 16); gps.lon = (double) lon32 / 1e7; break; case AltosLib.AO_LOG_GPS_ALT: - gps = state.make_temp_gps(); + gps = state.make_temp_gps(false); gps.alt = a; break; case AltosLib.AO_LOG_GPS_SAT: - gps = state.make_temp_gps(); + gps = state.make_temp_gps(true); int svid = a; int c_n0 = b >> 8; gps.add_sat(svid, c_n0); break; case AltosLib.AO_LOG_GPS_DATE: - gps = state.make_temp_gps(); + gps = state.make_temp_gps(false); gps.year = (a & 0xff) + 2000; gps.month = a >> 8; gps.day = b & 0xff; diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java index b80d7b2e..7817c76a 100644 --- a/altoslib/AltosState.java +++ b/altoslib/AltosState.java @@ -30,6 +30,8 @@ public class AltosState implements Cloneable { public int set; + static final double filter_len = 0.5; + /* derived data */ public long received_time; @@ -38,8 +40,199 @@ public class AltosState implements Cloneable { public double prev_time; public double time_change; public int tick; + private int prev_tick; public int boost_tick; + class AltosValue { + private double value; + private double prev_value; + private double max_value; + private double set_time; + private double prev_set_time; + private double max_rate = 1000.0; + + void set(double new_value, double time) { + if (new_value != AltosRecord.MISSING) { + value = new_value; + if (max_value == AltosRecord.MISSING || value > max_value) { + max_value = value; + } + set_time = time; + } + } + + double value() { + return value; + } + + double max() { + return max_value; + } + + double prev() { + return prev_value; + } + + double change() { + if (value != AltosRecord.MISSING && prev_value != AltosRecord.MISSING) + return value - prev_value; + return AltosRecord.MISSING; + } + + double rate() { + double c = change(); + double t = set_time - prev_set_time; + + if (c != AltosRecord.MISSING && t != 0) + return c / t; + return AltosRecord.MISSING; + } + + double integrate() { + if (value == AltosRecord.MISSING) + return AltosRecord.MISSING; + if (prev_value == AltosRecord.MISSING) + return AltosRecord.MISSING; + + return (value + prev_value) / 2 * (set_time - prev_set_time); + } + + double time() { + return set_time; + } + + void set_derivative(AltosValue in) { + double new_value = in.rate(); + + if (new_value == AltosRecord.MISSING) + return; + + /* Clip changes to reduce noise */ + if (prev_value != AltosRecord.MISSING) { + double ddt = in.time() - prev_set_time; + double ddv = (new_value - prev_value) / ddt; + + /* 100gs */ + if (Math.abs(ddv) > 1000) { + if (new_value > prev_value) + new_value = prev_value + ddt * 1000; + else + new_value = prev_value - ddt * 1000; + } + + double f = 1/Math.exp(ddt/ filter_len); + new_value = prev_value * f + new_value * (1-f); + } + + set(new_value, in.time()); + } + + void set_integral(AltosValue in) { + double change = in.integrate(); + + if (change != AltosRecord.MISSING) + set(prev_value + change, in.time()); + } + + void copy(AltosValue old) { + value = old.value; + set_time = old.set_time; + prev_value = old.value; + prev_set_time = old.set_time; + max_value = old.max_value; + } + + AltosValue() { + value = AltosRecord.MISSING; + prev_value = AltosRecord.MISSING; + max_value = AltosRecord.MISSING; + } + } + + class AltosCValue { + AltosValue measured; + AltosValue computed; + + double value() { + double v = measured.value(); + if (v != AltosRecord.MISSING) + return v; + return computed.value(); + } + + boolean is_measured() { + return measured.value() != AltosRecord.MISSING; + } + + double max() { + double m = measured.max(); + + if (m != AltosRecord.MISSING) + return m; + return computed.max(); + } + + double prev_value() { + if (measured.value != AltosRecord.MISSING && measured.prev_value != AltosRecord.MISSING) + return measured.prev_value; + return computed.prev_value; + } + + AltosValue altos_value() { + if (measured.value() != AltosRecord.MISSING) + return measured; + return computed; + } + + double change() { + double c = measured.change(); + if (c == AltosRecord.MISSING) + c = computed.change(); + return c; + } + + double rate() { + double r = measured.rate(); + if (r == AltosRecord.MISSING) + r = computed.rate(); + return r; + } + + void set_measured(double new_value, double time) { + measured.set(new_value, time); + } + + void set_computed(double new_value, double time) { + computed.set(new_value, time); + } + + void set_derivative(AltosValue in) { + computed.set_derivative(in); + } + + void set_derivative(AltosCValue in) { + set_derivative(in.altos_value()); + } + + void set_integral(AltosValue in) { + computed.set_integral(in); + } + + void set_integral(AltosCValue in) { + set_integral(in.altos_value()); + } + + void copy(AltosCValue old) { + measured.copy(old.measured); + computed.copy(old.computed); + } + + AltosCValue() { + measured = new AltosValue(); + computed = new AltosValue(); + } + } + public int state; public int flight; public int serial; @@ -55,36 +248,191 @@ public class AltosState implements Cloneable { public int main_deploy; public int flight_log_max; - public double ground_altitude; - public double ground_pressure; - public double altitude; - public double height; - public double pressure; - public double acceleration; + private double pressure_to_altitude(double p) { + if (p == AltosRecord.MISSING) + return AltosRecord.MISSING; + return AltosConvert.pressure_to_altitude(p); + } + + private AltosCValue ground_altitude; + + public double ground_altitude() { + return ground_altitude.value(); + } + + public void set_ground_altitude(double a) { + ground_altitude.set_measured(a, time); + } + + class AltosGroundPressure extends AltosValue { + void set(double p, double time) { + super.set(p, time); + ground_altitude.set_computed(pressure_to_altitude(p), time); + } + } + + private AltosGroundPressure ground_pressure; + + public double ground_pressure() { + return ground_pressure.value(); + } + + public void set_ground_pressure (double pressure) { + ground_pressure.set(pressure, time); + } + + class AltosAltitude extends AltosCValue { + + private void set_speed(AltosValue v) { + if (!acceleration.is_measured() || !ascent) + speed.set_derivative(this); + } + + void set_computed(double a, double time) { + super.set_computed(a,time); + set_speed(computed); + set |= set_position; + } + + void set_measured(double a, double time) { + super.set_measured(a,time); + set_speed(measured); + set |= set_position; + } + } + + private AltosAltitude altitude; + + public double altitude() { + double a = altitude.value(); + if (a != AltosRecord.MISSING) + return a; + if (gps != null) + return gps.alt; + return AltosRecord.MISSING; + } + + public double max_altitude() { + double a = altitude.max(); + if (a != AltosRecord.MISSING) + return a; + return AltosRecord.MISSING; + } + + public void set_altitude(double new_altitude) { + altitude.set_measured(new_altitude, time); + } + + class AltosPressure extends AltosValue { + void set(double p, double time) { + super.set(p, time); + altitude.set_computed(pressure_to_altitude(p), time); + } + } + + private AltosPressure pressure; + + public double pressure() { + return pressure.value(); + } + + public void set_pressure(double p) { + pressure.set(p, time); + } + + public double height() { + double k = kalman_height.value(); + if (k != AltosRecord.MISSING) + return k; + + double a = altitude(); + double g = ground_altitude(); + if (a != AltosRecord.MISSING && g != AltosRecord.MISSING) + return a - g; + return AltosRecord.MISSING; + } + + public double max_height() { + double k = kalman_height.max(); + if (k != AltosRecord.MISSING) + return k; + + double a = altitude.max(); + double g = ground_altitude(); + if (a != AltosRecord.MISSING && g != AltosRecord.MISSING) + return a - g; + return AltosRecord.MISSING; + } + + class AltosSpeed extends AltosCValue { + + void set_accel() { + acceleration.set_derivative(this); + } + + void set_derivative(AltosCValue in) { + super.set_derivative(in); + set_accel(); + } + + void set_computed(double new_value, double time) { + super.set_computed(new_value, time); + set_accel(); + } + + void set_measured(double new_value, double time) { + super.set_measured(new_value, time); + set_accel(); + } + } + + private AltosSpeed speed; + + public double speed() { + return speed.value(); + } + + public double max_speed() { + return speed.max(); + } + + class AltosAccel extends AltosCValue { + void set_measured(double a, double time) { + super.set_measured(a, time); + if (ascent) + speed.set_integral(this.measured); + } + } + + AltosAccel acceleration; + + public double acceleration() { + return acceleration.value(); + } + + public double max_acceleration() { + return acceleration.max(); + } + + public AltosValue kalman_height, kalman_speed, kalman_acceleration; + + public void set_kalman(double height, double speed, double acceleration) { + kalman_height.set(height, time); + kalman_speed.set(speed, time); + kalman_acceleration.set(acceleration, time); + } + public double battery_voltage; public double pyro_voltage; public double temperature; public double apogee_voltage; public double main_voltage; - public double speed; - public double ignitor_voltage[]; - - public double prev_height; - public double prev_speed; - public double prev_acceleration; - public double prev_max_height; - public double prev_max_acceleration; - public double prev_max_speed; - - public double max_height; - public double max_acceleration; - public double max_speed; - - public double kalman_height, kalman_speed, kalman_acceleration; + public double ignitor_voltage[]; public AltosGPS gps; public AltosGPS temp_gps; + public boolean temp_gps_clear_sats_pending; public boolean gps_pending; public int gps_sequence; @@ -125,14 +473,6 @@ public class AltosState implements Cloneable { public AltosRecordCompanion companion; - public double speed() { - return speed; - } - - public double max_speed() { - return max_speed; - } - public void set_npad(int npad) { this.npad = npad; gps_waiting = MIN_PAD_SAMPLES - npad; @@ -151,6 +491,7 @@ public class AltosState implements Cloneable { time_change = AltosRecord.MISSING; prev_time = AltosRecord.MISSING; tick = AltosRecord.MISSING; + prev_tick = AltosRecord.MISSING; boost_tick = AltosRecord.MISSING; state = AltosLib.ao_flight_invalid; flight = AltosRecord.MISSING; @@ -165,40 +506,27 @@ public class AltosState implements Cloneable { main_deploy = AltosRecord.MISSING; flight_log_max = AltosRecord.MISSING; - ground_altitude = AltosRecord.MISSING; - ground_pressure = AltosRecord.MISSING; - altitude = AltosRecord.MISSING; - height = AltosRecord.MISSING; - pressure = AltosRecord.MISSING; - acceleration = AltosRecord.MISSING; - temperature = AltosRecord.MISSING; - - prev_height = AltosRecord.MISSING; - prev_speed = AltosRecord.MISSING; - prev_acceleration = AltosRecord.MISSING; - - prev_max_height = 0; - prev_max_speed = 0; - prev_max_acceleration = 0; + ground_altitude = new AltosCValue(); + ground_pressure = new AltosGroundPressure(); + altitude = new AltosAltitude(); + pressure = new AltosPressure(); + speed = new AltosSpeed(); + acceleration = new AltosAccel(); + temperature = AltosRecord.MISSING; battery_voltage = AltosRecord.MISSING; pyro_voltage = AltosRecord.MISSING; apogee_voltage = AltosRecord.MISSING; main_voltage = AltosRecord.MISSING; ignitor_voltage = null; - speed = AltosRecord.MISSING; - - kalman_height = AltosRecord.MISSING; - kalman_speed = AltosRecord.MISSING; - kalman_acceleration = AltosRecord.MISSING; - - max_speed = 0; - max_height = 0; - max_acceleration = 0; + kalman_height = new AltosValue(); + kalman_speed = new AltosValue(); + kalman_acceleration = new AltosValue(); gps = null; temp_gps = null; + temp_gps_clear_sats_pending = false; gps_sequence = 0; gps_pending = false; @@ -225,8 +553,10 @@ public class AltosState implements Cloneable { accel_plus_g = AltosRecord.MISSING; accel_minus_g = AltosRecord.MISSING; accel = AltosRecord.MISSING; + ground_accel = AltosRecord.MISSING; ground_accel_avg = AltosRecord.MISSING; + log_format = AltosRecord.MISSING; serial = AltosRecord.MISSING; @@ -247,6 +577,7 @@ public class AltosState implements Cloneable { time = old.time; time_change = 0; tick = old.tick; + prev_tick = old.tick; boost_tick = old.boost_tick; state = old.state; @@ -265,35 +596,22 @@ public class AltosState implements Cloneable { set = 0; - ground_altitude = old.ground_altitude; - altitude = old.altitude; - height = old.height; - pressure = old.pressure; - acceleration = old.acceleration; + ground_altitude.copy(old.ground_altitude); + altitude.copy(old.altitude); + pressure.copy(old.pressure); + speed.copy(old.speed); + acceleration.copy(old.acceleration); + battery_voltage = old.battery_voltage; pyro_voltage = old.pyro_voltage; temperature = old.temperature; apogee_voltage = old.apogee_voltage; main_voltage = old.main_voltage; ignitor_voltage = old.ignitor_voltage; - speed = old.speed; - - prev_height = old.height; - prev_speed = old.speed; - prev_acceleration = old.acceleration; - - prev_max_height = old.max_height; - prev_max_speed = old.max_speed; - prev_max_acceleration = old.max_acceleration; - prev_time = old.time; - max_height = old.max_height; - max_acceleration = old.max_acceleration; - max_speed = old.max_speed; - - kalman_height = old.kalman_height; - kalman_speed = old.kalman_speed; - kalman_acceleration = old.kalman_acceleration; + kalman_height.copy(old.kalman_height); + kalman_speed.copy(old.kalman_speed); + kalman_acceleration.copy(old.kalman_acceleration); if (old.gps != null) gps = old.gps.clone(); @@ -303,6 +621,7 @@ public class AltosState implements Cloneable { temp_gps = old.temp_gps.clone(); else temp_gps = null; + temp_gps_clear_sats_pending = old.temp_gps_clear_sats_pending; gps_sequence = old.gps_sequence; gps_pending = old.gps_pending; @@ -351,122 +670,8 @@ public class AltosState implements Cloneable { baro = old.baro; companion = old.companion; } - - double altitude() { - if (altitude != AltosRecord.MISSING) - return altitude; - if (gps != null) - return gps.alt; - return AltosRecord.MISSING; - } - - void update_vertical_pos() { - - double alt = altitude(); - - if (state == AltosLib.ao_flight_pad && alt != AltosRecord.MISSING && ground_pressure == AltosRecord.MISSING) { - if (ground_altitude == AltosRecord.MISSING) - ground_altitude = alt; - else - ground_altitude = (ground_altitude * 7 + alt) / 8; - } - - if (kalman_height != AltosRecord.MISSING) - height = kalman_height; - else if (altitude != AltosRecord.MISSING && ground_altitude != AltosRecord.MISSING) - height = altitude - ground_altitude; - else - height = AltosRecord.MISSING; - - if (height != AltosRecord.MISSING && height > prev_max_height) - max_height = height; - - update_speed(); - } - - double motion_filter_value() { - return 1/ Math.exp(time_change/2.0); - } - - void update_speed() { - if (kalman_speed != AltosRecord.MISSING) - speed = kalman_speed; - else if (state != AltosLib.ao_flight_invalid && - time_change != AltosRecord.MISSING) - { - if (ascent && acceleration != AltosRecord.MISSING) - { - if (prev_speed == AltosRecord.MISSING) - speed = acceleration * time_change; - else - speed = prev_speed + acceleration * time_change; - } - else if (height != AltosRecord.MISSING && - prev_height != AltosRecord.MISSING && - time_change != 0) - { - double new_speed = (height - prev_height) / time_change; - - if (prev_speed == AltosRecord.MISSING) - speed = new_speed; - else { - double filter = motion_filter_value(); - - speed = prev_speed * filter + new_speed * (1-filter); - } - } - } - if (acceleration == AltosRecord.MISSING) { - if (prev_speed != AltosRecord.MISSING && time_change != 0) { - double new_acceleration = (speed - prev_speed) / time_change; - - if (prev_acceleration == AltosRecord.MISSING) - acceleration = new_acceleration; - else { - double filter = motion_filter_value(); - - acceleration = prev_acceleration * filter + new_acceleration * (1-filter); - } - } - } - if (boost && speed != AltosRecord.MISSING && speed > prev_max_speed) - max_speed = speed; - } - void update_accel() { - if (kalman_acceleration != AltosRecord.MISSING) { - acceleration = kalman_acceleration; - } else { - double ground = ground_accel; - - if (ground == AltosRecord.MISSING) - ground = ground_accel_avg; - if (accel == AltosRecord.MISSING) - return; - if (ground == AltosRecord.MISSING) - return; - if (accel_plus_g == AltosRecord.MISSING) - return; - if (accel_minus_g == AltosRecord.MISSING) - return; - - double counts_per_g = (accel_minus_g - accel_plus_g) / 2.0; - double counts_per_mss = counts_per_g / 9.80665; - acceleration = (ground - accel) / counts_per_mss; - } - - /* Only look at accelerometer data under boost */ - if (boost && acceleration != AltosRecord.MISSING && acceleration > prev_max_acceleration) - max_acceleration = acceleration; - update_speed(); - } - void update_time() { - if (tick != AltosRecord.MISSING) { - time = tick / 100.0; - if (prev_time != AltosRecord.MISSING) - time_change = time - prev_time; - } } void update_gps() { @@ -497,7 +702,7 @@ public class AltosState implements Cloneable { pad_lat != AltosRecord.MISSING && pad_lon != AltosRecord.MISSING) { - double h = height; + double h = height(); if (h == AltosRecord.MISSING) h = 0; @@ -508,16 +713,15 @@ public class AltosState implements Cloneable { } } - public void set_tick(int tick) { - if (tick != AltosRecord.MISSING) { - if (this.tick != AltosRecord.MISSING) { - while (tick < this.tick) - tick += 65536; - time_change = (tick - this.tick) / 100.0; - } else - time_change = 0; - this.tick = tick; - update_time(); + public void set_tick(int new_tick) { + if (new_tick != AltosRecord.MISSING) { + if (prev_tick != AltosRecord.MISSING) { + while (new_tick < prev_tick - 32767) { + new_tick += 65536; + } + } + tick = new_tick; + time = tick / 100.0; } } @@ -600,57 +804,15 @@ public class AltosState implements Cloneable { received_time = ms; } - public void set_altitude(double altitude) { - if (altitude != AltosRecord.MISSING) { - this.altitude = altitude; - update_vertical_pos(); - set |= set_position; - } - } - - public void set_ground_altitude(double ground_altitude) { - if (ground_altitude != AltosRecord.MISSING) { - this.ground_altitude = ground_altitude; - update_vertical_pos(); - } - } - - public void set_ground_pressure (double pressure) { - if (pressure != AltosRecord.MISSING) { - this.ground_pressure = pressure; - set_ground_altitude(AltosConvert.pressure_to_altitude(pressure)); - update_vertical_pos(); - } - } - public void set_gps(AltosGPS gps, int sequence) { if (gps != null) { this.gps = gps.clone(); gps_sequence = sequence; update_gps(); - update_vertical_pos(); set |= set_gps; } } - public void set_kalman(double height, double speed, double acceleration) { - if (height != AltosRecord.MISSING) { - kalman_height = height; - kalman_speed = speed; - kalman_acceleration = acceleration; - update_vertical_pos(); - update_speed(); - update_accel(); - } - } - - public void set_pressure(double pressure) { - if (pressure != AltosRecord.MISSING) { - this.pressure = pressure; - set_altitude(AltosConvert.pressure_to_altitude(pressure)); - } - } - public void make_baro() { if (baro == null) baro = new AltosMs5607(); @@ -674,6 +836,25 @@ public class AltosState implements Cloneable { this.companion = companion; } + void update_accel() { + double ground = ground_accel; + + if (ground == AltosRecord.MISSING) + ground = ground_accel_avg; + if (accel == AltosRecord.MISSING) + return; + if (ground == AltosRecord.MISSING) + return; + if (accel_plus_g == AltosRecord.MISSING) + return; + if (accel_minus_g == AltosRecord.MISSING) + return; + + double counts_per_g = (accel_minus_g - accel_plus_g) / 2.0; + double counts_per_mss = counts_per_g / 9.80665; + acceleration.set_computed((ground - accel) / counts_per_mss, time); + } + public void set_accel_g(double accel_plus_g, double accel_minus_g) { if (accel_plus_g != AltosRecord.MISSING) { this.accel_plus_g = accel_plus_g; @@ -681,6 +862,7 @@ public class AltosState implements Cloneable { update_accel(); } } + public void set_ground_accel(double ground_accel) { if (ground_accel != AltosRecord.MISSING) { this.ground_accel = ground_accel; @@ -754,12 +936,17 @@ public class AltosState implements Cloneable { return tick != AltosRecord.MISSING && serial != AltosRecord.MISSING; } - public AltosGPS make_temp_gps() { + public AltosGPS make_temp_gps(boolean sats) { if (temp_gps == null) { temp_gps = new AltosGPS(gps); - temp_gps.cc_gps_sat = null; } gps_pending = true; + if (!sats) + temp_gps_clear_sats_pending = true; + else if (temp_gps_clear_sats_pending) { + temp_gps.cc_gps_sat = null; + temp_gps_clear_sats_pending = false; + } return temp_gps; } diff --git a/altoslib/AltosTelemetry.java b/altoslib/AltosTelemetry.java index 82e5400e..642e7421 100644 --- a/altoslib/AltosTelemetry.java +++ b/altoslib/AltosTelemetry.java @@ -43,6 +43,8 @@ public abstract class AltosTelemetry implements AltosStateUpdate { } public void update_state(AltosState state) { + if (state.state == AltosLib.ao_flight_invalid) + state.set_state(AltosLib.ao_flight_startup); state.set_serial(serial); state.set_tick(tick); state.set_rssi(rssi, status); diff --git a/altoslib/AltosTelemetryFile.java b/altoslib/AltosTelemetryFile.java index 9e992576..33872688 100644 --- a/altoslib/AltosTelemetryFile.java +++ b/altoslib/AltosTelemetryFile.java @@ -72,13 +72,16 @@ public class AltosTelemetryFile extends AltosStateIterable { /* Find boost tick */ AltosState state = start.clone(); + System.out.printf ("Searching for boost\n"); for (AltosTelemetry telem : telems) { telem.update_state(state); - if (state.state >= AltosLib.ao_flight_boost) { + if (state.state != AltosLib.ao_flight_invalid && state.state >= AltosLib.ao_flight_boost) { + System.out.printf ("boost tick %d\n", state.tick); start.set_boost_tick(state.tick); break; } } + System.out.printf ("Found boost %d\n", start.boost_tick); } public Iterator iterator() { diff --git a/altoslib/AltosTelemetryLocation.java b/altoslib/AltosTelemetryLocation.java index fa3c24d0..50b9dcfc 100644 --- a/altoslib/AltosTelemetryLocation.java +++ b/altoslib/AltosTelemetryLocation.java @@ -61,7 +61,7 @@ public class AltosTelemetryLocation extends AltosTelemetryStandard { public void update_state(AltosState state) { super.update_state(state); - AltosGPS gps = state.make_temp_gps(); + AltosGPS gps = state.make_temp_gps(false); gps.nsat = flags & 0xf; gps.locked = (flags & (1 << 4)) != 0; diff --git a/altoslib/AltosTelemetrySatellite.java b/altoslib/AltosTelemetrySatellite.java index 3f70f212..bd94740f 100644 --- a/altoslib/AltosTelemetrySatellite.java +++ b/altoslib/AltosTelemetrySatellite.java @@ -42,7 +42,7 @@ public class AltosTelemetrySatellite extends AltosTelemetryStandard { public void update_state(AltosState state) { super.update_state(state); - AltosGPS gps = state.make_temp_gps(); + AltosGPS gps = state.make_temp_gps(true); gps.cc_gps_sat = sats; state.set_temp_gps(); diff --git a/altosui/AltosAscent.java b/altosui/AltosAscent.java index ceba2d1d..20474f52 100644 --- a/altosui/AltosAscent.java +++ b/altosui/AltosAscent.java @@ -240,7 +240,7 @@ public class AltosAscent extends JComponent implements AltosFlightDisplay { class Height extends AscentValueHold { void show (AltosState state, AltosListenerState listener_state) { - show(AltosConvert.height, state.height); + show(AltosConvert.height, state.height()); } public Height (GridBagLayout layout, int y) { super (layout, y, "Height"); @@ -251,7 +251,7 @@ public class AltosAscent extends JComponent implements AltosFlightDisplay { class Speed extends AscentValueHold { void show (AltosState state, AltosListenerState listener_state) { - show(AltosConvert.speed, state.speed); + show(AltosConvert.speed, state.speed()); } public Speed (GridBagLayout layout, int y) { super (layout, y, "Speed"); @@ -262,7 +262,7 @@ public class AltosAscent extends JComponent implements AltosFlightDisplay { class Accel extends AscentValueHold { void show (AltosState state, AltosListenerState listener_state) { - show(AltosConvert.accel, state.acceleration); + show(AltosConvert.accel, state.acceleration()); } public Accel (GridBagLayout layout, int y) { super (layout, y, "Acceleration"); diff --git a/altosui/AltosCSV.java b/altosui/AltosCSV.java index c96c815e..bcff393f 100644 --- a/altosui/AltosCSV.java +++ b/altosui/AltosCSV.java @@ -127,12 +127,12 @@ public class AltosCSV implements AltosWriter { void write_basic(AltosState state) { out.printf("%8.2f,%10.2f,%8.2f,%8.2f,%8.2f,%8.2f,%5.1f,%5.2f,%5.2f,%5.2f", - state.acceleration, - state.pressure, - state.altitude, - state.height, - state.speed, - state.speed, + state.acceleration(), + state.pressure(), + state.altitude(), + state.height(), + state.speed(), + state.speed(), state.temperature, state.battery_voltage, state.apogee_voltage, diff --git a/altosui/AltosDescent.java b/altosui/AltosDescent.java index 35efce16..e85717bb 100644 --- a/altosui/AltosDescent.java +++ b/altosui/AltosDescent.java @@ -245,7 +245,7 @@ public class AltosDescent extends JComponent implements AltosFlightDisplay { class Height extends DescentValue { void show (AltosState state, AltosListenerState listener_state) { - show(AltosConvert.height, state.height); + show(AltosConvert.height, state.height()); } public Height (GridBagLayout layout, int x, int y) { super (layout, x, y, "Height"); @@ -256,7 +256,7 @@ public class AltosDescent extends JComponent implements AltosFlightDisplay { class Speed extends DescentValue { void show (AltosState state, AltosListenerState listener_state) { - show(AltosConvert.speed, state.speed); + show(AltosConvert.speed, state.speed()); } public Speed (GridBagLayout layout, int x, int y) { super (layout, x, y, "Speed"); diff --git a/altosui/AltosDisplayThread.java b/altosui/AltosDisplayThread.java index 7a750c86..c894c2d0 100644 --- a/altosui/AltosDisplayThread.java +++ b/altosui/AltosDisplayThread.java @@ -92,14 +92,14 @@ public class AltosDisplayThread extends Thread { state.range >= 0) { voice.speak("Height %s, bearing %s %d, elevation %d, range %s.\n", - AltosConvert.height.say(state.height), + AltosConvert.height.say(state.height()), state.from_pad.bearing_words( AltosGreatCircle.BEARING_VOICE), (int) (state.from_pad.bearing + 0.5), (int) (state.elevation + 0.5), AltosConvert.distance.say(state.range)); } else if (state.state > Altos.ao_flight_pad) { - voice.speak(AltosConvert.height.say_units(state.height)); + voice.speak(AltosConvert.height.say_units(state.height())); } else { reported_landing = 0; } @@ -113,7 +113,7 @@ public class AltosDisplayThread extends Thread { System.currentTimeMillis() - state.received_time >= 15000 || state.state == Altos.ao_flight_landed)) { - if (Math.abs(state.speed) < 20 && state.height < 100) + if (Math.abs(state.speed()) < 20 && state.height() < 100) voice.speak("rocket landed safely"); else voice.speak("rocket may have crashed"); @@ -185,12 +185,12 @@ public class AltosDisplayThread extends Thread { if ((old_state == null || old_state.state <= Altos.ao_flight_boost) && state.state > Altos.ao_flight_boost) { voice.speak("max speed: %s.", - AltosConvert.speed.say_units(state.max_speed + 0.5)); + AltosConvert.speed.say_units(state.max_speed() + 0.5)); ret = true; } else if ((old_state == null || old_state.state < Altos.ao_flight_drogue) && state.state >= Altos.ao_flight_drogue) { voice.speak("max height: %s.", - AltosConvert.height.say_units(state.max_height + 0.5)); + AltosConvert.height.say_units(state.max_height() + 0.5)); ret = true; } } diff --git a/altosui/AltosFlightStats.java b/altosui/AltosFlightStats.java index 50deb6c8..f278012f 100644 --- a/altosui/AltosFlightStats.java +++ b/altosui/AltosFlightStats.java @@ -51,7 +51,7 @@ public class AltosFlightStats { if (state == null) return 0; - double landed_height = state.height; + double landed_height = state.height(); state = null; @@ -62,10 +62,10 @@ public class AltosFlightStats { for (AltosState s : states) { state = s; - if (state.height > landed_height + 10) { + if (state.height() > landed_height + 10) { above = true; } else { - if (above && state.height < landed_height + 2) { + if (above && state.height() < landed_height + 2) { above = false; landed_time = state.time; } @@ -82,7 +82,7 @@ public class AltosFlightStats { for (AltosState s : states) { state = s; - if (state.acceleration < 1) + if (state.acceleration() < 1) boost_time = state.time; if (state.state >= Altos.ao_flight_boost) break; @@ -131,16 +131,16 @@ public class AltosFlightStats { second = state.gps.second; } if (0 <= state.state && state.state < Altos.ao_flight_invalid) { - state_accel[state.state] += state.acceleration; - state_speed[state.state] += state.speed; + state_accel[state.state] += state.acceleration(); + state_speed[state.state] += state.speed(); state_count[state.state]++; if (state_start[state.state] == 0.0) state_start[state.state] = state.time; if (state_end[state.state] < state.time) state_end[state.state] = state.time; - max_height = state.max_height; - max_speed = state.max_speed; - max_acceleration = state.max_acceleration; + max_height = state.max_height(); + max_speed = state.max_speed(); + max_acceleration = state.max_acceleration(); } if (state.gps != null && state.gps.locked && state.gps.nsat >= 4) { if (state.state <= Altos.ao_flight_pad) { diff --git a/altosui/AltosFlightStatsTable.java b/altosui/AltosFlightStatsTable.java index f8a2d4de..b5a92683 100644 --- a/altosui/AltosFlightStatsTable.java +++ b/altosui/AltosFlightStatsTable.java @@ -76,15 +76,15 @@ public class AltosFlightStatsTable extends JComponent { int y = 0; new FlightStat(layout, y++, "Serial", String.format("%d", stats.serial)); new FlightStat(layout, y++, "Flight", String.format("%d", stats.flight)); - if (stats.year > 0 && stats.hour > 0) + if (stats.year != AltosRecord.MISSING && stats.hour != AltosRecord.MISSING) new FlightStat(layout, y++, "Date/Time", String.format("%04d-%02d-%02d", stats.year, stats.month, stats.day), String.format("%02d:%02d:%02d UTC", stats.hour, stats.minute, stats.second)); else { - if (stats.year > 0) + if (stats.year != AltosRecord.MISSING) new FlightStat(layout, y++, "Date", String.format("%04d-%02d-%02d", stats.year, stats.month, stats.day)); - if (stats.hour > 0) + if (stats.hour != AltosRecord.MISSING) new FlightStat(layout, y++, "Time", String.format("%02d:%02d:%02d UTC", stats.hour, stats.minute, stats.second)); } diff --git a/altosui/AltosGraphDataPoint.java b/altosui/AltosGraphDataPoint.java index 537efc44..85a19b00 100644 --- a/altosui/AltosGraphDataPoint.java +++ b/altosui/AltosGraphDataPoint.java @@ -52,13 +52,13 @@ public class AltosGraphDataPoint implements AltosUIDataPoint { double y = AltosRecord.MISSING; switch (index) { case data_height: - y = state.height; + y = state.height(); break; case data_speed: y = state.speed(); break; case data_accel: - y = state.acceleration; + y = state.acceleration(); break; case data_temp: y = state.temperature; @@ -97,7 +97,7 @@ public class AltosGraphDataPoint implements AltosUIDataPoint { y = state.from_pad.distance; break; case data_pressure: - y = state.pressure; + y = state.pressure(); break; } if (y == AltosRecord.MISSING) diff --git a/altosui/AltosInfoTable.java b/altosui/AltosInfoTable.java index 8906920b..cf4642bc 100644 --- a/altosui/AltosInfoTable.java +++ b/altosui/AltosInfoTable.java @@ -107,22 +107,22 @@ public class AltosInfoTable extends JTable { public void show(AltosState state, AltosListenerState listener_state) { info_reset(); if (state != null) { - if (state.altitude != AltosRecord.MISSING) - info_add_row(0, "Altitude", "%6.0f m", state.altitude); - if (state.ground_altitude != AltosRecord.MISSING) - info_add_row(0, "Pad altitude", "%6.0f m", state.ground_altitude); - if (state.height != AltosRecord.MISSING) - info_add_row(0, "Height", "%6.0f m", state.height); - if (state.height != AltosRecord.MISSING) - info_add_row(0, "Max height", "%6.0f m", state.max_height); - if (state.acceleration != AltosRecord.MISSING) - info_add_row(0, "Acceleration", "%8.1f m/s²", state.acceleration); - if (state.acceleration != AltosRecord.MISSING) - info_add_row(0, "Max acceleration", "%8.1f m/s²", state.max_acceleration); + if (state.altitude() != AltosRecord.MISSING) + info_add_row(0, "Altitude", "%6.0f m", state.altitude()); + if (state.ground_altitude() != AltosRecord.MISSING) + info_add_row(0, "Pad altitude", "%6.0f m", state.ground_altitude()); + if (state.height() != AltosRecord.MISSING) + info_add_row(0, "Height", "%6.0f m", state.height()); + if (state.max_height() != AltosRecord.MISSING) + info_add_row(0, "Max height", "%6.0f m", state.max_height()); + if (state.acceleration() != AltosRecord.MISSING) + info_add_row(0, "Acceleration", "%8.1f m/s²", state.acceleration()); + if (state.max_acceleration() != AltosRecord.MISSING) + info_add_row(0, "Max acceleration", "%8.1f m/s²", state.max_acceleration()); if (state.speed() != AltosRecord.MISSING) info_add_row(0, "Speed", "%8.1f m/s", state.speed()); - if (state.speed() != AltosRecord.MISSING) - info_add_row(0, "Max Speed", "%8.1f m/s", state.max_speed); + if (state.max_speed() != AltosRecord.MISSING) + info_add_row(0, "Max Speed", "%8.1f m/s", state.max_speed()); if (state.temperature != AltosRecord.MISSING) info_add_row(0, "Temperature", "%9.2f °C", state.temperature); if (state.battery_voltage != AltosRecord.MISSING) diff --git a/altosui/AltosKML.java b/altosui/AltosKML.java index b79f5c9e..8679178f 100644 --- a/altosui/AltosKML.java +++ b/altosui/AltosKML.java @@ -110,8 +110,8 @@ public class AltosKML implements AltosWriter { AltosGPS gps = state.gps; double altitude; - if (state.height != AltosRecord.MISSING) - altitude = state.height + gps_start_altitude; + if (state.height() != AltosRecord.MISSING) + altitude = state.height() + gps_start_altitude; else altitude = gps.alt; out.printf(kml_coord_fmt, diff --git a/altosui/AltosLanded.java b/altosui/AltosLanded.java index 4cdaa3df..630527a0 100644 --- a/altosui/AltosLanded.java +++ b/altosui/AltosLanded.java @@ -163,7 +163,7 @@ public class AltosLanded extends JComponent implements AltosFlightDisplay, Actio class Height extends LandedValue { void show (AltosState state, AltosListenerState listener_state) { - show(AltosConvert.height, state.max_height); + show(AltosConvert.height, state.max_height()); } public Height (GridBagLayout layout, int y) { super (layout, y, "Maximum Height"); @@ -185,7 +185,7 @@ public class AltosLanded extends JComponent implements AltosFlightDisplay, Actio class Accel extends LandedValue { void show (AltosState state, AltosListenerState listener_state) { - show(AltosConvert.accel, state.max_acceleration); + show(AltosConvert.accel, state.max_acceleration()); } public Accel (GridBagLayout layout, int y) { super (layout, y, "Maximum Acceleration"); diff --git a/altosui/AltosUI.java b/altosui/AltosUI.java index 151f68fd..31d5a54d 100644 --- a/altosui/AltosUI.java +++ b/altosui/AltosUI.java @@ -521,7 +521,7 @@ public class AltosUI extends AltosUIFrame { System.out.printf ("process cat\n"); for (AltosState state : eef) { System.out.printf ("tick %d state %d height %g\n", - state.tick, state.state, state.height); + state.tick, state.state, state.height()); if ((state.set & AltosState.set_gps) != 0) System.out.printf ("time %g lat %g lon %g alt %g\n", state.time_since_boost(), -- 2.30.2