X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=altoslib%2FAltosState.java;h=28fdbb244c3afffd6da9e530da4c84fce71bafec;hp=9ee3d57ddf814fe340ef53df61fa2e31758d5cae;hb=HEAD;hpb=6dbb362b2d1df4d8c2d301e90624aceef8051ef5 diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java index 9ee3d57d..28fdbb24 100644 --- a/altoslib/AltosState.java +++ b/altoslib/AltosState.java @@ -20,7 +20,7 @@ * Track flight state from telemetry or eeprom data stream */ -package org.altusmetrum.altoslib_12; +package org.altusmetrum.altoslib_14; public class AltosState extends AltosDataListener { @@ -307,13 +307,12 @@ public class AltosState extends AltosDataListener { 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(); } @@ -480,7 +479,7 @@ public class AltosState extends AltosDataListener { class AltosPressure extends AltosValue { void set(double p, double time) { super.set(p, time); - if (state == AltosLib.ao_flight_pad) + if (state() == AltosLib.ao_flight_pad) ground_pressure.set_filtered(p, time); double a = pressure_to_altitude(p); altitude.set_computed(a, time); @@ -513,26 +512,27 @@ public class AltosState extends AltosDataListener { } public double height() { - double k = kalman_height.value(); - if (k != AltosLib.MISSING) - return k; - double b = baro_height(); if (b != AltosLib.MISSING) return b; + double k = kalman_height.value(); + if (k != AltosLib.MISSING) + return k; + return gps_height(); } public double max_height() { - double k = kalman_height.max(); - if (k != AltosLib.MISSING) - return k; - double a = altitude.max(); double g = ground_altitude(); if (a != AltosLib.MISSING && g != AltosLib.MISSING) return a - g; + + double k = kalman_height.max(); + if (k != AltosLib.MISSING) + return k; + return max_gps_height(); } @@ -557,7 +557,7 @@ public class AltosState extends AltosDataListener { class AltosSpeed extends AltosCValue { boolean can_max() { - return state < AltosLib.ao_flight_fast || state == AltosLib.ao_flight_stateless; + return state() < AltosLib.ao_flight_fast || state() == AltosLib.ao_flight_stateless; } void set_accel() { @@ -615,7 +615,7 @@ public class AltosState extends AltosDataListener { class AltosAccel extends AltosCValue { boolean can_max() { - return state < AltosLib.ao_flight_fast || state == AltosLib.ao_flight_stateless; + return state() < AltosLib.ao_flight_fast || state() == AltosLib.ao_flight_stateless; } void set_measured(double a, double time) { @@ -692,7 +692,7 @@ public class AltosState extends AltosDataListener { public double gps_height; - public double pad_lat, pad_lon, pad_alt; + public double pad_lat, pad_lon; public int speak_tick; public double speak_altitude; @@ -703,6 +703,8 @@ public class AltosState extends AltosDataListener { public int pyro_fired; + public double motor_pressure; + public void set_npad(int npad) { this.npad = npad; gps_waiting = MIN_PAD_SAMPLES - npad; @@ -712,11 +714,11 @@ public class AltosState extends AltosDataListener { } public void init() { + super.init(); + set = 0; received_time = System.currentTimeMillis(); - time = AltosLib.MISSING; - state = AltosLib.ao_flight_invalid; landed = false; boost = false; rssi = AltosLib.MISSING; @@ -774,7 +776,6 @@ public class AltosState extends AltosDataListener { pad_lat = AltosLib.MISSING; pad_lon = AltosLib.MISSING; - pad_alt = AltosLib.MISSING; gps_altitude = new AltosGpsAltitude(); gps_ground_altitude = new AltosGpsGroundAltitude(); @@ -819,9 +820,9 @@ public class AltosState extends AltosDataListener { if (gps.locked && gps.nsat >= 4) { /* Track consecutive 'good' gps reports, waiting for 10 of them */ - if (state == AltosLib.ao_flight_pad || state == AltosLib.ao_flight_stateless) { + if (state() == AltosLib.ao_flight_pad || state() == AltosLib.ao_flight_stateless) { set_npad(npad+1); - if (pad_lat != AltosLib.MISSING && (npad < 10 || state == AltosLib.ao_flight_pad)) { + 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; gps_ground_altitude.set_filtered(gps.alt, time); @@ -842,6 +843,8 @@ public class AltosState extends AltosDataListener { gps.climb_rate * gps.climb_rate), time); if (gps.course != AltosLib.MISSING) gps_course.set(gps.course, time); + } else if (state() == AltosLib.ao_flight_pad || state() == AltosLib.ao_flight_stateless) { + set_npad(0); } if (gps.lat != 0 && gps.lon != 0 && pad_lat != AltosLib.MISSING && @@ -858,25 +861,11 @@ public class AltosState extends AltosDataListener { } } - public String state_name() { - return AltosLib.state_name(state); - } - public void set_state(int state) { - if (state != AltosLib.ao_flight_invalid) { - this.state = state; - ascent = (AltosLib.ao_flight_boost <= state && - state <= AltosLib.ao_flight_coast); - boost = (AltosLib.ao_flight_boost == state); - } - } - - public int state() { - return state; - } - - private void re_init() { - init(); + super.set_state(state); + ascent = (AltosLib.ao_flight_boost <= state() && + state() <= AltosLib.ao_flight_coast); + boost = (AltosLib.ao_flight_boost == state()); } public int rssi() { @@ -896,7 +885,8 @@ public class AltosState extends AltosDataListener { received_time = ms; } - public void set_gps(AltosGPS gps) { + public void set_gps(AltosGPS gps, boolean set_location, boolean set_sats) { + super.set_gps(gps, set_location, set_sats); if (gps != null) { this.gps = gps; update_gps(); @@ -910,9 +900,9 @@ public class AltosState extends AltosDataListener { void update_pad_rotation() { if (cal_data().pad_orientation != AltosLib.MISSING && accel_ground_along != AltosLib.MISSING) { - rotation = new AltosRotation(AltosIMU.convert_accel(accel_ground_across - cal_data().accel_zero_across), - AltosIMU.convert_accel(accel_ground_through - cal_data().accel_zero_through), - AltosIMU.convert_accel(accel_ground_along - cal_data().accel_zero_along), + rotation = new AltosRotation(accel_ground_across, + accel_ground_through, + accel_ground_along, cal_data().pad_orientation); orient.set_computed(rotation.tilt(), time); } @@ -1059,8 +1049,11 @@ public class AltosState extends AltosDataListener { this.pyro_fired = fired; } + public void set_motor_pressure(double motor_pressure) { + this.motor_pressure = motor_pressure; + } + public AltosState() { - Thread.dumpStack(); init(); }