X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=altoslib%2FAltosState.java;h=758fd6366b7b7f40d1b3967bcc24bdcb8f4d9313;hb=324073d14ed8215b74d9df51f2229827cd4401d0;hp=a9bb1e70833392321b01097da560eb6819304f42;hpb=be7f56b86478ef4a23a2af77338c580b9c9e5e3b;p=fw%2Faltos diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java index a9bb1e70..758fd636 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_3; 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,7 +49,6 @@ public class AltosState implements Cloneable { 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 != AltosLib.MISSING) { @@ -125,12 +124,14 @@ public class AltosState implements Cloneable { 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; @@ -298,6 +299,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); @@ -343,26 +368,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); @@ -407,6 +462,24 @@ 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 { void set_accel() { @@ -463,6 +536,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) { @@ -559,6 +646,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; @@ -592,6 +680,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; @@ -620,6 +711,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(); @@ -664,6 +756,7 @@ public class AltosState implements Cloneable { 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; @@ -712,6 +805,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; @@ -741,7 +838,6 @@ public class AltosState implements Cloneable { void update_gps() { elevation = 0; range = -1; - gps_height = 0; if (gps == null) return; @@ -753,14 +849,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 && @@ -773,7 +870,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; } } @@ -835,7 +931,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; } @@ -846,7 +944,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; } @@ -927,14 +1027,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) @@ -942,7 +1036,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) { @@ -954,10 +1048,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) { @@ -1016,10 +1108,9 @@ public class AltosState implements Cloneable { 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() {