X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=altoslib%2FAltosState.java;h=7a64f8a188182704c5aa6fbd01e17fb627eef5e5;hp=f46b12eaa7b00b3973f4b1c6f4995515093e1409;hb=e64b1bc108bd75bcd6271631e48abde84af4631f;hpb=f26cfe417c6977cf1e7e75a4f050e25f64d41859 diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java index f46b12ea..7a64f8a1 100644 --- a/altoslib/AltosState.java +++ b/altoslib/AltosState.java @@ -20,9 +20,7 @@ * Track flight state from telemetry or eeprom data stream */ -package org.altusmetrum.altoslib_11; - -import java.io.*; +package org.altusmetrum.altoslib_13; public class AltosState extends AltosDataListener { @@ -43,10 +41,6 @@ public class AltosState extends AltosDataListener { public int rssi; public int status; - public double time; - public double prev_time; - public double time_change; - class AltosValue { double value; double prev_value; @@ -290,17 +284,9 @@ public class AltosState extends AltosDataListener { } } - private int state; - public int altitude_32; - public int receiver_serial; public boolean landed; public boolean ascent; /* going up? */ public boolean boost; /* under power */ - 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) @@ -494,7 +480,7 @@ public class AltosState extends AltosDataListener { class AltosPressure extends AltosValue { void set(double p, double time) { super.set(p, time); - if (state == AltosLib.ao_flight_pad) + if (state() == AltosLib.ao_flight_pad) ground_pressure.set_filtered(p, time); double a = pressure_to_altitude(p); altitude.set_computed(a, time); @@ -515,23 +501,7 @@ public class AltosState extends AltosDataListener { 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() { @@ -587,7 +557,7 @@ public class AltosState extends AltosDataListener { class AltosSpeed extends AltosCValue { boolean can_max() { - return state < AltosLib.ao_flight_fast || state == AltosLib.ao_flight_stateless; + return state() < AltosLib.ao_flight_fast || state() == AltosLib.ao_flight_stateless; } void set_accel() { @@ -645,7 +615,7 @@ public class AltosState extends AltosDataListener { class AltosAccel extends AltosCValue { boolean can_max() { - return state < AltosLib.ao_flight_fast || state == AltosLib.ao_flight_stateless; + return state() < AltosLib.ao_flight_fast || state() == AltosLib.ao_flight_stateless; } void set_measured(double a, double time) { @@ -702,16 +672,11 @@ public class AltosState extends AltosDataListener { 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 AltosIMU imu; - public AltosMag mag; - public static final int MIN_PAD_SAMPLES = 10; public int npad; @@ -722,6 +687,7 @@ public class AltosState extends AltosDataListener { public AltosGreatCircle from_pad; public double elevation; /* from pad */ + public double distance; /* distance along ground */ public double range; /* total distance */ public double gps_height; @@ -731,17 +697,8 @@ public class AltosState extends AltosDataListener { public int speak_tick; public double speak_altitude; - public String callsign; - public String firmware_version; - public double ground_accel; - public int log_format; - public int log_space; - public String product; - - public AltosMs5607 baro; - public AltosCompanion companion; public int pyro_fired; @@ -755,28 +712,20 @@ public class AltosState extends AltosDataListener { } public void init() { + super.init(); + set = 0; received_time = System.currentTimeMillis(); - time = AltosLib.MISSING; - time_change = AltosLib.MISSING; - prev_time = AltosLib.MISSING; - state = AltosLib.ao_flight_invalid; landed = false; boost = false; rssi = AltosLib.MISSING; status = 0; - 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(); @@ -786,38 +735,40 @@ public class AltosState extends AltosDataListener { 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_pending = false; - imu = null; last_imu_time = AltosLib.MISSING; rotation = null; - 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; accel_ground_through = AltosLib.MISSING; - pad_orientation = AltosLib.MISSING; + accel_along = AltosLib.MISSING; + accel_across = AltosLib.MISSING; + accel_through = AltosLib.MISSING; + + gyro_roll = AltosLib.MISSING; + gyro_pitch = AltosLib.MISSING; + gyro_yaw = AltosLib.MISSING; + + mag_along = AltosLib.MISSING; + mag_across = AltosLib.MISSING; + mag_through = AltosLib.MISSING; set_npad(0); ngps = 0; from_pad = null; elevation = AltosLib.MISSING; + distance = AltosLib.MISSING; range = AltosLib.MISSING; gps_height = AltosLib.MISSING; @@ -835,18 +786,8 @@ public class AltosState extends AltosDataListener { speak_tick = AltosLib.MISSING; speak_altitude = AltosLib.MISSING; - callsign = null; - firmware_version = null; - ground_accel = AltosLib.MISSING; - log_format = AltosLib.MISSING; - log_space = AltosLib.MISSING; - product = null; - receiver_serial = AltosLib.MISSING; - altitude_32 = AltosLib.MISSING; - - baro = null; companion = null; pyro_fired = 0; @@ -870,6 +811,7 @@ public class AltosState extends AltosDataListener { void update_gps() { elevation = AltosLib.MISSING; + distance = AltosLib.MISSING; range = AltosLib.MISSING; if (gps == null) @@ -877,9 +819,9 @@ public class AltosState extends AltosDataListener { if (gps.locked && gps.nsat >= 4) { /* Track consecutive 'good' gps reports, waiting for 10 of them */ - if (state == AltosLib.ao_flight_pad || state == AltosLib.ao_flight_stateless) { + if (state() == AltosLib.ao_flight_pad || state() == AltosLib.ao_flight_stateless) { set_npad(npad+1); - if (pad_lat != AltosLib.MISSING && (npad < 10 || state == AltosLib.ao_flight_pad)) { + if (pad_lat != AltosLib.MISSING && (npad < 10 || state() == AltosLib.ao_flight_pad)) { pad_lat = (pad_lat * 31 + gps.lat) / 32; pad_lon = (pad_lon * 31 + gps.lon) / 32; gps_ground_altitude.set_filtered(gps.alt, time); @@ -900,6 +842,8 @@ public class AltosState extends AltosDataListener { gps.climb_rate * gps.climb_rate), time); if (gps.course != AltosLib.MISSING) gps_course.set(gps.course, time); + } else if (state() == AltosLib.ao_flight_pad || state() == AltosLib.ao_flight_stateless) { + set_npad(0); } if (gps.lat != 0 && gps.lon != 0 && pad_lat != AltosLib.MISSING && @@ -911,110 +855,20 @@ public class AltosState extends AltosDataListener { h = 0; from_pad = new AltosGreatCircle(pad_lat, pad_lon, 0, gps.lat, gps.lon, h); elevation = from_pad.elevation; + distance = from_pad.distance; range = from_pad.range; } } public String state_name() { - return AltosLib.state_name(state); - } - - public void set_product(String product) { - this.product = product; + return AltosLib.state_name(state()); } public void set_state(int state) { - if (state != AltosLib.ao_flight_invalid) { - this.state = state; - ascent = (AltosLib.ao_flight_boost <= state && - state <= AltosLib.ao_flight_coast); - boost = (AltosLib.ao_flight_boost == state); - } - } - - public int state() { - return state; - } - - 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_log_space(int log_space) { - this.log_space = log_space; - } - - 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 rs = receiver_serial; - init(); - 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; + super.set_state(state); + ascent = (AltosLib.ao_flight_boost <= state() && + state() <= AltosLib.ao_flight_coast); + boost = (AltosLib.ao_flight_boost == state()); } public int rssi() { @@ -1035,6 +889,7 @@ public class AltosState extends AltosDataListener { } public void set_gps(AltosGPS gps) { + super.set_gps(gps); if (gps != null) { this.gps = gps; update_gps(); @@ -1042,33 +897,16 @@ public class AltosState extends AltosDataListener { } } - - 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), - pad_orientation); - ground_rotation = 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); } } @@ -1080,29 +918,18 @@ public class AltosState extends AltosDataListener { update_pad_rotation(); } - public void set_pad_orientation(int pad_orientation) { - this.pad_orientation = pad_orientation; - update_pad_rotation(); - } - public double last_imu_time; - private double radians(double degrees) { - if (degrees == AltosLib.MISSING) - return AltosLib.MISSING; - return degrees * Math.PI / 180.0; - } - private void update_orient() { if (last_imu_time != AltosLib.MISSING) { double t = time - last_imu_time; - double pitch = radians(gyro_pitch()); - double yaw = radians(gyro_yaw()); - double roll = radians(gyro_roll()); + if (t > 0 && gyro_pitch != AltosLib.MISSING && rotation != null) { + double pitch = AltosConvert.degrees_to_radians(gyro_pitch) * t; + double yaw = AltosConvert.degrees_to_radians(gyro_yaw) * t; + double roll = AltosConvert.degrees_to_radians(gyro_roll) * t; - if (t > 0 & pitch != AltosLib.MISSING && rotation != null) { - rotation.rotate(t, pitch, yaw, roll); + rotation.rotate(pitch, yaw, roll); orient.set_computed(rotation.tilt(), time); } } @@ -1113,8 +940,8 @@ public class AltosState extends AltosDataListener { public void set_gyro(double roll, double pitch, double yaw) { gyro_roll = roll; - gyro_pitch = roll; - gyro_roll = roll; + gyro_pitch = pitch; + gyro_yaw = yaw; update_orient(); } @@ -1164,15 +991,11 @@ public class AltosState extends AltosDataListener { } public double mag_across() { - if (mag != null) - return AltosMag.convert_gauss(mag.across); - return AltosLib.MISSING; + return mag_across; } public double mag_through() { - if (mag != null) - return AltosMag.convert_gauss(mag.through); - return AltosLib.MISSING; + return mag_through; } public void set_companion(AltosCompanion companion) { @@ -1221,14 +1044,18 @@ public class AltosState extends AltosDataListener { } } - 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 AltosState() { + init(); + } + public AltosState (AltosCalData cal_data) { super(cal_data); init();