X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=altoslib%2FAltosState.java;h=5fce15c43865e63b43447db93f59faf725f838f7;hp=4ddcc4ed0bb1f2d01f31e7a5ae2cd068d54af467;hb=e447e1e5c90d3fc1be9c5a1c966c7c688a87ba18;hpb=796cb90e3241103f79ae12565cf6f0dad21cb57c diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java index 4ddcc4ed..5fce15c4 100644 --- a/altoslib/AltosState.java +++ b/altoslib/AltosState.java @@ -19,7 +19,7 @@ * Track flight state from telemetry or eeprom data stream */ -package org.altusmetrum.altoslib_2; +package org.altusmetrum.altoslib_5; public class AltosState implements Cloneable { @@ -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); @@ -270,6 +271,7 @@ public class AltosState implements Cloneable { public int state; public int flight; public int serial; + public int altitude_32; public int receiver_serial; public boolean landed; public boolean ascent; /* going up? */ @@ -299,6 +301,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 +339,7 @@ public class AltosState implements Cloneable { } private AltosGroundPressure ground_pressure; - + public double ground_pressure() { return ground_pressure.value(); } @@ -344,26 +370,89 @@ 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; + + private AltosValue gps_ground_speed; + private AltosValue gps_ascent_rate; + private AltosValue gps_course; + private AltosValue gps_speed; + 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); + } + + public double gps_ground_speed() { + return gps_ground_speed.value(); + } + + public double max_gps_ground_speed() { + return gps_ground_speed.max(); + } + + public double gps_ascent_rate() { + return gps_ascent_rate.value(); + } + + public double max_gps_ascent_rate() { + return gps_ascent_rate.max(); + } + + public double gps_course() { + return gps_course.value(); + } + + public double gps_speed() { + return gps_speed.value(); + } + + public double max_gps_speed() { + return gps_speed.max(); + } + class AltosPressure extends AltosValue { void set(double p, double time) { super.set(p, time); @@ -384,11 +473,7 @@ public class AltosState implements Cloneable { pressure.set(p, time); } - public double height() { - double k = kalman_height.value(); - if (k != AltosLib.MISSING) - return k; - + public double baro_height() { double a = altitude(); double g = ground_altitude(); if (a != AltosLib.MISSING && g != AltosLib.MISSING) @@ -396,6 +481,18 @@ public class AltosState implements Cloneable { return AltosLib.MISSING; } + public double height() { + double k = kalman_height.value(); + if (k != AltosLib.MISSING) + return k; + + double b = baro_height(); + if (b != AltosLib.MISSING) + return b; + + return gps_height(); + } + public double max_height() { double k = kalman_height.max(); if (k != AltosLib.MISSING) @@ -403,13 +500,35 @@ public class AltosState implements Cloneable { double a = altitude.max(); double g = ground_altitude(); + if (a != AltosLib.MISSING && g != AltosLib.MISSING) + return a - g; + return max_gps_height(); + } + + 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 || state == AltosLib.ao_flight_stateless; + } + void set_accel() { acceleration.set_derivative(this); } @@ -436,17 +555,34 @@ public class AltosState implements Cloneable { double v = kalman_speed.value(); if (v != AltosLib.MISSING) return v; - return speed.value(); + v = speed.value(); + if (v != AltosLib.MISSING) + return v; + v = gps_speed(); + if (v != AltosLib.MISSING) + return v; + return AltosLib.MISSING; } public double max_speed() { double v = kalman_speed.max(); if (v != AltosLib.MISSING) return v; - return speed.max(); + v = speed.max(); + if (v != AltosLib.MISSING) + return v; + v = max_gps_speed(); + if (v != AltosLib.MISSING) + return v; + return AltosLib.MISSING; } class AltosAccel extends AltosCValue { + + boolean can_max() { + return state < AltosLib.ao_flight_fast || state == AltosLib.ao_flight_stateless; + } + void set_measured(double a, double time) { super.set_measured(a, time); if (ascent) @@ -474,6 +610,10 @@ public class AltosState implements Cloneable { return orient.value(); } + public double max_orient() { + return orient.max(); + } + public AltosValue kalman_height, kalman_speed, kalman_acceleration; public void set_kalman(double height, double speed, double acceleration) { @@ -528,11 +668,14 @@ public class AltosState implements Cloneable { public double ground_accel_avg; public int log_format; + public String product; public AltosMs5607 baro; public AltosCompanion companion; + public int pyro_fired; + public void set_npad(int npad) { this.npad = npad; gps_waiting = MIN_PAD_SAMPLES - npad; @@ -604,10 +747,18 @@ public class AltosState implements Cloneable { pad_lon = AltosLib.MISSING; pad_alt = AltosLib.MISSING; + gps_altitude = new AltosGpsAltitude(); + gps_ground_altitude = new AltosGpsGroundAltitude(); + gps_ground_speed = new AltosValue(); + gps_speed = new AltosValue(); + gps_ascent_rate = new AltosValue(); + gps_course = new AltosValue(); + speak_tick = AltosLib.MISSING; speak_altitude = AltosLib.MISSING; callsign = null; + firmware_version = null; accel_plus_g = AltosLib.MISSING; accel_minus_g = AltosLib.MISSING; @@ -617,11 +768,15 @@ public class AltosState implements Cloneable { ground_accel_avg = AltosLib.MISSING; log_format = AltosLib.MISSING; + product = null; serial = AltosLib.MISSING; receiver_serial = AltosLib.MISSING; + altitude_32 = AltosLib.MISSING; baro = null; companion = null; + + pyro_fired = 0; } void finish_update() { @@ -650,7 +805,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; @@ -668,7 +823,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); @@ -726,6 +881,14 @@ 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); + gps_ground_speed.copy(old.gps_ground_speed); + gps_ascent_rate.copy(old.gps_ascent_rate); + gps_course.copy(old.gps_course); + gps_speed.copy(old.gps_speed); + pad_lat = old.pad_lat; pad_lon = old.pad_lon; pad_alt = old.pad_alt; @@ -734,6 +897,7 @@ public class AltosState implements Cloneable { speak_altitude = old.speak_altitude; callsign = old.callsign; + firmware_version = old.firmware_version; accel_plus_g = old.accel_plus_g; accel_minus_g = old.accel_minus_g; @@ -742,39 +906,52 @@ public class AltosState implements Cloneable { ground_accel_avg = old.ground_accel_avg; log_format = old.log_format; + product = old.product; serial = old.serial; receiver_serial = old.receiver_serial; + altitude_32 = old.altitude_32; baro = old.baro; companion = old.companion; + + pyro_fired = old.pyro_fired; } - + void update_time() { } void update_gps() { - elevation = 0; - range = -1; - gps_height = 0; + elevation = AltosLib.MISSING; + range = AltosLib.MISSING; if (gps == null) return; if (gps.locked && gps.nsat >= 4) { /* Track consecutive 'good' gps reports, waiting for 10 of them */ - if (state == AltosLib.ao_flight_pad) { + if (state == AltosLib.ao_flight_pad || state == AltosLib.ao_flight_stateless) { set_npad(npad+1); - if (pad_lat != AltosLib.MISSING) { + if (pad_lat != AltosLib.MISSING && (npad < 10 || state == AltosLib.ao_flight_pad)) { 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.climb_rate != AltosLib.MISSING) + gps_ascent_rate.set(gps.climb_rate, time); + if (gps.ground_speed != AltosLib.MISSING) + gps_ground_speed.set(gps.ground_speed, time); + if (gps.climb_rate != AltosLib.MISSING && gps.ground_speed != AltosLib.MISSING) + gps_speed.set(Math.sqrt(gps.ground_speed * gps.ground_speed + + gps.climb_rate * gps.climb_rate), time); + if (gps.course != AltosLib.MISSING) + gps_course.set(gps.course, time); } if (gps.lat != 0 && gps.lon != 0 && pad_lat != AltosLib.MISSING && @@ -787,7 +964,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; } } @@ -820,18 +996,34 @@ public class AltosState implements Cloneable { state <= AltosLib.ao_flight_coast); boost = (AltosLib.ao_flight_boost == state); } - } public void set_device_type(int device_type) { this.device_type = device_type; + switch (device_type) { + case AltosLib.product_telegps: + this.state = AltosLib.ao_flight_stateless; + break; + } } - public void set_config(int major, int minor, int apogee_delay, int main_deploy, int flight_log_max) { - config_major = major; - config_minor = minor; + public void set_log_format(int log_format) { + this.log_format = log_format; + switch (log_format) { + case AltosLib.AO_LOG_FORMAT_TELEGPS: + this.state = AltosLib.ao_flight_stateless; + break; + } + } + + public void set_flight_params(int apogee_delay, int main_deploy) { this.apogee_delay = apogee_delay; this.main_deploy = main_deploy; + } + + public void set_config(int major, int minor, int flight_log_max) { + config_major = major; + config_minor = minor; this.flight_log_max = flight_log_max; } @@ -843,15 +1035,27 @@ public class AltosState implements Cloneable { firmware_version = version; } + public int compare_version(String other_version) { + if (firmware_version == null) + return AltosLib.MISSING; + return AltosLib.compare_version(firmware_version, other_version); + } + + private void re_init() { + int bt = boost_tick; + int rs = receiver_serial; + init(); + boost_tick = bt; + receiver_serial = rs; + } + public void set_flight(int flight) { /* When the flight changes, reset the state */ - if (flight != AltosLib.MISSING && flight != 0) { + if (flight != AltosLib.MISSING) { if (this.flight != AltosLib.MISSING && this.flight != flight) { - int bt = boost_tick; - init(); - boost_tick = bt; + re_init(); } this.flight = flight; } @@ -862,9 +1066,7 @@ public class AltosState implements Cloneable { if (serial != AltosLib.MISSING) { if (this.serial != AltosLib.MISSING && this.serial != serial) { - int bt = boost_tick; - init(); - boost_tick = bt; + re_init(); } this.serial = serial; } @@ -875,6 +1077,15 @@ public class AltosState implements Cloneable { receiver_serial = serial; } + public boolean altitude_32() { + return altitude_32 == 1; + } + + public void set_altitude_32(int altitude_32) { + if (altitude_32 != AltosLib.MISSING) + this.altitude_32 = altitude_32; + } + public int rssi() { if (rssi == AltosLib.MISSING) return 0; @@ -945,14 +1156,8 @@ public class AltosState implements Cloneable { } void update_accel() { - double ground = ground_accel; - - if (ground == AltosLib.MISSING) - ground = ground_accel_avg; if (accel == AltosLib.MISSING) return; - if (ground == AltosLib.MISSING) - return; if (accel_plus_g == AltosLib.MISSING) return; if (accel_minus_g == AltosLib.MISSING) @@ -960,7 +1165,7 @@ public class AltosState implements Cloneable { double counts_per_g = (accel_minus_g - accel_plus_g) / 2.0; double counts_per_mss = counts_per_g / 9.80665; - acceleration.set_measured((ground - accel) / counts_per_mss, time); + acceleration.set_measured((accel_plus_g - accel) / counts_per_mss, time); } public void set_accel_g(double accel_plus_g, double accel_minus_g) { @@ -972,10 +1177,8 @@ public class AltosState implements Cloneable { } public void set_ground_accel(double ground_accel) { - if (ground_accel != AltosLib.MISSING) { + if (ground_accel != AltosLib.MISSING) this.ground_accel = ground_accel; - update_accel(); - } } public void set_accel(double accel) { @@ -1030,6 +1233,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;