X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=altoslib%2FAltosState.java;h=d3929b8f7e047480ddcd02cef4eed0953863f26c;hp=0970a88e60063d581bae6d474ae95c6dcc8c4ff0;hb=0f32d1a2e2fb5989d124189a7eb1156a00b0ad8f;hpb=b1a90adac9f6e2a609ce1ccd6749462bb5c9adbe diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java index 0970a88e..d3929b8f 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 @@ -23,7 +24,7 @@ package org.altusmetrum.altoslib_11; import java.io.*; -public class AltosState implements Cloneable, AltosHashable { +public class AltosState extends AltosDataListener { public static final int set_position = 1; public static final int set_gps = 2; @@ -39,14 +40,12 @@ public class AltosState implements Cloneable, AltosHashable { public long received_time; + public int rssi; + public int status; + public double time; - public double prev_time; - public double time_change; - public int tick; - private int prev_tick; - public int boost_tick; - class AltosValue implements AltosHashable { + class AltosValue { double value; double prev_value; private double max_value; @@ -177,28 +176,6 @@ public class AltosState implements Cloneable, AltosHashable { prev_set_time = set_time; } - public AltosHashSet hashSet() { - AltosHashSet h = new AltosHashSet(); - - h.putDouble("value", value); - h.putDouble("prev_value", prev_value); - h.putDouble("max_value", max_value); - h.putDouble("set_time", set_time); - h.putDouble("prev_set_time", prev_set_time); - return h; - } - - AltosValue(AltosHashSet h) { - this(); - if (h != null) { - value = h.getDouble("value", value); - prev_value = h.getDouble("prev_value", prev_value); - max_value = h.getDouble("max_value", max_value); - set_time = h.getDouble("set_time", 0); - prev_set_time = h.getDouble("prev_set_time", 0); - } - } - AltosValue() { value = AltosLib.MISSING; prev_value = AltosLib.MISSING; @@ -207,15 +184,9 @@ public class AltosState implements Cloneable, AltosHashable { } - AltosValue AltosValue_fromHashSet(AltosHashSet h, AltosValue def) { - if (h == null) - return def; - return new AltosValue(h); - } - - class AltosCValue implements AltosHashable { + class AltosCValue { - class AltosIValue extends AltosValue implements AltosHashable { + class AltosIValue extends AltosValue { boolean can_max() { return c_can_max(); } @@ -223,10 +194,6 @@ public class AltosState implements Cloneable, AltosHashable { AltosIValue() { super(); } - - AltosIValue(AltosHashSet h) { - super(h); - } }; public AltosIValue measured; @@ -315,48 +282,16 @@ public class AltosState implements Cloneable, AltosHashable { computed.finish_update(); } - AltosCValue() { + public AltosCValue() { measured = new AltosIValue(); computed = new AltosIValue(); } - - - public AltosHashSet hashSet() { - AltosHashSet h = new AltosHashSet(); - - h.putHashable("measured", measured); - h.putHashable("computed", computed); - return h; - } - - AltosCValue(AltosHashSet h) { - measured = new AltosIValue(h.getHash("measured")); - computed = new AltosIValue(h.getHash("computed")); - } - } - - AltosCValue AltosCValue_fromHashSet(AltosHashSet h, AltosCValue def) { - if (h == null) - return def; - return new AltosCValue(h); } private int state; - public int flight; - public int serial; - public int altitude_32; - 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) @@ -390,15 +325,6 @@ public class AltosState implements Cloneable, AltosHashable { AltosGpsGroundAltitude() { super(); } - - AltosGpsGroundAltitude (AltosHashSet h) { - super(h); - } - } - - AltosGpsGroundAltitude AltosGpsGroundAltitude_fromHashSet(AltosHashSet h, AltosGpsGroundAltitude def) { - if (h == null) return def; - return new AltosGpsGroundAltitude(h); } private AltosGpsGroundAltitude gps_ground_altitude; @@ -426,15 +352,6 @@ public class AltosState implements Cloneable, AltosHashable { AltosGroundPressure () { super(); } - - AltosGroundPressure (AltosHashSet h) { - super(h); - } - } - - AltosGroundPressure AltosGroundPressure_fromHashSet(AltosHashSet h, AltosGroundPressure def) { - if (h == null) return def; - return new AltosGroundPressure(h); } private AltosGroundPressure ground_pressure; @@ -447,7 +364,7 @@ public class AltosState implements Cloneable, AltosHashable { ground_pressure.set_measured(pressure, time); } - class AltosAltitude extends AltosCValue implements AltosHashable { + class AltosAltitude extends AltosCValue { private void set_speed(AltosValue v) { if (!acceleration.is_measured() || !ascent) @@ -469,20 +386,11 @@ public class AltosState implements Cloneable, AltosHashable { AltosAltitude() { super(); } - - AltosAltitude (AltosHashSet h) { - super(h); - } - } - - AltosAltitude AltosAltitude_fromHashSet(AltosHashSet h, AltosAltitude def) { - if (h == null) return def; - return new AltosAltitude(h); } private AltosAltitude altitude; - class AltosGpsAltitude extends AltosValue implements AltosHashable { + class AltosGpsAltitude extends AltosValue { private void set_gps_height() { double a = value(); @@ -502,15 +410,6 @@ public class AltosState implements Cloneable, AltosHashable { AltosGpsAltitude() { super(); } - - AltosGpsAltitude (AltosHashSet h) { - super(h); - } - } - - AltosGpsAltitude AltosGpsAltitude_fromHashSet(AltosHashSet h, AltosGpsAltitude def) { - if (h == null) return def; - return new AltosGpsAltitude(h); } private AltosGpsAltitude gps_altitude; @@ -535,6 +434,11 @@ public class AltosState implements Cloneable, AltosHashable { } 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); } @@ -590,15 +494,6 @@ public class AltosState implements Cloneable, AltosHashable { AltosPressure() { super(); } - - AltosPressure (AltosHashSet h) { - super(h); - } - } - - AltosPressure AltosPressure_fromHashSet(AltosHashSet h, AltosPressure def) { - if (h == null) return def; - return new AltosPressure(h); } private AltosPressure pressure; @@ -611,6 +506,25 @@ public class AltosState implements Cloneable, AltosHashable { pressure.set(p, time); } + class AltosForce extends AltosValue { + void set(double p, double time) { + super.set(p, time); + } + + AltosForce() { + super(); + } + } + private AltosForce thrust; + + public double thrust() { + return thrust.value(); + } + + public void set_thrust(double N) { + thrust.set(N, time); + } + public double baro_height() { double a = altitude(); double g = ground_altitude(); @@ -661,7 +575,7 @@ public class AltosState implements Cloneable, AltosHashable { return AltosLib.MISSING; } - class AltosSpeed extends AltosCValue implements AltosHashable { + class AltosSpeed extends AltosCValue { boolean can_max() { return state < AltosLib.ao_flight_fast || state == AltosLib.ao_flight_stateless; @@ -689,15 +603,6 @@ public class AltosState implements Cloneable, AltosHashable { AltosSpeed() { super(); } - - AltosSpeed (AltosHashSet h) { - super(h); - } - } - - AltosSpeed AltosSpeed_fromHashSet(AltosHashSet h, AltosSpeed def) { - if (h == null) return def; - return new AltosSpeed(h); } private AltosSpeed speed; @@ -728,7 +633,7 @@ public class AltosState implements Cloneable, AltosHashable { return AltosLib.MISSING; } - class AltosAccel extends AltosCValue implements AltosHashable { + class AltosAccel extends AltosCValue { boolean can_max() { return state < AltosLib.ao_flight_fast || state == AltosLib.ao_flight_stateless; @@ -743,15 +648,6 @@ public class AltosState implements Cloneable, AltosHashable { AltosAccel() { super(); } - - AltosAccel (AltosHashSet h) { - super(h); - } - } - - AltosAccel AltosAccel_fromHashSet(AltosHashSet h, AltosAccel def) { - if (h == null) return def; - return new AltosAccel(h); } AltosAccel acceleration; @@ -781,6 +677,11 @@ public class AltosState implements Cloneable, AltosHashable { 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); @@ -792,13 +693,10 @@ public class AltosState implements Cloneable, AltosHashable { public double apogee_voltage; public double main_voltage; - public double ignitor_voltage[]; + public double igniter_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; public AltosMag mag; @@ -825,17 +723,12 @@ public class AltosState implements Cloneable, AltosHashable { 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 log_space; public String product; - public AltosMs5607 baro; - public AltosCompanion companion; public int pyro_fired; @@ -853,28 +746,17 @@ public class AltosState implements Cloneable, AltosHashable { 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; 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(); + thrust = new AltosForce(); speed = new AltosSpeed(); acceleration = new AltosAccel(); orient = new AltosCValue(); @@ -884,16 +766,13 @@ public class AltosState implements Cloneable, AltosHashable { pyro_voltage = AltosLib.MISSING; apogee_voltage = AltosLib.MISSING; main_voltage = AltosLib.MISSING; - ignitor_voltage = null; + igniter_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; @@ -902,9 +781,6 @@ public class AltosState implements Cloneable, AltosHashable { ground_rotation = null; mag = null; - accel_zero_along = AltosLib.MISSING; - accel_zero_across = AltosLib.MISSING; - accel_zero_through = AltosLib.MISSING; accel_ground_along = AltosLib.MISSING; accel_ground_across = AltosLib.MISSING; @@ -912,10 +788,6 @@ public class AltosState implements Cloneable, AltosHashable { pad_orientation = AltosLib.MISSING; - gyro_zero_roll = AltosLib.MISSING; - gyro_zero_pitch = AltosLib.MISSING; - gyro_zero_yaw = AltosLib.MISSING; - set_npad(0); ngps = 0; @@ -941,28 +813,18 @@ public class AltosState implements Cloneable, AltosHashable { callsign = null; firmware_version = null; - 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; + log_space = AltosLib.MISSING; product = null; - serial = AltosLib.MISSING; - receiver_serial = AltosLib.MISSING; - altitude_32 = AltosLib.MISSING; - baro = null; companion = null; pyro_fired = 0; } void finish_update() { - prev_tick = tick; - ground_altitude.finish_update(); altitude.finish_update(); pressure.finish_update(); @@ -975,150 +837,6 @@ public class AltosState implements Cloneable, AltosHashable { kalman_acceleration.finish_update(); } - void copy(AltosState old) { - - if (old == null) { - init(); - return; - } - - 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_pressure.copy(old.ground_pressure); - ground_altitude.copy(old.ground_altitude); - altitude.copy(old.altitude); - pressure.copy(old.pressure); - speed.copy(old.speed); - acceleration.copy(old.acceleration); - orient.copy(old.orient); - - battery_voltage = old.battery_voltage; - pyro_voltage = old.pyro_voltage; - temperature = old.temperature; - apogee_voltage = old.apogee_voltage; - main_voltage = old.main_voltage; - ignitor_voltage = old.ignitor_voltage; - - 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(); - else - imu = null; - last_imu_time = old.last_imu_time; - - if (old.rotation != null) - rotation = new AltosRotation (old.rotation); - - if (old.ground_rotation != null) { - ground_rotation = new AltosRotation(old.ground_rotation); - } - - accel_zero_along = old.accel_zero_along; - accel_zero_across = old.accel_zero_across; - accel_zero_through = old.accel_zero_through; - - accel_ground_along = old.accel_ground_along; - accel_ground_across = old.accel_ground_across; - accel_ground_through = old.accel_ground_through; - pad_orientation = old.pad_orientation; - - gyro_zero_roll = old.gyro_zero_roll; - gyro_zero_pitch = old.gyro_zero_pitch; - gyro_zero_yaw = old.gyro_zero_yaw; - - 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; - - gps_altitude.copy(old.gps_altitude); - gps_ground_altitude.copy(old.gps_ground_altitude); - gps_ground_speed.copy(old.gps_ground_speed); - gps_ascent_rate.copy(old.gps_ascent_rate); - gps_course.copy(old.gps_course); - gps_speed.copy(old.gps_speed); - - 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; - firmware_version = old.firmware_version; - - 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; - product = old.product; - serial = old.serial; - receiver_serial = old.receiver_serial; - altitude_32 = old.altitude_32; - - baro = old.baro; - companion = old.companion; - - pyro_fired = old.pyro_fired; - } - void update_time() { } @@ -1169,32 +887,10 @@ public class AltosState implements Cloneable, AltosHashable { } } - 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; - } - } - - public void set_boost_tick(int boost_tick) { - if (boost_tick != AltosLib.MISSING) - this.boost_tick = boost_tick; - } - public String state_name() { return AltosLib.state_name(state); } - public void set_product(String product) { - this.product = product; - } - public void set_state(int state) { if (state != AltosLib.ao_flight_invalid) { this.state = state; @@ -1208,92 +904,8 @@ public class AltosState implements Cloneable, AltosHashable { return state; } - public void set_device_type(int device_type) { - this.device_type = device_type; - switch (device_type) { - case AltosLib.product_telegps: - this.state = AltosLib.ao_flight_stateless; - break; - } - } - - public void set_log_format(int log_format) { - this.log_format = log_format; - switch (log_format) { - case AltosLib.AO_LOG_FORMAT_TELEGPS: - this.state = AltosLib.ao_flight_stateless; - break; - } - } - - public void set_flight_params(int apogee_delay, int main_deploy) { - this.apogee_delay = apogee_delay; - this.main_deploy = main_deploy; - } - - public void set_config(int major, int minor, int flight_log_max) { - config_major = major; - config_minor = minor; - 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 int compare_version(String other_version) { - if (firmware_version == null) - return AltosLib.MISSING; - return AltosLib.compare_version(firmware_version, other_version); - } - private void re_init() { - int bt = boost_tick; - int rs = receiver_serial; init(); - boost_tick = bt; - receiver_serial = rs; - } - - public void set_flight(int flight) { - - /* When the flight changes, reset the state */ - if (flight != AltosLib.MISSING) { - if (this.flight != AltosLib.MISSING && - this.flight != flight) { - re_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) { - re_init(); - } - this.serial = serial; - } - } - - public void set_receiver_serial(int serial) { - if (serial != AltosLib.MISSING) - receiver_serial = serial; - } - - public boolean altitude_32() { - return altitude_32 == 1; - } - - public void set_altitude_32(int altitude_32) { - if (altitude_32 != AltosLib.MISSING) - this.altitude_32 = altitude_32; } public int rssi() { @@ -1313,40 +925,27 @@ public class AltosState implements Cloneable, AltosHashable { received_time = ms; } - public void set_gps(AltosGPS gps, int sequence) { + public void set_gps(AltosGPS gps) { if (gps != null) { - this.gps = gps.clone(); - gps_sequence = sequence; + this.gps = gps; update_gps(); set |= set_gps; } } - public double accel_zero_along; - public double accel_zero_across; - public double accel_zero_through; - public AltosRotation rotation; public AltosRotation ground_rotation; - public void set_accel_zero(double zero_along, double zero_across, double zero_through) { - if (zero_along != AltosLib.MISSING) { - accel_zero_along = zero_along; - accel_zero_across = zero_across; - accel_zero_through = zero_through; - } - } - public int pad_orientation; public double accel_ground_along, accel_ground_across, accel_ground_through; void update_pad_rotation() { if (pad_orientation != AltosLib.MISSING && accel_ground_along != AltosLib.MISSING) { - rotation = new AltosRotation(AltosIMU.convert_accel(accel_ground_across - accel_zero_across), - AltosIMU.convert_accel(accel_ground_through - accel_zero_through), - AltosIMU.convert_accel(accel_ground_along - accel_zero_along), + 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), pad_orientation); ground_rotation = rotation; orient.set_computed(rotation.tilt(), time); @@ -1365,18 +964,6 @@ public class AltosState implements Cloneable, AltosHashable { update_pad_rotation(); } - public double gyro_zero_roll; - public double gyro_zero_pitch; - public double gyro_zero_yaw; - - public void set_gyro_zero(double roll, double pitch, double yaw) { - if (roll != AltosLib.MISSING) { - gyro_zero_roll = roll; - gyro_zero_pitch = pitch; - gyro_zero_yaw = yaw; - } - } - public double last_imu_time; private double radians(double degrees) { @@ -1401,77 +988,58 @@ public class AltosState implements Cloneable, AltosHashable { last_imu_time = time; } - public void set_imu(AltosIMU imu) { - if (imu != null) - imu = imu.clone(); - this.imu = imu; + private double gyro_roll, gyro_pitch, gyro_yaw; + + public void set_gyro(double roll, double pitch, double yaw) { + gyro_roll = roll; + gyro_pitch = roll; + gyro_roll = roll; update_orient(); } - private double gyro_zero_overflow(double first) { - double v = first / 128.0; - if (v < 0) - v = Math.ceil(v); - else - v = Math.floor(v); - return v * 128.0; - } + private double accel_along, accel_across, accel_through; - public void check_imu_wrap(AltosIMU imu) { - if (this.imu == null) { - gyro_zero_roll += gyro_zero_overflow(imu.gyro_roll); - gyro_zero_pitch += gyro_zero_overflow(imu.gyro_pitch); - gyro_zero_yaw += gyro_zero_overflow(imu.gyro_yaw); - } + 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() { - if (imu != null && accel_zero_along != AltosLib.MISSING) - return AltosIMU.convert_accel(imu.accel_along - accel_zero_along); - return AltosLib.MISSING; + return accel_along; } public double accel_across() { - if (imu != null && accel_zero_across != AltosLib.MISSING) - return AltosIMU.convert_accel(imu.accel_across - accel_zero_across); - return AltosLib.MISSING; + return accel_across; } public double accel_through() { - if (imu != null && accel_zero_through != AltosLib.MISSING) - return AltosIMU.convert_accel(imu.accel_through - accel_zero_through); - return AltosLib.MISSING; + return accel_through; } public double gyro_roll() { - if (imu != null && gyro_zero_roll != AltosLib.MISSING) { - return AltosIMU.convert_gyro(imu.gyro_roll - gyro_zero_roll); - } - return AltosLib.MISSING; + return gyro_roll; } public double gyro_pitch() { - if (imu != null && gyro_zero_pitch != AltosLib.MISSING) { - return AltosIMU.convert_gyro(imu.gyro_pitch - gyro_zero_pitch); - } - return AltosLib.MISSING; + return gyro_pitch; } public double gyro_yaw() { - if (imu != null && gyro_zero_yaw != AltosLib.MISSING) { - return AltosIMU.convert_gyro(imu.gyro_yaw - gyro_zero_yaw); - } - return AltosLib.MISSING; + return gyro_yaw; } - public void set_mag(AltosMag mag) { - this.mag = mag.clone(); + 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() { - if (mag != null) - return AltosMag.convert_gauss(mag.along); - return AltosLib.MISSING; + return mag_along; } public double mag_across() { @@ -1486,71 +1054,15 @@ public class AltosState implements Cloneable, AltosHashable { return AltosLib.MISSING; } - 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_ms5607(int pres, int temp) { - if (baro != null) { - baro.set(pres, temp); - - set_pressure(baro.pa); - set_temperature(baro.cc / 100.0); - } - } - public void set_companion(AltosCompanion companion) { this.companion = companion; } - void update_accel() { - if (accel == 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((accel_plus_g - accel) / counts_per_mss, time); - } - - public void set_accel_g(double accel_plus_g, double accel_minus_g) { - 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 != AltosLib.MISSING) - this.ground_accel = ground_accel; - } - - public void set_accel(double accel) { - 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; - } + 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) { @@ -1588,281 +1100,16 @@ public class AltosState implements Cloneable, AltosHashable { } } - public void set_ignitor_voltage(double[] voltage) { - this.ignitor_voltage = voltage; + public void set_igniter_voltage(double[] voltage) { + this.igniter_voltage = voltage; } public void set_pyro_fired(int fired) { this.pyro_fired = fired; } - public double time_since_boost() { - if (tick == AltosLib.MISSING) - return 0.0; - - if (boost_tick == AltosLib.MISSING) - return tick / 100.0; - return (tick - boost_tick) / 100.0; - } - - public boolean valid() { - return tick != AltosLib.MISSING && serial != AltosLib.MISSING; - } - - public AltosGPS make_temp_gps(boolean sats) { - if (temp_gps == null) { - temp_gps = new AltosGPS(gps); - } - 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 void set_temp_gps() { - set_gps(temp_gps, gps_sequence + 1); - gps_pending = false; - temp_gps = null; - } - - public AltosState clone() { - AltosState s = new AltosState(); - s.copy(this); - - AltosHashSet hash = hashSet(); - String onetrip = hash.toString(); - AltosHashSet back = AltosHashSet.fromString(onetrip); - AltosState tripstate = AltosState.fromHashSet(back); - AltosHashSet triphash = tripstate.hashSet(); - String twotrip = triphash.toString(); - - if (!onetrip.equals(twotrip)) { - System.out.printf("%s\n%s\n", onetrip, twotrip); - System.exit(1); - } - return s; - } - - public AltosState () { + public AltosState (AltosCalData cal_data) { + super(cal_data); init(); } - - public AltosHashSet hashSet() { - AltosHashSet h = new AltosHashSet(); - - h.putBoolean("valid", true); - h.putInt("set", set); - h.putLong("received_time", received_time); - h.putDouble("time", time); - h.putDouble("prev_time", prev_time); - h.putDouble("time_change", time_change); - h.putInt("tick", tick); - h.putInt("prev_tick", prev_tick); - h.putInt("boost_tick", boost_tick); - h.putInt("state", state); - h.putInt("flight", flight); - h.putInt("serial", serial); - h.putInt("altitude_32", altitude_32); - h.putInt("receiver_serial", receiver_serial); - h.putBoolean("landed", landed); - h.putBoolean("ascent", ascent); - h.putBoolean("boost", boost); - h.putInt("rssi", rssi); - h.putInt("status", status); - h.putInt("device_type", device_type); - h.putInt("config_major", config_major); - h.putInt("config_minor", config_minor); - h.putInt("apogee_delay", apogee_delay); - h.putInt("main_deploy", main_deploy); - h.putInt("flight_log_max", flight_log_max); - h.putHashable("ground_altitude", ground_altitude); - h.putHashable("gps_ground_altitude", gps_ground_altitude); - h.putHashable("ground_pressure", ground_pressure); - h.putHashable("altitude", altitude); - h.putHashable("gps_altitude", gps_altitude); - h.putHashable("gps_ground_speed", gps_ground_speed); - h.putHashable("gps_ascent_rate", gps_ascent_rate); - h.putHashable("gps_course", gps_course); - h.putHashable("gps_speed", gps_speed); - h.putHashable("pressure", pressure); - h.putHashable("speed", speed); - h.putHashable("acceleration", acceleration); - h.putHashable("orient", orient); - h.putHashable("kalman_height", kalman_height); - h.putHashable("kalman_speed", kalman_speed); - h.putHashable("kalman_acceleration", kalman_acceleration); - - h.putDouble("battery_voltage",battery_voltage); - h.putDouble("pyro_voltage",pyro_voltage); - h.putDouble("temperature",temperature); - h.putDouble("apogee_voltage",apogee_voltage); - h.putDouble("main_voltage",main_voltage); - h.putDoubleArray("ignitor_voltage",ignitor_voltage); - h.putHashable("gps", gps); - h.putHashable("temp_gps", temp_gps); - h.putInt("temp_gps_sat_tick", temp_gps_sat_tick); - h.putBoolean("gps_pending", gps_pending); - h.putInt("gps_sequence", gps_sequence); - h.putHashable("imu", imu); - h.putHashable("mag", mag); - - h.putInt("npad", npad); - h.putInt("gps_waiting", gps_waiting); - h.putBoolean("gps_ready", gps_ready); - h.putInt("ngps", ngps); - h.putHashable("from_pad", from_pad); - h.putDouble("elevation", elevation); - h.putDouble("range", range); - h.putDouble("gps_height", gps_height); - h.putDouble("pad_lat", pad_lat); - h.putDouble("pad_lon", pad_lon); - h.putDouble("pad_alt", pad_alt); - h.putInt("speak_tick", speak_tick); - h.putDouble("speak_altitude", speak_altitude); - h.putString("callsign", callsign); - h.putString("firmware_version", firmware_version); - h.putDouble("accel_plus_g", accel_plus_g); - h.putDouble("accel_minus_g", accel_minus_g); - h.putDouble("accel", accel); - h.putDouble("ground_accel", ground_accel); - h.putDouble("ground_accel_avg", ground_accel_avg); - h.putInt("log_format", log_format); - h.putString("product", product); - h.putHashable("baro", baro); - h.putHashable("companion", companion); - h.putInt("pyro_fired", pyro_fired); - h.putDouble("accel_zero_along", accel_zero_along); - h.putDouble("accel_zero_across", accel_zero_across); - h.putDouble("accel_zero_through", accel_zero_through); - - h.putHashable("rotation", rotation); - h.putHashable("ground_rotation", ground_rotation); - - h.putInt("pad_orientation", pad_orientation); - - h.putDouble("accel_ground_along", accel_ground_along); - h.putDouble("accel_ground_across", accel_ground_across); - h.putDouble("accel_ground_through", accel_ground_through); - - h.putDouble("gyro_zero_roll", gyro_zero_roll); - h.putDouble("gyro_zero_pitch", gyro_zero_pitch); - h.putDouble("gyro_zero_yaw", gyro_zero_yaw); - - h.putDouble("last_imu_time", last_imu_time); - return h; - } - - public AltosState(AltosHashSet h) { - this(); - - set = h.getInt("set", set); - received_time = h.getLong("received_time", received_time); - time = h.getDouble("time", time); - prev_time = h.getDouble("prev_time", prev_time); - time_change = h.getDouble("time_change", time_change); - tick = h.getInt("tick", tick); - prev_tick = h.getInt("prev_tick", prev_tick); - boost_tick = h.getInt("boost_tick", boost_tick); - state = h.getInt("state", state); - flight = h.getInt("flight", flight); - serial = h.getInt("serial", serial); - altitude_32 = h.getInt("altitude_32", altitude_32); - receiver_serial = h.getInt("receiver_serial", receiver_serial); - landed = h.getBoolean("landed", landed); - ascent = h.getBoolean("ascent", ascent); - boost = h.getBoolean("boost", boost); - rssi = h.getInt("rssi", rssi); - status = h.getInt("status", status); - device_type = h.getInt("device_type", device_type); - config_major = h.getInt("config_major", config_major); - config_minor = h.getInt("config_minor", config_minor); - apogee_delay = h.getInt("apogee_delay", apogee_delay); - main_deploy = h.getInt("main_deploy", main_deploy); - flight_log_max = h.getInt("flight_log_max", flight_log_max); - ground_altitude = AltosCValue_fromHashSet(h.getHash("ground_altitude"), ground_altitude); - gps_ground_altitude = AltosGpsGroundAltitude_fromHashSet(h.getHash("gps_ground_altitude"), gps_ground_altitude); - ground_pressure = AltosGroundPressure_fromHashSet(h.getHash("ground_pressure"), ground_pressure); - altitude = AltosAltitude_fromHashSet(h.getHash("altitude"), altitude); - gps_altitude = AltosGpsAltitude_fromHashSet(h.getHash("gps_altitude"), gps_altitude); - gps_ground_speed = AltosValue_fromHashSet(h.getHash("gps_ground_speed"), gps_ground_speed); - gps_ascent_rate = AltosValue_fromHashSet(h.getHash("gps_ascent_rate"), gps_ascent_rate); - gps_course = AltosValue_fromHashSet(h.getHash("gps_course"), gps_course); - gps_speed = AltosValue_fromHashSet(h.getHash("gps_speed"), gps_speed); - pressure = AltosPressure_fromHashSet(h.getHash("pressure"), pressure); - speed = AltosSpeed_fromHashSet(h.getHash("speed"), speed); - acceleration = AltosAccel_fromHashSet(h.getHash("acceleration"), acceleration); - orient = AltosCValue_fromHashSet(h.getHash("orient"), orient); - kalman_height = AltosValue_fromHashSet(h.getHash("kalman_height"), kalman_height); - kalman_speed = AltosValue_fromHashSet(h.getHash("kalman_speed"), kalman_speed); - kalman_acceleration = AltosValue_fromHashSet(h.getHash("kalman_acceleration"), kalman_acceleration); - - battery_voltage = h.getDouble("battery_voltage", battery_voltage); - pyro_voltage = h.getDouble("pyro_voltage", pyro_voltage); - temperature = h.getDouble("temperature", temperature); - apogee_voltage = h.getDouble("apogee_voltage", apogee_voltage); - main_voltage= h.getDouble("main_voltage", main_voltage); - ignitor_voltage = h.getDoubleArray("ignitor_voltage", ignitor_voltage); - gps = AltosGPS.fromHashSet(h.getHash("gps"), gps); - temp_gps = AltosGPS.fromHashSet(h.getHash("temp_gps"), temp_gps); - temp_gps_sat_tick = h.getInt("temp_gps_sat_tick", temp_gps_sat_tick); - gps_pending = h.getBoolean("gps_pending", gps_pending); - gps_sequence = h.getInt("gps_sequence", gps_sequence); - imu = AltosIMU.fromHashSet(h.getHash("imu"), imu); - mag = AltosMag.fromHashSet(h.getHash("mag"), mag); - - npad = h.getInt("npad", npad); - gps_waiting = h.getInt("gps_waiting", gps_waiting); - gps_ready = h.getBoolean("gps_ready", gps_ready); - ngps = h.getInt("ngps", ngps); - from_pad = AltosGreatCircle.fromHashSet(h.getHash("from_pad"), from_pad); - elevation = h.getDouble("elevation", elevation); - range = h.getDouble("range", range); - gps_height = h.getDouble("gps_height", gps_height); - pad_lat = h.getDouble("pad_lat", pad_lat); - pad_lon = h.getDouble("pad_lon", pad_lon); - pad_alt = h.getDouble("pad_alt", pad_alt); - speak_tick = h.getInt("speak_tick", speak_tick); - speak_altitude = h.getDouble("speak_altitude", speak_altitude); - callsign = h.getString("callsign", callsign); - firmware_version = h.getString("firmware_version", firmware_version); - accel_plus_g = h.getDouble("accel_plus_g", accel_plus_g); - accel_minus_g = h.getDouble("accel_minus_g", accel_minus_g); - accel = h.getDouble("accel", accel); - ground_accel = h.getDouble("ground_accel", ground_accel); - ground_accel_avg = h.getDouble("ground_accel_avg", ground_accel_avg); - log_format = h.getInt("log_format", log_format); - product = h.getString("product", product); - baro = AltosMs5607.fromHashSet(h.getHash("baro"), baro); - companion = AltosCompanion.fromHashSet(h.getHash("companion"), companion); - pyro_fired = h.getInt("pyro_fired", pyro_fired); - accel_zero_along = h.getDouble("accel_zero_along", accel_zero_along); - accel_zero_across = h.getDouble("accel_zero_across", accel_zero_across); - accel_zero_through = h.getDouble("accel_zero_through", accel_zero_through); - - rotation = AltosRotation.fromHashSet(h.getHash("rotation"), rotation); - ground_rotation = AltosRotation.fromHashSet(h.getHash("ground_rotation"), ground_rotation); - - pad_orientation = h.getInt("pad_orientation", pad_orientation); - - accel_ground_along = h.getDouble("accel_ground_along", accel_ground_along); - accel_ground_across = h.getDouble("accel_ground_across", accel_ground_across); - accel_ground_through = h.getDouble("accel_ground_through", accel_ground_through); - - gyro_zero_roll = h.getDouble("gyro_zero_roll", gyro_zero_roll); - gyro_zero_pitch = h.getDouble("gyro_zero_pitch", gyro_zero_pitch); - gyro_zero_yaw = h.getDouble("gyro_zero_yaw", gyro_zero_yaw); - - last_imu_time = h.getDouble("last_imu_time", last_imu_time); - } - - public static AltosState fromHashSet(AltosHashSet h) { - if (h == null) - return null; - if (!h.getBoolean("valid", false)) - return null; - return new AltosState(h); - } }