X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=altoslib%2FAltosState.java;h=5be008b7ed524e694886fb3487fe20f5113786e6;hb=master;hp=a75c240c41334d58e4ae4661749a43b9095a4d0c;hpb=cb1e757d4d486d04af0219b3f36ad17ee8095110;p=fw%2Faltos diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java index a75c240c..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_11; +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(); @@ -909,11 +899,11 @@ public class AltosState extends AltosDataListener { public double accel_ground_along, accel_ground_across, accel_ground_through; 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), - cal_data.pad_orientation); + if (cal_data().pad_orientation != AltosLib.MISSING && accel_ground_along != AltosLib.MISSING) { + rotation = new AltosRotation(accel_ground_across, + accel_ground_through, + accel_ground_along, + cal_data().pad_orientation); orient.set_computed(rotation.tilt(), time); } } @@ -931,12 +921,12 @@ public class AltosState extends AltosDataListener { if (last_imu_time != AltosLib.MISSING) { double t = time - last_imu_time; - double pitch = AltosConvert.degrees_to_radians(gyro_pitch); - double yaw = AltosConvert.degrees_to_radians(gyro_yaw); - double roll = AltosConvert.degrees_to_radians(gyro_roll); + if (t > 0 && gyro_pitch != AltosLib.MISSING && rotation != null) { + double pitch = AltosConvert.degrees_to_radians(gyro_pitch) * t; + double yaw = AltosConvert.degrees_to_radians(gyro_yaw) * t; + double roll = AltosConvert.degrees_to_radians(gyro_roll) * t; - if (t > 0 && pitch != AltosLib.MISSING && rotation != null) { - rotation.rotate(t, pitch, yaw, roll); + rotation.rotate(pitch, yaw, roll); orient.set_computed(rotation.tilt(), time); } } @@ -1059,15 +1049,16 @@ 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(); } public AltosState (AltosCalData cal_data) { super(cal_data); - if (cal_data == null) - Thread.dumpStack(); init(); } }