X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=altoslib%2FAltosState.java;h=846bda42dd834f454c8dc225de5bc4a42a44643f;hp=830e95f311c19d6af0c0ef78821aa565bff55ef2;hb=e8794ab1d512418e312a1a779777119e20dc8c38;hpb=00ae706dab6e8fddef4c5730a17c433a022228b7 diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java index 830e95f3..846bda42 100644 --- a/altoslib/AltosState.java +++ b/altoslib/AltosState.java @@ -3,7 +3,8 @@ * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. * * This program is distributed in the hope that it will be useful, but * WITHOUT ANY WARRANTY; without even the implied warranty of @@ -19,9 +20,11 @@ * Track flight state from telemetry or eeprom data stream */ -package org.altusmetrum.altoslib_5; +package org.altusmetrum.altoslib_11; -public class AltosState implements Cloneable { +import java.io.*; + +public class AltosState extends AltosFlightListener implements Cloneable { public static final int set_position = 1; public static final int set_gps = 2; @@ -29,8 +32,9 @@ public class AltosState implements Cloneable { public int set; + static final double filter_len = 2.0; static final double ascent_filter_len = 0.5; - static final double descent_filter_len = 0.5; + static final double descent_filter_len = 5.0; /* derived data */ @@ -39,9 +43,7 @@ public class AltosState implements Cloneable { public double time; public double prev_time; public double time_change; - public int tick; private int prev_tick; - public int boost_tick; class AltosValue { double value; @@ -62,8 +64,10 @@ public class AltosState implements Cloneable { } void set_filtered(double new_value, double time) { - if (prev_value != AltosLib.MISSING) - new_value = (prev_value * 15.0 + new_value) / 16.0; + if (prev_value != AltosLib.MISSING) { + double f = 1/Math.exp((time - prev_set_time) / filter_len); + new_value = f * new_value + (1-f) * prev_value; + } set(new_value, time); } @@ -177,6 +181,7 @@ public class AltosState implements Cloneable { prev_value = AltosLib.MISSING; max_value = AltosLib.MISSING; } + } class AltosCValue { @@ -185,6 +190,10 @@ public class AltosState implements Cloneable { boolean can_max() { return c_can_max(); } + + AltosIValue() { + super(); + } }; public AltosIValue measured; @@ -273,13 +282,13 @@ public class AltosState implements Cloneable { computed.finish_update(); } - AltosCValue() { + public AltosCValue() { measured = new AltosIValue(); computed = new AltosIValue(); } } - public int state; + private int state; public int flight; public int serial; public int altitude_32; @@ -324,6 +333,10 @@ public class AltosState implements Cloneable { pad_alt = value(); gps_altitude.set_gps_height(); } + + AltosGpsGroundAltitude() { + super(); + } } private AltosGpsGroundAltitude gps_ground_altitude; @@ -347,6 +360,10 @@ public class AltosState implements Cloneable { super.set_measured(p, time); ground_altitude.set_computed(pressure_to_altitude(p), time); } + + AltosGroundPressure () { + super(); + } } private AltosGroundPressure ground_pressure; @@ -377,6 +394,10 @@ public class AltosState implements Cloneable { set_speed(measured); set |= set_position; } + + AltosAltitude() { + super(); + } } private AltosAltitude altitude; @@ -397,6 +418,10 @@ public class AltosState implements Cloneable { super.set(a, t); set_gps_height(); } + + AltosGpsAltitude() { + super(); + } } private AltosGpsAltitude gps_altitude; @@ -472,6 +497,10 @@ public class AltosState implements Cloneable { double a = pressure_to_altitude(p); altitude.set_computed(a, time); } + + AltosPressure() { + super(); + } } private AltosPressure pressure; @@ -484,6 +513,25 @@ public class AltosState implements Cloneable { pressure.set(p, time); } + class AltosForce extends AltosValue { + void set(double p, double time) { + super.set(p, time); + } + + AltosForce() { + super(); + } + } + private AltosForce thrust; + + public double thrust() { + return thrust.value(); + } + + public void set_thrust(double N) { + thrust.set(N, time); + } + public double baro_height() { double a = altitude(); double g = ground_altitude(); @@ -558,6 +606,10 @@ public class AltosState implements Cloneable { super.set_measured(new_value, time); set_accel(); } + + AltosSpeed() { + super(); + } } private AltosSpeed speed; @@ -599,6 +651,10 @@ public class AltosState implements Cloneable { if (ascent) speed.set_integral(this.measured); } + + AltosAccel() { + super(); + } } AltosAccel acceleration; @@ -679,6 +735,7 @@ public class AltosState implements Cloneable { public double ground_accel_avg; public int log_format; + public int log_space; public String product; public AltosMs5607 baro; @@ -722,6 +779,7 @@ public class AltosState implements Cloneable { ground_pressure = new AltosGroundPressure(); altitude = new AltosAltitude(); pressure = new AltosPressure(); + thrust = new AltosForce(); speed = new AltosSpeed(); acceleration = new AltosAccel(); orient = new AltosCValue(); @@ -796,6 +854,7 @@ public class AltosState implements Cloneable { ground_accel_avg = AltosLib.MISSING; log_format = AltosLib.MISSING; + log_space = AltosLib.MISSING; product = null; serial = AltosLib.MISSING; receiver_serial = AltosLib.MISSING; @@ -829,6 +888,8 @@ public class AltosState implements Cloneable { return; } + super.copy(old); + received_time = old.received_time; time = old.time; time_change = old.time_change; @@ -955,6 +1016,7 @@ public class AltosState implements Cloneable { ground_accel_avg = old.ground_accel_avg; log_format = old.log_format; + log_space = old.log_space; product = old.product; serial = old.serial; receiver_serial = old.receiver_serial; @@ -1029,15 +1091,14 @@ public class AltosState implements Cloneable { } } - public void set_boost_tick(int boost_tick) { - if (boost_tick != AltosLib.MISSING) - this.boost_tick = boost_tick; - } - public String state_name() { return AltosLib.state_name(state); } + public void set_product(String product) { + this.product = product; + } + public void set_state(int state) { if (state != AltosLib.ao_flight_invalid) { this.state = state; @@ -1047,6 +1108,10 @@ public class AltosState implements Cloneable { } } + public int state() { + return state; + } + public void set_device_type(int device_type) { this.device_type = device_type; switch (device_type) { @@ -1065,6 +1130,10 @@ public class AltosState implements Cloneable { } } + public void set_log_space(int log_space) { + this.log_space = log_space; + } + public void set_flight_params(int apogee_delay, int main_deploy) { this.apogee_delay = apogee_delay; this.main_deploy = main_deploy; @@ -1334,7 +1403,7 @@ public class AltosState implements Cloneable { public void set_ms5607(AltosMs5607 ms5607) { baro = ms5607; - if (baro != null) { + if (baro != null && baro.pa != AltosLib.MISSING && baro.cc != AltosLib.MISSING) { set_pressure(baro.pa); set_temperature(baro.cc / 100.0); } @@ -1349,11 +1418,6 @@ public class AltosState implements Cloneable { } } - public void make_companion (int nchannels) { - if (companion == null) - companion = new AltosCompanion(nchannels); - } - public void set_companion(AltosCompanion companion) { this.companion = companion; } @@ -1453,28 +1517,57 @@ public class AltosState implements Cloneable { return tick != AltosLib.MISSING && serial != AltosLib.MISSING; } - public AltosGPS make_temp_gps(boolean sats) { - if (temp_gps == null) { - temp_gps = new AltosGPS(gps); - } - gps_pending = true; - if (sats) { - if (tick != temp_gps_sat_tick) - temp_gps.cc_gps_sat = null; - temp_gps_sat_tick = tick; - } - return temp_gps; - } - public void set_temp_gps() { set_gps(temp_gps, gps_sequence + 1); gps_pending = false; - temp_gps = null; + super.set_temp_gps(); + } + + public void set_config_data(AltosConfigData config_data) { + if (config_data.callsign != null) + set_callsign(config_data.callsign); + if (config_data.accel_cal_plus != AltosLib.MISSING && + config_data.accel_cal_minus != AltosLib.MISSING) + set_accel_g(config_data.accel_cal_plus, config_data.accel_cal_minus); + if (config_data.product != null) + set_product(config_data.product); + if (config_data.log_format != AltosLib.MISSING) + set_log_format(config_data.log_format); + if (config_data.serial != AltosLib.MISSING) + set_serial(config_data.serial); + AltosMs5607 ms5607 = new AltosMs5607(config_data); + if (ms5607.valid_config()) + set_ms5607(ms5607); } public AltosState clone() { AltosState s = new AltosState(); s.copy(this); + + /* Code to test state save/restore. Enable only for that purpose + */ + if (false) { + AltosJson json = new AltosJson(this); + String onetrip = json.toPrettyString(); + AltosJson back = AltosJson.fromString(onetrip); + AltosState tripstate = (AltosState) back.make(this.getClass()); + AltosJson tripjson = new AltosJson(tripstate); + String twotrip = tripjson.toPrettyString(); + + if (!onetrip.equals(twotrip)) { + try { + FileWriter one_file = new FileWriter("one.json", true); + one_file.write(onetrip); + one_file.flush(); + FileWriter two_file = new FileWriter("two.json", true); + two_file.write(twotrip); + two_file.flush(); + } catch (Exception e) { + } + System.out.printf("json error\n"); + System.exit(1); + } + } return s; }