X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=altoslib%2FAltosState.java;h=7a64f8a188182704c5aa6fbd01e17fb627eef5e5;hp=aa3de432a75fa2e7feaaf9522d967ba1555213ed;hb=e64b1bc108bd75bcd6271631e48abde84af4631f;hpb=7ec1b97d278c7aec3199fb7270f0dcf9484c879f diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java index aa3de432..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,10 +20,9 @@ * 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 record; +public class AltosState extends AltosDataListener { public static final int set_position = 1; public static final int set_gps = 2; @@ -30,55 +30,652 @@ public class AltosState implements Cloneable { public int set; + static final double filter_len = 2.0; + static final double ascent_filter_len = 0.5; + static final double descent_filter_len = 5.0; + /* derived data */ - public long report_time; + public long received_time; + + public int rssi; + public int status; + + class AltosValue { + double value; + double prev_value; + private double max_value; + private double set_time; + private double prev_set_time; + + boolean can_max() { return true; } + + 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; + } + } + + 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); + } + + double value() { + return value; + } + + double max() { + return max_value; + } + + double prev() { + return prev_value; + } + + double change() { + if (value != AltosLib.MISSING && prev_value != AltosLib.MISSING) + return value - prev_value; + return AltosLib.MISSING; + } + + double rate() { + double c = change(); + double t = set_time - prev_set_time; + + if (c != AltosLib.MISSING && t != 0) + return c / t; + return AltosLib.MISSING; + } + + double integrate() { + if (value == AltosLib.MISSING) + return AltosLib.MISSING; + if (prev_value == AltosLib.MISSING) + return AltosLib.MISSING; + + return (value + prev_value) / 2 * (set_time - prev_set_time); + } + + double time() { + return set_time; + } + + void set_derivative(AltosValue in) { + double n = in.rate(); + + if (n == AltosLib.MISSING) + return; + + double p = prev_value; + double pt = prev_set_time; + + if (p == AltosLib.MISSING) { + p = 0; + pt = in.time() - 0.01; + } + + /* Clip changes to reduce noise */ + double ddt = in.time() - pt; + double ddv = (n - p) / ddt; + + final double max = 100000; + + /* 100gs */ + if (Math.abs(ddv) > max) { + if (n > p) + n = p + ddt * max; + else + n = p - ddt * max; + } + + double filter_len; + + 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); + + set(n, in.time()); + } + + void set_integral(AltosValue in) { + double change = in.integrate(); + + if (change != AltosLib.MISSING) { + double prev = prev_value; + if (prev == AltosLib.MISSING) + prev = 0; + set(prev + change, in.time()); + } + } + + 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; + } + + void finish_update() { + prev_value = value; + prev_set_time = set_time; + } + + AltosValue() { + value = AltosLib.MISSING; + prev_value = AltosLib.MISSING; + max_value = AltosLib.MISSING; + } + + } + + class AltosCValue { + + class AltosIValue extends AltosValue { + boolean can_max() { + return c_can_max(); + } + + AltosIValue() { + super(); + } + }; + + public AltosIValue measured; + public AltosIValue computed; + + boolean can_max() { return true; } + + boolean c_can_max() { return can_max(); } - public double time; - public double prev_time; - public double time_change; - public int tick; - public int boost_tick; + double value() { + double v = measured.value(); + if (v != AltosLib.MISSING) + return v; + return computed.value(); + } + + boolean is_measured() { + return measured.value() != AltosLib.MISSING; + } + + double max() { + double m = measured.max(); + + if (m != AltosLib.MISSING) + return m; + return computed.max(); + } + + double prev_value() { + if (measured.value != AltosLib.MISSING && measured.prev_value != AltosLib.MISSING) + return measured.prev_value; + return computed.prev_value; + } + + AltosValue altos_value() { + if (measured.value() != AltosLib.MISSING) + return measured; + return computed; + } + + double change() { + double c = measured.change(); + if (c == AltosLib.MISSING) + c = computed.change(); + return c; + } + + double rate() { + double r = measured.rate(); + if (r == AltosLib.MISSING) + r = computed.rate(); + return r; + } + + void set_measured(double new_value, double time) { + measured.set(new_value, time); + } + + void set_computed(double new_value, double time) { + computed.set(new_value, time); + } + + void set_derivative(AltosValue in) { + computed.set_derivative(in); + } + + void set_derivative(AltosCValue in) { + set_derivative(in.altos_value()); + } + + 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); + } + + void finish_update() { + measured.finish_update(); + computed.finish_update(); + } + + public AltosCValue() { + measured = new AltosIValue(); + computed = new AltosIValue(); + } + } - public int state; - public int flight; - public int serial; public boolean landed; public boolean ascent; /* going up? */ public boolean boost; /* under power */ - public int rssi; - public int status; - public double ground_altitude; - public double ground_pressure; - public double altitude; - public double height; - public double pressure; - public double acceleration; + private double pressure_to_altitude(double p) { + if (p == AltosLib.MISSING) + return AltosLib.MISSING; + return AltosConvert.pressure_to_altitude(p); + } + + private AltosCValue ground_altitude; + + public double ground_altitude() { + return ground_altitude.value(); + } + + public void set_ground_altitude(double a) { + 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(); + } + + AltosGpsGroundAltitude() { + super(); + } + } + + 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); + if (!is_measured()) + ground_altitude.set_computed(pressure_to_altitude(computed.value()), time); + } + + void set_measured(double p, double time) { + super.set_measured(p, time); + ground_altitude.set_computed(pressure_to_altitude(p), time); + } + + AltosGroundPressure () { + super(); + } + } + + private AltosGroundPressure ground_pressure; + + public double ground_pressure() { + return ground_pressure.value(); + } + + public void set_ground_pressure (double pressure) { + ground_pressure.set_measured(pressure, time); + } + + 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; + } + + void set_measured(double a, double time) { + super.set_measured(a,time); + set_speed(measured); + set |= set_position; + } + + AltosAltitude() { + super(); + } + } + + 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(); + } + + 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 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_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 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; + } + + 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(); + } + } + + AltosAccel acceleration; + + public double acceleration() { + return acceleration.value(); + } + + public double max_acceleration() { + return acceleration.max(); + } + + public AltosCValue orient; + + public void set_orient(double new_orient) { + orient.set_measured(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) { + 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); + } + public double battery_voltage; public double pyro_voltage; public double temperature; public double apogee_voltage; public double main_voltage; - public double speed; - - public double prev_height; - public double prev_speed; - public double prev_acceleration; - - public double max_height; - public double max_acceleration; - public double max_speed; - public double kalman_height, kalman_speed, kalman_acceleration; + public double igniter_voltage[]; public AltosGPS gps; - public AltosGPS temp_gps; public boolean gps_pending; - public int gps_sequence; - - public AltosIMU imu; - public AltosMag mag; public static final int MIN_PAD_SAMPLES = 10; @@ -90,6 +687,7 @@ public class AltosState implements Cloneable { public AltosGreatCircle from_pad; public double elevation; /* from pad */ + public double distance; /* distance along ground */ public double range; /* total distance */ public double gps_height; @@ -99,26 +697,11 @@ public class AltosState implements Cloneable { public int speak_tick; public double speak_altitude; - public String callsign; - public double accel_plus_g; - public double accel_minus_g; - public double accel; public double ground_accel; - public double ground_accel_avg; - - public int log_format; - - public AltosMs5607 baro; - public AltosRecordCompanion companion; - - public double speed() { - return speed; - } + public AltosCompanion companion; - public double max_speed() { - return max_speed; - } + public int pyro_fired; public void set_npad(int npad) { this.npad = npad; @@ -129,634 +712,352 @@ public class AltosState implements Cloneable { } public void init() { - record = null; + super.init(); set = 0; - report_time = System.currentTimeMillis(); - time = AltosRecord.MISSING; - time_change = AltosRecord.MISSING; - prev_time = AltosRecord.MISSING; - tick = AltosRecord.MISSING; - boost_tick = AltosRecord.MISSING; - state = AltosLib.ao_flight_invalid; - flight = AltosRecord.MISSING; + received_time = System.currentTimeMillis(); landed = false; boost = false; - rssi = AltosRecord.MISSING; + rssi = AltosLib.MISSING; status = 0; - ground_altitude = AltosRecord.MISSING; - ground_pressure = AltosRecord.MISSING; - altitude = AltosRecord.MISSING; - height = AltosRecord.MISSING; - pressure = AltosRecord.MISSING; - acceleration = AltosRecord.MISSING; - temperature = AltosRecord.MISSING; - - prev_height = AltosRecord.MISSING; - prev_speed = AltosRecord.MISSING; - prev_acceleration = AltosRecord.MISSING; + 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(); - battery_voltage = AltosRecord.MISSING; - pyro_voltage = AltosRecord.MISSING; - apogee_voltage = AltosRecord.MISSING; - main_voltage = AltosRecord.MISSING; + gps = null; + gps_pending = false; - speed = AltosRecord.MISSING; + last_imu_time = AltosLib.MISSING; + rotation = null; - kalman_height = AltosRecord.MISSING; - kalman_speed = AltosRecord.MISSING; - kalman_acceleration = AltosRecord.MISSING; + accel_ground_along = AltosLib.MISSING; + accel_ground_across = AltosLib.MISSING; + accel_ground_through = AltosLib.MISSING; - max_speed = 0; - max_height = 0; - max_acceleration = 0; + accel_along = AltosLib.MISSING; + accel_across = AltosLib.MISSING; + accel_through = AltosLib.MISSING; - gps = null; - temp_gps = null; - gps_sequence = 0; - gps_pending = false; + gyro_roll = AltosLib.MISSING; + gyro_pitch = AltosLib.MISSING; + gyro_yaw = AltosLib.MISSING; - imu = null; - mag = null; + mag_along = AltosLib.MISSING; + mag_across = AltosLib.MISSING; + mag_through = AltosLib.MISSING; set_npad(0); ngps = 0; from_pad = null; - elevation = AltosRecord.MISSING; - range = AltosRecord.MISSING; - gps_height = AltosRecord.MISSING; + elevation = AltosLib.MISSING; + distance = AltosLib.MISSING; + range = AltosLib.MISSING; + gps_height = AltosLib.MISSING; - pad_lat = AltosRecord.MISSING; - pad_lon = AltosRecord.MISSING; - pad_alt = AltosRecord.MISSING; + pad_lat = AltosLib.MISSING; + pad_lon = AltosLib.MISSING; + pad_alt = AltosLib.MISSING; - speak_tick = AltosRecord.MISSING; - speak_altitude = AltosRecord.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(); - callsign = null; + speak_tick = AltosLib.MISSING; + speak_altitude = AltosLib.MISSING; - accel_plus_g = AltosRecord.MISSING; - accel_minus_g = AltosRecord.MISSING; - accel = AltosRecord.MISSING; - ground_accel = AltosRecord.MISSING; - ground_accel_avg = AltosRecord.MISSING; - log_format = AltosRecord.MISSING; - serial = AltosRecord.MISSING; + ground_accel = AltosLib.MISSING; - baro = null; companion = null; - } - - void copy(AltosState old) { - - record = null; - - if (old == null) { - init(); - return; - } - - report_time = old.report_time; - time = old.time; - time_change = 0; - tick = old.tick; - boost_tick = old.boost_tick; - - state = old.state; - flight = old.flight; - landed = old.landed; - ascent = old.ascent; - boost = old.boost; - rssi = old.rssi; - status = old.status; - - set = 0; - - 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; - speed = old.speed; - - prev_height = old.height; - prev_speed = old.speed; - prev_acceleration = old.acceleration; - prev_time = old.time; - - max_height = old.max_height; - max_acceleration = old.max_acceleration; - max_speed = old.max_speed; - - kalman_height = old.kalman_height; - kalman_speed = old.kalman_speed; - kalman_acceleration = old.kalman_acceleration; - - if (old.gps != null) - gps = old.gps.clone(); - else - gps = null; - if (old.temp_gps != null) - temp_gps = old.temp_gps.clone(); - else - temp_gps = null; - gps_sequence = old.gps_sequence; - gps_pending = old.gps_pending; - - if (old.imu != null) - imu = old.imu.clone(); - else - imu = null; - - if (old.mag != null) - mag = old.mag.clone(); - else - mag = null; - - npad = old.npad; - gps_waiting = old.gps_waiting; - gps_ready = old.gps_ready; - ngps = old.ngps; - - if (old.from_pad != null) - from_pad = old.from_pad.clone(); - else - from_pad = null; - - elevation = old.elevation; - range = old.range; - - gps_height = old.gps_height; - pad_lat = old.pad_lat; - pad_lon = old.pad_lon; - pad_alt = old.pad_alt; - - speak_tick = old.speak_tick; - speak_altitude = old.speak_altitude; - - callsign = old.callsign; - - accel_plus_g = old.accel_plus_g; - accel_minus_g = old.accel_minus_g; - accel = old.accel; - ground_accel = old.ground_accel; - ground_accel_avg = old.ground_accel_avg; - - log_format = old.log_format; - serial = old.serial; - - baro = old.baro; - companion = old.companion; - } - - double altitude() { - if (altitude != AltosRecord.MISSING) - return altitude; - if (gps != null) - return gps.alt; - return AltosRecord.MISSING; - } - - void update_vertical_pos() { - - double alt = altitude(); - - if (state == AltosLib.ao_flight_pad && alt != AltosRecord.MISSING && ground_pressure == AltosRecord.MISSING) { - if (ground_altitude == AltosRecord.MISSING) - ground_altitude = alt; - else - ground_altitude = (ground_altitude * 7 + alt) / 8; - } - - 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; - - if (height != AltosRecord.MISSING && height > max_height) - max_height = height; - - update_speed(); - } - - double motion_filter_value() { - return 1/ Math.exp(time_change/2.0); - } - - 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); - } - } - } - 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(); - acceleration = prev_acceleration * filter + new_acceleration * (1-filter); - } - } - } - if (boost && speed != AltosRecord.MISSING && speed > max_speed) - max_speed = speed; + pyro_fired = 0; } - - void update_accel() { - double ground = ground_accel; - if (ground == AltosRecord.MISSING) - ground = ground_accel_avg; - if (accel == AltosRecord.MISSING) - return; - if (ground == 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; - - acceleration = (ground - accel) / counts_per_mss; - - /* Only look at accelerometer data under boost */ - if (boost && acceleration != AltosRecord.MISSING && (max_acceleration == AltosRecord.MISSING || acceleration > max_acceleration)) - max_acceleration = acceleration; - update_speed(); + 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() { - if (tick != AltosRecord.MISSING) { - time = tick / 100.0; - if (prev_time != AltosRecord.MISSING) - time_change = time - prev_time; - } } void update_gps() { - elevation = 0; - range = -1; - gps_height = 0; + 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 (state == AltosLib.ao_flight_pad) { + if (state() == AltosLib.ao_flight_pad || state() == AltosLib.ao_flight_stateless) { set_npad(npad+1); - if (pad_lat != AltosRecord.MISSING) { + 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; - pad_alt = (pad_alt * 31 + gps.alt) / 32; + gps_ground_altitude.set_filtered(gps.alt, time); } } - if (pad_lat == AltosRecord.MISSING) { + 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.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 != AltosRecord.MISSING && - pad_lon != AltosRecord.MISSING) + pad_lat != AltosLib.MISSING && + pad_lon != AltosLib.MISSING) { - double h = height; + double h = height(); - if (h == AltosRecord.MISSING) + 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; - gps_height = gps.alt - pad_alt; } } - 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 String state_name() { + return AltosLib.state_name(state()); } - public void set_boost_tick(int boost_tick) { - if (boost_tick != AltosRecord.MISSING) - this.boost_tick = boost_tick; + 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 String state_name() { - return AltosLib.state_name(state); + public int rssi() { + if (rssi == AltosLib.MISSING) + return 0; + return rssi; } - 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 void set_rssi(int rssi, int status) { + if (rssi != AltosLib.MISSING) { + this.rssi = rssi; + this.status = status; } - } - public void set_flight(int flight) { + public void set_received_time(long ms) { + received_time = ms; + } - /* When the flight changes, reset the state */ - if (flight != AltosRecord.MISSING) { - if (this.flight != AltosRecord.MISSING && - this.flight != flight) { - init(); - } - this.flight = flight; + public void set_gps(AltosGPS gps) { + super.set_gps(gps); + if (gps != null) { + this.gps = gps; + update_gps(); + set |= set_gps; } } - public void set_serial(int serial) { - /* When the serial changes, reset the state */ - if (serial != AltosRecord.MISSING) { - if (this.serial != AltosRecord.MISSING && - this.serial != serial) { - init(); - } - this.serial = serial; + 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 int rssi() { - if (rssi == AltosRecord.MISSING) - return 0; - return rssi; + 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 void set_rssi(int rssi, int status) { - if (rssi != AltosRecord.MISSING) { - this.rssi = rssi; - this.status = status; + 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; } - public void set_altitude(double altitude) { - if (altitude != AltosRecord.MISSING) { - this.altitude = altitude; - update_vertical_pos(); - set |= set_position; - } + 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(); } - public void set_ground_altitude(double ground_altitude) { - if (ground_altitude != AltosRecord.MISSING) { - this.ground_altitude = ground_altitude; - update_vertical_pos(); - } + 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 void set_ground_pressure (double pressure) { - if (pressure != AltosRecord.MISSING) { - this.ground_pressure = pressure; - set_ground_altitude(AltosConvert.pressure_to_altitude(pressure)); - update_vertical_pos(); - } + public double accel_along() { + return accel_along; } - public void set_gps(AltosGPS gps, int sequence) { - if (gps != null) { - this.gps = gps.clone(); - gps_sequence = sequence; - update_gps(); - update_vertical_pos(); - set |= set_gps; - } + public double accel_across() { + return accel_across; } - public void set_kalman(double height, double speed, double acceleration) { - if (height != AltosRecord.MISSING) { - kalman_height = height; - kalman_speed = speed; - kalman_acceleration = acceleration; - update_vertical_pos(); - } + public double accel_through() { + return accel_through; } - public void set_pressure(double pressure) { - if (pressure != AltosRecord.MISSING) { - this.pressure = pressure; - set_altitude(AltosConvert.pressure_to_altitude(pressure)); - } + public double gyro_roll() { + return gyro_roll; + } + + public double gyro_pitch() { + return gyro_pitch; } - public void make_baro() { - if (baro == null) - baro = new AltosMs5607(); + public double gyro_yaw() { + return gyro_yaw; } - public void set_ms5607(int pres, int temp) { - if (baro != null) { - baro.set(pres, temp); + private double mag_along, mag_across, mag_through; - set_pressure(baro.pa); - set_temperature(baro.cc / 100.0); - } + public void set_mag(double along, double across, double through) { + mag_along = along; + mag_across = across; + mag_through = through; } - public void make_companion (int nchannels) { - if (companion == null) - companion = new AltosRecordCompanion(nchannels); + public double mag_along() { + return mag_along; } - public void set_companion(AltosRecordCompanion companion) { - this.companion = companion; + public double mag_across() { + return mag_across; } - 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 double mag_through() { + return mag_through; } - public void set_ground_accel(double ground_accel) { - if (ground_accel != AltosRecord.MISSING) { - this.ground_accel = ground_accel; - update_accel(); - } + + public void set_companion(AltosCompanion companion) { + this.companion = companion; } - public void set_accel(double accel) { - if (accel != AltosRecord.MISSING) { - this.accel = accel; - if (state == AltosLib.ao_flight_pad) { - if (ground_accel_avg == AltosRecord.MISSING) - ground_accel_avg = accel; - else - ground_accel_avg = (ground_accel_avg * 7 + accel) / 8; - } + public void set_acceleration(double acceleration) { + if (acceleration != AltosLib.MISSING) { + this.acceleration.set_measured(acceleration, time); + set |= set_data; } - update_accel(); } public void set_temperature(double temperature) { - if (temperature != AltosRecord.MISSING) { + if (temperature != AltosLib.MISSING) { this.temperature = temperature; set |= set_data; } } public void set_battery_voltage(double battery_voltage) { - if (battery_voltage != AltosRecord.MISSING) { + if (battery_voltage != AltosLib.MISSING) { this.battery_voltage = battery_voltage; set |= set_data; } } public void set_pyro_voltage(double pyro_voltage) { - if (pyro_voltage != AltosRecord.MISSING) { + if (pyro_voltage != AltosLib.MISSING) { this.pyro_voltage = pyro_voltage; set |= set_data; } } public void set_apogee_voltage(double apogee_voltage) { - if (apogee_voltage != AltosRecord.MISSING) { + if (apogee_voltage != AltosLib.MISSING) { this.apogee_voltage = apogee_voltage; set |= set_data; } } public void set_main_voltage(double main_voltage) { - if (main_voltage != AltosRecord.MISSING) { + if (main_voltage != AltosLib.MISSING) { this.main_voltage = main_voltage; set |= set_data; } } - - public double time_since_boost() { - if (tick == AltosRecord.MISSING) - return 0.0; - - if (boost_tick != AltosRecord.MISSING) { - return (tick - boost_tick) / 100.0; - } - return tick / 100.0; - } - - public boolean valid() { - return tick != AltosRecord.MISSING && serial != AltosRecord.MISSING; + public void set_igniter_voltage(double[] voltage) { + this.igniter_voltage = voltage; } - public AltosGPS make_temp_gps() { - if (temp_gps == null) { - temp_gps = new AltosGPS(gps); - temp_gps.cc_gps_sat = null; - } - gps_pending = true; - return temp_gps; - } - - public void set_temp_gps() { - set_gps(temp_gps, gps_sequence + 1); - gps_pending = false; - temp_gps = null; - } - - public void init (AltosRecord cur, AltosState prev_state) { - - System.out.printf ("init\n"); - if (cur == null) - cur = new AltosRecord(); - - record = cur; - - /* Discard previous state if it was for a different board */ - if (prev_state != null && prev_state.serial != cur.serial) - prev_state = null; - - copy(prev_state); - - set_ground_altitude(cur.ground_altitude()); - set_altitude(cur.altitude()); - - set_kalman(cur.kalman_height, cur.kalman_speed, cur.kalman_acceleration); - - report_time = System.currentTimeMillis(); - - set_temperature(cur.temperature()); - set_apogee_voltage(cur.drogue_voltage()); - set_main_voltage(cur.main_voltage()); - set_battery_voltage(cur.battery_voltage()); - - set_pressure(cur.pressure()); - - set_tick(cur.tick); - set_state(cur.state); - - set_accel_g (cur.accel_minus_g, cur.accel_plus_g); - set_ground_accel(cur.ground_accel); - set_accel (cur.accel); - - if (cur.gps_sequence != gps_sequence) - set_gps(cur.gps, cur.gps_sequence); - - } - - public AltosState clone() { - AltosState s = new AltosState(); - s.copy(this); - return s; + public void set_pyro_fired(int fired) { + this.pyro_fired = fired; } - public AltosState(AltosRecord cur) { - init(cur, null); - } - - public AltosState (AltosRecord cur, AltosState prev) { - init(cur, prev); + public AltosState() { + init(); } - public AltosState () { + public AltosState (AltosCalData cal_data) { + super(cal_data); init(); } }