X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=altoslib%2FAltosState.java;h=4dbd751bee8d74f78562f4832d2be0fcf656897e;hb=ca338a897ba3237652a3ae092e660f26c9e35074;hp=08dafbb4c8ad6d12333e458638e6732abc04aede;hpb=cc06242e882cba462791962c199b7c89e79adc65;p=fw%2Faltos diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java index 08dafbb4..4dbd751b 100644 --- a/altoslib/AltosState.java +++ b/altoslib/AltosState.java @@ -50,12 +50,13 @@ public class AltosState implements Cloneable { private double set_time; private double prev_set_time; + boolean can_max() { return true; } + void set(double new_value, double time) { if (new_value != AltosLib.MISSING) { value = new_value; - if (max_value == AltosLib.MISSING || value > max_value) { + if (can_max() && (max_value == AltosLib.MISSING || value > max_value)) max_value = value; - } set_time = time; } } @@ -108,7 +109,7 @@ public class AltosState implements Cloneable { void set_derivative(AltosValue in) { double n = in.rate(); - + if (n == AltosLib.MISSING) return; @@ -123,7 +124,7 @@ public class AltosState implements Cloneable { /* Clip changes to reduce noise */ double ddt = in.time() - pt; double ddv = (n - p) / ddt; - + final double max = 100000; /* 100gs */ @@ -246,11 +247,11 @@ public class AltosState implements Cloneable { 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); @@ -299,6 +300,30 @@ public class AltosState implements Cloneable { ground_altitude.set_measured(a, time); } + class AltosGpsGroundAltitude extends AltosValue { + void set(double a, double t) { + super.set(a, t); + pad_alt = value(); + gps_altitude.set_gps_height(); + } + + void set_filtered(double a, double t) { + super.set_filtered(a, t); + pad_alt = value(); + gps_altitude.set_gps_height(); + } + } + + private AltosGpsGroundAltitude gps_ground_altitude; + + public double gps_ground_altitude() { + return gps_ground_altitude.value(); + } + + public void set_gps_ground_altitude(double a) { + gps_ground_altitude.set(a, time); + } + class AltosGroundPressure extends AltosCValue { void set_filtered(double p, double time) { computed.set_filtered(p, time); @@ -313,7 +338,7 @@ public class AltosState implements Cloneable { } private AltosGroundPressure ground_pressure; - + public double ground_pressure() { return ground_pressure.value(); } @@ -344,26 +369,56 @@ public class AltosState implements Cloneable { private AltosAltitude altitude; + class AltosGpsAltitude extends AltosValue { + + private void set_gps_height() { + double a = value(); + double g = gps_ground_altitude.value(); + + if (a != AltosLib.MISSING && g != AltosLib.MISSING) + gps_height = a - g; + else + gps_height = AltosLib.MISSING; + } + + void set(double a, double t) { + super.set(a, t); + set_gps_height(); + } + } + + private AltosGpsAltitude gps_altitude; + public double altitude() { double a = altitude.value(); if (a != AltosLib.MISSING) return a; - if (gps != null) - return gps.alt; - return AltosLib.MISSING; + return gps_altitude.value(); } public double max_altitude() { double a = altitude.max(); if (a != AltosLib.MISSING) return a; - return AltosLib.MISSING; + return gps_altitude.max(); } public void set_altitude(double new_altitude) { altitude.set_measured(new_altitude, time); } + public double gps_altitude() { + return gps_altitude.value(); + } + + public double max_gps_altitude() { + return gps_altitude.max(); + } + + public void set_gps_altitude(double new_gps_altitude) { + gps_altitude.set(new_gps_altitude, time); + } + class AltosPressure extends AltosValue { void set(double p, double time) { super.set(p, time); @@ -408,8 +463,30 @@ public class AltosState implements Cloneable { return AltosLib.MISSING; } + public double gps_height() { + double a = gps_altitude(); + double g = gps_ground_altitude(); + + if (a != AltosLib.MISSING && g != AltosLib.MISSING) + return a - g; + return AltosLib.MISSING; + } + + public double max_gps_height() { + double a = gps_altitude.max(); + double g = gps_ground_altitude(); + + if (a != AltosLib.MISSING && g != AltosLib.MISSING) + return a - g; + return AltosLib.MISSING; + } + class AltosSpeed extends AltosCValue { - + + boolean can_max() { + return state < AltosLib.ao_flight_fast; + } + void set_accel() { acceleration.set_derivative(this); } @@ -447,6 +524,11 @@ public class AltosState implements Cloneable { } class AltosAccel extends AltosCValue { + + boolean can_max() { + return state < AltosLib.ao_flight_fast; + } + void set_measured(double a, double time) { super.set_measured(a, time); if (ascent) @@ -537,6 +619,8 @@ public class AltosState implements Cloneable { public AltosCompanion companion; + public int pyro_fired; + public void set_npad(int npad) { this.npad = npad; gps_waiting = MIN_PAD_SAMPLES - npad; @@ -608,6 +692,9 @@ public class AltosState implements Cloneable { pad_lon = AltosLib.MISSING; pad_alt = AltosLib.MISSING; + gps_altitude = new AltosGpsAltitude(); + gps_ground_altitude = new AltosGpsGroundAltitude(); + speak_tick = AltosLib.MISSING; speak_altitude = AltosLib.MISSING; @@ -626,6 +713,8 @@ public class AltosState implements Cloneable { baro = null; companion = null; + + pyro_fired = 0; } void finish_update() { @@ -654,7 +743,7 @@ public class AltosState implements Cloneable { time = old.time; time_change = old.time_change; prev_time = old.time; - + tick = old.tick; prev_tick = old.tick; boost_tick = old.boost_tick; @@ -672,7 +761,7 @@ public class AltosState implements Cloneable { apogee_delay = old.apogee_delay; main_deploy = old.main_deploy; flight_log_max = old.flight_log_max; - + set = 0; ground_pressure.copy(old.ground_pressure); @@ -730,6 +819,10 @@ public class AltosState implements Cloneable { range = old.range; gps_height = old.gps_height; + + gps_altitude.copy(old.gps_altitude); + gps_ground_altitude.copy(old.gps_ground_altitude); + pad_lat = old.pad_lat; pad_lon = old.pad_lon; pad_alt = old.pad_alt; @@ -751,15 +844,16 @@ public class AltosState implements Cloneable { baro = old.baro; companion = old.companion; + + pyro_fired = old.pyro_fired; } - + void update_time() { } void update_gps() { elevation = 0; range = -1; - gps_height = 0; if (gps == null) return; @@ -771,14 +865,15 @@ public class AltosState implements Cloneable { if (pad_lat != AltosLib.MISSING) { pad_lat = (pad_lat * 31 + gps.lat) / 32; pad_lon = (pad_lon * 31 + gps.lon) / 32; - pad_alt = (pad_alt * 31 + gps.alt) / 32; + gps_ground_altitude.set_filtered(gps.alt, time); } } if (pad_lat == AltosLib.MISSING) { pad_lat = gps.lat; pad_lon = gps.lon; - pad_alt = gps.alt; + gps_ground_altitude.set(gps.alt, time); } + gps_altitude.set(gps.alt, time); } if (gps.lat != 0 && gps.lon != 0 && pad_lat != AltosLib.MISSING && @@ -791,7 +886,6 @@ public class AltosState implements Cloneable { from_pad = new AltosGreatCircle(pad_lat, pad_lon, 0, gps.lat, gps.lon, h); elevation = from_pad.elevation; range = from_pad.range; - gps_height = gps.alt - pad_alt; } } @@ -1026,6 +1120,10 @@ public class AltosState implements Cloneable { this.ignitor_voltage = voltage; } + public void set_pyro_fired(int fired) { + this.pyro_fired = fired; + } + public double time_since_boost() { if (tick == AltosLib.MISSING) return 0.0;