From 0e917f3ff822616adb147517ac961422e5fedbfd Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Thu, 5 Aug 2010 22:49:53 -0400 Subject: [PATCH] altosui: Compute flight state from eeprom data This lets eeprom files be used to replay flights. Signed-off-by: Keith Packard --- ao-tools/altosui/Altos.java | 20 +- ao-tools/altosui/AltosEepromReader.java | 178 +++++++++++++++--- ao-tools/altosui/AltosEepromRecord.java | 56 +++++- .../altosui/AltosFlightStatusTableModel.java | 2 +- ao-tools/altosui/AltosGPS.java | 31 +-- ao-tools/altosui/AltosRecord.java | 11 +- ao-tools/altosui/AltosState.java | 13 +- ao-tools/altosui/AltosTelemetry.java | 2 +- ao-tools/altosui/AltosUI.java | 34 +++- 9 files changed, 277 insertions(+), 70 deletions(-) diff --git a/ao-tools/altosui/Altos.java b/ao-tools/altosui/Altos.java index 6ea7b43c..53359e23 100644 --- a/ao-tools/altosui/Altos.java +++ b/ao-tools/altosui/Altos.java @@ -35,8 +35,18 @@ public class Altos { static final int AO_LOG_GPS_SAT = 'V'; static final int AO_LOG_GPS_DATE = 'Y'; - /* Added for 'serial-number' entry in eeprom files */ - static final int AO_LOG_SERIAL_NUMBER = 1000; + /* Added for header fields in eeprom files */ + static final int AO_LOG_CONFIG_VERSION = 1000; + static final int AO_LOG_MAIN_DEPLOY = 1001; + static final int AO_LOG_APOGEE_DELAY = 1002; + static final int AO_LOG_RADIO_CHANNEL = 1003; + static final int AO_LOG_CALLSIGN = 1004; + static final int AO_LOG_ACCEL_CAL = 1005; + static final int AO_LOG_RADIO_CAL = 1006; + static final int AO_LOG_MANUFACTURER = 1007; + static final int AO_LOG_PRODUCT = 1008; + static final int AO_LOG_SERIAL_NUMBER = 1009; + static final int AO_LOG_SOFTWARE_VERSION = 1010; /* Added to flag invalid records */ static final int AO_LOG_INVALID = -1; @@ -98,4 +108,10 @@ public class Altos { return "invalid"; return state_to_string[state]; } + + static final int AO_GPS_VALID = (1 << 4); + static final int AO_GPS_RUNNING = (1 << 5); + static final int AO_GPS_DATE_VALID = (1 << 6); + static final int AO_GPS_NUM_SAT_SHIFT = 0; + static final int AO_GPS_NUM_SAT_MASK = 0xf; } diff --git a/ao-tools/altosui/AltosEepromReader.java b/ao-tools/altosui/AltosEepromReader.java index 6fe9bfe4..7a8ff5b0 100644 --- a/ao-tools/altosui/AltosEepromReader.java +++ b/ao-tools/altosui/AltosEepromReader.java @@ -36,75 +36,181 @@ import altosui.AltosLog; import altosui.AltosVoice; import altosui.AltosEepromMonitor; +class AltosOrderedRecord extends AltosEepromRecord implements Comparable { + + int index; + + public AltosOrderedRecord(String line, int in_index, int prev_tick) + throws ParseException { + super(line); + int new_tick = tick | (prev_tick & ~0xffff); + if (new_tick < prev_tick) { + if (prev_tick - new_tick > 0x8000) + new_tick += 0x10000; + } + tick = new_tick; + index = in_index; + } + + public int compareTo(AltosOrderedRecord o) { + int tick_diff = tick - o.tick; + if (tick_diff != 0) + return tick_diff; + return index - o.index; + } +} + public class AltosEepromReader { static final int seen_flight = 1; static final int seen_sensor = 2; static final int seen_temp_volt = 4; static final int seen_deploy = 8; + static final int seen_gps_time = 16; + static final int seen_gps_lat = 32; + static final int seen_gps_lon = 64; static final int seen_basic = seen_flight|seen_sensor|seen_temp_volt|seen_deploy; - static final int seen_gps_time = 16; - AltosRecord state; - AltosEepromRecord record; + AltosOrderedRecord record; + + TreeSet records; + + Iterator record_iterator; int seen; - int tick; + int index; + + boolean last_reported; + + double ground_pres; + double ground_accel; + + int n_pad_samples; - boolean done; + int gps_tick; FileInputStream input; public AltosRecord read() throws IOException, ParseException { for (;;) { if (record == null) { - record = new AltosEepromRecord(AltosRecord.gets(input)); - if (record == null) { - if (done) + if (!record_iterator.hasNext()) { + if (last_reported) return null; + last_reported = true; return state; } + record = record_iterator.next(); - /* eeprom only records low 16 bits of tick count */ - int tick = record.tick | (state.tick & ~0xffff); - - if (tick < state.tick) { - if (state.tick - tick > 0x8000) - tick += 0x10000; - else - tick = state.tick; - } - - /* Accumulate data in the state record while - * the time stamp is not increasing - */ - - if ((seen & seen_basic) == seen_basic && tick > state.tick) + if ((seen & seen_basic) == seen_basic && record.tick != state.tick) return new AltosRecord(state); } - state.tick = tick; + state.tick = record.tick; switch (record.cmd) { case Altos.AO_LOG_FLIGHT: state.ground_accel = record.a; state.flight = record.b; + seen |= seen_flight; break; case Altos.AO_LOG_SENSOR: state.accel = record.a; state.pres = record.b; + if (state.state < Altos.ao_flight_boost) { + n_pad_samples++; + ground_pres += state.pres; + state.ground_pres = (int) (ground_pres / n_pad_samples); + state.flight_pres = state.ground_pres; + System.out.printf("ground pressure %d altitude %f\n", + record.b, state.altitude()); + ground_accel += state.accel; + state.ground_accel = (int) (ground_accel / n_pad_samples); + state.flight_accel = state.ground_accel; + } else { + state.flight_pres = (state.flight_pres * 15 + state.pres) / 16; + state.flight_accel = (state.flight_accel * 15 + state.accel) / 16; + state.flight_vel += (state.accel_plus_g - state.accel); + } + seen |= seen_sensor; break; case Altos.AO_LOG_TEMP_VOLT: state.temp = record.a; state.batt = record.b; + seen |= seen_temp_volt; break; case Altos.AO_LOG_DEPLOY: state.drogue = record.a; state.main = record.b; + seen |= seen_deploy; + break; + case Altos.AO_LOG_STATE: + System.out.printf("state %d\n", record.a); + state.state = record.a; break; case Altos.AO_LOG_GPS_TIME: + gps_tick = state.tick; + state.gps = new AltosGPS(); + state.gps.hour = (record.a & 0xff); + state.gps.minute = (record.a >> 8); + state.gps.second = (record.b & 0xff); + int flags = (record.b >> 8); + state.gps.connected = (flags & Altos.AO_GPS_RUNNING) != 0; + state.gps.locked = (flags & Altos.AO_GPS_VALID) != 0; + state.gps.date_valid = (flags & Altos.AO_GPS_DATE_VALID) != 0; + state.gps.nsat = (flags & Altos.AO_GPS_NUM_SAT_MASK) >> + Altos.AO_GPS_NUM_SAT_SHIFT; + break; + case Altos.AO_LOG_GPS_LAT: + int lat32 = record.a | (record.b << 16); + state.gps.lat = (double) lat32 / 1e7; + break; + case Altos.AO_LOG_GPS_LON: + int lon32 = record.a | (record.b << 16); + state.gps.lon = (double) lon32 / 1e7; + break; + case Altos.AO_LOG_GPS_ALT: + state.gps.alt = record.a; + break; + case Altos.AO_LOG_GPS_SAT: + if (state.tick == gps_tick) { + int svid = record.a; + int c_n0 = record.b >> 8; + state.gps.add_sat(svid, c_n0); + } + break; + case Altos.AO_LOG_GPS_DATE: + state.gps.year = record.a & 0xff; + state.gps.month = record.a >> 8; + state.gps.day = record.b & 0xff; + break; + + case Altos.AO_LOG_CONFIG_VERSION: + break; + case Altos.AO_LOG_MAIN_DEPLOY: + break; + case Altos.AO_LOG_APOGEE_DELAY: + break; + case Altos.AO_LOG_RADIO_CHANNEL: + break; + case Altos.AO_LOG_CALLSIGN: + state.callsign = record.data; + break; + case Altos.AO_LOG_ACCEL_CAL: + state.accel_plus_g = record.a; + state.accel_minus_g = record.b; + break; + case Altos.AO_LOG_RADIO_CAL: + break; + case Altos.AO_LOG_MANUFACTURER: + break; + case Altos.AO_LOG_PRODUCT: + break; + case Altos.AO_LOG_SERIAL_NUMBER: + break; + case Altos.AO_LOG_SOFTWARE_VERSION: break; } record = null; @@ -113,8 +219,30 @@ public class AltosEepromReader { public AltosEepromReader (FileInputStream in_input) { state = new AltosRecord(); + state.state = Altos.ao_flight_pad; + state.accel_plus_g = 15758; + state.accel_minus_g = 16294; input = in_input; seen = 0; - done = false; + records = new TreeSet(); + + int index = 0; + int tick = 0; + + try { + for (;;) { + String line = AltosRecord.gets(input); + if (line == null) + break; + AltosOrderedRecord record = new AltosOrderedRecord(line, index++, tick); + if (record == null) + break; + tick = record.tick; + records.add(record); + } + } catch (IOException io) { + } catch (ParseException pe) { + } + record_iterator = records.iterator(); } } diff --git a/ao-tools/altosui/AltosEepromRecord.java b/ao-tools/altosui/AltosEepromRecord.java index 5b359352..86ac1fd2 100644 --- a/ao-tools/altosui/AltosEepromRecord.java +++ b/ao-tools/altosui/AltosEepromRecord.java @@ -44,25 +44,65 @@ public class AltosEepromRecord { public int tick; public int a; public int b; + String data; + public boolean tick_valid; public AltosEepromRecord (String line) throws ParseException { + tick_valid = false; + tick = 0; + a = 0; + b = 0; + data = null; if (line == null) { cmd = Altos.AO_LOG_INVALID; } else { String[] tokens = line.split("\\s+"); - if (tokens[0].equals("serial-number")) { - cmd = Altos.AO_LOG_SERIAL_NUMBER; - tick = 0; - a = Integer.parseInt(tokens[1]); - b = 0; - } else { + if (tokens[0].length() == 1) { if (tokens.length != 4) throw new ParseException(line, 0); cmd = tokens[0].codePointAt(0); - tick = Integer.parseInt(tokens[1]); + tick = Integer.parseInt(tokens[1],16); + tick_valid = true; + a = Integer.parseInt(tokens[2],16); + b = Integer.parseInt(tokens[3],16); + } else if (tokens[0].equals("Config") && tokens[1].equals("version:")) { + cmd = Altos.AO_LOG_CONFIG_VERSION; + data = tokens[2]; + } else if (tokens[0].equals("Main") && tokens[1].equals("deploy:")) { + cmd = Altos.AO_LOG_MAIN_DEPLOY; + a = Integer.parseInt(tokens[2]); + } else if (tokens[0].equals("Apogee") && tokens[1].equals("delay:")) { + cmd = Altos.AO_LOG_APOGEE_DELAY; + a = Integer.parseInt(tokens[2]); + } else if (tokens[0].equals("Radio") && tokens[1].equals("channel:")) { + cmd = Altos.AO_LOG_RADIO_CHANNEL; a = Integer.parseInt(tokens[2]); - b = Integer.parseInt(tokens[3]); + } else if (tokens[0].equals("Callsign:")) { + cmd = Altos.AO_LOG_CALLSIGN; + data = tokens[1].replaceAll("\"",""); + } else if (tokens[0].equals("Accel") && tokens[1].equals("cal")) { + cmd = Altos.AO_LOG_ACCEL_CAL; + a = Integer.parseInt(tokens[3]); + b = Integer.parseInt(tokens[5]); + } else if (tokens[0].equals("Radio") && tokens[1].equals("cal:")) { + cmd = Altos.AO_LOG_RADIO_CAL; + a = Integer.parseInt(tokens[2]); + } else if (tokens[0].equals("manufacturer")) { + cmd = Altos.AO_LOG_MANUFACTURER; + data = tokens[1]; + } else if (tokens[0].equals("product")) { + cmd = Altos.AO_LOG_PRODUCT; + data = tokens[1]; + } else if (tokens[0].equals("serial-number")) { + cmd = Altos.AO_LOG_SERIAL_NUMBER; + a = Integer.parseInt(tokens[1]); + } else if (tokens[0].equals("software-version")) { + cmd = Altos.AO_LOG_SOFTWARE_VERSION; + data = tokens[1]; + } else { + cmd = Altos.AO_LOG_INVALID; + data = line; } } } diff --git a/ao-tools/altosui/AltosFlightStatusTableModel.java b/ao-tools/altosui/AltosFlightStatusTableModel.java index 174dd42c..4c24b6ac 100644 --- a/ao-tools/altosui/AltosFlightStatusTableModel.java +++ b/ao-tools/altosui/AltosFlightStatusTableModel.java @@ -51,7 +51,7 @@ public class AltosFlightStatusTableModel extends AbstractTableModel { public void set(AltosState state) { setValueAt(String.format("%1.0f", state.height), 0); - setValueAt(state.data.state, 1); + setValueAt(state.data.state(), 1); setValueAt(state.data.rssi, 2); double speed = state.baro_speed; if (state.ascent) diff --git a/ao-tools/altosui/AltosGPS.java b/ao-tools/altosui/AltosGPS.java index 6b84d7a5..b3ee67e8 100644 --- a/ao-tools/altosui/AltosGPS.java +++ b/ao-tools/altosui/AltosGPS.java @@ -29,8 +29,9 @@ public class AltosGPS { } int nsat; - boolean gps_locked; - boolean gps_connected; + boolean locked; + boolean connected; + boolean date_valid; double lat; /* degrees (+N -S) */ double lon; /* degrees (+E -W) */ int alt; /* m */ @@ -77,19 +78,19 @@ public class AltosGPS { nsat = AltosParse.parse_int(words[i++]); AltosParse.word(words[i++], "sat"); - gps_connected = false; - gps_locked = false; + connected = false; + locked = false; lat = lon = 0; alt = 0; ClearGPSTime(); if ((words[i]).equals("unlocked")) { - gps_connected = true; + connected = true; i++; } else if ((words[i]).equals("not-connected")) { i++; } else if (words.length >= 40) { - gps_locked = true; - gps_connected = true; + locked = true; + connected = true; ParseGPSTime(words[i], words[i+1]); i += 2; lat = AltosParse.parse_coord(words[i++]); @@ -169,8 +170,8 @@ public class AltosGPS { public AltosGPS(AltosGPS old) { nsat = old.nsat; - gps_locked = old.gps_locked; - gps_connected = old.gps_connected; + locked = old.locked; + connected = old.connected; lat = old.lat; /* degrees (+N -S) */ lon = old.lon; /* degrees (+E -W) */ alt = old.alt; /* m */ @@ -189,11 +190,13 @@ public class AltosGPS { h_error = old.h_error; /* m */ v_error = old.v_error; /* m */ - AltosGPSSat[] cc_gps_sat; /* tracking data */ - cc_gps_sat = new AltosGPSSat[old.cc_gps_sat.length]; - for (int i = 0; i < old.cc_gps_sat.length; i++) { - cc_gps_sat[i].svid = old.cc_gps_sat[i].svid; - cc_gps_sat[i].c_n0 = old.cc_gps_sat[i].c_n0; + if (old.cc_gps_sat != null) { + cc_gps_sat = new AltosGPSSat[old.cc_gps_sat.length]; + for (int i = 0; i < old.cc_gps_sat.length; i++) { + cc_gps_sat[i] = new AltosGPSSat(); + cc_gps_sat[i].svid = old.cc_gps_sat[i].svid; + cc_gps_sat[i].c_n0 = old.cc_gps_sat[i].c_n0; + } } } } diff --git a/ao-tools/altosui/AltosRecord.java b/ao-tools/altosui/AltosRecord.java index c3e9d211..3440d935 100644 --- a/ao-tools/altosui/AltosRecord.java +++ b/ao-tools/altosui/AltosRecord.java @@ -31,7 +31,7 @@ public class AltosRecord { int flight; int rssi; int status; - String state; + int state; int tick; int accel; int pres; @@ -72,7 +72,7 @@ public class AltosRecord { } public double pressure() { - return barometer_to_pressure(pres); + return barometer_to_pressure(flight_pres); } public double ground_pressure() { @@ -136,9 +136,8 @@ public class AltosRecord { return speed; } - public int state() { - System.out.printf("state: %s -> %d\n", state, Altos.state(state)); - return Altos.state(state); + public String state() { + return Altos.state_name(state); } public static String gets(FileInputStream s) throws IOException { @@ -188,7 +187,7 @@ public class AltosRecord { flight = 0; rssi = 0; status = 0; - state = "startup"; + state = Altos.ao_flight_startup; tick = 0; accel = 0; pres = 0; diff --git a/ao-tools/altosui/AltosState.java b/ao-tools/altosui/AltosState.java index fc06f839..deeb4c77 100644 --- a/ao-tools/altosui/AltosState.java +++ b/ao-tools/altosui/AltosState.java @@ -89,7 +89,7 @@ public class AltosState { main_sense = data.main_voltage(); battery = data.battery_voltage(); tick = data.tick; - state = data.state(); + state = data.state; if (prev_state != null) { @@ -124,7 +124,12 @@ public class AltosState { } if (state == Altos.ao_flight_pad) { - if (data.gps != null && data.gps.gps_locked && data.gps.nsat >= 4) { + if (data.gps == null) + System.out.printf("on pad, gps null\n"); + else + System.out.printf ("on pad gps lat %f lon %f locked %d nsat %d\n", + data.gps.lat, data.gps.lon, data.gps.locked ? 1 : 0, data.gps.nsat); + if (data.gps != null && data.gps.locked && data.gps.nsat >= 4) { npad++; if (npad > 1) { /* filter pad position */ @@ -159,9 +164,9 @@ public class AltosState { if (height > max_height) max_height = height; if (data.gps != null) { - if (gps == null || !gps.gps_locked || data.gps.gps_locked) + if (gps == null || !gps.locked || data.gps.locked) gps = data.gps; - if (npad > 0 && gps.gps_locked) + if (npad > 0 && gps.locked) from_pad = new AltosGreatCircle(pad_lat, pad_lon, gps.lat, gps.lon); } if (npad > 0) { diff --git a/ao-tools/altosui/AltosTelemetry.java b/ao-tools/altosui/AltosTelemetry.java index f495be1d..af29b8c0 100644 --- a/ao-tools/altosui/AltosTelemetry.java +++ b/ao-tools/altosui/AltosTelemetry.java @@ -76,7 +76,7 @@ public class AltosTelemetry extends AltosRecord { status = AltosParse.parse_hex(words[i++]); AltosParse.word(words[i++], "STATE"); - state = words[i++]; + state = Altos.state(words[i++]); tick = AltosParse.parse_int(words[i++]); diff --git a/ao-tools/altosui/AltosUI.java b/ao-tools/altosui/AltosUI.java index 40663882..2cb0c479 100644 --- a/ao-tools/altosui/AltosUI.java +++ b/ao-tools/altosui/AltosUI.java @@ -173,7 +173,7 @@ public class AltosUI extends JFrame { else info_add_row(0, "Ground state", "wait (%d)", state.gps_waiting); - info_add_row(0, "Rocket state", "%s", state.data.state); + info_add_row(0, "Rocket state", "%s", state.data.state()); info_add_row(0, "Callsign", "%s", state.data.callsign); info_add_row(0, "Rocket serial", "%6d", state.data.serial); info_add_row(0, "Rocket flight", "%6d", state.data.flight); @@ -193,9 +193,9 @@ public class AltosUI extends JFrame { if (state.gps == null) { info_add_row(1, "GPS", "not available"); } else { - if (state.data.gps.gps_locked) + if (state.data.gps.locked) info_add_row(1, "GPS", " locked"); - else if (state.data.gps.gps_connected) + else if (state.data.gps.connected) info_add_row(1, "GPS", " unlocked"); else info_add_row(1, "GPS", " missing"); @@ -321,7 +321,7 @@ public class AltosUI extends JFrame { private void tell(AltosState state, AltosState old_state) { if (old_state == null || old_state.state != state.state) { - voice.speak(state.data.state); + voice.speak(state.data.state()); if ((old_state == null || old_state.state <= Altos.ao_flight_boost) && state.state > Altos.ao_flight_boost) { voice.speak("max speed: %d meters per second.", @@ -493,7 +493,23 @@ public class AltosUI extends JFrame { class ReplayEepromThread extends DisplayThread { FileInputStream replay; - AltosRecord read () { + AltosEepromReader reader; + + ReplayEepromThread(FileInputStream in, String in_name) { + replay = in; + name = in_name; + reader = new AltosEepromReader (in); + } + + AltosRecord read () throws ParseException { + try { + return reader.read(); + } catch (IOException ee) { + JOptionPane.showMessageDialog(AltosUI.this, + name, + "error reading", + JOptionPane.ERROR_MESSAGE); + } return null; } @@ -504,10 +520,10 @@ public class AltosUI extends JFrame { } report(); } - - ReplayEepromThread(FileInputStream in, String in_name) { - replay = in; - name = in_name; + void update(AltosState state) throws InterruptedException { + /* Make it run in realtime after the rocket leaves the pad */ + if (state.state > Altos.ao_flight_pad) + Thread.sleep((int) (Math.min(state.time_change,10) * 1000)); } } -- 2.30.2