X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=altoslib%2FAltosState.java;h=7a64f8a188182704c5aa6fbd01e17fb627eef5e5;hp=b40b744fd7b5ae13894095aa2bf278758c309d62;hb=e64b1bc108bd75bcd6271631e48abde84af4631f;hpb=04d7d0f829ba953ffeca8ad9887a4b6b2b5d5087 diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java index b40b744f..7a64f8a1 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,623 +20,1044 @@ * Track flight state from telemetry or eeprom data stream */ -package org.altusmetrum.altoslib_1; +package org.altusmetrum.altoslib_13; -public class AltosState implements Cloneable { - public AltosRecord data; +public class AltosState extends AltosDataListener { - /* derived data */ + public static final int set_position = 1; + public static final int set_gps = 2; + public static final int set_data = 4; - public long report_time; + public int set; - public double time; - public double time_change; - public int tick; + static final double filter_len = 2.0; + static final double ascent_filter_len = 0.5; + static final double descent_filter_len = 5.0; - public int state; - public boolean landed; - public boolean ascent; /* going up? */ - public boolean boost; /* under power */ + /* derived data */ - public double ground_altitude; - public double altitude; - public double height; - public double pressure; - public double acceleration; - public double battery_voltage; - public double pyro_voltage; - public double temperature; - public double apogee_voltage; - public double main_voltage; - public double speed; + public long received_time; - public double prev_height; - public double prev_speed; - public double prev_acceleration; + public int rssi; + public int status; - public double max_height; - public double max_acceleration; - public double max_speed; + class AltosValue { + double value; + double prev_value; + private double max_value; + private double set_time; + private double prev_set_time; - public double kalman_height, kalman_speed, kalman_acceleration; + boolean can_max() { return true; } - public AltosGPS gps; - public int gps_sequence; + void set(double new_value, double time) { + if (new_value != AltosLib.MISSING) { + value = new_value; + if (can_max() && (max_value == AltosLib.MISSING || value > max_value)) + max_value = value; + set_time = time; + } + } - public AltosIMU imu; - public AltosMag mag; + void set_filtered(double new_value, double time) { + 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); + } - public static final int MIN_PAD_SAMPLES = 10; + double value() { + return value; + } - public int npad; - public int gps_waiting; - public boolean gps_ready; + double max() { + return max_value; + } - public int ngps; + double prev() { + return prev_value; + } - public AltosGreatCircle from_pad; - public double elevation; /* from pad */ - public double range; /* total distance */ + double change() { + if (value != AltosLib.MISSING && prev_value != AltosLib.MISSING) + return value - prev_value; + return AltosLib.MISSING; + } - public double gps_height; + double rate() { + double c = change(); + double t = set_time - prev_set_time; - public double pad_lat, pad_lon, pad_alt; + if (c != AltosLib.MISSING && t != 0) + return c / t; + return AltosLib.MISSING; + } - public int speak_tick; - public double speak_altitude; + double integrate() { + if (value == AltosLib.MISSING) + return AltosLib.MISSING; + if (prev_value == AltosLib.MISSING) + return AltosLib.MISSING; - public String callsign; - public double accel_plus_g; - public double accel_minus_g; - public double accel; - public double ground_accel; + return (value + prev_value) / 2 * (set_time - prev_set_time); + } - public int log_format; - public int serial; + double time() { + return set_time; + } - public AltosMs5607 baro; + void set_derivative(AltosValue in) { + double n = in.rate(); - public double speed() { - return speed; - } + if (n == AltosLib.MISSING) + return; - public double max_speed() { - return max_speed; - } + double p = prev_value; + double pt = prev_set_time; - public void set_npad(int npad) { - this.npad = npad; - gps_waiting = MIN_PAD_SAMPLES - npad; - if (this.gps_waiting < 0) - gps_waiting = 0; - gps_ready = gps_waiting == 0; - } + if (p == AltosLib.MISSING) { + p = 0; + pt = in.time() - 0.01; + } - public void init() { - data = new AltosRecord(); + /* Clip changes to reduce noise */ + double ddt = in.time() - pt; + double ddv = (n - p) / ddt; - report_time = System.currentTimeMillis(); - time = AltosRecord.MISSING; - time_change = AltosRecord.MISSING; - tick = AltosRecord.MISSING; - state = AltosLib.ao_flight_invalid; - landed = false; - boost = false; + final double max = 100000; - ground_altitude = AltosRecord.MISSING; - altitude = AltosRecord.MISSING; - height = AltosRecord.MISSING; - pressure = AltosRecord.MISSING; - acceleration = AltosRecord.MISSING; - temperature = AltosRecord.MISSING; + /* 100gs */ + if (Math.abs(ddv) > max) { + if (n > p) + n = p + ddt * max; + else + n = p - ddt * max; + } - prev_height = AltosRecord.MISSING; - prev_speed = AltosRecord.MISSING; - prev_acceleration = AltosRecord.MISSING; + double filter_len; - battery_voltage = AltosRecord.MISSING; - pyro_voltage = AltosRecord.MISSING; - apogee_voltage = AltosRecord.MISSING; - main_voltage = AltosRecord.MISSING; + if (ascent) + filter_len = ascent_filter_len; + else + filter_len = descent_filter_len; + double f = 1/Math.exp(ddt/ filter_len); + n = p * f + n * (1-f); - accel_speed = AltosRecord.MISSING; - baro_speed = AltosRecord.MISSING; + set(n, in.time()); + } - kalman_height = AltosRecord.MISSING; - kalman_speed = AltosRecord.MISSING; - kalman_acceleration = AltosRecord.MISSING; + void set_integral(AltosValue in) { + double change = in.integrate(); - max_baro_speed = 0; - max_accel_speed = 0; - max_height = 0; - max_acceleration = 0; + if (change != AltosLib.MISSING) { + double prev = prev_value; + if (prev == AltosLib.MISSING) + prev = 0; + set(prev + change, in.time()); + } + } - gps = null; - gps_sequence = 0; + void copy(AltosValue old) { + value = old.value; + set_time = old.set_time; + prev_value = old.value; + prev_set_time = old.set_time; + max_value = old.max_value; + } - imu = null; - mag = null; + void finish_update() { + prev_value = value; + prev_set_time = set_time; + } - set_npad(0); - ngps = 0; + AltosValue() { + value = AltosLib.MISSING; + prev_value = AltosLib.MISSING; + max_value = AltosLib.MISSING; + } - from_pad = null; - elevation = AltosRecord.MISSING; - range = AltosRecord.MISSING; - gps_height = AltosRecord.MISSING; + } - pat_lat = AltosRecord.MISSING; - pad_lon = AltosRecord.MISSING; - pad_alt = AltosRecord.MISSING; + class AltosCValue { - speak_tick = AltosRecord.MISSING; - speak_altitude = AltosRecord.MISSING; + class AltosIValue extends AltosValue { + boolean can_max() { + return c_can_max(); + } - callsign = null; + AltosIValue() { + super(); + } + }; - accel_plus_g = AltosRecord.MISSING; - accel_minus_g = AltosRecord.MISSING; - log_format = AltosRecord.MISSING; - serial = AltosRecord.MISSING; + public AltosIValue measured; + public AltosIValue computed; - baro = null; - } + boolean can_max() { return true; } - void copy(AltosState old) { + boolean c_can_max() { return can_max(); } - data = null; + double value() { + double v = measured.value(); + if (v != AltosLib.MISSING) + return v; + return computed.value(); + } - if (old == null) { - init(); - return; + boolean is_measured() { + return measured.value() != AltosLib.MISSING; } - report_time = old.report_time; - time = old.time; - time_change = old.time_change; - tick = old.tick; + double max() { + double m = measured.max(); - state = old.state; - landed = old.landed; - ascent = old.ascent; - boost = old.boost; + if (m != AltosLib.MISSING) + return m; + return computed.max(); + } - ground_altitude = old.ground_altitude; - altitude = old.altitude; - height = old.height; - pressure = old.pressure; - acceleration = old.acceleration; - battery_voltage = old.battery_voltage; - pyro_voltage = old.pyro_voltage; - temperature = old.temperature; - apogee_voltage = old.apogee_voltage; - main_voltage = old.main_voltage; - accel_speed = old.accel_speed; - baro_speed = old.baro_speed; + double prev_value() { + if (measured.value != AltosLib.MISSING && measured.prev_value != AltosLib.MISSING) + return measured.prev_value; + return computed.prev_value; + } - prev_height = old.height; - prev_speed = old.speed; - prev_acceleration = old.acceleration; + AltosValue altos_value() { + if (measured.value() != AltosLib.MISSING) + return measured; + return computed; + } - max_height = old.max_height; - max_acceleration = old.max_acceleration; - max_accel_speed = old.max_accel_speed; - max_baro_speed = old.max_baro_speed; + double change() { + double c = measured.change(); + if (c == AltosLib.MISSING) + c = computed.change(); + return c; + } - kalman_height = old.kalman_height; - kalman_speed = old.kalman_speed; - kalman_acceleration = old.kalman_acceleration; + double rate() { + double r = measured.rate(); + if (r == AltosLib.MISSING) + r = computed.rate(); + return r; + } - if (old.gps != null) - gps = old.gps.clone(); - else - gps = null; - gps_sequence = old.gps_sequence; + void set_measured(double new_value, double time) { + measured.set(new_value, time); + } - if (old.imu != null) - imu = old.imu.clone(); - else - imu = null; + void set_computed(double new_value, double time) { + computed.set(new_value, time); + } - if (old.mag != null) - mag = old.mag.clone(); - else - mag = null; + void set_derivative(AltosValue in) { + computed.set_derivative(in); + } - npad = old.npad; - gps_waiting = old.gps_waiting; - gps_ready = old.gps_ready; - ngps = old.ngps; + void set_derivative(AltosCValue in) { + set_derivative(in.altos_value()); + } - if (old.from_pad != null) - from_pad = old.from_pad.clone(); - else - from_pad = null; + void set_integral(AltosValue in) { + computed.set_integral(in); + } - elevation = old.elevation; - range = old.range; + void set_integral(AltosCValue in) { + set_integral(in.altos_value()); + } - gps_height = old.gps_height; - pad_lat = old.pad_lat; - pad_lon = old.pad_lon; - pad_alt = old.pad_alt; + void copy(AltosCValue old) { + measured.copy(old.measured); + computed.copy(old.computed); + } - speak_tick = old.speak_tick; - speak_altitude = old.speak_altitude; + void finish_update() { + measured.finish_update(); + computed.finish_update(); + } - callsign = old.callsign; + public AltosCValue() { + measured = new AltosIValue(); + computed = new AltosIValue(); + } + } - accel_plus_g = old.accel_plus_g; - accel_minus_g = old.accel_minus_g; - log_format = old.log_format; - serial = old.serial; + public boolean landed; + public boolean ascent; /* going up? */ + public boolean boost; /* under power */ - baro = old.baro; + private double pressure_to_altitude(double p) { + if (p == AltosLib.MISSING) + return AltosLib.MISSING; + return AltosConvert.pressure_to_altitude(p); } - double ground_altitude() { - + private AltosCValue ground_altitude; + + public double ground_altitude() { + return ground_altitude.value(); } - double altitude() { - if (altitude != AltosRecord.MISSING) - return altitude; - if (gps != null) - return gps.alt; - return AltosRecord.MISSING; + public void set_ground_altitude(double a) { + ground_altitude.set_measured(a, time); } - void update_vertical_pos() { + 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(); + } - double alt = altitude(); - if (state == AltosLib.ao_flight_pad) { - + AltosGpsGroundAltitude() { + super(); } + } - if (kalman_height != AltosRecord.MISSING) - height = kalman_height; - else if (altitude != AltosRecord.MISSING && ground_altitude != AltosRecord.MISSING) - height = altitude - ground_altitude; - else - height = AltosRecord.MISSING; + private AltosGpsGroundAltitude gps_ground_altitude; - update_speed(); + public double gps_ground_altitude() { + return gps_ground_altitude.value(); } - double motion_filter_value() { - return 1/ Math.exp(time_change/10.0); + public void set_gps_ground_altitude(double a) { + gps_ground_altitude.set(a, time); } - void update_speed() { - if (kalman_speed != AltosRecord.MISSING) - speed = kalman_speed; - else if (state != AltosLib.ao_flight_invalid && - time_change != AltosRecord.MISSING) - { - if (ascent && acceleration != AltosRecord.MISSING) - { - if (prev_speed == AltosRecord.MISSING) - speed = acceleration * time_change; - else - speed = prev_speed + acceleration * time_change; - } - else if (height != AltosRecord.MISSING && - prev_height != AltosRecord.MISSING && - time_change != 0) - { - double new_speed = (height - prev_height) / time_change; - - if (prev_speed == AltosRecord.MISSING) - speed = new_speed; - else { - double filter = motion_filter_value(); - - speed = prev_speed * filter + new_speed * (1-filter); - } - } + class AltosGroundPressure extends AltosCValue { + void set_filtered(double p, double time) { + computed.set_filtered(p, time); + if (!is_measured()) + ground_altitude.set_computed(pressure_to_altitude(computed.value()), time); } - if (acceleration == AltosRecord.MISSING) { - if (prev_speed != AltosRecord.MISSING && time_change != 0) { - double new_acceleration = (speed - prev_speed) / time_change; - if (prev_acceleration == AltosRecord.MISSING) - acceleration = new_acceleration; - else { - double filter = motion_filter_value(); + void set_measured(double p, double time) { + super.set_measured(p, time); + ground_altitude.set_computed(pressure_to_altitude(p), time); + } - acceleration = prev_acceleration * filter + new_acceleration * (1-filter); - } - } + AltosGroundPressure () { + super(); } } - - void update_accel() { - if (accel == AltosRecord.MISSING) - return; - if (ground_Accel == AltosRecord.MISSING) - return; - if (accel_plus_g == AltosRecord.MISSING) - return; - if (accel_minus_g == AltosRecord.MISSING) - return; - double counts_per_g = (accel_minus_g - accel_plus_g) / 2.0; - double counts_per_mss = counts_per_g / 9.80665; + private AltosGroundPressure ground_pressure; - acceleration = (ground_accel - accel) / counts_per_mss; - update_speed(); + public double ground_pressure() { + return ground_pressure.value(); } - public void set_tick(int tick) { - if (tick != AltosRecord.MISSING) { - if (this.tick != AltosRecord.MISSING) { - while (tick < this.tick) - tick += 65536; - time_change = (tick - this.tick) / 100.0; - } else - time_change = 0; - this.tick = tick; - update_time(); - } + public void set_ground_pressure (double pressure) { + ground_pressure.set_measured(pressure, time); } - 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); + class AltosAltitude extends AltosCValue { + + private void set_speed(AltosValue v) { + if (!acceleration.is_measured() || !ascent) + speed.set_derivative(this); } - } + void set_computed(double a, double time) { + super.set_computed(a,time); + set_speed(computed); + set |= set_position; + } - public void set_altitude(double altitude) { - if (altitude != AltosRecord.MISSING) { - this.altitude = altitude; - update_vertical_pos(); + void set_measured(double a, double time) { + super.set_measured(a,time); + set_speed(measured); + set |= set_position; } - } - public void set_ground_altitude(double ground_altitude) { - if (ground_altitude != AltosRecord.MISSING) { - this.ground_altitude = ground_altitude; - update_vertical_pos(); + AltosAltitude() { + super(); } } - public void set_gps(AltosGPS gps, int sequence) { - if (gps != null) { - this.gps = gps.clone(); - gps_sequence = sequence; - update_vertical_pos(); + 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; } - } - public void set_kalman(double height, double speed, double acceleration) { - if (height != AltosRecord.MISSING) { - kalman_height = height; - kalman_speed = speed; - kalman_acceleration = acceleration; - baro_speed = accel_speed = speed; - update_vertical_pos(); + void set(double a, double t) { + super.set(a, t); + set_gps_height(); } + + AltosGpsAltitude() { + super(); + } + } + + private AltosGpsAltitude gps_altitude; + + private AltosValue gps_ground_speed; + private AltosValue gps_ascent_rate; + private AltosValue gps_course; + private AltosValue gps_speed; + + public double altitude() { + double a = altitude.value(); + if (a != AltosLib.MISSING) + return a; + return gps_altitude.value(); } - public void set_pressure(double pressure) { - if (pressure != AltosRecord.MISSING) { - this.pressure = pressure; - set_altitude(AltosConvert.pressure_to_altitude(pressure)); + public double max_altitude() { + double a = altitude.max(); + if (a != AltosLib.MISSING) + return a; + return gps_altitude.max(); + } + + public void set_altitude(double new_altitude) { + double old_altitude = altitude.value(); + if (old_altitude != AltosLib.MISSING) { + while (old_altitude - new_altitude > 32000) + new_altitude += 65536.0; } + 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_accel_g(double accel_plus_g, double accel_minus_g) { - if (accel_plus_g != AltosRecord.MISSING) { - this.accel_plus_g = accel_plus_g; - this.accel_minus_g = accel_minus_g; - update_accel(); + 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 max_gps_ground_speed() { + return gps_ground_speed.max(); + } + + public double gps_ascent_rate() { + return gps_ascent_rate.value(); + } + + public double max_gps_ascent_rate() { + return gps_ascent_rate.max(); + } + + public double gps_course() { + return gps_course.value(); + } + + public double gps_speed() { + return gps_speed.value(); + } + + public double max_gps_speed() { + return gps_speed.max(); + } + + class AltosPressure extends AltosValue { + void set(double p, double time) { + super.set(p, time); + if (state() == AltosLib.ao_flight_pad) + ground_pressure.set_filtered(p, time); + double a = pressure_to_altitude(p); + altitude.set_computed(a, time); } + + AltosPressure() { + super(); + } + } + + private AltosPressure pressure; + + public double pressure() { + return pressure.value(); + } + + public void set_pressure(double p) { + pressure.set(p, time); + } + + public void set_thrust(double N) { + } + + public double baro_height() { + double a = altitude(); + double g = ground_altitude(); + if (a != AltosLib.MISSING && g != AltosLib.MISSING) + return a - g; + return AltosLib.MISSING; } - public void set_ground_accel(double ground_accel) { - if (ground_accel != AltosRecord.MISSING) { - this.ground_accel = ground_accel; - update_accel(); + + public double height() { + double k = kalman_height.value(); + if (k != AltosLib.MISSING) + return k; + + double b = baro_height(); + if (b != AltosLib.MISSING) + return b; + + 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; + return max_gps_height(); + } + + 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 || state() == AltosLib.ao_flight_stateless; + } + + void set_accel() { + acceleration.set_derivative(this); } + + void set_derivative(AltosCValue in) { + super.set_derivative(in); + set_accel(); + } + + void set_computed(double new_value, double time) { + super.set_computed(new_value, time); + set_accel(); + } + + void set_measured(double new_value, double time) { + super.set_measured(new_value, time); + set_accel(); + } + + AltosSpeed() { + super(); + } + } + + private AltosSpeed speed; + + public double speed() { + double v = kalman_speed.value(); + if (v != AltosLib.MISSING) + return v; + v = speed.value(); + if (v != AltosLib.MISSING) + return v; + v = gps_speed(); + if (v != AltosLib.MISSING) + return v; + return AltosLib.MISSING; + } + + public double max_speed() { + double v = kalman_speed.max(); + if (v != AltosLib.MISSING) + return v; + v = speed.max(); + if (v != AltosLib.MISSING) + return v; + v = max_gps_speed(); + if (v != AltosLib.MISSING) + return v; + return AltosLib.MISSING; } - public void set_accel(double accel) { - if (accel != AltosRecord.MISSING) { - this.accel = accel; + class AltosAccel extends AltosCValue { + + boolean can_max() { + return state() < AltosLib.ao_flight_fast || state() == AltosLib.ao_flight_stateless; + } + + void set_measured(double a, double time) { + super.set_measured(a, time); + if (ascent) + speed.set_integral(this.measured); + } + + AltosAccel() { + super(); } - update_accel(); } - public void set_temperature(double temperature) { - if (temperature != AltosRecord.MISSING) - this.temperature = temperature; + AltosAccel acceleration; + + public double acceleration() { + return acceleration.value(); } - public void set_battery_voltage(double battery_voltage) { - if (battery_voltage != AltosRecord.MISSING) - this.battery_voltage = battery_voltage; + public double max_acceleration() { + return acceleration.max(); } - public void set_pyro_voltage(double pyro_voltage) { - if (pyro_voltage != AltosRecord.MISSING) - this.pyro_voltage = pyro_voltage; + public AltosCValue orient; + + public void set_orient(double new_orient) { + orient.set_measured(new_orient, time); } - public void set_apogee_voltage(double apogee_voltage) { - if (apogee_voltage != AltosRecord.MISSING) - this.apogee_voltage = apogee_voltage; + public double orient() { + return orient.value(); } - public void set_main_voltage(double main_voltage) { - if (main_voltage != AltosRecord.MISSING) - this.main_voltage = main_voltage; + public double max_orient() { + return orient.max(); } - public void init (AltosRecord cur, AltosState prev_state) { + public AltosValue kalman_height, kalman_speed, kalman_acceleration; - if (cur == null) - cur = new AltosRecord(); + public void set_kalman(double height, double speed, double acceleration) { + double old_height = kalman_height.value(); + if (old_height != AltosLib.MISSING) { + while (old_height - height > 32000) + height += 65536; + } + kalman_height.set(height, time); + kalman_speed.set(speed, time); + kalman_acceleration.set(acceleration, time); + } - data = cur; + public double battery_voltage; + public double pyro_voltage; + public double temperature; + public double apogee_voltage; + public double main_voltage; - /* Discard previous state if it was for a different board */ - if (prev_state != null && prev_state.serial != cur.serial) - prev_state = null; + public double igniter_voltage[]; - copy(prev_state); + public AltosGPS gps; + public boolean gps_pending; - set_ground_altitude(data.ground_altitude()); - set_altitude(data.altitude()); + public static final int MIN_PAD_SAMPLES = 10; - set_kalman(data.kalman_height, data.kalman_speed, data.kalman_acceleration); + public int npad; + public int gps_waiting; + public boolean gps_ready; - report_time = System.currentTimeMillis(); + public int ngps; - set_temperature(data.temperature()); - set_apogee_voltage(data.drogue_voltage()); - set_main_voltage(data.main_voltage()); - set_battery_voltage(data.battery_voltage()); + public AltosGreatCircle from_pad; + public double elevation; /* from pad */ + public double distance; /* distance along ground */ + public double range; /* total distance */ - set_pressure(data.pressure()); + public double gps_height; - set_tick(data.tick); - set_state(data.state); + public double pad_lat, pad_lon, pad_alt; + + public int speak_tick; + public double speak_altitude; + + public double ground_accel; - set_accel_g (data.accel_minus_g, data.accel_plus_g); - set_ground_accel(data.ground_accel); - set_accel (data.accel); + public AltosCompanion companion; + + public int pyro_fired; + + public void set_npad(int npad) { + this.npad = npad; + gps_waiting = MIN_PAD_SAMPLES - npad; + if (this.gps_waiting < 0) + gps_waiting = 0; + gps_ready = gps_waiting == 0; + } - set_gps(data.gps, data.gps_sequence); + public void init() { + super.init(); - if (prev_state != null) { + set = 0; - if (data.kalman_speed != AltosRecord.MISSING) { - baro_speed = accel_speed = data.kalman_speed; - } else { - /* compute barometric speed */ + received_time = System.currentTimeMillis(); + landed = false; + boost = false; + rssi = AltosLib.MISSING; + status = 0; + + ground_altitude = new AltosCValue(); + ground_pressure = new AltosGroundPressure(); + altitude = new AltosAltitude(); + pressure = new AltosPressure(); + speed = new AltosSpeed(); + acceleration = new AltosAccel(); + orient = new AltosCValue(); + + temperature = AltosLib.MISSING; + battery_voltage = AltosLib.MISSING; + pyro_voltage = AltosLib.MISSING; + apogee_voltage = AltosLib.MISSING; + main_voltage = AltosLib.MISSING; + igniter_voltage = null; + + kalman_height = new AltosValue(); + kalman_speed = new AltosValue(); + kalman_acceleration = new AltosValue(); - double height_change = height - prev_state.height; + gps = null; + gps_pending = false; - double prev_baro_speed = prev_state.baro_speed; - if (prev_baro_speed == AltosRecord.MISSING) - prev_baro_speed = 0; + last_imu_time = AltosLib.MISSING; + rotation = null; - if (time_change > 0) - baro_speed = (prev_baro_speed * 3 + (height_change / time_change)) / 4.0; - else - baro_speed = prev_state.baro_speed; + accel_ground_along = AltosLib.MISSING; + accel_ground_across = AltosLib.MISSING; + accel_ground_through = AltosLib.MISSING; - double prev_accel_speed = prev_state.accel_speed; + accel_along = AltosLib.MISSING; + accel_across = AltosLib.MISSING; + accel_through = AltosLib.MISSING; - if (prev_accel_speed == AltosRecord.MISSING) - prev_accel_speed = 0; + gyro_roll = AltosLib.MISSING; + gyro_pitch = AltosLib.MISSING; + gyro_yaw = AltosLib.MISSING; - if (acceleration == AltosRecord.MISSING) { - /* Fill in mising acceleration value */ - accel_speed = baro_speed; + mag_along = AltosLib.MISSING; + mag_across = AltosLib.MISSING; + mag_through = AltosLib.MISSING; - if (time_change > 0 && accel_speed != AltosRecord.MISSING) - acceleration = (accel_speed - prev_accel_speed) / time_change; - else - acceleration = prev_state.acceleration; - } else { - /* compute accelerometer speed */ - accel_speed = prev_accel_speed + acceleration * time_change; - } - } - } else { - npad = 0; - ngps = 0; - gps = null; - gps_sequence = 0; - baro_speed = AltosRecord.MISSING; - accel_speed = AltosRecord.MISSING; - pad_alt = AltosRecord.MISSING; - max_baro_speed = 0; - max_accel_speed = 0; - max_height = 0; - max_acceleration = 0; - time_change = 0; - baro = new AltosMs5607(); - callsign = ""; - accel_plus_g = AltosRecord.MISSING; - accel_minus_g = AltosRecord.MISSING; - log_format = AltosRecord.MISSING; - serial = AltosRecord.MISSING; - } - - time = tick / 100.0; - - if (data.gps != null && data.gps_sequence != gps_sequence && (state < AltosLib.ao_flight_boost)) { + set_npad(0); + ngps = 0; + + from_pad = null; + elevation = AltosLib.MISSING; + distance = AltosLib.MISSING; + range = AltosLib.MISSING; + gps_height = AltosLib.MISSING; + + pad_lat = AltosLib.MISSING; + pad_lon = AltosLib.MISSING; + pad_alt = AltosLib.MISSING; + + gps_altitude = new AltosGpsAltitude(); + gps_ground_altitude = new AltosGpsGroundAltitude(); + gps_ground_speed = new AltosValue(); + gps_speed = new AltosValue(); + gps_ascent_rate = new AltosValue(); + gps_course = new AltosValue(); + + speak_tick = AltosLib.MISSING; + speak_altitude = AltosLib.MISSING; + + ground_accel = AltosLib.MISSING; + + companion = null; + + pyro_fired = 0; + } + void finish_update() { + ground_altitude.finish_update(); + altitude.finish_update(); + pressure.finish_update(); + speed.finish_update(); + acceleration.finish_update(); + orient.finish_update(); + + kalman_height.finish_update(); + kalman_speed.finish_update(); + kalman_acceleration.finish_update(); + } + + void update_time() { + } + + void update_gps() { + elevation = AltosLib.MISSING; + distance = AltosLib.MISSING; + range = AltosLib.MISSING; + + if (gps == null) + return; + + if (gps.locked && gps.nsat >= 4) { /* Track consecutive 'good' gps reports, waiting for 10 of them */ - if (data.gps != null && data.gps.locked && data.gps.nsat >= 4) + if (state() == AltosLib.ao_flight_pad || state() == AltosLib.ao_flight_stateless) { set_npad(npad+1); - else - set_npad(0); - - /* Average GPS data while on the pad */ - if (data.gps != null && data.gps.locked && data.gps.nsat >= 4) { - if (ngps > 1 && state == AltosLib.ao_flight_pad) { - /* filter pad position */ - pad_lat = (pad_lat * 31.0 + data.gps.lat) / 32.0; - pad_lon = (pad_lon * 31.0 + data.gps.lon) / 32.0; - pad_alt = (pad_alt * 31.0 + data.gps.alt) / 32.0; - } else { - pad_lat = data.gps.lat; - pad_lon = data.gps.lon; - pad_alt = data.gps.alt; + 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); } - ngps++; } - } else { - if (ngps == 0 && ground_altitude != AltosRecord.MISSING) - pad_alt = ground_altitude; - } - - gps_sequence = data.gps_sequence; - - /* Only look at accelerometer data under boost */ - if (boost && acceleration != AltosRecord.MISSING && (max_acceleration == AltosRecord.MISSING || acceleration > max_acceleration)) - max_acceleration = acceleration; - if (boost && accel_speed != AltosRecord.MISSING && accel_speed > max_accel_speed) - max_accel_speed = accel_speed; - if (boost && baro_speed != AltosRecord.MISSING && baro_speed > max_baro_speed) - max_baro_speed = baro_speed; - - if (height != AltosRecord.MISSING && height > max_height) - max_height = height; - elevation = 0; - range = -1; - gps_height = 0; - if (data.gps != null) { - gps = data.gps; - if (ngps > 0 && gps.locked) { - double h = height; - - if (h == AltosRecord.MISSING) h = 0; - 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; + if (pad_lat == AltosLib.MISSING) { + pad_lat = gps.lat; + pad_lon = gps.lon; + 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.climb_rate != AltosLib.MISSING && gps.ground_speed != AltosLib.MISSING) + gps_speed.set(Math.sqrt(gps.ground_speed * gps.ground_speed + + 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 && + pad_lon != AltosLib.MISSING) + { + double h = height(); + + if (h == AltosLib.MISSING) + h = 0; + from_pad = new AltosGreatCircle(pad_lat, pad_lon, 0, gps.lat, gps.lon, h); + elevation = from_pad.elevation; + distance = from_pad.distance; + range = from_pad.range; + } + } + + public String state_name() { + return AltosLib.state_name(state()); + } + + public void set_state(int state) { + super.set_state(state); + ascent = (AltosLib.ao_flight_boost <= state() && + state() <= AltosLib.ao_flight_coast); + boost = (AltosLib.ao_flight_boost == state()); + } + + public int rssi() { + if (rssi == AltosLib.MISSING) + return 0; + return rssi; + } + + public void set_rssi(int rssi, int status) { + if (rssi != AltosLib.MISSING) { + this.rssi = rssi; + this.status = status; + } + } + + public void set_received_time(long ms) { + received_time = ms; + } + + public void set_gps(AltosGPS gps) { + super.set_gps(gps); + if (gps != null) { + this.gps = gps; + update_gps(); + set |= set_gps; + } + } + + public AltosRotation rotation; + + 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); + orient.set_computed(rotation.tilt(), time); + } + } + + public void set_accel_ground(double ground_along, double ground_across, double ground_through) { + accel_ground_along = ground_along; + accel_ground_across = ground_across; + accel_ground_through = ground_through; + update_pad_rotation(); + } + + public double last_imu_time; + + private void update_orient() { + if (last_imu_time != AltosLib.MISSING) { + double t = time - last_imu_time; + + 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; + + rotation.rotate(pitch, yaw, roll); + orient.set_computed(rotation.tilt(), time); + } + } + last_imu_time = time; + } + + private double gyro_roll, gyro_pitch, gyro_yaw; + + public void set_gyro(double roll, double pitch, double yaw) { + gyro_roll = roll; + gyro_pitch = pitch; + gyro_yaw = yaw; + update_orient(); + } + + private double accel_along, accel_across, accel_through; + + public void set_accel(double along, double across, double through) { + accel_along = along; + accel_across = across; + accel_through = through; + update_orient(); + } + + public double accel_along() { + return accel_along; + } + + public double accel_across() { + return accel_across; + } + + public double accel_through() { + return accel_through; + } + + public double gyro_roll() { + return gyro_roll; + } + + public double gyro_pitch() { + return gyro_pitch; + } + + public double gyro_yaw() { + return gyro_yaw; + } + + private double mag_along, mag_across, mag_through; + + public void set_mag(double along, double across, double through) { + mag_along = along; + mag_across = across; + mag_through = through; + } + + public double mag_along() { + return mag_along; + } + + public double mag_across() { + return mag_across; + } + + public double mag_through() { + return mag_through; + } + + public void set_companion(AltosCompanion companion) { + this.companion = companion; + } + + public void set_acceleration(double acceleration) { + if (acceleration != AltosLib.MISSING) { + this.acceleration.set_measured(acceleration, time); + set |= set_data; + } + } + + public void set_temperature(double temperature) { + if (temperature != AltosLib.MISSING) { + this.temperature = temperature; + set |= set_data; + } + } + + public void set_battery_voltage(double battery_voltage) { + if (battery_voltage != AltosLib.MISSING) { + this.battery_voltage = battery_voltage; + set |= set_data; + } + } + + public void set_pyro_voltage(double pyro_voltage) { + if (pyro_voltage != AltosLib.MISSING) { + this.pyro_voltage = pyro_voltage; + set |= set_data; + } + } + + public void set_apogee_voltage(double apogee_voltage) { + if (apogee_voltage != AltosLib.MISSING) { + this.apogee_voltage = apogee_voltage; + set |= set_data; + } + } + + public void set_main_voltage(double main_voltage) { + if (main_voltage != AltosLib.MISSING) { + this.main_voltage = main_voltage; + set |= set_data; + } + } + + public void set_igniter_voltage(double[] voltage) { + this.igniter_voltage = voltage; } - public AltosState clone() { - AltosState s = new AltosState(data, this); - return s; + public void set_pyro_fired(int fired) { + this.pyro_fired = fired; } - public AltosState(AltosRecord cur) { - init(cur, null); + public AltosState() { + init(); } - public AltosState (AltosRecord cur, AltosState prev) { - init(cur, prev); + public AltosState (AltosCalData cal_data) { + super(cal_data); + init(); } }