X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=altoslib%2FAltosState.java;h=ddda82b9e4fd3a686ff8cac1eead1b3e8a0a38bd;hp=5a805fc6bf6864b75a146b64a411746583084c48;hb=71715337eb532a1fbe1a753240e7417d5223489f;hpb=4e22b34bde421a9df090c9196fd4347468c8176a diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java index 5a805fc6..ddda82b9 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_4; public class AltosState implements Cloneable { @@ -29,8 +29,8 @@ public class AltosState implements Cloneable { public int set; - static final double ascent_filter_len = 0.1; - static final double descent_filter_len = 2.0; + static final double ascent_filter_len = 0.5; + static final double descent_filter_len = 0.5; /* derived data */ @@ -49,14 +49,14 @@ public class AltosState implements Cloneable { private double max_value; private double set_time; private double prev_set_time; - private double max_rate = 1000.0; + + 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; } } @@ -109,7 +109,7 @@ public class AltosState implements Cloneable { void set_derivative(AltosValue in) { double n = in.rate(); - + if (n == AltosLib.MISSING) return; @@ -124,13 +124,15 @@ 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 */ - if (Math.abs(ddv) > 1000) { + if (Math.abs(ddv) > max) { if (n > p) - n = p + ddt * 1000; + n = p + ddt * max; else - n = p - ddt * 1000; + n = p - ddt * max; } double filter_len; @@ -245,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); @@ -298,10 +300,35 @@ 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); - ground_altitude.set_computed(pressure_to_altitude(computed.value()), time); + if (!is_measured()) + ground_altitude.set_computed(pressure_to_altitude(computed.value()), time); } void set_measured(double p, double time) { @@ -311,7 +338,7 @@ public class AltosState implements Cloneable { } private AltosGroundPressure ground_pressure; - + public double ground_pressure() { return ground_pressure.value(); } @@ -342,26 +369,72 @@ 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; + 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 gps_ascent_rate() { + return gps_ascent_rate.value(); + } + + public double gps_course() { + return gps_course.value(); + } + class AltosPressure extends AltosValue { void set(double p, double time) { super.set(p, time); @@ -406,8 +479,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); } @@ -445,6 +540,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) @@ -462,6 +562,20 @@ public class AltosState implements Cloneable { return acceleration.max(); } + public AltosValue orient; + + public void set_orient(double new_orient) { + orient.set(new_orient, time); + } + + public double orient() { + 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) { @@ -516,11 +630,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; @@ -558,6 +675,7 @@ public class AltosState implements Cloneable { pressure = new AltosPressure(); speed = new AltosSpeed(); acceleration = new AltosAccel(); + orient = new AltosValue(); temperature = AltosLib.MISSING; battery_voltage = AltosLib.MISSING; @@ -591,6 +709,12 @@ 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_ascent_rate = new AltosValue(); + gps_course = new AltosValue(); + speak_tick = AltosLib.MISSING; speak_altitude = AltosLib.MISSING; @@ -604,11 +728,14 @@ public class AltosState implements Cloneable { ground_accel_avg = AltosLib.MISSING; log_format = AltosLib.MISSING; + product = null; serial = AltosLib.MISSING; receiver_serial = AltosLib.MISSING; baro = null; companion = null; + + pyro_fired = 0; } void finish_update() { @@ -619,6 +746,7 @@ public class AltosState implements Cloneable { pressure.finish_update(); speed.finish_update(); acceleration.finish_update(); + orient.finish_update(); kalman_height.finish_update(); kalman_speed.finish_update(); @@ -636,7 +764,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; @@ -654,14 +782,16 @@ 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); ground_altitude.copy(old.ground_altitude); altitude.copy(old.altitude); pressure.copy(old.pressure); speed.copy(old.speed); acceleration.copy(old.acceleration); + orient.copy(old.orient); battery_voltage = old.battery_voltage; pyro_voltage = old.pyro_voltage; @@ -710,6 +840,13 @@ 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); + pad_lat = old.pad_lat; pad_lon = old.pad_lon; pad_alt = old.pad_alt; @@ -726,20 +863,22 @@ 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; 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; @@ -751,14 +890,21 @@ 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.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.course != AltosLib.MISSING) + gps_course.set(gps.course, time); } if (gps.lat != 0 && gps.lon != 0 && pad_lat != AltosLib.MISSING && @@ -771,7 +917,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; } } @@ -833,7 +978,9 @@ public class AltosState implements Cloneable { if (flight != AltosLib.MISSING && flight != 0) { if (this.flight != AltosLib.MISSING && this.flight != flight) { + int bt = boost_tick; init(); + boost_tick = bt; } this.flight = flight; } @@ -844,7 +991,9 @@ 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; } this.serial = serial; } @@ -925,14 +1074,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) @@ -940,7 +1083,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) { @@ -952,10 +1095,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) { @@ -1010,14 +1151,17 @@ 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; - if (boost_tick != AltosLib.MISSING) { - return (tick - boost_tick) / 100.0; - } - return tick / 100.0; + if (boost_tick == AltosLib.MISSING) + return tick / 100.0; + return (tick - boost_tick) / 100.0; } public boolean valid() {