X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=altoslib%2FAltosState.java;h=5a805fc6bf6864b75a146b64a411746583084c48;hp=b40b744fd7b5ae13894095aa2bf278758c309d62;hb=4e22b34bde421a9df090c9196fd4347468c8176a;hpb=04d7d0f829ba953ffeca8ad9887a4b6b2b5d5087 diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java index b40b744f..5a805fc6 100644 --- a/altoslib/AltosState.java +++ b/altoslib/AltosState.java @@ -19,47 +19,469 @@ * Track flight state from telemetry or eeprom data stream */ -package org.altusmetrum.altoslib_1; +package org.altusmetrum.altoslib_2; public class AltosState implements Cloneable { - public AltosRecord data; + + public static final int set_position = 1; + public static final int set_gps = 2; + public static final int set_data = 4; + + public int set; + + static final double ascent_filter_len = 0.1; + static final double descent_filter_len = 2.0; /* derived data */ - public long report_time; + public long received_time; public double time; + public double prev_time; public double time_change; public int tick; + private int prev_tick; + public int boost_tick; + + class AltosValue { + private double value; + private double prev_value; + 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) { + value = new_value; + if (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) + new_value = (prev_value * 15.0 + new_value) / 16.0; + 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; + + /* 100gs */ + if (Math.abs(ddv) > 1000) { + if (n > p) + n = p + ddt * 1000; + else + n = p - ddt * 1000; + } + + 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 { + AltosValue measured; + AltosValue computed; + + 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(); + } + + AltosCValue() { + measured = new AltosValue(); + computed = new AltosValue(); + } + } public int state; + public int flight; + public int serial; + public int receiver_serial; public boolean landed; public boolean ascent; /* going up? */ public boolean boost; /* under power */ + public int rssi; + public int status; + public int device_type; + public int config_major; + public int config_minor; + public int apogee_delay; + public int main_deploy; + public int flight_log_max; + + 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 AltosGroundPressure extends AltosCValue { + void set_filtered(double p, double time) { + computed.set_filtered(p, time); + 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); + } + } + + 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; + } + } + + private AltosAltitude altitude; + + public double altitude() { + double a = altitude.value(); + if (a != AltosLib.MISSING) + return a; + if (gps != null) + return gps.alt; + return AltosLib.MISSING; + } + + public double max_altitude() { + double a = altitude.max(); + if (a != AltosLib.MISSING) + return a; + return AltosLib.MISSING; + } + + public void set_altitude(double new_altitude) { + altitude.set_measured(new_altitude, time); + } + + 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); + } + } + + private AltosPressure pressure; + + public double pressure() { + return pressure.value(); + } + + public void set_pressure(double p) { + pressure.set(p, time); + } + + public double height() { + double k = kalman_height.value(); + if (k != AltosLib.MISSING) + return k; + + double a = altitude(); + double g = ground_altitude(); + if (a != AltosLib.MISSING && g != AltosLib.MISSING) + return a - g; + return AltosLib.MISSING; + } + + 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 AltosLib.MISSING; + } + + class AltosSpeed extends AltosCValue { + + 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(); + } + } + + private AltosSpeed speed; + + public double speed() { + double v = kalman_speed.value(); + if (v != AltosLib.MISSING) + return v; + return speed.value(); + } + + public double max_speed() { + double v = kalman_speed.max(); + if (v != AltosLib.MISSING) + return v; + return speed.max(); + } + + class AltosAccel extends AltosCValue { + void set_measured(double a, double time) { + super.set_measured(a, time); + if (ascent) + speed.set_integral(this.measured); + } + } + + AltosAccel acceleration; + + public double acceleration() { + return acceleration.value(); + } + + public double max_acceleration() { + return acceleration.max(); + } + + public AltosValue kalman_height, kalman_speed, kalman_acceleration; + + public void set_kalman(double height, double speed, double acceleration) { + kalman_height.set(height, time); + kalman_speed.set(speed, time); + kalman_acceleration.set(acceleration, time); + } - 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 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 ignitor_voltage[]; public AltosGPS gps; + public AltosGPS temp_gps; + public int temp_gps_sat_tick; + public boolean gps_pending; public int gps_sequence; public AltosIMU imu; @@ -85,23 +507,19 @@ public class AltosState implements Cloneable { public double speak_altitude; public String callsign; + public String firmware_version; + 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 int serial; public AltosMs5607 baro; - public double speed() { - return speed; - } - - public double max_speed() { - return max_speed; - } + public AltosCompanion companion; public void set_npad(int npad) { this.npad = npad; @@ -112,47 +530,51 @@ public class AltosState implements Cloneable { } public void init() { - data = new AltosRecord(); - - report_time = System.currentTimeMillis(); - time = AltosRecord.MISSING; - time_change = AltosRecord.MISSING; - tick = AltosRecord.MISSING; + set = 0; + + received_time = System.currentTimeMillis(); + time = AltosLib.MISSING; + time_change = AltosLib.MISSING; + prev_time = AltosLib.MISSING; + tick = AltosLib.MISSING; + prev_tick = AltosLib.MISSING; + boost_tick = AltosLib.MISSING; state = AltosLib.ao_flight_invalid; + flight = AltosLib.MISSING; landed = false; boost = false; - - ground_altitude = 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; - - battery_voltage = AltosRecord.MISSING; - pyro_voltage = AltosRecord.MISSING; - apogee_voltage = AltosRecord.MISSING; - main_voltage = AltosRecord.MISSING; - - - accel_speed = AltosRecord.MISSING; - baro_speed = AltosRecord.MISSING; - - kalman_height = AltosRecord.MISSING; - kalman_speed = AltosRecord.MISSING; - kalman_acceleration = AltosRecord.MISSING; - - max_baro_speed = 0; - max_accel_speed = 0; - max_height = 0; - max_acceleration = 0; + rssi = AltosLib.MISSING; + status = 0; + device_type = AltosLib.MISSING; + config_major = AltosLib.MISSING; + config_minor = AltosLib.MISSING; + apogee_delay = AltosLib.MISSING; + main_deploy = AltosLib.MISSING; + flight_log_max = AltosLib.MISSING; + + ground_altitude = new AltosCValue(); + ground_pressure = new AltosGroundPressure(); + altitude = new AltosAltitude(); + pressure = new AltosPressure(); + speed = new AltosSpeed(); + acceleration = new AltosAccel(); + + temperature = AltosLib.MISSING; + battery_voltage = AltosLib.MISSING; + pyro_voltage = AltosLib.MISSING; + apogee_voltage = AltosLib.MISSING; + main_voltage = AltosLib.MISSING; + ignitor_voltage = null; + + kalman_height = new AltosValue(); + kalman_speed = new AltosValue(); + kalman_acceleration = new AltosValue(); gps = null; + temp_gps = null; + temp_gps_sat_tick = 0; gps_sequence = 0; + gps_pending = false; imu = null; mag = null; @@ -161,77 +583,108 @@ public class AltosState implements Cloneable { ngps = 0; from_pad = null; - elevation = AltosRecord.MISSING; - range = AltosRecord.MISSING; - gps_height = AltosRecord.MISSING; + elevation = AltosLib.MISSING; + range = AltosLib.MISSING; + gps_height = AltosLib.MISSING; - pat_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; + speak_tick = AltosLib.MISSING; + speak_altitude = AltosLib.MISSING; callsign = null; - accel_plus_g = AltosRecord.MISSING; - accel_minus_g = AltosRecord.MISSING; - log_format = AltosRecord.MISSING; - serial = AltosRecord.MISSING; + accel_plus_g = AltosLib.MISSING; + accel_minus_g = AltosLib.MISSING; + accel = AltosLib.MISSING; + + ground_accel = AltosLib.MISSING; + ground_accel_avg = AltosLib.MISSING; + + log_format = AltosLib.MISSING; + serial = AltosLib.MISSING; + receiver_serial = AltosLib.MISSING; baro = null; + companion = null; } - void copy(AltosState old) { + void finish_update() { + prev_tick = tick; + + ground_altitude.finish_update(); + altitude.finish_update(); + pressure.finish_update(); + speed.finish_update(); + acceleration.finish_update(); - data = null; + kalman_height.finish_update(); + kalman_speed.finish_update(); + kalman_acceleration.finish_update(); + } + + void copy(AltosState old) { if (old == null) { init(); return; } - report_time = old.report_time; + received_time = old.received_time; time = old.time; time_change = old.time_change; + prev_time = old.time; + tick = old.tick; + prev_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; + device_type = old.device_type; + config_major = old.config_major; + config_minor = old.config_minor; + apogee_delay = old.apogee_delay; + main_deploy = old.main_deploy; + flight_log_max = old.flight_log_max; + + set = 0; + + ground_altitude.copy(old.ground_altitude); + altitude.copy(old.altitude); + pressure.copy(old.pressure); + speed.copy(old.speed); + acceleration.copy(old.acceleration); - 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; - - prev_height = old.height; - prev_speed = old.speed; - prev_acceleration = old.acceleration; - - max_height = old.max_height; - max_acceleration = old.max_acceleration; - max_accel_speed = old.max_accel_speed; - max_baro_speed = old.max_baro_speed; + ignitor_voltage = old.ignitor_voltage; - kalman_height = old.kalman_height; - kalman_speed = old.kalman_speed; - kalman_acceleration = old.kalman_acceleration; + kalman_height.copy(old.kalman_height); + kalman_speed.copy(old.kalman_speed); + kalman_acceleration.copy(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; + temp_gps_sat_tick = old.temp_gps_sat_tick; gps_sequence = old.gps_sequence; + gps_pending = old.gps_pending; if (old.imu != null) imu = old.imu.clone(); @@ -268,116 +721,80 @@ public class AltosState implements Cloneable { 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; + receiver_serial = old.receiver_serial; baro = old.baro; + companion = old.companion; } - - double ground_altitude() { - - } - - double altitude() { - if (altitude != AltosRecord.MISSING) - return altitude; - if (gps != null) - return gps.alt; - return AltosRecord.MISSING; + + void update_time() { } - void update_vertical_pos() { - - double alt = altitude(); - if (state == AltosLib.ao_flight_pad) { - - } - - 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; - - update_speed(); - } + void update_gps() { + elevation = 0; + range = -1; + gps_height = 0; - double motion_filter_value() { - return 1/ Math.exp(time_change/10.0); - } + if (gps == null) + return; - 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 (gps.locked && gps.nsat >= 4) { + /* Track consecutive 'good' gps reports, waiting for 10 of them */ + if (state == AltosLib.ao_flight_pad) { + set_npad(npad+1); + 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; } } + if (pad_lat == AltosLib.MISSING) { + pad_lat = gps.lat; + pad_lon = gps.lon; + pad_alt = gps.alt; + } } - 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(); + 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; + range = from_pad.range; + gps_height = gps.alt - pad_alt; + } + } - acceleration = prev_acceleration * filter + new_acceleration * (1-filter); + public void set_tick(int new_tick) { + if (new_tick != AltosLib.MISSING) { + if (prev_tick != AltosLib.MISSING) { + while (new_tick < prev_tick - 1000) { + new_tick += 65536; } } + tick = new_tick; + time = tick / 100.0; + time_change = time - prev_time; } } - - 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; - - acceleration = (ground_accel - accel) / counts_per_mss; - update_speed(); + public void set_boost_tick(int boost_tick) { + if (boost_tick != AltosLib.MISSING) + this.boost_tick = boost_tick; } - 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_state(int state) { @@ -390,252 +807,249 @@ public class AltosState implements Cloneable { } - public void set_altitude(double altitude) { - if (altitude != AltosRecord.MISSING) { - this.altitude = altitude; - update_vertical_pos(); + public void set_device_type(int device_type) { + this.device_type = device_type; + } + + public void set_config(int major, int minor, int apogee_delay, int main_deploy, int flight_log_max) { + config_major = major; + config_minor = minor; + this.apogee_delay = apogee_delay; + this.main_deploy = main_deploy; + this.flight_log_max = flight_log_max; + } + + public void set_callsign(String callsign) { + this.callsign = callsign; + } + + public void set_firmware_version(String version) { + firmware_version = version; + } + + public void set_flight(int flight) { + + /* When the flight changes, reset the state */ + if (flight != AltosLib.MISSING && flight != 0) { + if (this.flight != AltosLib.MISSING && + this.flight != flight) { + init(); + } + this.flight = flight; + } + } + + public void set_serial(int serial) { + /* When the serial changes, reset the state */ + if (serial != AltosLib.MISSING) { + if (this.serial != AltosLib.MISSING && + this.serial != serial) { + init(); + } + this.serial = serial; } } - public void set_ground_altitude(double ground_altitude) { - if (ground_altitude != AltosRecord.MISSING) { - this.ground_altitude = ground_altitude; - update_vertical_pos(); + public void set_receiver_serial(int serial) { + if (serial != AltosLib.MISSING) + receiver_serial = serial; + } + + 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, int sequence) { if (gps != null) { this.gps = gps.clone(); gps_sequence = sequence; - update_vertical_pos(); + update_gps(); + set |= set_gps; } } - 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(); + public void set_imu(AltosIMU imu) { + if (imu != null) + imu = imu.clone(); + this.imu = imu; + } + + public void set_mag(AltosMag mag) { + this.mag = mag.clone(); + } + + public AltosMs5607 make_baro() { + if (baro == null) + baro = new AltosMs5607(); + return baro; + } + + public void set_ms5607(AltosMs5607 ms5607) { + baro = ms5607; + + if (baro != null) { + set_pressure(baro.pa); + set_temperature(baro.cc / 100.0); } } - public void set_pressure(double pressure) { - if (pressure != AltosRecord.MISSING) { - this.pressure = pressure; - set_altitude(AltosConvert.pressure_to_altitude(pressure)); + public void set_ms5607(int pres, int temp) { + if (baro != null) { + baro.set(pres, temp); + + set_pressure(baro.pa); + set_temperature(baro.cc / 100.0); } } + public void make_companion (int nchannels) { + if (companion == null) + companion = new AltosCompanion(nchannels); + } + + public void set_companion(AltosCompanion companion) { + this.companion = companion; + } + + 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) + return; + + 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); + } + public void set_accel_g(double accel_plus_g, double accel_minus_g) { - if (accel_plus_g != AltosRecord.MISSING) { + if (accel_plus_g != AltosLib.MISSING) { this.accel_plus_g = accel_plus_g; this.accel_minus_g = accel_minus_g; update_accel(); } } + public void set_ground_accel(double ground_accel) { - if (ground_accel != AltosRecord.MISSING) { + if (ground_accel != AltosLib.MISSING) { this.ground_accel = ground_accel; update_accel(); } } public void set_accel(double accel) { - if (accel != AltosRecord.MISSING) { + if (accel != AltosLib.MISSING) { this.accel = accel; + if (state == AltosLib.ao_flight_pad) { + if (ground_accel_avg == AltosLib.MISSING) + ground_accel_avg = accel; + else + ground_accel_avg = (ground_accel_avg * 7 + accel) / 8; + } } 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 void init (AltosRecord cur, AltosState prev_state) { - - if (cur == null) - cur = new AltosRecord(); - - data = 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(data.ground_altitude()); - set_altitude(data.altitude()); - - set_kalman(data.kalman_height, data.kalman_speed, data.kalman_acceleration); - - report_time = System.currentTimeMillis(); - - set_temperature(data.temperature()); - set_apogee_voltage(data.drogue_voltage()); - set_main_voltage(data.main_voltage()); - set_battery_voltage(data.battery_voltage()); - - set_pressure(data.pressure()); - - set_tick(data.tick); - set_state(data.state); - - set_accel_g (data.accel_minus_g, data.accel_plus_g); - set_ground_accel(data.ground_accel); - set_accel (data.accel); - - set_gps(data.gps, data.gps_sequence); - - if (prev_state != null) { - - if (data.kalman_speed != AltosRecord.MISSING) { - baro_speed = accel_speed = data.kalman_speed; - } else { - /* compute barometric speed */ - - double height_change = height - prev_state.height; - - double prev_baro_speed = prev_state.baro_speed; - if (prev_baro_speed == AltosRecord.MISSING) - prev_baro_speed = 0; - - if (time_change > 0) - baro_speed = (prev_baro_speed * 3 + (height_change / time_change)) / 4.0; - else - baro_speed = prev_state.baro_speed; - - double prev_accel_speed = prev_state.accel_speed; - - if (prev_accel_speed == AltosRecord.MISSING) - prev_accel_speed = 0; + public void set_ignitor_voltage(double[] voltage) { + this.ignitor_voltage = voltage; + } - if (acceleration == AltosRecord.MISSING) { - /* Fill in mising acceleration value */ - accel_speed = baro_speed; + public double time_since_boost() { + if (tick == AltosLib.MISSING) + return 0.0; - 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; + if (boost_tick != AltosLib.MISSING) { + return (tick - boost_tick) / 100.0; } + return tick / 100.0; + } - time = tick / 100.0; - - if (data.gps != null && data.gps_sequence != gps_sequence && (state < AltosLib.ao_flight_boost)) { + public boolean valid() { + return tick != AltosLib.MISSING && serial != AltosLib.MISSING; + } - /* Track consecutive 'good' gps reports, waiting for 10 of them */ - if (data.gps != null && data.gps.locked && data.gps.nsat >= 4) - 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; - } - ngps++; - } - } else { - if (ngps == 0 && ground_altitude != AltosRecord.MISSING) - pad_alt = ground_altitude; + public AltosGPS make_temp_gps(boolean sats) { + if (temp_gps == null) { + temp_gps = new AltosGPS(gps); } - - 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; - } + gps_pending = true; + if (sats) { + if (tick != temp_gps_sat_tick) + temp_gps.cc_gps_sat = null; + temp_gps_sat_tick = tick; } + return temp_gps; } - public AltosState clone() { - AltosState s = new AltosState(data, this); - return s; + public void set_temp_gps() { + set_gps(temp_gps, gps_sequence + 1); + gps_pending = false; + temp_gps = null; } - public AltosState(AltosRecord cur) { - init(cur, null); + public AltosState clone() { + AltosState s = new AltosState(); + s.copy(this); + return s; } - public AltosState (AltosRecord cur, AltosState prev) { - init(cur, prev); + public AltosState () { + init(); } }