diff options
Diffstat (limited to 'altoslib')
43 files changed, 4228 insertions, 1011 deletions
diff --git a/altoslib/AltosCompanion.java b/altoslib/AltosCompanion.java new file mode 100644 index 00000000..1572fdae --- /dev/null +++ b/altoslib/AltosCompanion.java @@ -0,0 +1,38 @@ +/* + * Copyright © 2011 Keith Packard <keithp@keithp.com> + * + * 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. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +package org.altusmetrum.altoslib_1; + +public class AltosCompanion { + public final static int board_id_telescience = 0x0a; + public final static int MAX_CHANNELS = 12; + + public int tick; + public int board_id; + public int update_period; + public int channels; + public int[] companion_data; + + public AltosCompanion(int in_channels) { + channels = in_channels; + if (channels < 0) + channels = 0; + if (channels > MAX_CHANNELS) + channels = MAX_CHANNELS; + companion_data = new int[channels]; + } +} diff --git a/altoslib/AltosConvert.java b/altoslib/AltosConvert.java index 8cd478e2..a1e2cdca 100644 --- a/altoslib/AltosConvert.java +++ b/altoslib/AltosConvert.java @@ -190,6 +190,12 @@ public class AltosConvert { return ignite / 32767 * 15.0; } + public static double + barometer_to_pressure(double count) + { + return ((count / 16.0) / 2047.0 + 0.095) / 0.009 * 1000.0; + } + public static double radio_to_frequency(int freq, int setting, int cal, int channel) { double f; diff --git a/altoslib/AltosEeprom.java b/altoslib/AltosEeprom.java new file mode 100644 index 00000000..31646c7e --- /dev/null +++ b/altoslib/AltosEeprom.java @@ -0,0 +1,89 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * 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. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +package org.altusmetrum.altoslib_1; + +import java.io.*; +import java.util.*; +import java.text.*; + +public abstract class AltosEeprom implements AltosStateUpdate { + public int cmd; + public int tick; + public int data8[]; + public boolean valid; + + public final static int header_length = 4; + + public abstract void update_state(AltosState state); + + public void write(PrintStream out) { + out.printf("%c %04x", cmd, tick); + if (data8 != null) { + for (int i = 0; i < data8.length; i++) + out.printf (" %02x", data8[i]); + } + out.printf ("\n"); + } + + void parse_chunk(AltosEepromChunk chunk, int start, int record_length) throws ParseException { + cmd = chunk.data(start); + + int data_length = record_length - header_length; + + valid = !chunk.erased(start, record_length); + if (valid) { + if (AltosConvert.checksum(chunk.data, start, record_length) != 0) + throw new ParseException(String.format("invalid checksum at 0x%x", + chunk.address + start), 0); + } else { + cmd = AltosLib.AO_LOG_INVALID; + } + + tick = chunk.data16(start+2); + + data8 = new int[data_length]; + for (int i = 0; i < data_length; i++) + data8[i] = chunk.data(start + header_length + i); + } + + void parse_string(String line, int record_length) { + valid = false; + tick = 0; + cmd = AltosLib.AO_LOG_INVALID; + + int data_length = record_length - header_length; + + if (line == null) + return; + try { + String[] tokens = line.split("\\s+"); + + if (tokens[0].length() == 1) { + if (tokens.length == 2 + data_length) { + cmd = tokens[0].codePointAt(0); + tick = Integer.parseInt(tokens[1],16); + valid = true; + data8 = new int[data_length]; + for (int i = 0; i < data_length; i++) + data8[i] = Integer.parseInt(tokens[2 + i],16); + } + } + } catch (NumberFormatException ne) { + } + } +} diff --git a/altoslib/AltosEepromBody.java b/altoslib/AltosEepromBody.java new file mode 100644 index 00000000..60aa8881 --- /dev/null +++ b/altoslib/AltosEepromBody.java @@ -0,0 +1,31 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * 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. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +package org.altusmetrum.altoslib_1; + +import java.io.*; +import java.util.*; +import java.text.*; + +public class AltosEepromBody implements AltosEeprom, AltosStateUpdate { + + public void update_state(AltosState state) { + } + + public void write(PrintStream out) { + } +}
\ No newline at end of file diff --git a/altoslib/AltosEepromBodyIterable.java b/altoslib/AltosEepromBodyIterable.java new file mode 100644 index 00000000..33dc0ac8 --- /dev/null +++ b/altoslib/AltosEepromBodyIterable.java @@ -0,0 +1,28 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * 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. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +package org.altusmetrum.altoslib_1; + +import java.io.*; +import java.util.*; +import java.text.*; + +public class AltosEepromBodyIterable { + LinkedList<AltosEepromBody> bodies; + + +}
\ No newline at end of file diff --git a/altoslib/AltosEepromFile.java b/altoslib/AltosEepromFile.java new file mode 100644 index 00000000..2f4c54d7 --- /dev/null +++ b/altoslib/AltosEepromFile.java @@ -0,0 +1,110 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * 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. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +package org.altusmetrum.altoslib_1; + +import java.io.*; +import java.util.*; +import java.text.*; + +class AltosEepromIterator implements Iterator<AltosState> { + AltosState state; + Iterator<AltosEeprom> body; + AltosEeprom next; + boolean seen; + + public boolean hasNext() { + return !seen || body.hasNext(); + } + + public AltosState next() { + if (seen) { + AltosState n = state.clone(); + AltosEeprom e = body.next(); + + e.update_state(n); + state = n; + } + seen = true; + return state; + } + + public void remove () { + } + + public AltosEepromIterator(AltosState start, Iterator<AltosEeprom> body) { + this.state = start; + this.body = body; + this.seen = false; + } +} + +public class AltosEepromFile extends AltosStateIterable { + + AltosEepromIterable headers; + AltosEepromIterable body; + AltosState start; + + public void write_comments(PrintStream out) { + headers.write(out); + } + + public void write(PrintStream out) { + headers.write(out); + body.write(out); + } + + public AltosEepromFile(FileInputStream input) { + headers = new AltosEepromIterable(AltosEepromHeader.read(input)); + + start = headers.state(); + + switch (start.log_format) { + case AltosLib.AO_LOG_FORMAT_FULL: + body = new AltosEepromIterable(AltosEepromTM.read(input)); + break; + case AltosLib.AO_LOG_FORMAT_TINY: + case AltosLib.AO_LOG_FORMAT_TELEMETRY: + case AltosLib.AO_LOG_FORMAT_TELESCIENCE: + case AltosLib.AO_LOG_FORMAT_TELEMEGA: + break; + case AltosLib.AO_LOG_FORMAT_TELEMINI: + case AltosLib.AO_LOG_FORMAT_EASYMINI: + body = new AltosEepromIterable(AltosEepromMini.read(input)); + break; + } + + /* Find boost tick */ + AltosState state = start.clone(); + for (AltosEeprom eeprom : body) { + eeprom.update_state(state); + if (state.state >= AltosLib.ao_flight_boost) { + start.set_boost_tick(state.tick); + break; + } + } + } + + public Iterator<AltosState> iterator() { + AltosState state = start.clone(); + Iterator<AltosEeprom> i = body.iterator(); + + while (i.hasNext() && !state.valid()) + i.next().update_state(state); + return new AltosEepromIterator(state, i); + } +}
\ No newline at end of file diff --git a/altoslib/AltosEepromHeader.java b/altoslib/AltosEepromHeader.java new file mode 100644 index 00000000..a06f05ed --- /dev/null +++ b/altoslib/AltosEepromHeader.java @@ -0,0 +1,274 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * 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. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +package org.altusmetrum.altoslib_1; + +import java.io.*; +import java.util.*; +import java.text.*; + +public class AltosEepromHeader extends AltosEeprom { + + public int cmd; + public String data; + public int config_a, config_b; + public boolean last; + public boolean valid; + + public void update_state(AltosState state) { + switch (cmd) { + case AltosLib.AO_LOG_CONFIG_VERSION: + break; + case AltosLib.AO_LOG_MAIN_DEPLOY: + break; + case AltosLib.AO_LOG_APOGEE_DELAY: + break; + case AltosLib.AO_LOG_RADIO_CHANNEL: + break; + case AltosLib.AO_LOG_CALLSIGN: + state.callsign = data; + break; + case AltosLib.AO_LOG_ACCEL_CAL: + state.set_accel_g(config_a, config_b); + break; + case AltosLib.AO_LOG_RADIO_CAL: + break; + case AltosLib.AO_LOG_MANUFACTURER: + break; + case AltosLib.AO_LOG_PRODUCT: + break; + case AltosLib.AO_LOG_LOG_FORMAT: + state.log_format = config_a; + break; + case AltosLib.AO_LOG_SERIAL_NUMBER: + state.set_serial(config_a); + break; + case AltosLib.AO_LOG_BARO_RESERVED: + state.make_baro(); + state.baro.reserved = config_a; + break; + case AltosLib.AO_LOG_BARO_SENS: + state.make_baro(); + state.baro.sens = config_a; + break; + case AltosLib.AO_LOG_BARO_OFF: + state.make_baro(); + state.baro.off = config_a; + break; + case AltosLib.AO_LOG_BARO_TCS: + state.make_baro(); + state.baro.tcs = config_a; + break; + case AltosLib.AO_LOG_BARO_TCO: + state.make_baro(); + state.baro.tco = config_a; + break; + case AltosLib.AO_LOG_BARO_TREF: + state.make_baro(); + state.baro.tref = config_a; + break; + case AltosLib.AO_LOG_BARO_TEMPSENS: + state.make_baro(); + state.baro.tempsens = config_a; + break; + case AltosLib.AO_LOG_BARO_CRC: + state.make_baro(); + state.baro.crc = config_a; + break; + case AltosLib.AO_LOG_SOFTWARE_VERSION: + break; + } + } + + public void write(PrintStream out) { + switch (cmd) { + case AltosLib.AO_LOG_CONFIG_VERSION: + out.printf("# Config version: %s\n", data); + break; + case AltosLib.AO_LOG_MAIN_DEPLOY: + out.printf("# Main deploy: %s\n", config_a); + break; + case AltosLib.AO_LOG_APOGEE_DELAY: + out.printf("# Apogee delay: %s\n", config_a); + break; + case AltosLib.AO_LOG_RADIO_CHANNEL: + out.printf("# Radio channel: %s\n", config_a); + break; + case AltosLib.AO_LOG_CALLSIGN: + out.printf("# Callsign: %s\n", data); + break; + case AltosLib.AO_LOG_ACCEL_CAL: + out.printf ("# Accel cal: %d %d\n", config_a, config_b); + break; + case AltosLib.AO_LOG_RADIO_CAL: + out.printf ("# Radio cal: %d\n", config_a); + break; + case AltosLib.AO_LOG_MAX_FLIGHT_LOG: + out.printf ("# Max flight log: %d\n", config_a); + break; + case AltosLib.AO_LOG_MANUFACTURER: + out.printf ("# Manufacturer: %s\n", data); + break; + case AltosLib.AO_LOG_PRODUCT: + out.printf ("# Product: %s\n", data); + break; + case AltosLib.AO_LOG_SERIAL_NUMBER: + out.printf ("# Serial number: %d\n", config_a); + break; + case AltosLib.AO_LOG_SOFTWARE_VERSION: + out.printf ("# Software version: %s\n", data); + break; + case AltosLib.AO_LOG_BARO_RESERVED: + out.printf ("# Baro reserved: %d\n", config_a); + break; + case AltosLib.AO_LOG_BARO_SENS: + out.printf ("# Baro sens: %d\n", config_a); + break; + case AltosLib.AO_LOG_BARO_OFF: + out.printf ("# Baro off: %d\n", config_a); + break; + case AltosLib.AO_LOG_BARO_TCS: + out.printf ("# Baro tcs: %d\n", config_a); + break; + case AltosLib.AO_LOG_BARO_TCO: + out.printf ("# Baro tco: %d\n", config_a); + break; + case AltosLib.AO_LOG_BARO_TREF: + out.printf ("# Baro tref: %d\n", config_a); + break; + case AltosLib.AO_LOG_BARO_TEMPSENS: + out.printf ("# Baro tempsens: %d\n", config_a); + break; + case AltosLib.AO_LOG_BARO_CRC: + out.printf ("# Baro crc: %d\n", config_a); + break; + } + } + + public AltosEepromHeader (String[] tokens) { + last = false; + valid = true; + try { + if (tokens[0].equals("Config") && tokens[1].equals("version:")) { + cmd = AltosLib.AO_LOG_CONFIG_VERSION; + data = tokens[2]; + } else if (tokens[0].equals("Main") && tokens[1].equals("deploy:")) { + cmd = AltosLib.AO_LOG_MAIN_DEPLOY; + config_a = Integer.parseInt(tokens[2]); + } else if (tokens[0].equals("Apogee") && tokens[1].equals("delay:")) { + cmd = AltosLib.AO_LOG_APOGEE_DELAY; + config_a = Integer.parseInt(tokens[2]); + } else if (tokens[0].equals("Radio") && tokens[1].equals("channel:")) { + cmd = AltosLib.AO_LOG_RADIO_CHANNEL; + config_a = Integer.parseInt(tokens[2]); + } else if (tokens[0].equals("Callsign:")) { + cmd = AltosLib.AO_LOG_CALLSIGN; + data = tokens[1].replaceAll("\"",""); + } else if (tokens[0].equals("Accel") && tokens[1].equals("cal")) { + cmd = AltosLib.AO_LOG_ACCEL_CAL; + config_a = Integer.parseInt(tokens[3]); + config_b = Integer.parseInt(tokens[5]); + } else if (tokens[0].equals("Radio") && tokens[1].equals("cal:")) { + cmd = AltosLib.AO_LOG_RADIO_CAL; + config_a = Integer.parseInt(tokens[2]); + } else if (tokens[0].equals("Max") && tokens[1].equals("flight") && tokens[2].equals("log:")) { + cmd = AltosLib.AO_LOG_MAX_FLIGHT_LOG; + config_a = Integer.parseInt(tokens[3]); + } else if (tokens[0].equals("manufacturer")) { + cmd = AltosLib.AO_LOG_MANUFACTURER; + data = tokens[1]; + } else if (tokens[0].equals("product")) { + cmd = AltosLib.AO_LOG_PRODUCT; + data = tokens[1]; + } else if (tokens[0].equals("serial-number")) { + cmd = AltosLib.AO_LOG_SERIAL_NUMBER; + config_a = Integer.parseInt(tokens[1]); + } else if (tokens[0].equals("log-format")) { + cmd = AltosLib.AO_LOG_LOG_FORMAT; + config_a = Integer.parseInt(tokens[1]); + } else if (tokens[0].equals("software-version")) { + cmd = AltosLib.AO_LOG_SOFTWARE_VERSION; + data = tokens[1]; + last = true; + } else if (tokens[0].equals("ms5607")) { + if (tokens[1].equals("reserved:")) { + cmd = AltosLib.AO_LOG_BARO_RESERVED; + config_a = Integer.parseInt(tokens[2]); + } else if (tokens[1].equals("sens:")) { + cmd = AltosLib.AO_LOG_BARO_SENS; + config_a = Integer.parseInt(tokens[2]); + } else if (tokens[1].equals("off:")) { + cmd = AltosLib.AO_LOG_BARO_OFF; + config_a = Integer.parseInt(tokens[2]); + } else if (tokens[1].equals("tcs:")) { + cmd = AltosLib.AO_LOG_BARO_TCS; + config_a = Integer.parseInt(tokens[2]); + } else if (tokens[1].equals("tco:")) { + cmd = AltosLib.AO_LOG_BARO_TCO; + config_a = Integer.parseInt(tokens[2]); + } else if (tokens[1].equals("tref:")) { + cmd = AltosLib.AO_LOG_BARO_TREF; + config_a = Integer.parseInt(tokens[2]); + } else if (tokens[1].equals("tempsens:")) { + cmd = AltosLib.AO_LOG_BARO_TEMPSENS; + config_a = Integer.parseInt(tokens[2]); + } else if (tokens[1].equals("crc:")) { + cmd = AltosLib.AO_LOG_BARO_CRC; + config_a = Integer.parseInt(tokens[2]); + } else { + cmd = AltosLib.AO_LOG_INVALID; + data = tokens[2]; + } + } else + valid = false; + } catch (Exception e) { + valid = false; + } + } + + static public LinkedList<AltosEeprom> read(FileInputStream input) { + LinkedList<AltosEeprom> headers = new LinkedList<AltosEeprom>(); + + for (;;) { + try { + String line = AltosLib.gets(input); + if (line == null) + break; + AltosEepromHeader header = new AltosEepromHeader(line); + headers.add(header); + if (header.last) + break; + } catch (IOException ie) { + break; + } + } + + return headers; + } + + static public void write (PrintStream out, LinkedList<AltosEepromHeader> headers) { + out.printf("# Comments\n"); + for (AltosEepromHeader header : headers) { + header.write(out); + } + + } + + public AltosEepromHeader (String line) { + this(line.split("\\s+")); + } +} diff --git a/altoslib/AltosEepromHeaderIterable.java b/altoslib/AltosEepromHeaderIterable.java new file mode 100644 index 00000000..01953f0e --- /dev/null +++ b/altoslib/AltosEepromHeaderIterable.java @@ -0,0 +1,48 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * 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. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +package org.altusmetrum.altoslib_1; + +import java.io.*; +import java.util.*; +import java.text.*; + +public class AltosEepromHeaderIterable implements Iterable<AltosEepromHeader> { + public LinkedList<AltosEepromHeader> headers; + + public void write(PrintStream out) { + AltosEepromHeader.write(out, headers); + } + + public AltosState state() { + AltosState state = new AltosState(); + + for (AltosEepromHeader header : headers) + header.update_state(state); + return state; + } + + public AltosEepromHeaderIterable(FileInputStream input) { + headers = AltosEepromHeader.read(input); + } + + public Iterator<AltosEepromHeader> iterator() { + if (headers == null) + headers = new LinkedList<AltosEepromHeader>(); + return headers.iterator(); + } +}
\ No newline at end of file diff --git a/altoslib/AltosEepromIterable.java b/altoslib/AltosEepromIterable.java index b84574ef..8e6a2313 100644 --- a/altoslib/AltosEepromIterable.java +++ b/altoslib/AltosEepromIterable.java @@ -1,5 +1,5 @@ /* - * Copyright © 2010 Keith Packard <keithp@keithp.com> + * Copyright © 2013 Keith Packard <keithp@keithp.com> * * 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 @@ -21,415 +21,97 @@ import java.io.*; import java.util.*; import java.text.*; -public class AltosEepromIterable extends AltosRecordIterable { +class AltosEepromOrdered implements Comparable<AltosEepromOrdered> { + AltosEeprom eeprom; + int index; + int tick; - static final int seen_basic = AltosRecord.seen_flight|AltosRecord.seen_sensor; - - boolean has_accel; - boolean has_gps; - boolean has_ignite; - - AltosEepromRecord flight_record; - AltosEepromRecord gps_date_record; - - TreeSet<AltosOrderedRecord> records; - - LinkedList<AltosRecord> list; - - class EepromState { - int seen; - int n_pad_samples; - double ground_pres; - int gps_tick; - int boost_tick; - int sensor_tick; - - EepromState() { - seen = 0; - n_pad_samples = 0; - ground_pres = 0.0; - gps_tick = 0; - } + int cmdi() { + if (eeprom.cmd == AltosLib.AO_LOG_FLIGHT) + return 0; + return 1; } - void update_state(AltosRecordTM state, AltosEepromRecord record, EepromState eeprom) { - state.tick = record.tick; - switch (record.cmd) { - case AltosLib.AO_LOG_FLIGHT: - eeprom.seen |= AltosRecord.seen_flight; - state.ground_accel = record.a; - state.flight_accel = record.a; - state.flight = record.b; - eeprom.boost_tick = record.tick; - break; - case AltosLib.AO_LOG_SENSOR: - state.accel = record.a; - state.pres = record.b; - if (state.state < AltosLib.ao_flight_boost) { - eeprom.n_pad_samples++; - eeprom.ground_pres += state.pres; - state.ground_pres = (int) (eeprom.ground_pres / eeprom.n_pad_samples); - state.flight_pres = state.ground_pres; - } else { - state.flight_pres = (state.flight_pres * 15 + state.pres) / 16; - } - state.flight_accel = (state.flight_accel * 15 + state.accel) / 16; - if ((eeprom.seen & AltosRecord.seen_sensor) == 0) - eeprom.sensor_tick = record.tick - 1; - state.flight_vel += (state.accel_plus_g - state.accel) * (record.tick - eeprom.sensor_tick); - eeprom.seen |= AltosRecord.seen_sensor; - eeprom.sensor_tick = record.tick; - has_accel = true; - break; - case AltosLib.AO_LOG_PRESSURE: - state.pres = record.b; - state.flight_pres = state.pres; - if (eeprom.n_pad_samples == 0) { - eeprom.n_pad_samples++; - state.ground_pres = state.pres; - } - eeprom.seen |= AltosRecord.seen_sensor; - break; - case AltosLib.AO_LOG_TEMP_VOLT: - state.temp = record.a; - state.batt = record.b; - eeprom.seen |= AltosRecord.seen_temp_volt; - break; - case AltosLib.AO_LOG_DEPLOY: - state.drogue = record.a; - state.main = record.b; - eeprom.seen |= AltosRecord.seen_deploy; - has_ignite = true; - break; - case AltosLib.AO_LOG_STATE: - state.state = record.a; - break; - case AltosLib.AO_LOG_GPS_TIME: - eeprom.gps_tick = state.tick; - eeprom.seen |= AltosRecord.seen_gps_time; - AltosGPS old = state.gps; - state.gps = new AltosGPS(); + public int compareTo(AltosEepromOrdered o) { + int cmd_diff = cmdi() - o.cmdi(); - /* GPS date doesn't get repeated through the file */ - if (old != null) { - state.gps.year = old.year; - state.gps.month = old.month; - state.gps.day = old.day; - } - state.gps.hour = (record.a & 0xff); - state.gps.minute = (record.a >> 8); - state.gps.second = (record.b & 0xff); + if (cmd_diff != 0) + return cmd_diff; - int flags = (record.b >> 8); - state.gps.connected = (flags & AltosLib.AO_GPS_RUNNING) != 0; - state.gps.locked = (flags & AltosLib.AO_GPS_VALID) != 0; - state.gps.nsat = (flags & AltosLib.AO_GPS_NUM_SAT_MASK) >> - AltosLib.AO_GPS_NUM_SAT_SHIFT; - state.gps_sequence++; - has_gps = true; - break; - case AltosLib.AO_LOG_GPS_LAT: - eeprom.seen |= AltosRecord.seen_gps_lat; - int lat32 = record.a | (record.b << 16); - if (state.gps == null) - state.gps = new AltosGPS(); - state.gps.lat = (double) lat32 / 1e7; - break; - case AltosLib.AO_LOG_GPS_LON: - eeprom.seen |= AltosRecord.seen_gps_lon; - int lon32 = record.a | (record.b << 16); - if (state.gps == null) - state.gps = new AltosGPS(); - state.gps.lon = (double) lon32 / 1e7; - break; - case AltosLib.AO_LOG_GPS_ALT: - if (state.gps == null) - state.gps = new AltosGPS(); - state.gps.alt = record.a; - break; - case AltosLib.AO_LOG_GPS_SAT: - if (state.tick == eeprom.gps_tick) { - int svid = record.a; - int c_n0 = record.b >> 8; - if (state.gps == null) - state.gps = new AltosGPS(); - state.gps.add_sat(svid, c_n0); - } - break; - case AltosLib.AO_LOG_GPS_DATE: - if (state.gps == null) - state.gps = new AltosGPS(); - state.gps.year = (record.a & 0xff) + 2000; - state.gps.month = record.a >> 8; - state.gps.day = record.b & 0xff; - break; + int tick_diff = tick - o.tick; - case AltosLib.AO_LOG_CONFIG_VERSION: - break; - case AltosLib.AO_LOG_MAIN_DEPLOY: - break; - case AltosLib.AO_LOG_APOGEE_DELAY: - break; - case AltosLib.AO_LOG_RADIO_CHANNEL: - break; - case AltosLib.AO_LOG_CALLSIGN: - state.callsign = record.data; - break; - case AltosLib.AO_LOG_ACCEL_CAL: - state.accel_plus_g = record.a; - state.accel_minus_g = record.b; - break; - case AltosLib.AO_LOG_RADIO_CAL: - break; - case AltosLib.AO_LOG_MANUFACTURER: - break; - case AltosLib.AO_LOG_PRODUCT: - break; - case AltosLib.AO_LOG_SERIAL_NUMBER: - state.serial = record.a; - break; - case AltosLib.AO_LOG_SOFTWARE_VERSION: - break; - } - state.seen |= eeprom.seen; + if (tick_diff != 0) + return tick_diff; + return index - o.index; } - LinkedList<AltosRecord> make_list() { - LinkedList<AltosRecord> list = new LinkedList<AltosRecord>(); - Iterator<AltosOrderedRecord> iterator = records.iterator(); - AltosOrderedRecord record = null; - AltosRecordTM state = new AltosRecordTM(); - //boolean last_reported = false; - EepromState eeprom = new EepromState(); - - state.state = AltosLib.ao_flight_pad; - state.accel_plus_g = 15758; - state.accel_minus_g = 16294; - state.flight_vel = 0; - - /* Pull in static data from the flight and gps_date records */ - if (flight_record != null) - update_state(state, flight_record, eeprom); - if (gps_date_record != null) - update_state(state, gps_date_record, eeprom); + AltosEepromOrdered (AltosEeprom eeprom, int index, int tick) { + this.eeprom = eeprom; + this.index = index; + this.tick = tick; + } +} - while (iterator.hasNext()) { - record = iterator.next(); - if ((eeprom.seen & seen_basic) == seen_basic && record.tick != state.tick) { - AltosRecordTM r = state.clone(); - r.time = (r.tick - eeprom.boost_tick) / 100.0; - list.add(r); +class AltosEepromOrderedIterator implements Iterator<AltosEeprom> { + TreeSet<AltosEepromOrdered> olist; + Iterator<AltosEepromOrdered> oiterator; + + public AltosEepromOrderedIterator(Iterable<AltosEeprom> eeproms) { + olist = new TreeSet<AltosEepromOrdered>(); + + int tick = 0; + int index = 0; + boolean first = true; + + for (AltosEeprom e : eeproms) { + int t = e.tick; + if (first) + tick = t; + else { + while (t < tick - 32767) + t += 65536; + tick = t; } - update_state(state, record, eeprom); + olist.add(new AltosEepromOrdered(e, index++, tick)); } - AltosRecordTM r = state.clone(); - r.time = (r.tick - eeprom.boost_tick) / 100.0; - list.add(r); - return list; + oiterator = olist.iterator(); } - public Iterator<AltosRecord> iterator() { - if (list == null) - list = make_list(); - return list.iterator(); + public boolean hasNext() { + return oiterator.hasNext(); } - public boolean has_gps() { return has_gps; } - public boolean has_accel() { return has_accel; } - public boolean has_ignite() { return has_ignite; } - - public void write_comments(PrintStream out) { - Iterator<AltosOrderedRecord> iterator = records.iterator(); - out.printf("# Comments\n"); - while (iterator.hasNext()) { - AltosOrderedRecord record = iterator.next(); - switch (record.cmd) { - case AltosLib.AO_LOG_CONFIG_VERSION: - out.printf("# Config version: %s\n", record.data); - break; - case AltosLib.AO_LOG_MAIN_DEPLOY: - out.printf("# Main deploy: %s\n", record.a); - break; - case AltosLib.AO_LOG_APOGEE_DELAY: - out.printf("# Apogee delay: %s\n", record.a); - break; - case AltosLib.AO_LOG_RADIO_CHANNEL: - out.printf("# Radio channel: %s\n", record.a); - break; - case AltosLib.AO_LOG_CALLSIGN: - out.printf("# Callsign: %s\n", record.data); - break; - case AltosLib.AO_LOG_ACCEL_CAL: - out.printf ("# Accel cal: %d %d\n", record.a, record.b); - break; - case AltosLib.AO_LOG_RADIO_CAL: - out.printf ("# Radio cal: %d\n", record.a); - break; - case AltosLib.AO_LOG_MAX_FLIGHT_LOG: - out.printf ("# Max flight log: %d\n", record.a); - break; - case AltosLib.AO_LOG_MANUFACTURER: - out.printf ("# Manufacturer: %s\n", record.data); - break; - case AltosLib.AO_LOG_PRODUCT: - out.printf ("# Product: %s\n", record.data); - break; - case AltosLib.AO_LOG_SERIAL_NUMBER: - out.printf ("# Serial number: %d\n", record.a); - break; - case AltosLib.AO_LOG_SOFTWARE_VERSION: - out.printf ("# Software version: %s\n", record.data); - break; - case AltosLib.AO_LOG_BARO_RESERVED: - out.printf ("# Baro reserved: %d\n", record.a); - break; - case AltosLib.AO_LOG_BARO_SENS: - out.printf ("# Baro sens: %d\n", record.a); - break; - case AltosLib.AO_LOG_BARO_OFF: - out.printf ("# Baro off: %d\n", record.a); - break; - case AltosLib.AO_LOG_BARO_TCS: - out.printf ("# Baro tcs: %d\n", record.a); - break; - case AltosLib.AO_LOG_BARO_TCO: - out.printf ("# Baro tco: %d\n", record.a); - break; - case AltosLib.AO_LOG_BARO_TREF: - out.printf ("# Baro tref: %d\n", record.a); - break; - case AltosLib.AO_LOG_BARO_TEMPSENS: - out.printf ("# Baro tempsens: %d\n", record.a); - break; - case AltosLib.AO_LOG_BARO_CRC: - out.printf ("# Baro crc: %d\n", record.a); - break; - } - } + public AltosEeprom next() { + return oiterator.next().eeprom; } - /* - * Given an AO_LOG_GPS_TIME record with correct time, and one - * missing time, rewrite the missing time values with the good - * ones, assuming that the difference between them is 'diff' seconds - */ - void update_time(AltosOrderedRecord good, AltosOrderedRecord bad) { - - int diff = (bad.tick - good.tick + 50) / 100; - - int hour = (good.a & 0xff); - int minute = (good.a >> 8); - int second = (good.b & 0xff); - int flags = (good.b >> 8); - int seconds = hour * 3600 + minute * 60 + second; - - /* Make sure this looks like a good GPS value */ - if ((flags & AltosLib.AO_GPS_NUM_SAT_MASK) >> AltosLib.AO_GPS_NUM_SAT_SHIFT < 4) - flags = (flags & ~AltosLib.AO_GPS_NUM_SAT_MASK) | (4 << AltosLib.AO_GPS_NUM_SAT_SHIFT); - flags |= AltosLib.AO_GPS_RUNNING; - flags |= AltosLib.AO_GPS_VALID; - - int new_seconds = seconds + diff; - if (new_seconds < 0) - new_seconds += 24 * 3600; - int new_second = (new_seconds % 60); - int new_minutes = (new_seconds / 60); - int new_minute = (new_minutes % 60); - int new_hours = (new_minutes / 60); - int new_hour = (new_hours % 24); - - bad.a = new_hour + (new_minute << 8); - bad.b = new_second + (flags << 8); + public void remove () { } +} - /* - * Read the whole file, dumping records into a RB tree so - * we can enumerate them in time order -- the eeprom data - * are sometimes out of order with GPS data getting timestamps - * matching the first packet out of the GPS unit but not - * written until the final GPS packet has been received. - */ - public AltosEepromIterable (FileInputStream input) { - records = new TreeSet<AltosOrderedRecord>(); - - AltosOrderedRecord last_gps_time = null; - - int index = 0; - int prev_tick = 0; - boolean prev_tick_valid = false; - boolean missing_time = false; - - try { - for (;;) { - String line = AltosLib.gets(input); - if (line == null) - break; - AltosOrderedRecord record = new AltosOrderedRecord(line, index++, prev_tick, prev_tick_valid); - if (record.cmd == AltosLib.AO_LOG_INVALID) - continue; - prev_tick = record.tick; - if (record.cmd < AltosLib.AO_LOG_CONFIG_VERSION) - prev_tick_valid = true; - if (record.cmd == AltosLib.AO_LOG_FLIGHT) { - flight_record = record; - continue; - } +public class AltosEepromIterable implements Iterable<AltosEeprom> { + public LinkedList<AltosEeprom> eeproms; - /* Two firmware bugs caused the loss of some GPS data. - * The flight date would never be recorded, and often - * the flight time would get overwritten by another - * record. Detect the loss of the GPS date and fix up the - * missing time records - */ - if (record.cmd == AltosLib.AO_LOG_GPS_DATE) { - gps_date_record = record; - continue; - } + public void write(PrintStream out) { + for (AltosEeprom eeprom : eeproms) + eeprom.write(out); + } - /* go back and fix up any missing time values */ - if (record.cmd == AltosLib.AO_LOG_GPS_TIME) { - last_gps_time = record; - if (missing_time) { - Iterator<AltosOrderedRecord> iterator = records.iterator(); - while (iterator.hasNext()) { - AltosOrderedRecord old = iterator.next(); - if (old.cmd == AltosLib.AO_LOG_GPS_TIME && - old.a == -1 && old.b == -1) - { - update_time(record, old); - } - } - missing_time = false; - } - } + public AltosState state() { + AltosState state = new AltosState(); - if (record.cmd == AltosLib.AO_LOG_GPS_LAT) { - if (last_gps_time == null || last_gps_time.tick != record.tick) { - AltosOrderedRecord add_gps_time = new AltosOrderedRecord(AltosLib.AO_LOG_GPS_TIME, - record.tick, - -1, -1, index-1); - if (last_gps_time != null) - update_time(last_gps_time, add_gps_time); - else - missing_time = true; + for (AltosEeprom header : eeproms) + header.update_state(state); + return state; + } - records.add(add_gps_time); - record.index = index++; - } - } - records.add(record); + public AltosEepromIterable(LinkedList<AltosEeprom> eeproms) { + this.eeproms = eeproms; + } - /* Bail after reading the 'landed' record; we're all done */ - if (record.cmd == AltosLib.AO_LOG_STATE && - record.a == AltosLib.ao_flight_landed) - break; - } - } catch (IOException io) { - } catch (ParseException pe) { - } - try { - input.close(); - } catch (IOException ie) { - } + public Iterator<AltosEeprom> iterator() { + if (eeproms == null) + eeproms = new LinkedList<AltosEeprom>(); + return new AltosEepromOrderedIterator(eeproms); } -} +}
\ No newline at end of file diff --git a/altoslib/AltosEepromMetrum.java b/altoslib/AltosEepromMetrum.java new file mode 100644 index 00000000..72887032 --- /dev/null +++ b/altoslib/AltosEepromMetrum.java @@ -0,0 +1,214 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * 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. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +package org.altusmetrum.altoslib_1; + +import java.text.*; + +public class AltosEepromMetrum { + public int cmd; + public int tick; + public boolean valid; + public String data; + public int config_a, config_b; + + public int data8[]; + + public static final int record_length = 16; + static final int header_length = 4; + static final int data_length = record_length - header_length; + + public int data8(int i) { + return data8[i]; + } + + public int data16(int i) { + return ((data8[i] | (data8[i+1] << 8)) << 16) >> 16; + } + + public int data32(int i) { + return data8[i] | (data8[i+1] << 8) | (data8[i+2] << 16) | (data8[i+3] << 24); + } + + /* AO_LOG_FLIGHT elements */ + public int flight() { return data16(0); } + public int ground_accel() { return data16(2); } + public int ground_pres() { return data32(4); } + public int ground_temp() { return data32(8); } + + /* AO_LOG_STATE elements */ + public int state() { return data16(0); } + public int reason() { return data16(2); } + + /* AO_LOG_SENSOR elements */ + public int pres() { return data32(0); } + public int temp() { return data32(4); } + public int accel() { return data16(8); } + + /* AO_LOG_TEMP_VOLT elements */ + public int v_batt() { return data16(0); } + public int sense_a() { return data16(2); } + public int sense_m() { return data16(4); } + + /* AO_LOG_GPS_POS elements */ + public int latitude() { return data32(0); } + public int longitude() { return data32(4); } + public int altitude() { return data16(8); } + + /* AO_LOG_GPS_TIME elements */ + public int hour() { return data8(0); } + public int minute() { return data8(1); } + public int second() { return data8(2); } + public int flags() { return data8(3); } + public int year() { return data8(4); } + public int month() { return data8(5); } + public int day() { return data8(6); } + + /* AO_LOG_GPS_SAT elements */ + public int channels() { return data8(0); } + public int more() { return data8(1); } + public int svid(int n) { return data8(2 + n * 2); } + public int c_n(int n) { return data8(2 + n * 2 + 1); } + + public AltosEepromMetrum (AltosEepromChunk chunk, int start) throws ParseException { + cmd = chunk.data(start); + + valid = !chunk.erased(start, record_length); + if (valid) { + if (AltosConvert.checksum(chunk.data, start, record_length) != 0) + throw new ParseException(String.format("invalid checksum at 0x%x", + chunk.address + start), 0); + } else { + cmd = AltosLib.AO_LOG_INVALID; + } + + tick = chunk.data16(start+2); + + data8 = new int[data_length]; + for (int i = 0; i < data_length; i++) + data8[i] = chunk.data(start + header_length + i); + } + + public AltosEepromMetrum (String line) { + valid = false; + tick = 0; + + if (line == null) { + cmd = AltosLib.AO_LOG_INVALID; + line = ""; + } else { + try { + String[] tokens = line.split("\\s+"); + + if (tokens[0].length() == 1) { + if (tokens.length != 2 + data_length) { + cmd = AltosLib.AO_LOG_INVALID; + data = line; + } else { + cmd = tokens[0].codePointAt(0); + tick = Integer.parseInt(tokens[1],16); + valid = true; + data8 = new int[data_length]; + for (int i = 0; i < data_length; i++) + data8[i] = Integer.parseInt(tokens[2 + i],16); + } + } else if (tokens[0].equals("Config") && tokens[1].equals("version:")) { + cmd = AltosLib.AO_LOG_CONFIG_VERSION; + data = tokens[2]; + } else if (tokens[0].equals("Main") && tokens[1].equals("deploy:")) { + cmd = AltosLib.AO_LOG_MAIN_DEPLOY; + config_a = Integer.parseInt(tokens[2]); + } else if (tokens[0].equals("Apogee") && tokens[1].equals("delay:")) { + cmd = AltosLib.AO_LOG_APOGEE_DELAY; + config_a = Integer.parseInt(tokens[2]); + } else if (tokens[0].equals("Radio") && tokens[1].equals("channel:")) { + cmd = AltosLib.AO_LOG_RADIO_CHANNEL; + config_a = Integer.parseInt(tokens[2]); + } else if (tokens[0].equals("Callsign:")) { + cmd = AltosLib.AO_LOG_CALLSIGN; + data = tokens[1].replaceAll("\"",""); + } else if (tokens[0].equals("Accel") && tokens[1].equals("cal")) { + cmd = AltosLib.AO_LOG_ACCEL_CAL; + config_a = Integer.parseInt(tokens[3]); + config_b = Integer.parseInt(tokens[5]); + } else if (tokens[0].equals("Radio") && tokens[1].equals("cal:")) { + cmd = AltosLib.AO_LOG_RADIO_CAL; + config_a = Integer.parseInt(tokens[2]); + } else if (tokens[0].equals("Max") && tokens[1].equals("flight") && tokens[2].equals("log:")) { + cmd = AltosLib.AO_LOG_MAX_FLIGHT_LOG; + config_a = Integer.parseInt(tokens[3]); + } else if (tokens[0].equals("manufacturer")) { + cmd = AltosLib.AO_LOG_MANUFACTURER; + data = tokens[1]; + } else if (tokens[0].equals("product")) { + cmd = AltosLib.AO_LOG_PRODUCT; + data = tokens[1]; + } else if (tokens[0].equals("serial-number")) { + cmd = AltosLib.AO_LOG_SERIAL_NUMBER; + config_a = Integer.parseInt(tokens[1]); + } else if (tokens[0].equals("log-format")) { + cmd = AltosLib.AO_LOG_LOG_FORMAT; + config_a = Integer.parseInt(tokens[1]); + } else if (tokens[0].equals("software-version")) { + cmd = AltosLib.AO_LOG_SOFTWARE_VERSION; + data = tokens[1]; + } else if (tokens[0].equals("ms5607")) { + if (tokens[1].equals("reserved:")) { + cmd = AltosLib.AO_LOG_BARO_RESERVED; + config_a = Integer.parseInt(tokens[2]); + } else if (tokens[1].equals("sens:")) { + cmd = AltosLib.AO_LOG_BARO_SENS; + config_a = Integer.parseInt(tokens[2]); + } else if (tokens[1].equals("off:")) { + cmd = AltosLib.AO_LOG_BARO_OFF; + config_a = Integer.parseInt(tokens[2]); + } else if (tokens[1].equals("tcs:")) { + cmd = AltosLib.AO_LOG_BARO_TCS; + config_a = Integer.parseInt(tokens[2]); + } else if (tokens[1].equals("tco:")) { + cmd = AltosLib.AO_LOG_BARO_TCO; + config_a = Integer.parseInt(tokens[2]); + } else if (tokens[1].equals("tref:")) { + cmd = AltosLib.AO_LOG_BARO_TREF; + config_a = Integer.parseInt(tokens[2]); + } else if (tokens[1].equals("tempsens:")) { + cmd = AltosLib.AO_LOG_BARO_TEMPSENS; + config_a = Integer.parseInt(tokens[2]); + } else if (tokens[1].equals("crc:")) { + cmd = AltosLib.AO_LOG_BARO_CRC; + config_a = Integer.parseInt(tokens[2]); + } else { + cmd = AltosLib.AO_LOG_INVALID; + data = line; + } + } else { + cmd = AltosLib.AO_LOG_INVALID; + data = line; + } + } catch (NumberFormatException ne) { + cmd = AltosLib.AO_LOG_INVALID; + data = line; + } + } + } + + public AltosEepromMetrum(int in_cmd, int in_tick) { + cmd = in_cmd; + tick = in_tick; + valid = true; + } +} diff --git a/altoslib/AltosEepromMetrumIterable.java b/altoslib/AltosEepromMetrumIterable.java new file mode 100644 index 00000000..0387319e --- /dev/null +++ b/altoslib/AltosEepromMetrumIterable.java @@ -0,0 +1,358 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * 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. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +package org.altusmetrum.altoslib_1; + +import java.io.*; +import java.util.*; +import java.text.*; + +public class AltosEepromMetrumIterable extends AltosRecordIterable { + + 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; + + boolean has_accel; + boolean has_gps; + boolean has_ignite; + + AltosEepromMetrum flight_record; + AltosEepromMetrum gps_date_record; + + TreeSet<AltosOrderedMetrumRecord> records; + + AltosMs5607 baro; + + LinkedList<AltosRecord> list; + + class EepromState { + int seen; + int n_pad_samples; + double ground_pres; + int gps_tick; + int boost_tick; + int sensor_tick; + + EepromState() { + seen = 0; + n_pad_samples = 0; + ground_pres = 0.0; + gps_tick = 0; + } + } + + void update_state(AltosRecordTM2 state, AltosEepromMetrum record, EepromState eeprom) { + state.tick = record.tick; + switch (record.cmd) { + case AltosLib.AO_LOG_FLIGHT: + eeprom.seen |= seen_flight; + state.ground_accel = record.ground_accel(); + state.flight_accel = record.ground_accel(); + state.ground_pres = baro.set(record.ground_pres(), record.ground_temp()); + state.flight_pres = state.ground_pres; + state.flight = record.data16(0); + eeprom.boost_tick = record.tick; + break; + case AltosLib.AO_LOG_STATE: + state.state = record.state(); + break; + case AltosLib.AO_LOG_SENSOR: + state.accel = record.accel(); + baro.set(record.pres(), record.temp()); + state.pres = baro.pa; + state.temp = baro.cc; + if (state.state < AltosLib.ao_flight_boost) { + eeprom.n_pad_samples++; + eeprom.ground_pres += state.pres; + state.ground_pres = (int) (eeprom.ground_pres / eeprom.n_pad_samples); + state.flight_pres = state.ground_pres; + } else { + state.flight_pres = (state.flight_pres * 15 + state.pres) / 16; + } + state.flight_accel = (state.flight_accel * 15 + state.accel) / 16; + if ((eeprom.seen & seen_sensor) == 0) + eeprom.sensor_tick = record.tick - 1; + state.flight_vel += (state.accel_plus_g - state.accel) * (record.tick - eeprom.sensor_tick); + eeprom.seen |= seen_sensor; + eeprom.sensor_tick = record.tick; + has_accel = true; + break; + case AltosLib.AO_LOG_TEMP_VOLT: + state.v_batt = record.v_batt(); + state.sense_a = record.sense_a(); + state.sense_m = record.sense_m(); + eeprom.seen |= seen_temp_volt; + break; + case AltosLib.AO_LOG_GPS_POS: + eeprom.gps_tick = state.tick; + state.gps = new AltosGPS(); + + state.gps.lat = record.latitude() / 1e7; + state.gps.lon = record.longitude() / 1e7; + state.gps.alt = record.altitude(); + break; + + case AltosLib.AO_LOG_GPS_TIME: + state.gps.year = record.year() + 2000; + state.gps.month = record.month(); + state.gps.day = record.day(); + + state.gps.hour = record.hour(); + state.gps.minute = record.minute(); + state.gps.second = record.second(); + + int flags = record.flags(); + state.gps.connected = (flags & AltosLib.AO_GPS_RUNNING) != 0; + state.gps.locked = (flags & AltosLib.AO_GPS_VALID) != 0; + state.gps.nsat = (flags & AltosLib.AO_GPS_NUM_SAT_MASK) >> + AltosLib.AO_GPS_NUM_SAT_SHIFT; + state.gps_sequence++; + has_gps = true; + eeprom.seen |= seen_gps_time | seen_gps_lat | seen_gps_lon; + break; + case AltosLib.AO_LOG_GPS_SAT: + if (state.tick == eeprom.gps_tick) { + int nsat = record.channels(); + for (int i = 0; i < nsat; i++) + state.gps.add_sat(record.svid(i), record.c_n(i)); + } + break; + case AltosLib.AO_LOG_CONFIG_VERSION: + break; + case AltosLib.AO_LOG_MAIN_DEPLOY: + break; + case AltosLib.AO_LOG_APOGEE_DELAY: + break; + case AltosLib.AO_LOG_RADIO_CHANNEL: + break; + case AltosLib.AO_LOG_CALLSIGN: + state.callsign = record.data; + break; + case AltosLib.AO_LOG_ACCEL_CAL: + state.accel_plus_g = record.config_a; + state.accel_minus_g = record.config_b; + break; + case AltosLib.AO_LOG_RADIO_CAL: + break; + case AltosLib.AO_LOG_MANUFACTURER: + break; + case AltosLib.AO_LOG_PRODUCT: + break; + case AltosLib.AO_LOG_SERIAL_NUMBER: + state.serial = record.config_a; + break; + case AltosLib.AO_LOG_SOFTWARE_VERSION: + break; + case AltosLib.AO_LOG_BARO_RESERVED: + baro.reserved = record.config_a; + break; + case AltosLib.AO_LOG_BARO_SENS: + baro.sens =record.config_a; + break; + case AltosLib.AO_LOG_BARO_OFF: + baro.off =record.config_a; + break; + case AltosLib.AO_LOG_BARO_TCS: + baro.tcs =record.config_a; + break; + case AltosLib.AO_LOG_BARO_TCO: + baro.tco =record.config_a; + break; + case AltosLib.AO_LOG_BARO_TREF: + baro.tref =record.config_a; + break; + case AltosLib.AO_LOG_BARO_TEMPSENS: + baro.tempsens =record.config_a; + break; + case AltosLib.AO_LOG_BARO_CRC: + baro.crc =record.config_a; + break; + } + state.seen |= eeprom.seen; + } + + LinkedList<AltosRecord> make_list() { + LinkedList<AltosRecord> list = new LinkedList<AltosRecord>(); + Iterator<AltosOrderedMetrumRecord> iterator = records.iterator(); + AltosOrderedMetrumRecord record = null; + AltosRecordTM2 state = new AltosRecordTM2(); + //boolean last_reported = false; + EepromState eeprom = new EepromState(); + + state.state = AltosLib.ao_flight_pad; + state.accel_plus_g = 15758; + state.accel_minus_g = 16294; + + /* Pull in static data from the flight and gps_date records */ + if (flight_record != null) + update_state(state, flight_record, eeprom); + if (gps_date_record != null) + update_state(state, gps_date_record, eeprom); + + while (iterator.hasNext()) { + record = iterator.next(); + if ((eeprom.seen & seen_basic) == seen_basic && record.tick != state.tick) { + AltosRecordTM2 r = state.clone(); + r.time = (r.tick - eeprom.boost_tick) / 100.0; + list.add(r); + } + update_state(state, record, eeprom); + } + AltosRecordTM2 r = state.clone(); + r.time = (r.tick - eeprom.boost_tick) / 100.0; + list.add(r); + return list; + } + + public Iterator<AltosRecord> iterator() { + if (list == null) + list = make_list(); + return list.iterator(); + } + + public boolean has_gps() { return has_gps; } + public boolean has_accel() { return has_accel; } + public boolean has_ignite() { return has_ignite; } + + public void write_comments(PrintStream out) { + Iterator<AltosOrderedMetrumRecord> iterator = records.iterator(); + out.printf("# Comments\n"); + while (iterator.hasNext()) { + AltosOrderedMetrumRecord record = iterator.next(); + switch (record.cmd) { + case AltosLib.AO_LOG_CONFIG_VERSION: + out.printf("# Config version: %s\n", record.data); + break; + case AltosLib.AO_LOG_MAIN_DEPLOY: + out.printf("# Main deploy: %s\n", record.config_a); + break; + case AltosLib.AO_LOG_APOGEE_DELAY: + out.printf("# Apogee delay: %s\n", record.config_a); + break; + case AltosLib.AO_LOG_RADIO_CHANNEL: + out.printf("# Radio channel: %s\n", record.config_a); + break; + case AltosLib.AO_LOG_CALLSIGN: + out.printf("# Callsign: %s\n", record.data); + break; + case AltosLib.AO_LOG_ACCEL_CAL: + out.printf ("# Accel cal: %d %d\n", record.config_a, record.config_b); + break; + case AltosLib.AO_LOG_RADIO_CAL: + out.printf ("# Radio cal: %d\n", record.config_a); + break; + case AltosLib.AO_LOG_MAX_FLIGHT_LOG: + out.printf ("# Max flight log: %d\n", record.config_a); + break; + case AltosLib.AO_LOG_MANUFACTURER: + out.printf ("# Manufacturer: %s\n", record.data); + break; + case AltosLib.AO_LOG_PRODUCT: + out.printf ("# Product: %s\n", record.data); + break; + case AltosLib.AO_LOG_SERIAL_NUMBER: + out.printf ("# Serial number: %d\n", record.config_a); + break; + case AltosLib.AO_LOG_SOFTWARE_VERSION: + out.printf ("# Software version: %s\n", record.data); + break; + case AltosLib.AO_LOG_BARO_RESERVED: + out.printf ("# Baro reserved: %d\n", record.config_a); + break; + case AltosLib.AO_LOG_BARO_SENS: + out.printf ("# Baro sens: %d\n", record.config_a); + break; + case AltosLib.AO_LOG_BARO_OFF: + out.printf ("# Baro off: %d\n", record.config_a); + break; + case AltosLib.AO_LOG_BARO_TCS: + out.printf ("# Baro tcs: %d\n", record.config_a); + break; + case AltosLib.AO_LOG_BARO_TCO: + out.printf ("# Baro tco: %d\n", record.config_a); + break; + case AltosLib.AO_LOG_BARO_TREF: + out.printf ("# Baro tref: %d\n", record.config_a); + break; + case AltosLib.AO_LOG_BARO_TEMPSENS: + out.printf ("# Baro tempsens: %d\n", record.config_a); + break; + case AltosLib.AO_LOG_BARO_CRC: + out.printf ("# Baro crc: %d\n", record.config_a); + break; + } + } + } + + /* + * Read the whole file, dumping records into a RB tree so + * we can enumerate them in time order -- the eeprom data + * are sometimes out of order with GPS data getting timestamps + * matching the first packet out of the GPS unit but not + * written until the final GPS packet has been received. + */ + public AltosEepromMetrumIterable (FileInputStream input) { + records = new TreeSet<AltosOrderedMetrumRecord>(); + + AltosOrderedMetrumRecord last_gps_time = null; + + baro = new AltosMs5607(); + + int index = 0; + int prev_tick = 0; + boolean prev_tick_valid = false; + boolean missing_time = false; + + try { + for (;;) { + String line = AltosLib.gets(input); + if (line == null) + break; + AltosOrderedMetrumRecord record = new AltosOrderedMetrumRecord(line, index++, prev_tick, prev_tick_valid); + if (record.cmd == AltosLib.AO_LOG_INVALID) + continue; + prev_tick = record.tick; + if (record.cmd < AltosLib.AO_LOG_CONFIG_VERSION) + prev_tick_valid = true; + if (record.cmd == AltosLib.AO_LOG_FLIGHT) { + flight_record = record; + continue; + } + + records.add(record); + + /* Bail after reading the 'landed' record; we're all done */ + if (record.cmd == AltosLib.AO_LOG_STATE && + record.state() == AltosLib.ao_flight_landed) + break; + } + } catch (IOException io) { + } catch (ParseException pe) { + } + try { + input.close(); + } catch (IOException ie) { + } + } +} diff --git a/altoslib/AltosEepromMini.java b/altoslib/AltosEepromMini.java index 215cd3d9..1e0ff1b9 100644 --- a/altoslib/AltosEepromMini.java +++ b/altoslib/AltosEepromMini.java @@ -17,20 +17,12 @@ package org.altusmetrum.altoslib_1; +import java.io.*; +import java.util.*; import java.text.*; -public class AltosEepromMini { - public int cmd; - public int tick; - public boolean valid; - public String data; - public int config_a, config_b; - - public int data8[]; - +public class AltosEepromMini extends AltosEeprom { public static final int record_length = 16; - static final int header_length = 4; - static final int data_length = record_length - header_length; public int data8(int i) { return data8[i]; @@ -63,126 +55,40 @@ public class AltosEepromMini { public int sense_m() { return data16(8); } public int v_batt() { return data16(10); } - public AltosEepromMini (AltosEepromChunk chunk, int start) throws ParseException { - cmd = chunk.data(start); - - valid = !chunk.erased(start, record_length); - if (valid) { - if (AltosConvert.checksum(chunk.data, start, record_length) != 0) - throw new ParseException(String.format("invalid checksum at 0x%x", - chunk.address + start), 0); - } else { - cmd = AltosLib.AO_LOG_INVALID; - } + double voltage(AltosState state, int sensor) { + double supply; - tick = chunk.data16(start+2); + if (state.log_format == AltosLib.AO_LOG_FORMAT_EASYMINI) + supply = 3.0; + else + supply = 3.3; + return sensor / 32767.0 * supply * 127/27; + } - data8 = new int[data_length]; - for (int i = 0; i < data_length; i++) - data8[i] = chunk.data(start + header_length + i); + public void update_state(AltosState state) { + switch (cmd) { + case AltosLib.AO_LOG_FLIGHT: + state.set_flight(flight()); + state.set_ground_pressure(ground_pres()); + break; + case AltosLib.AO_LOG_STATE: + state.set_state(state()); + break; + case AltosLib.AO_LOG_SENSOR: + state.set_ms5607(pres(), temp()); + state.set_apogee_voltage(voltage(state, sense_a())); + state.set_main_voltage(voltage(state, sense_m())); + state.set_battery_voltage(voltage(state, v_batt())); + break; + } } - public AltosEepromMini (String line) { - valid = false; - tick = 0; + public AltosEepromMini (AltosEepromChunk chunk, int start) throws ParseException { + parse_chunk(chunk, start, record_length); + } - if (line == null) { - cmd = AltosLib.AO_LOG_INVALID; - line = ""; - } else { - try { - String[] tokens = line.split("\\s+"); - - if (tokens[0].length() == 1) { - if (tokens.length != 2 + data_length) { - cmd = AltosLib.AO_LOG_INVALID; - data = line; - } else { - cmd = tokens[0].codePointAt(0); - tick = Integer.parseInt(tokens[1],16); - valid = true; - data8 = new int[data_length]; - for (int i = 0; i < data_length; i++) - data8[i] = Integer.parseInt(tokens[2 + i],16); - } - } else if (tokens[0].equals("Config") && tokens[1].equals("version:")) { - cmd = AltosLib.AO_LOG_CONFIG_VERSION; - data = tokens[2]; - } else if (tokens[0].equals("Main") && tokens[1].equals("deploy:")) { - cmd = AltosLib.AO_LOG_MAIN_DEPLOY; - config_a = Integer.parseInt(tokens[2]); - } else if (tokens[0].equals("Apogee") && tokens[1].equals("delay:")) { - cmd = AltosLib.AO_LOG_APOGEE_DELAY; - config_a = Integer.parseInt(tokens[2]); - } else if (tokens[0].equals("Radio") && tokens[1].equals("channel:")) { - cmd = AltosLib.AO_LOG_RADIO_CHANNEL; - config_a = Integer.parseInt(tokens[2]); - } else if (tokens[0].equals("Callsign:")) { - cmd = AltosLib.AO_LOG_CALLSIGN; - data = tokens[1].replaceAll("\"",""); - } else if (tokens[0].equals("Accel") && tokens[1].equals("cal")) { - cmd = AltosLib.AO_LOG_ACCEL_CAL; - config_a = Integer.parseInt(tokens[3]); - config_b = Integer.parseInt(tokens[5]); - } else if (tokens[0].equals("Radio") && tokens[1].equals("cal:")) { - cmd = AltosLib.AO_LOG_RADIO_CAL; - config_a = Integer.parseInt(tokens[2]); - } else if (tokens[0].equals("Max") && tokens[1].equals("flight") && tokens[2].equals("log:")) { - cmd = AltosLib.AO_LOG_MAX_FLIGHT_LOG; - config_a = Integer.parseInt(tokens[3]); - } else if (tokens[0].equals("manufacturer")) { - cmd = AltosLib.AO_LOG_MANUFACTURER; - data = tokens[1]; - } else if (tokens[0].equals("product")) { - cmd = AltosLib.AO_LOG_PRODUCT; - data = tokens[1]; - } else if (tokens[0].equals("serial-number")) { - cmd = AltosLib.AO_LOG_SERIAL_NUMBER; - config_a = Integer.parseInt(tokens[1]); - } else if (tokens[0].equals("log-format")) { - cmd = AltosLib.AO_LOG_LOG_FORMAT; - config_a = Integer.parseInt(tokens[1]); - } else if (tokens[0].equals("software-version")) { - cmd = AltosLib.AO_LOG_SOFTWARE_VERSION; - data = tokens[1]; - } else if (tokens[0].equals("ms5607")) { - if (tokens[1].equals("reserved:")) { - cmd = AltosLib.AO_LOG_BARO_RESERVED; - config_a = Integer.parseInt(tokens[2]); - } else if (tokens[1].equals("sens:")) { - cmd = AltosLib.AO_LOG_BARO_SENS; - config_a = Integer.parseInt(tokens[2]); - } else if (tokens[1].equals("off:")) { - cmd = AltosLib.AO_LOG_BARO_OFF; - config_a = Integer.parseInt(tokens[2]); - } else if (tokens[1].equals("tcs:")) { - cmd = AltosLib.AO_LOG_BARO_TCS; - config_a = Integer.parseInt(tokens[2]); - } else if (tokens[1].equals("tco:")) { - cmd = AltosLib.AO_LOG_BARO_TCO; - config_a = Integer.parseInt(tokens[2]); - } else if (tokens[1].equals("tref:")) { - cmd = AltosLib.AO_LOG_BARO_TREF; - config_a = Integer.parseInt(tokens[2]); - } else if (tokens[1].equals("tempsens:")) { - cmd = AltosLib.AO_LOG_BARO_TEMPSENS; - config_a = Integer.parseInt(tokens[2]); - } else if (tokens[1].equals("crc:")) { - cmd = AltosLib.AO_LOG_BARO_CRC; - config_a = Integer.parseInt(tokens[2]); - } else { - cmd = AltosLib.AO_LOG_INVALID; - data = line; - } - } else { - cmd = AltosLib.AO_LOG_INVALID; - data = line; - } - } catch (NumberFormatException ne) { - cmd = AltosLib.AO_LOG_INVALID; - data = line; - } - } + public AltosEepromMini (String line) { + parse_string(line, record_length); } public AltosEepromMini(int in_cmd, int in_tick) { @@ -190,4 +96,22 @@ public class AltosEepromMini { tick = in_tick; valid = true; } + + static public LinkedList<AltosEeprom> read(FileInputStream input) { + LinkedList<AltosEeprom> minis = new LinkedList<AltosEeprom>(); + + for (;;) { + try { + String line = AltosLib.gets(input); + if (line == null) + break; + AltosEepromMini mini = new AltosEepromMini(line); + minis.add(mini); + } catch (IOException ie) { + break; + } + } + + return minis; + } } diff --git a/altoslib/AltosEepromMiniIterable.java b/altoslib/AltosEepromMiniIterable.java index 1f221187..495495eb 100644 --- a/altoslib/AltosEepromMiniIterable.java +++ b/altoslib/AltosEepromMiniIterable.java @@ -21,7 +21,7 @@ import java.io.*; import java.util.*; import java.text.*; -public class AltosEepromMiniIterable extends AltosRecordIterable { +public class AltosEepromMiniIterable implements Iterable<AltosEepromMini> { static final int seen_flight = 1; static final int seen_sensor = 2; diff --git a/altoslib/AltosEepromOldIterable.java b/altoslib/AltosEepromOldIterable.java new file mode 100644 index 00000000..ef82828b --- /dev/null +++ b/altoslib/AltosEepromOldIterable.java @@ -0,0 +1,435 @@ +/* + * Copyright © 2010 Keith Packard <keithp@keithp.com> + * + * 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. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +package org.altusmetrum.altoslib_1; + +import java.io.*; +import java.util.*; +import java.text.*; + +public class AltosEepromOldIterable extends AltosRecordIterable { + + static final int seen_basic = AltosRecord.seen_flight|AltosRecord.seen_sensor; + + boolean has_accel; + boolean has_gps; + boolean has_ignite; + + AltosEepromRecord flight_record; + AltosEepromRecord gps_date_record; + + TreeSet<AltosOrderedRecord> records; + + LinkedList<AltosRecord> list; + + class EepromState { + int seen; + int n_pad_samples; + double ground_pres; + int gps_tick; + int boost_tick; + int sensor_tick; + + EepromState() { + seen = 0; + n_pad_samples = 0; + ground_pres = 0.0; + gps_tick = 0; + } + } + + void update_state(AltosRecordTM state, AltosEepromRecord record, EepromState eeprom) { + state.tick = record.tick; + switch (record.cmd) { + case AltosLib.AO_LOG_FLIGHT: + eeprom.seen |= AltosRecord.seen_flight; + state.ground_accel = record.a; + state.flight_accel = record.a; + state.flight = record.b; + eeprom.boost_tick = record.tick; + break; + case AltosLib.AO_LOG_SENSOR: + state.accel = record.a; + state.pres = record.b; + if (state.state < AltosLib.ao_flight_boost) { + eeprom.n_pad_samples++; + eeprom.ground_pres += state.pres; + state.ground_pres = (int) (eeprom.ground_pres / eeprom.n_pad_samples); + state.flight_pres = state.ground_pres; + } else { + state.flight_pres = (state.flight_pres * 15 + state.pres) / 16; + } + state.flight_accel = (state.flight_accel * 15 + state.accel) / 16; + if ((eeprom.seen & AltosRecord.seen_sensor) == 0) + eeprom.sensor_tick = record.tick - 1; + state.flight_vel += (state.accel_plus_g - state.accel) * (record.tick - eeprom.sensor_tick); + eeprom.seen |= AltosRecord.seen_sensor; + eeprom.sensor_tick = record.tick; + has_accel = true; + break; + case AltosLib.AO_LOG_PRESSURE: + state.pres = record.b; + state.flight_pres = state.pres; + if (eeprom.n_pad_samples == 0) { + eeprom.n_pad_samples++; + state.ground_pres = state.pres; + } + eeprom.seen |= AltosRecord.seen_sensor; + break; + case AltosLib.AO_LOG_TEMP_VOLT: + state.temp = record.a; + state.batt = record.b; + eeprom.seen |= AltosRecord.seen_temp_volt; + break; + case AltosLib.AO_LOG_DEPLOY: + state.drogue = record.a; + state.main = record.b; + eeprom.seen |= AltosRecord.seen_deploy; + has_ignite = true; + break; + case AltosLib.AO_LOG_STATE: + state.state = record.a; + break; + case AltosLib.AO_LOG_GPS_TIME: + eeprom.gps_tick = state.tick; + eeprom.seen |= AltosRecord.seen_gps_time; + AltosGPS old = state.gps; + state.gps = new AltosGPS(); + + /* GPS date doesn't get repeated through the file */ + if (old != null) { + state.gps.year = old.year; + state.gps.month = old.month; + state.gps.day = old.day; + } + 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 & AltosLib.AO_GPS_RUNNING) != 0; + state.gps.locked = (flags & AltosLib.AO_GPS_VALID) != 0; + state.gps.nsat = (flags & AltosLib.AO_GPS_NUM_SAT_MASK) >> + AltosLib.AO_GPS_NUM_SAT_SHIFT; + state.gps_sequence++; + has_gps = true; + break; + case AltosLib.AO_LOG_GPS_LAT: + eeprom.seen |= AltosRecord.seen_gps_lat; + int lat32 = record.a | (record.b << 16); + if (state.gps == null) + state.gps = new AltosGPS(); + state.gps.lat = (double) lat32 / 1e7; + break; + case AltosLib.AO_LOG_GPS_LON: + eeprom.seen |= AltosRecord.seen_gps_lon; + int lon32 = record.a | (record.b << 16); + if (state.gps == null) + state.gps = new AltosGPS(); + state.gps.lon = (double) lon32 / 1e7; + break; + case AltosLib.AO_LOG_GPS_ALT: + if (state.gps == null) + state.gps = new AltosGPS(); + state.gps.alt = record.a; + break; + case AltosLib.AO_LOG_GPS_SAT: + if (state.tick == eeprom.gps_tick) { + int svid = record.a; + int c_n0 = record.b >> 8; + if (state.gps == null) + state.gps = new AltosGPS(); + state.gps.add_sat(svid, c_n0); + } + break; + case AltosLib.AO_LOG_GPS_DATE: + if (state.gps == null) + state.gps = new AltosGPS(); + state.gps.year = (record.a & 0xff) + 2000; + state.gps.month = record.a >> 8; + state.gps.day = record.b & 0xff; + break; + + case AltosLib.AO_LOG_CONFIG_VERSION: + break; + case AltosLib.AO_LOG_MAIN_DEPLOY: + break; + case AltosLib.AO_LOG_APOGEE_DELAY: + break; + case AltosLib.AO_LOG_RADIO_CHANNEL: + break; + case AltosLib.AO_LOG_CALLSIGN: + state.callsign = record.data; + break; + case AltosLib.AO_LOG_ACCEL_CAL: + state.accel_plus_g = record.a; + state.accel_minus_g = record.b; + break; + case AltosLib.AO_LOG_RADIO_CAL: + break; + case AltosLib.AO_LOG_MANUFACTURER: + break; + case AltosLib.AO_LOG_PRODUCT: + break; + case AltosLib.AO_LOG_SERIAL_NUMBER: + state.serial = record.a; + break; + case AltosLib.AO_LOG_SOFTWARE_VERSION: + break; + } + state.seen |= eeprom.seen; + } + + LinkedList<AltosRecord> make_list() { + LinkedList<AltosRecord> list = new LinkedList<AltosRecord>(); + Iterator<AltosOrderedRecord> iterator = records.iterator(); + AltosOrderedRecord record = null; + AltosRecordTM state = new AltosRecordTM(); + //boolean last_reported = false; + EepromState eeprom = new EepromState(); + + state.state = AltosLib.ao_flight_pad; + state.accel_plus_g = 15758; + state.accel_minus_g = 16294; + state.flight_vel = 0; + + /* Pull in static data from the flight and gps_date records */ + if (flight_record != null) + update_state(state, flight_record, eeprom); + if (gps_date_record != null) + update_state(state, gps_date_record, eeprom); + + while (iterator.hasNext()) { + record = iterator.next(); + if ((eeprom.seen & seen_basic) == seen_basic && record.tick != state.tick) { + AltosRecordTM r = state.clone(); + r.time = (r.tick - eeprom.boost_tick) / 100.0; + list.add(r); + } + update_state(state, record, eeprom); + } + AltosRecordTM r = state.clone(); + r.time = (r.tick - eeprom.boost_tick) / 100.0; + list.add(r); + return list; + } + + public Iterator<AltosRecord> iterator() { + if (list == null) + list = make_list(); + return list.iterator(); + } + + public boolean has_gps() { return has_gps; } + public boolean has_accel() { return has_accel; } + public boolean has_ignite() { return has_ignite; } + + public void write_comments(PrintStream out) { + Iterator<AltosOrderedRecord> iterator = records.iterator(); + out.printf("# Comments\n"); + while (iterator.hasNext()) { + AltosOrderedRecord record = iterator.next(); + switch (record.cmd) { + case AltosLib.AO_LOG_CONFIG_VERSION: + out.printf("# Config version: %s\n", record.data); + break; + case AltosLib.AO_LOG_MAIN_DEPLOY: + out.printf("# Main deploy: %s\n", record.a); + break; + case AltosLib.AO_LOG_APOGEE_DELAY: + out.printf("# Apogee delay: %s\n", record.a); + break; + case AltosLib.AO_LOG_RADIO_CHANNEL: + out.printf("# Radio channel: %s\n", record.a); + break; + case AltosLib.AO_LOG_CALLSIGN: + out.printf("# Callsign: %s\n", record.data); + break; + case AltosLib.AO_LOG_ACCEL_CAL: + out.printf ("# Accel cal: %d %d\n", record.a, record.b); + break; + case AltosLib.AO_LOG_RADIO_CAL: + out.printf ("# Radio cal: %d\n", record.a); + break; + case AltosLib.AO_LOG_MAX_FLIGHT_LOG: + out.printf ("# Max flight log: %d\n", record.a); + break; + case AltosLib.AO_LOG_MANUFACTURER: + out.printf ("# Manufacturer: %s\n", record.data); + break; + case AltosLib.AO_LOG_PRODUCT: + out.printf ("# Product: %s\n", record.data); + break; + case AltosLib.AO_LOG_SERIAL_NUMBER: + out.printf ("# Serial number: %d\n", record.a); + break; + case AltosLib.AO_LOG_SOFTWARE_VERSION: + out.printf ("# Software version: %s\n", record.data); + break; + case AltosLib.AO_LOG_BARO_RESERVED: + out.printf ("# Baro reserved: %d\n", record.a); + break; + case AltosLib.AO_LOG_BARO_SENS: + out.printf ("# Baro sens: %d\n", record.a); + break; + case AltosLib.AO_LOG_BARO_OFF: + out.printf ("# Baro off: %d\n", record.a); + break; + case AltosLib.AO_LOG_BARO_TCS: + out.printf ("# Baro tcs: %d\n", record.a); + break; + case AltosLib.AO_LOG_BARO_TCO: + out.printf ("# Baro tco: %d\n", record.a); + break; + case AltosLib.AO_LOG_BARO_TREF: + out.printf ("# Baro tref: %d\n", record.a); + break; + case AltosLib.AO_LOG_BARO_TEMPSENS: + out.printf ("# Baro tempsens: %d\n", record.a); + break; + case AltosLib.AO_LOG_BARO_CRC: + out.printf ("# Baro crc: %d\n", record.a); + break; + } + } + } + + /* + * Given an AO_LOG_GPS_TIME record with correct time, and one + * missing time, rewrite the missing time values with the good + * ones, assuming that the difference between them is 'diff' seconds + */ + void update_time(AltosOrderedRecord good, AltosOrderedRecord bad) { + + int diff = (bad.tick - good.tick + 50) / 100; + + int hour = (good.a & 0xff); + int minute = (good.a >> 8); + int second = (good.b & 0xff); + int flags = (good.b >> 8); + int seconds = hour * 3600 + minute * 60 + second; + + /* Make sure this looks like a good GPS value */ + if ((flags & AltosLib.AO_GPS_NUM_SAT_MASK) >> AltosLib.AO_GPS_NUM_SAT_SHIFT < 4) + flags = (flags & ~AltosLib.AO_GPS_NUM_SAT_MASK) | (4 << AltosLib.AO_GPS_NUM_SAT_SHIFT); + flags |= AltosLib.AO_GPS_RUNNING; + flags |= AltosLib.AO_GPS_VALID; + + int new_seconds = seconds + diff; + if (new_seconds < 0) + new_seconds += 24 * 3600; + int new_second = (new_seconds % 60); + int new_minutes = (new_seconds / 60); + int new_minute = (new_minutes % 60); + int new_hours = (new_minutes / 60); + int new_hour = (new_hours % 24); + + bad.a = new_hour + (new_minute << 8); + bad.b = new_second + (flags << 8); + } + + /* + * Read the whole file, dumping records into a RB tree so + * we can enumerate them in time order -- the eeprom data + * are sometimes out of order with GPS data getting timestamps + * matching the first packet out of the GPS unit but not + * written until the final GPS packet has been received. + */ + public AltosEepromOldIterable (FileInputStream input) { + records = new TreeSet<AltosOrderedRecord>(); + + AltosOrderedRecord last_gps_time = null; + + int index = 0; + int prev_tick = 0; + boolean prev_tick_valid = false; + boolean missing_time = false; + + try { + for (;;) { + String line = AltosLib.gets(input); + if (line == null) + break; + AltosOrderedRecord record = new AltosOrderedRecord(line, index++, prev_tick, prev_tick_valid); + if (record.cmd == AltosLib.AO_LOG_INVALID) + continue; + prev_tick = record.tick; + if (record.cmd < AltosLib.AO_LOG_CONFIG_VERSION) + prev_tick_valid = true; + if (record.cmd == AltosLib.AO_LOG_FLIGHT) { + flight_record = record; + continue; + } + + /* Two firmware bugs caused the loss of some GPS data. + * The flight date would never be recorded, and often + * the flight time would get overwritten by another + * record. Detect the loss of the GPS date and fix up the + * missing time records + */ + if (record.cmd == AltosLib.AO_LOG_GPS_DATE) { + gps_date_record = record; + continue; + } + + /* go back and fix up any missing time values */ + if (record.cmd == AltosLib.AO_LOG_GPS_TIME) { + last_gps_time = record; + if (missing_time) { + Iterator<AltosOrderedRecord> iterator = records.iterator(); + while (iterator.hasNext()) { + AltosOrderedRecord old = iterator.next(); + if (old.cmd == AltosLib.AO_LOG_GPS_TIME && + old.a == -1 && old.b == -1) + { + update_time(record, old); + } + } + missing_time = false; + } + } + + if (record.cmd == AltosLib.AO_LOG_GPS_LAT) { + if (last_gps_time == null || last_gps_time.tick != record.tick) { + AltosOrderedRecord add_gps_time = new AltosOrderedRecord(AltosLib.AO_LOG_GPS_TIME, + record.tick, + -1, -1, index-1); + if (last_gps_time != null) + update_time(last_gps_time, add_gps_time); + else + missing_time = true; + + records.add(add_gps_time); + record.index = index++; + } + } + records.add(record); + + /* Bail after reading the 'landed' record; we're all done */ + if (record.cmd == AltosLib.AO_LOG_STATE && + record.a == AltosLib.ao_flight_landed) + break; + } + } catch (IOException io) { + } catch (ParseException pe) { + } + try { + input.close(); + } catch (IOException ie) { + } + } +} diff --git a/altoslib/AltosEepromTM.java b/altoslib/AltosEepromTM.java new file mode 100644 index 00000000..6945468b --- /dev/null +++ b/altoslib/AltosEepromTM.java @@ -0,0 +1,212 @@ +/* + * Copyright © 2010 Keith Packard <keithp@keithp.com> + * + * 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. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +package org.altusmetrum.altoslib_1; + +import java.io.*; +import java.util.*; +import java.text.*; + +public class AltosEepromTM extends AltosEeprom { + public int cmd; + public int tick; + public int a; + public int b; + public boolean tick_valid; + + public static final int record_length = 8; + + static double + thermometer_to_temperature(double thermo) + { + return (thermo - 19791.268) / 32728.0 * 1.25 / 0.00247; + } + + public void write(PrintStream out) { + out.printf("%c %4x %4x %4x\n", cmd, tick, a, b); + } + + public void update_state(AltosState state) { + AltosGPS gps; + + /* Flush any pending GPS changes */ + if (state.gps_pending) { + switch (cmd) { + case AltosLib.AO_LOG_GPS_LAT: + case AltosLib.AO_LOG_GPS_LON: + case AltosLib.AO_LOG_GPS_ALT: + case AltosLib.AO_LOG_GPS_SAT: + case AltosLib.AO_LOG_GPS_DATE: + break; + default: + state.set_temp_gps(); + break; + } + } + + switch (cmd) { + case AltosLib.AO_LOG_FLIGHT: + state.set_state(AltosLib.ao_flight_pad); + state.set_ground_accel(a); + state.set_flight(b); + state.set_boost_tick(tick); + break; + case AltosLib.AO_LOG_SENSOR: + state.set_tick(tick); + state.set_accel(a); + double pressure = AltosConvert.barometer_to_pressure(b); + state.set_pressure(pressure); + break; + case AltosLib.AO_LOG_PRESSURE: + state.set_tick(tick); + state.set_pressure(AltosConvert.barometer_to_pressure(b)); + break; + case AltosLib.AO_LOG_TEMP_VOLT: + state.set_tick(tick); + state.set_temperature(thermometer_to_temperature(a)); + state.set_battery_voltage(AltosConvert.cc_battery_to_voltage(b)); + break; + case AltosLib.AO_LOG_DEPLOY: + state.set_tick(tick); + state.set_apogee_voltage(AltosConvert.cc_ignitor_to_voltage(a)); + state.set_main_voltage(AltosConvert.cc_ignitor_to_voltage(b)); + break; + case AltosLib.AO_LOG_STATE: + state.set_tick(tick); + state.set_state(a); + break; + case AltosLib.AO_LOG_GPS_TIME: + gps = state.make_temp_gps(); + + gps.hour = (a & 0xff); + gps.minute = (a >> 8); + gps.second = (b & 0xff); + + int flags = (b >> 8); + + gps.connected = (flags & AltosLib.AO_GPS_RUNNING) != 0; + gps.locked = (flags & AltosLib.AO_GPS_VALID) != 0; + gps.nsat = (flags & AltosLib.AO_GPS_NUM_SAT_MASK) >> + AltosLib.AO_GPS_NUM_SAT_SHIFT; + break; + case AltosLib.AO_LOG_GPS_LAT: + gps = state.make_temp_gps(); + + int lat32 = a | (b << 16); + gps.lat = (double) lat32 / 1e7; + break; + case AltosLib.AO_LOG_GPS_LON: + gps = state.make_temp_gps(); + + int lon32 = a | (b << 16); + gps.lon = (double) lon32 / 1e7; + break; + case AltosLib.AO_LOG_GPS_ALT: + gps = state.make_temp_gps(); + gps.alt = a; + break; + case AltosLib.AO_LOG_GPS_SAT: + gps = state.make_temp_gps(); + int svid = a; + int c_n0 = b >> 8; + gps.add_sat(svid, c_n0); + break; + case AltosLib.AO_LOG_GPS_DATE: + gps = state.make_temp_gps(); + gps.year = (a & 0xff) + 2000; + gps.month = a >> 8; + gps.day = b & 0xff; + break; + } + } + + public AltosEepromTM (AltosEepromChunk chunk, int start) throws ParseException { + + cmd = chunk.data(start); + tick_valid = true; + + tick_valid = !chunk.erased(start, record_length); + if (tick_valid) { + if (AltosConvert.checksum(chunk.data, start, record_length) != 0) + throw new ParseException(String.format("invalid checksum at 0x%x", + chunk.address + start), 0); + } else { + cmd = AltosLib.AO_LOG_INVALID; + } + + tick = chunk.data16(start + 2); + a = chunk.data16(start + 4); + b = chunk.data16(start + 6); + } + + public AltosEepromTM (String line) { + tick_valid = false; + tick = 0; + a = 0; + b = 0; + if (line == null) { + cmd = AltosLib.AO_LOG_INVALID; + } else { + try { + String[] tokens = line.split("\\s+"); + + if (tokens[0].length() == 1) { + if (tokens.length != 4) { + cmd = AltosLib.AO_LOG_INVALID; + } else { + cmd = tokens[0].codePointAt(0); + tick = Integer.parseInt(tokens[1],16); + tick_valid = true; + a = Integer.parseInt(tokens[2],16); + b = Integer.parseInt(tokens[3],16); + } + } else { + cmd = AltosLib.AO_LOG_INVALID; + } + } catch (NumberFormatException ne) { + cmd = AltosLib.AO_LOG_INVALID; + } + } + } + + public AltosEepromTM(int in_cmd, int in_tick, int in_a, int in_b) { + tick_valid = true; + cmd = in_cmd; + tick = in_tick; + a = in_a; + b = in_b; + } + + static public LinkedList<AltosEeprom> read(FileInputStream input) { + LinkedList<AltosEeprom> tms = new LinkedList<AltosEeprom>(); + + for (;;) { + try { + String line = AltosLib.gets(input); + if (line == null) + break; + AltosEepromTM tm = new AltosEepromTM(line); + tms.add(tm); + } catch (IOException ie) { + break; + } + } + + return tms; + } + +} diff --git a/altoslib/AltosFile.java b/altoslib/AltosFile.java index 90dbc6db..54c54824 100644 --- a/altoslib/AltosFile.java +++ b/altoslib/AltosFile.java @@ -22,10 +22,17 @@ import java.util.*; public class AltosFile extends File { + static String number(int n) { + if (n == AltosRecord.MISSING) + return "unk"; + else + return String.format("%03d", n); + } + public AltosFile(int year, int month, int day, int serial, int flight, String extension) { super (AltosPreferences.logdir(), - String.format("%04d-%02d-%02d-serial-%03d-flight-%03d.%s", - year, month, day, serial, flight, extension)); + String.format("%04d-%02d-%02d-serial-%s-flight-%s.%s", + year, month, day, number(serial), number(flight), extension)); } public AltosFile(int serial, int flight, String extension) { @@ -37,7 +44,7 @@ public class AltosFile extends File { extension); } - public AltosFile(AltosRecord telem) { - this(telem.serial, telem.flight, "telem"); + public AltosFile(AltosState state) { + this(state.serial, state.flight, "telem"); } } diff --git a/altoslib/AltosFlightReader.java b/altoslib/AltosFlightReader.java index 34526658..5a415274 100644 --- a/altoslib/AltosFlightReader.java +++ b/altoslib/AltosFlightReader.java @@ -28,7 +28,7 @@ public class AltosFlightReader { public void init() { } - public AltosRecord read() throws InterruptedException, ParseException, AltosCRCException, IOException { return null; } + public AltosState read() throws InterruptedException, ParseException, AltosCRCException, IOException { return null; } public void close(boolean interrupted) { } diff --git a/altoslib/AltosGPS.java b/altoslib/AltosGPS.java index f23842f3..399e95b1 100644 --- a/altoslib/AltosGPS.java +++ b/altoslib/AltosGPS.java @@ -19,7 +19,7 @@ package org.altusmetrum.altoslib_1; import java.text.*; -public class AltosGPS { +public class AltosGPS implements Cloneable { public final static int MISSING = AltosRecord.MISSING; @@ -28,7 +28,7 @@ public class AltosGPS { public boolean connected; public double lat; /* degrees (+N -S) */ public double lon; /* degrees (+E -W) */ - public int alt; /* m */ + public double alt; /* m */ public int year; public int month; public int day; @@ -70,35 +70,35 @@ public class AltosGPS { } public AltosGPS(AltosTelemetryMap map) throws ParseException { - String state = map.get_string(AltosTelemetry.AO_TELEM_GPS_STATE, - AltosTelemetry.AO_TELEM_GPS_STATE_ERROR); + String state = map.get_string(AltosTelemetryLegacy.AO_TELEM_GPS_STATE, + AltosTelemetryLegacy.AO_TELEM_GPS_STATE_ERROR); - nsat = map.get_int(AltosTelemetry.AO_TELEM_GPS_NUM_SAT, 0); - if (state.equals(AltosTelemetry.AO_TELEM_GPS_STATE_LOCKED)) { + nsat = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_NUM_SAT, 0); + if (state.equals(AltosTelemetryLegacy.AO_TELEM_GPS_STATE_LOCKED)) { connected = true; locked = true; - lat = map.get_double(AltosTelemetry.AO_TELEM_GPS_LATITUDE, MISSING, 1.0e-7); - lon = map.get_double(AltosTelemetry.AO_TELEM_GPS_LONGITUDE, MISSING, 1.0e-7); - alt = map.get_int(AltosTelemetry.AO_TELEM_GPS_ALTITUDE, MISSING); - year = map.get_int(AltosTelemetry.AO_TELEM_GPS_YEAR, MISSING); + lat = map.get_double(AltosTelemetryLegacy.AO_TELEM_GPS_LATITUDE, MISSING, 1.0e-7); + lon = map.get_double(AltosTelemetryLegacy.AO_TELEM_GPS_LONGITUDE, MISSING, 1.0e-7); + alt = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_ALTITUDE, MISSING); + year = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_YEAR, MISSING); if (year != MISSING) year += 2000; - month = map.get_int(AltosTelemetry.AO_TELEM_GPS_MONTH, MISSING); - day = map.get_int(AltosTelemetry.AO_TELEM_GPS_DAY, MISSING); + month = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_MONTH, MISSING); + day = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_DAY, MISSING); - hour = map.get_int(AltosTelemetry.AO_TELEM_GPS_HOUR, 0); - minute = map.get_int(AltosTelemetry.AO_TELEM_GPS_MINUTE, 0); - second = map.get_int(AltosTelemetry.AO_TELEM_GPS_SECOND, 0); + hour = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_HOUR, 0); + minute = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_MINUTE, 0); + second = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_SECOND, 0); - ground_speed = map.get_double(AltosTelemetry.AO_TELEM_GPS_HORIZONTAL_SPEED, + ground_speed = map.get_double(AltosTelemetryLegacy.AO_TELEM_GPS_HORIZONTAL_SPEED, AltosRecord.MISSING, 1/100.0); - course = map.get_int(AltosTelemetry.AO_TELEM_GPS_COURSE, + course = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_COURSE, AltosRecord.MISSING); - hdop = map.get_double(AltosTelemetry.AO_TELEM_GPS_HDOP, MISSING, 1.0); - vdop = map.get_double(AltosTelemetry.AO_TELEM_GPS_VDOP, MISSING, 1.0); - h_error = map.get_int(AltosTelemetry.AO_TELEM_GPS_HERROR, MISSING); - v_error = map.get_int(AltosTelemetry.AO_TELEM_GPS_VERROR, MISSING); - } else if (state.equals(AltosTelemetry.AO_TELEM_GPS_STATE_UNLOCKED)) { + hdop = map.get_double(AltosTelemetryLegacy.AO_TELEM_GPS_HDOP, MISSING, 1.0); + vdop = map.get_double(AltosTelemetryLegacy.AO_TELEM_GPS_VDOP, MISSING, 1.0); + h_error = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_HERROR, MISSING); + v_error = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_VERROR, MISSING); + } else if (state.equals(AltosTelemetryLegacy.AO_TELEM_GPS_STATE_UNLOCKED)) { connected = true; locked = false; } else { @@ -216,6 +216,39 @@ public class AltosGPS { cc_gps_sat = null; } + public AltosGPS clone() { + AltosGPS g = new AltosGPS(); + + g.nsat = nsat; + g.locked = locked; + g.connected = connected; + g.lat = lat; /* degrees (+N -S) */ + g.lon = lon; /* degrees (+E -W) */ + g.alt = alt; /* m */ + g.year = year; + g.month = month; + g.day = day; + g.hour = hour; + g.minute = minute; + g.second = second; + + g.ground_speed = ground_speed; /* m/s */ + g.course = course; /* degrees */ + g.climb_rate = climb_rate; /* m/s */ + g.hdop = hdop; /* unitless? */ + g.h_error = h_error; /* m */ + g.v_error = v_error; /* m */ + + if (cc_gps_sat != null) { + g.cc_gps_sat = new AltosGPSSat[cc_gps_sat.length]; + for (int i = 0; i < cc_gps_sat.length; i++) { + g.cc_gps_sat[i] = new AltosGPSSat(cc_gps_sat[i].svid, + cc_gps_sat[i].c_n0); + } + } + return g; + } + public AltosGPS(AltosGPS old) { if (old != null) { nsat = old.nsat; diff --git a/altoslib/AltosGreatCircle.java b/altoslib/AltosGreatCircle.java index f1cf0ae9..770c3c6c 100644 --- a/altoslib/AltosGreatCircle.java +++ b/altoslib/AltosGreatCircle.java @@ -19,7 +19,7 @@ package org.altusmetrum.altoslib_1; import java.lang.Math; -public class AltosGreatCircle { +public class AltosGreatCircle implements Cloneable { public double distance; public double bearing; public double range; @@ -95,6 +95,16 @@ public class AltosGreatCircle { elevation = Math.atan2(height_diff, distance) * 180 / Math.PI; } + public AltosGreatCircle clone() { + AltosGreatCircle n = new AltosGreatCircle(); + + n.distance = distance; + n.bearing = bearing; + n.range = range; + n.elevation = elevation; + return n; + } + public AltosGreatCircle (double start_lat, double start_lon, double end_lat, double end_lon) { this(start_lat, start_lon, 0, end_lat, end_lon, 0); diff --git a/altoslib/AltosIMU.java b/altoslib/AltosIMU.java index 8f6731fa..c5ebbb16 100644 --- a/altoslib/AltosIMU.java +++ b/altoslib/AltosIMU.java @@ -17,7 +17,7 @@ package org.altusmetrum.altoslib_1; -public class AltosIMU { +public class AltosIMU implements Cloneable { public int accel_x; public int accel_y; public int accel_z; @@ -25,5 +25,18 @@ public class AltosIMU { public int gyro_x; public int gyro_y; public int gyro_z; + + public AltosIMU clone() { + AltosIMU n = new AltosIMU(); + + n.accel_x = accel_x; + n.accel_y = accel_y; + n.accel_z = accel_z; + + n.gyro_x = gyro_x; + n.gyro_y = gyro_y; + n.gyro_z = gyro_z; + return n; + } }
\ No newline at end of file diff --git a/altoslib/AltosLib.java b/altoslib/AltosLib.java index d60ef492..4ca8ad9d 100644 --- a/altoslib/AltosLib.java +++ b/altoslib/AltosLib.java @@ -218,7 +218,9 @@ public class AltosLib { public static final int AO_LOG_FORMAT_TELEMETRY = 3; public static final int AO_LOG_FORMAT_TELESCIENCE = 4; public static final int AO_LOG_FORMAT_TELEMEGA = 5; - public static final int AO_LOG_FORMAT_MINI = 6; + public static final int AO_LOG_FORMAT_EASYMINI = 6; + public static final int AO_LOG_FORMAT_TELEMETRUM = 7; + public static final int AO_LOG_FORMAT_TELEMINI = 8; public static final int AO_LOG_FORMAT_NONE = 127; public static boolean isspace(int c) { diff --git a/altoslib/AltosLog.java b/altoslib/AltosLog.java index 974c9f0f..7f69bb65 100644 --- a/altoslib/AltosLog.java +++ b/altoslib/AltosLog.java @@ -57,8 +57,8 @@ public class AltosLog implements Runnable { return file; } - boolean open (AltosRecord telem) throws IOException { - AltosFile a = new AltosFile(telem); + boolean open (AltosState state) throws IOException { + AltosFile a = new AltosFile(state); log_file = new FileWriter(a, true); if (log_file != null) { @@ -78,22 +78,25 @@ public class AltosLog implements Runnable { public void run () { try { - AltosRecord previous = null; + AltosState state = null; for (;;) { AltosLine line = input_queue.take(); if (line.line == null) continue; try { - AltosRecord telem = AltosTelemetry.parse(line.line, previous); - if ((telem.seen & AltosRecord.seen_flight) != 0 && - (telem.serial != serial || telem.flight != flight || log_file == null)) + AltosTelemetry telem = new AltosTelemetryLegacy(line.line); + if (state != null) + state = state.clone(); + else + state = new AltosState(); + telem.update_state(state); + if (state.serial != serial || state.flight != flight || log_file == null) { close_log_file(); - serial = telem.serial; - flight = telem.flight; - open(telem); + serial = state.serial; + flight = state.flight; + open(state); } - previous = telem; } catch (ParseException pe) { } catch (AltosCRCException ce) { } diff --git a/altoslib/AltosMag.java b/altoslib/AltosMag.java index b3bbd92f..cb6826f3 100644 --- a/altoslib/AltosMag.java +++ b/altoslib/AltosMag.java @@ -17,9 +17,18 @@ package org.altusmetrum.altoslib_1; -public class AltosMag { +public class AltosMag implements Cloneable { public int x; public int y; public int z; + + public AltosMag clone() { + AltosMag n = new AltosMag(); + + n.x = x; + n.y = y; + n.z = z; + return n; + } }
\ No newline at end of file diff --git a/altoslib/AltosOrderedMetrumRecord.java b/altoslib/AltosOrderedMetrumRecord.java new file mode 100644 index 00000000..02cdf1fe --- /dev/null +++ b/altoslib/AltosOrderedMetrumRecord.java @@ -0,0 +1,52 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * 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. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +package org.altusmetrum.altoslib_1; + +import java.text.ParseException; + +/* + * AltosRecords with an index field so they can be sorted by tick while preserving + * the original ordering for elements with matching ticks + */ +class AltosOrderedMetrumRecord extends AltosEepromMetrum implements Comparable<AltosOrderedMetrumRecord> { + + public int index; + + public AltosOrderedMetrumRecord(String line, int in_index, int prev_tick, boolean prev_tick_valid) + throws ParseException { + super(line); + if (prev_tick_valid) { + tick |= (prev_tick & ~0xffff); + if (tick < prev_tick) { + if (prev_tick - tick > 0x8000) + tick += 0x10000; + } else { + if (tick - prev_tick > 0x8000) + tick -= 0x10000; + } + } + index = in_index; + } + + public int compareTo(AltosOrderedMetrumRecord o) { + int tick_diff = tick - o.tick; + if (tick_diff != 0) + return tick_diff; + return index - o.index; + } +} diff --git a/altoslib/AltosRecord.java b/altoslib/AltosRecord.java index 5e4ed927..0c8e1db9 100644 --- a/altoslib/AltosRecord.java +++ b/altoslib/AltosRecord.java @@ -56,6 +56,10 @@ public class AltosRecord implements Comparable <AltosRecord>, Cloneable { public int flight_log_max; public String firmware_version; + public double accel_plus_g, accel_minus_g; + public double ground_accel; + public double accel; + public AltosRecordCompanion companion; /* Telemetry sources have these values recorded from the flight computer */ @@ -167,5 +171,9 @@ public class AltosRecord implements Comparable <AltosRecord>, Cloneable { kalman_acceleration = MISSING; kalman_speed = MISSING; kalman_height = MISSING; + + accel_plus_g = MISSING; + accel_minus_g = MISSING; + } } diff --git a/altoslib/AltosRecordMini.java b/altoslib/AltosRecordMini.java index 253f3804..dacd89b8 100644 --- a/altoslib/AltosRecordMini.java +++ b/altoslib/AltosRecordMini.java @@ -31,6 +31,8 @@ public class AltosRecordMini extends AltosRecord { public int flight_accel; public int flight_vel; + public int flight_height; + public int flight_pres; static double adc(int raw) { @@ -89,6 +91,7 @@ public class AltosRecordMini extends AltosRecord { flight_accel = old.flight_accel; flight_vel = old.flight_vel; + flight_height = old.flight_height; flight_pres = old.flight_pres; } @@ -110,6 +113,7 @@ public class AltosRecordMini extends AltosRecord { flight_accel = 0; flight_vel = 0; + flight_height = 0; flight_pres = 0; } diff --git a/altoslib/AltosRecordTM2.java b/altoslib/AltosRecordTM2.java new file mode 100644 index 00000000..0cd54f2c --- /dev/null +++ b/altoslib/AltosRecordTM2.java @@ -0,0 +1,156 @@ +/* + * Copyright © 2012 Keith Packard <keithp@keithp.com> + * + * 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. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +package org.altusmetrum.altoslib_1; + +public class AltosRecordTM2 extends AltosRecord { + + /* Sensor values */ + public int accel; + public int pres; + public int temp; + + public int v_batt; + public int sense_a; + public int sense_m; + + public int ground_accel; + public int ground_pres; + public int accel_plus_g; + public int accel_minus_g; + + public int flight_accel; + public int flight_vel; + public int flight_pres; + + static double adc(int raw) { + return raw / 4095.0; + } + + public double pressure() { + if (pres != MISSING) + return pres; + return MISSING; + } + + public double ground_pressure() { + if (ground_pres != MISSING) + return ground_pres; + return MISSING; + } + + public double battery_voltage() { + if (v_batt != MISSING) + return 3.3 * adc(v_batt) * (15.0 + 27.0) / 27.0; + return MISSING; + } + + static double pyro(int raw) { + if (raw != MISSING) + return 3.3 * adc(raw) * (100.0 + 27.0) / 27.0; + return MISSING; + } + + public double main_voltage() { + return pyro(sense_m); + } + + public double drogue_voltage() { + return pyro(sense_a); + } + + public double temperature() { + if (temp != MISSING) + return temp / 100.0; + return MISSING; + } + + double accel_counts_per_mss() { + double counts_per_g = Math.abs(accel_minus_g - accel_plus_g) / 2; + + return counts_per_g / 9.80665; + } + + public double acceleration() { + if (ground_accel == MISSING || accel == MISSING) + return MISSING; + + if (accel_minus_g == MISSING || accel_plus_g == MISSING) + return MISSING; + + return (ground_accel - accel) / accel_counts_per_mss(); + } + + public void copy (AltosRecordTM2 old) { + super.copy(old); + + accel = old.accel; + pres = old.pres; + temp = old.temp; + + v_batt = old.v_batt; + sense_a = old.sense_a; + sense_m = old.sense_m; + + ground_accel = old.ground_accel; + ground_pres = old.ground_pres; + accel_plus_g = old.accel_plus_g; + accel_minus_g = old.accel_minus_g; + + flight_accel = old.flight_accel; + flight_vel = old.flight_vel; + flight_pres = old.flight_pres; + } + + public AltosRecordTM2 clone() { + return new AltosRecordTM2(this); + } + + void make_missing() { + + accel = MISSING; + pres = MISSING; + temp = MISSING; + + v_batt = MISSING; + sense_a = MISSING; + sense_m = MISSING; + + ground_accel = MISSING; + ground_pres = MISSING; + accel_plus_g = MISSING; + accel_minus_g = MISSING; + + flight_accel = 0; + flight_vel = 0; + flight_pres = 0; + } + + public AltosRecordTM2(AltosRecord old) { + super.copy(old); + make_missing(); + } + + public AltosRecordTM2(AltosRecordTM2 old) { + copy(old); + } + + public AltosRecordTM2() { + super(); + make_missing(); + } +} diff --git a/altoslib/AltosReplayReader.java b/altoslib/AltosReplayReader.java index a7e30370..0c14dee4 100644 --- a/altoslib/AltosReplayReader.java +++ b/altoslib/AltosReplayReader.java @@ -25,10 +25,10 @@ import java.util.*; */ public class AltosReplayReader extends AltosFlightReader { - Iterator<AltosRecord> iterator; + Iterator<AltosState> iterator; File file; - public AltosRecord read() { + public AltosState read() { if (iterator.hasNext()) return iterator.next(); return null; @@ -45,7 +45,7 @@ public class AltosReplayReader extends AltosFlightReader { public File backing_file() { return file; } - public AltosReplayReader(Iterator<AltosRecord> in_iterator, File in_file) { + public AltosReplayReader(Iterator<AltosState> in_iterator, File in_file) { iterator = in_iterator; file = in_file; name = file.getName(); diff --git a/altoslib/AltosSelfFlash.java b/altoslib/AltosSelfFlash.java new file mode 100644 index 00000000..07917d5d --- /dev/null +++ b/altoslib/AltosSelfFlash.java @@ -0,0 +1,149 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * 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. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +package org.altusmetrum.altoslib_1; + +import java.io.*; + +public class AltosSelfFlash { + File file; + FileInputStream input; + AltosHexfile image; + AltosLink link; + boolean aborted; + AltosFlashListener listener; + byte[] read_block, write_block; + + void action(String s, int percent) { + if (listener != null && !aborted) + listener.position(s, percent); + } + + void action(int part, int total) { + int percent = 100 * part / total; + action(String.format("%d/%d (%d%%)", + part, total, percent), + percent); + } + + void read_block(long addr) { + link.printf("R %x\n", addr); + + } + + void read_memory(long addr, int len) { + } + + void write_memory(long addr, byte[] data, int start, int len) { + + } + + void reboot() { + } + + public void flash() { + try { + int remain = image.data.length; + long flash_addr = image.address; + int image_start = 0; + + action("start", 0); + action(0, image.data.length); + while (remain > 0 && !aborted) { + int this_time = remain; + if (this_time > 0x100) + this_time = 0x100; + + if (link != null) { + /* write the data */ + write_memory(flash_addr, image.data, image_start, this_time); + + byte[] check = read_memory(flash_addr, this_time); + for (int i = 0; i < this_time; i++) + if (check[i] != image.data[image_start + i]) + throw new IOException(String.format("Flash write failed at 0x%x (%02x != %02x)", + image.address + image_start + i, + check[i], image.data[image_start + i])); + } else { + Thread.sleep(100); + } + + remain -= this_time; + flash_addr += this_time; + image_start += this_time; + + action(image.data.length - remain, image.data.length); + } + if (!aborted) { + action("done", 100); + if (link != null) { + reboot(); + } + } + if (link != null) + link.close(); + } catch (IOException ie) { + action(ie.getMessage(), -1); + abort(); + } catch (InterruptedException ie) { + abort(); + } + } + + public void close() { + if (link != null) + link.close(); + } + + synchronized public void abort() { + aborted = true; + close(); + } + + public boolean check_rom_config() { + if (link == null) + return true; + if (rom_config == null) + rom_config = debug.romconfig(); + return rom_config != null && rom_config.valid(); + } + + public void set_romconfig (AltosRomconfig romconfig) { + rom_config = romconfig; + } + + public AltosRomconfig romconfig() { + if (!check_rom_config()) + return null; + return rom_config; + } + + public AltosFlash(File file, AltosLink link, AltosFlashListener listener) + throws IOException, FileNotFoundException, InterruptedException { + this.file = file; + this.link = link; + this.listener = listener; + this.read_block = new byte[256]; + this.write_block = new byte[256]; + input = new FileInputStream(file); + image = new AltosHexfile(input); + if (link != null) { + debug.close(); + throw new IOException("Debug port not connected"); + } + } +}
\ No newline at end of file diff --git a/altoslib/AltosSensorMetrum.java b/altoslib/AltosSensorMetrum.java new file mode 100644 index 00000000..686c78a8 --- /dev/null +++ b/altoslib/AltosSensorMetrum.java @@ -0,0 +1,55 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * 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. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +package org.altusmetrum.altoslib_1; + +import java.util.concurrent.TimeoutException; + +class AltosSensorMetrum { + int tick; + int sense_a; + int sense_m; + int v_batt; + + public AltosSensorMetrum(AltosLink link) throws InterruptedException, TimeoutException { + String[] items = link.adc(); + for (int i = 0; i < items.length;) { + if (items[i].equals("tick:")) { + tick = Integer.parseInt(items[i+1]); + i += 2; + continue; + } + if (items[i].equals("drogue:")) { + sense_a = Integer.parseInt(items[i+1]); + i += 2; + continue; + } + if (items[i].equals("main:")) { + sense_m = Integer.parseInt(items[i+1]); + i += 2; + continue; + } + if (items[i].equals("batt:")) { + v_batt = Integer.parseInt(items[i+1]); + i += 2; + continue; + } + i++; + } + } +} + diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java index e0d9bb1f..aa3de432 100644 --- a/altoslib/AltosState.java +++ b/altoslib/AltosState.java @@ -21,40 +21,60 @@ package org.altusmetrum.altoslib_1; -public class AltosState { - public AltosRecord data; +public class AltosState implements Cloneable { + public AltosRecord record; + + public static final int set_position = 1; + public static final int set_gps = 2; + public static final int set_data = 4; + + public int set; /* derived data */ public long report_time; public double time; + public double prev_time; public double time_change; public int tick; + public int boost_tick; public int state; + public int flight; + public int serial; public boolean landed; public boolean ascent; /* going up? */ - public boolean boost; /* under power */ + public boolean boost; /* under power */ + public int rssi; + public int status; public double ground_altitude; + public double ground_pressure; public double altitude; public double height; public double pressure; public double acceleration; - public double battery; + public double battery_voltage; + public double pyro_voltage; public double temperature; - public double main_sense; - public double drogue_sense; - public double accel_speed; - public double baro_speed; + 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_accel_speed; - public double max_baro_speed; + public double max_speed; + + public double kalman_height, kalman_speed, kalman_acceleration; public AltosGPS gps; + public AltosGPS temp_gps; + public boolean gps_pending; public int gps_sequence; public AltosIMU imu; @@ -63,10 +83,11 @@ public class AltosState { public static final int MIN_PAD_SAMPLES = 10; public int npad; - public int ngps; public int gps_waiting; public boolean gps_ready; + public int ngps; + public AltosGreatCircle from_pad; public double elevation; /* from pad */ public double range; /* total distance */ @@ -78,196 +99,653 @@ public class AltosState { public int speak_tick; public double speak_altitude; + public String callsign; + public double accel_plus_g; + public double accel_minus_g; + public double accel; + public double ground_accel; + public double ground_accel_avg; + + public int log_format; + + public AltosMs5607 baro; + + public AltosRecordCompanion companion; + public double speed() { - if (ascent) - return accel_speed; - else - return baro_speed; + return speed; } public double max_speed() { - if (max_accel_speed != 0) - return max_accel_speed; - return max_baro_speed; + return max_speed; } - public void init (AltosRecord cur, AltosState prev_state) { - data = cur; + public void set_npad(int npad) { + this.npad = npad; + gps_waiting = MIN_PAD_SAMPLES - npad; + if (this.gps_waiting < 0) + gps_waiting = 0; + gps_ready = gps_waiting == 0; + } - /* Discard previous state if it was for a different board */ - if (prev_state != null && prev_state.data.serial != data.serial) - prev_state = null; - ground_altitude = data.ground_altitude(); + public void init() { + record = null; - altitude = data.altitude(); - if (altitude == AltosRecord.MISSING && data.gps != null) - altitude = data.gps.alt; + set = 0; + report_time = System.currentTimeMillis(); + time = AltosRecord.MISSING; + time_change = AltosRecord.MISSING; + prev_time = AltosRecord.MISSING; + tick = AltosRecord.MISSING; + boost_tick = AltosRecord.MISSING; + state = AltosLib.ao_flight_invalid; + flight = AltosRecord.MISSING; + landed = false; + boost = false; + rssi = AltosRecord.MISSING; + status = 0; + + ground_altitude = AltosRecord.MISSING; + ground_pressure = AltosRecord.MISSING; + altitude = AltosRecord.MISSING; height = AltosRecord.MISSING; - if (data.kalman_height != AltosRecord.MISSING) - height = data.kalman_height; - else { - if (altitude != AltosRecord.MISSING && ground_altitude != AltosRecord.MISSING) { - double cur_height = altitude - ground_altitude; - if (prev_state == null || prev_state.height == AltosRecord.MISSING) - height = cur_height; - else - height = (prev_state.height * 15 + cur_height) / 16.0; - } + 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; + + speed = AltosRecord.MISSING; + + kalman_height = AltosRecord.MISSING; + kalman_speed = AltosRecord.MISSING; + kalman_acceleration = AltosRecord.MISSING; + + max_speed = 0; + max_height = 0; + max_acceleration = 0; + + gps = null; + temp_gps = null; + gps_sequence = 0; + gps_pending = false; + + imu = null; + mag = null; + + set_npad(0); + ngps = 0; + + from_pad = null; + elevation = AltosRecord.MISSING; + range = AltosRecord.MISSING; + gps_height = AltosRecord.MISSING; + + pad_lat = AltosRecord.MISSING; + pad_lon = AltosRecord.MISSING; + pad_alt = AltosRecord.MISSING; + + speak_tick = AltosRecord.MISSING; + speak_altitude = AltosRecord.MISSING; + + callsign = null; + + accel_plus_g = AltosRecord.MISSING; + accel_minus_g = AltosRecord.MISSING; + accel = AltosRecord.MISSING; + ground_accel = AltosRecord.MISSING; + ground_accel_avg = AltosRecord.MISSING; + log_format = AltosRecord.MISSING; + serial = AltosRecord.MISSING; + + baro = null; + companion = null; + } + + void copy(AltosState old) { + + record = null; + + if (old == null) { + init(); + return; } - report_time = System.currentTimeMillis(); + report_time = old.report_time; + time = old.time; + time_change = 0; + tick = old.tick; + boost_tick = old.boost_tick; + + state = old.state; + flight = old.flight; + landed = old.landed; + ascent = old.ascent; + boost = old.boost; + rssi = old.rssi; + status = old.status; + + set = 0; + + ground_altitude = old.ground_altitude; + altitude = old.altitude; + height = old.height; + pressure = old.pressure; + acceleration = old.acceleration; + battery_voltage = old.battery_voltage; + pyro_voltage = old.pyro_voltage; + temperature = old.temperature; + apogee_voltage = old.apogee_voltage; + main_voltage = old.main_voltage; + speed = old.speed; + + prev_height = old.height; + prev_speed = old.speed; + prev_acceleration = old.acceleration; + prev_time = old.time; + + max_height = old.max_height; + max_acceleration = old.max_acceleration; + max_speed = old.max_speed; + + kalman_height = old.kalman_height; + kalman_speed = old.kalman_speed; + kalman_acceleration = old.kalman_acceleration; + + if (old.gps != null) + gps = old.gps.clone(); + else + gps = null; + if (old.temp_gps != null) + temp_gps = old.temp_gps.clone(); + else + temp_gps = null; + gps_sequence = old.gps_sequence; + gps_pending = old.gps_pending; - if (data.kalman_acceleration != AltosRecord.MISSING) - acceleration = data.kalman_acceleration; + if (old.imu != null) + imu = old.imu.clone(); else - acceleration = data.acceleration(); - temperature = data.temperature(); - drogue_sense = data.drogue_voltage(); - main_sense = data.main_voltage(); - battery = data.battery_voltage(); - pressure = data.pressure(); - tick = data.tick; - state = data.state; - - if (prev_state != null) { - - /* Preserve any existing gps data */ - npad = prev_state.npad; - ngps = prev_state.ngps; - gps = prev_state.gps; - gps_sequence = prev_state.gps_sequence; - pad_lat = prev_state.pad_lat; - pad_lon = prev_state.pad_lon; - pad_alt = prev_state.pad_alt; - max_height = prev_state.max_height; - max_acceleration = prev_state.max_acceleration; - max_accel_speed = prev_state.max_accel_speed; - max_baro_speed = prev_state.max_baro_speed; - imu = prev_state.imu; - mag = prev_state.mag; - - /* make sure the clock is monotonic */ - while (tick < prev_state.tick) - tick += 65536; - - time_change = (tick - prev_state.tick) / 100.0; - - 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; + imu = null; - double prev_accel_speed = prev_state.accel_speed; + if (old.mag != null) + mag = old.mag.clone(); + else + mag = null; - if (prev_accel_speed == AltosRecord.MISSING) - prev_accel_speed = 0; + npad = old.npad; + gps_waiting = old.gps_waiting; + gps_ready = old.gps_ready; + ngps = old.ngps; - if (acceleration == AltosRecord.MISSING) { - /* Fill in mising acceleration value */ - accel_speed = baro_speed; + if (old.from_pad != null) + from_pad = old.from_pad.clone(); + else + from_pad = null; - 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; - } + elevation = old.elevation; + range = old.range; - time = tick / 100.0; + gps_height = old.gps_height; + pad_lat = old.pad_lat; + pad_lon = old.pad_lon; + pad_alt = old.pad_alt; - if (data.gps != null && data.gps_sequence != gps_sequence && (state < AltosLib.ao_flight_boost)) { + speak_tick = old.speak_tick; + speak_altitude = old.speak_altitude; - /* Track consecutive 'good' gps reports, waiting for 10 of them */ - if (data.gps != null && data.gps.locked && data.gps.nsat >= 4) - npad++; + callsign = old.callsign; + + accel_plus_g = old.accel_plus_g; + accel_minus_g = old.accel_minus_g; + accel = old.accel; + ground_accel = old.ground_accel; + ground_accel_avg = old.ground_accel_avg; + + log_format = old.log_format; + serial = old.serial; + + baro = old.baro; + companion = old.companion; + } + + double altitude() { + if (altitude != AltosRecord.MISSING) + return altitude; + if (gps != null) + return gps.alt; + return AltosRecord.MISSING; + } + + void update_vertical_pos() { + + double alt = altitude(); + + if (state == AltosLib.ao_flight_pad && alt != AltosRecord.MISSING && ground_pressure == AltosRecord.MISSING) { + if (ground_altitude == AltosRecord.MISSING) + ground_altitude = alt; else - 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; + ground_altitude = (ground_altitude * 7 + alt) / 8; } - gps_sequence = data.gps_sequence; + 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; - gps_waiting = MIN_PAD_SAMPLES - npad; - if (gps_waiting < 0) - gps_waiting = 0; + if (height != AltosRecord.MISSING && height > max_height) + max_height = height; - gps_ready = gps_waiting == 0; + update_speed(); + } + + double motion_filter_value() { + return 1/ Math.exp(time_change/2.0); + } + + void update_speed() { + if (kalman_speed != AltosRecord.MISSING) + speed = kalman_speed; + else if (state != AltosLib.ao_flight_invalid && + time_change != AltosRecord.MISSING) + { + if (ascent && acceleration != AltosRecord.MISSING) + { + if (prev_speed == AltosRecord.MISSING) + speed = acceleration * time_change; + else + speed = prev_speed + acceleration * time_change; + } + else if (height != AltosRecord.MISSING && + prev_height != AltosRecord.MISSING && + time_change != 0) + { + double new_speed = (height - prev_height) / time_change; + + if (prev_speed == AltosRecord.MISSING) + speed = new_speed; + else { + double filter = motion_filter_value(); + + speed = prev_speed * filter + new_speed * (1-filter); + } + } + } + if (acceleration == AltosRecord.MISSING) { + if (prev_speed != AltosRecord.MISSING && time_change != 0) { + double new_acceleration = (speed - prev_speed) / time_change; - ascent = (AltosLib.ao_flight_boost <= state && - state <= AltosLib.ao_flight_coast); - boost = (AltosLib.ao_flight_boost == state); + if (prev_acceleration == AltosRecord.MISSING) + acceleration = new_acceleration; + else { + double filter = motion_filter_value(); + + acceleration = prev_acceleration * filter + new_acceleration * (1-filter); + } + } + } + if (boost && speed != AltosRecord.MISSING && speed > max_speed) + max_speed = speed; + } + + void update_accel() { + double ground = ground_accel; + + if (ground == AltosRecord.MISSING) + ground = ground_accel_avg; + if (accel == AltosRecord.MISSING) + return; + if (ground == AltosRecord.MISSING) + return; + if (accel_plus_g == AltosRecord.MISSING) + return; + if (accel_minus_g == AltosRecord.MISSING) + return; + + double counts_per_g = (accel_minus_g - accel_plus_g) / 2.0; + double counts_per_mss = counts_per_g / 9.80665; + + acceleration = (ground - accel) / counts_per_mss; /* Only look at accelerometer data under boost */ if (boost && acceleration != AltosRecord.MISSING && (max_acceleration == AltosRecord.MISSING || acceleration > max_acceleration)) max_acceleration = acceleration; - 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; + update_speed(); + } - if (height != AltosRecord.MISSING && height > max_height) - max_height = height; + void update_time() { + if (tick != AltosRecord.MISSING) { + time = tick / 100.0; + if (prev_time != AltosRecord.MISSING) + time_change = time - prev_time; + } + } + + void update_gps() { elevation = 0; range = -1; gps_height = 0; - 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; + + if (gps == null) + return; + + if (gps.locked && gps.nsat >= 4) { + /* Track consecutive 'good' gps reports, waiting for 10 of them */ + if (state == AltosLib.ao_flight_pad) { + set_npad(npad+1); + if (pad_lat != AltosRecord.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 == AltosRecord.MISSING) { + pad_lat = gps.lat; + pad_lon = gps.lon; + pad_alt = gps.alt; + } + } + if (gps.lat != 0 && gps.lon != 0 && + pad_lat != AltosRecord.MISSING && + pad_lon != AltosRecord.MISSING) + { + 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; + } + } + + 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 void set_boost_tick(int boost_tick) { + if (boost_tick != AltosRecord.MISSING) + this.boost_tick = boost_tick; + } + + public String state_name() { + 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 void set_flight(int flight) { + + /* When the flight changes, reset the state */ + if (flight != AltosRecord.MISSING) { + if (this.flight != AltosRecord.MISSING && + this.flight != flight) { + init(); + } + this.flight = flight; + } + } + + public void set_serial(int serial) { + /* When the serial changes, reset the state */ + if (serial != AltosRecord.MISSING) { + if (this.serial != AltosRecord.MISSING && + this.serial != serial) { + init(); + } + this.serial = serial; + } + } + + public int rssi() { + if (rssi == AltosRecord.MISSING) + return 0; + return rssi; + } + + public void set_rssi(int rssi, int status) { + if (rssi != AltosRecord.MISSING) { + this.rssi = rssi; + this.status = status; + } + } + + public void set_altitude(double altitude) { + if (altitude != AltosRecord.MISSING) { + this.altitude = altitude; + update_vertical_pos(); + set |= set_position; + } + } + + public void set_ground_altitude(double ground_altitude) { + if (ground_altitude != AltosRecord.MISSING) { + this.ground_altitude = ground_altitude; + update_vertical_pos(); + } + } + + public void set_ground_pressure (double pressure) { + if (pressure != AltosRecord.MISSING) { + this.ground_pressure = pressure; + set_ground_altitude(AltosConvert.pressure_to_altitude(pressure)); + update_vertical_pos(); + } + } + + public void set_gps(AltosGPS gps, int sequence) { + if (gps != null) { + this.gps = gps.clone(); + gps_sequence = sequence; + update_gps(); + update_vertical_pos(); + set |= set_gps; + } + } + + public void set_kalman(double height, double speed, double acceleration) { + if (height != AltosRecord.MISSING) { + kalman_height = height; + kalman_speed = speed; + kalman_acceleration = acceleration; + update_vertical_pos(); + } + } + + public void set_pressure(double pressure) { + if (pressure != AltosRecord.MISSING) { + this.pressure = pressure; + set_altitude(AltosConvert.pressure_to_altitude(pressure)); + } + } + + public void make_baro() { + if (baro == null) + baro = new AltosMs5607(); + } + + 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 AltosRecordCompanion(nchannels); + } + + public void set_companion(AltosRecordCompanion companion) { + this.companion = companion; + } + + public void set_accel_g(double accel_plus_g, double accel_minus_g) { + if (accel_plus_g != AltosRecord.MISSING) { + this.accel_plus_g = accel_plus_g; + this.accel_minus_g = accel_minus_g; + update_accel(); + } + } + public void set_ground_accel(double ground_accel) { + if (ground_accel != AltosRecord.MISSING) { + this.ground_accel = ground_accel; + update_accel(); + } + } + + public void set_accel(double accel) { + if (accel != AltosRecord.MISSING) { + this.accel = accel; + if (state == AltosLib.ao_flight_pad) { + if (ground_accel_avg == AltosRecord.MISSING) + ground_accel_avg = accel; + else + ground_accel_avg = (ground_accel_avg * 7 + accel) / 8; } } + update_accel(); + } + + public void set_temperature(double temperature) { + if (temperature != AltosRecord.MISSING) { + this.temperature = temperature; + set |= set_data; + } + } + + public void set_battery_voltage(double battery_voltage) { + if (battery_voltage != AltosRecord.MISSING) { + this.battery_voltage = battery_voltage; + set |= set_data; + } + } + + public void set_pyro_voltage(double pyro_voltage) { + if (pyro_voltage != AltosRecord.MISSING) { + this.pyro_voltage = pyro_voltage; + set |= set_data; + } + } + + public void set_apogee_voltage(double apogee_voltage) { + if (apogee_voltage != AltosRecord.MISSING) { + this.apogee_voltage = apogee_voltage; + set |= set_data; + } + } + + public void set_main_voltage(double main_voltage) { + if (main_voltage != AltosRecord.MISSING) { + this.main_voltage = main_voltage; + set |= set_data; + } + } + + + public double time_since_boost() { + if (tick == AltosRecord.MISSING) + return 0.0; + + if (boost_tick != AltosRecord.MISSING) { + return (tick - boost_tick) / 100.0; + } + return tick / 100.0; + } + + public boolean valid() { + return tick != AltosRecord.MISSING && serial != AltosRecord.MISSING; + } + + public AltosGPS make_temp_gps() { + if (temp_gps == null) { + temp_gps = new AltosGPS(gps); + temp_gps.cc_gps_sat = null; + } + gps_pending = true; + return temp_gps; + } + + public void set_temp_gps() { + set_gps(temp_gps, gps_sequence + 1); + gps_pending = false; + temp_gps = null; + } + + public void init (AltosRecord cur, AltosState prev_state) { + + System.out.printf ("init\n"); + if (cur == null) + cur = new AltosRecord(); + + record = cur; + + /* Discard previous state if it was for a different board */ + if (prev_state != null && prev_state.serial != cur.serial) + prev_state = null; + + copy(prev_state); + + set_ground_altitude(cur.ground_altitude()); + set_altitude(cur.altitude()); + + set_kalman(cur.kalman_height, cur.kalman_speed, cur.kalman_acceleration); + + report_time = System.currentTimeMillis(); + + set_temperature(cur.temperature()); + set_apogee_voltage(cur.drogue_voltage()); + set_main_voltage(cur.main_voltage()); + set_battery_voltage(cur.battery_voltage()); + + set_pressure(cur.pressure()); + + set_tick(cur.tick); + set_state(cur.state); + + set_accel_g (cur.accel_minus_g, cur.accel_plus_g); + set_ground_accel(cur.ground_accel); + set_accel (cur.accel); + + if (cur.gps_sequence != gps_sequence) + set_gps(cur.gps, cur.gps_sequence); + + } + + public AltosState clone() { + AltosState s = new AltosState(); + s.copy(this); + return s; } public AltosState(AltosRecord cur) { @@ -277,4 +755,8 @@ public class AltosState { public AltosState (AltosRecord cur, AltosState prev) { init(cur, prev); } + + public AltosState () { + init(); + } } diff --git a/altoslib/AltosStateIterable.java b/altoslib/AltosStateIterable.java new file mode 100644 index 00000000..db4a2568 --- /dev/null +++ b/altoslib/AltosStateIterable.java @@ -0,0 +1,29 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * 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. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +package org.altusmetrum.altoslib_1; + +import java.io.*; +import java.util.*; + +public abstract class AltosStateIterable implements Iterable<AltosState> { + + public void write_comments (PrintStream out) { + } + + public abstract void write(PrintStream out); +} diff --git a/altoslib/AltosStateUpdate.java b/altoslib/AltosStateUpdate.java new file mode 100644 index 00000000..50460e21 --- /dev/null +++ b/altoslib/AltosStateUpdate.java @@ -0,0 +1,22 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * 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. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +package org.altusmetrum.altoslib_1; + +public interface AltosStateUpdate { + public void update_state(AltosState state); +}
\ No newline at end of file diff --git a/altoslib/AltosTelemetry.java b/altoslib/AltosTelemetry.java index e7322349..b84455d3 100644 --- a/altoslib/AltosTelemetry.java +++ b/altoslib/AltosTelemetry.java @@ -23,217 +23,136 @@ import java.text.*; * Telemetry data contents */ +public abstract class AltosTelemetry implements AltosStateUpdate { + + /* All telemetry packets have these fields */ + public int tick; + public int serial; + public int rssi; + public int status; + + /* Mark when we received the packet */ + long received_time; + + static boolean cksum(int[] bytes) { + int sum = 0x5a; + for (int i = 1; i < bytes.length - 1; i++) + sum += bytes[i]; + sum &= 0xff; + return sum == bytes[bytes.length - 1]; + } + + public void update_state(AltosState state) { + } + final static int PKT_APPEND_STATUS_1_CRC_OK = (1 << 7); + final static int PKT_APPEND_STATUS_1_LQI_MASK = (0x7f); + final static int PKT_APPEND_STATUS_1_LQI_SHIFT = 0; + + final static int packet_type_TM_sensor = 0x01; + final static int packet_type_Tm_sensor = 0x02; + final static int packet_type_Tn_sensor = 0x03; + final static int packet_type_configuration = 0x04; + final static int packet_type_location = 0x05; + final static int packet_type_satellite = 0x06; + final static int packet_type_companion = 0x07; + final static int packet_type_MM_sensor = 0x08; + final static int packet_type_MM_data = 0x09; + final static int packet_type_Mini = 0x10; + + static AltosTelemetry parse_hex(String hex) throws ParseException, AltosCRCException { + AltosTelemetry telem = null; + + int[] bytes; + try { + bytes = AltosLib.hexbytes(hex); + } catch (NumberFormatException ne) { + throw new ParseException(ne.getMessage(), 0); + } + + /* one for length, one for checksum */ + if (bytes[0] != bytes.length - 2) + throw new ParseException(String.format("invalid length %d != %d\n", + bytes[0], + bytes.length - 2), 0); + if (!cksum(bytes)) + throw new ParseException(String.format("invalid line \"%s\"", hex), 0); + + int rssi = AltosLib.int8(bytes, bytes.length - 3) / 2 - 74; + int status = AltosLib.uint8(bytes, bytes.length - 2); + + if ((status & PKT_APPEND_STATUS_1_CRC_OK) == 0) + throw new AltosCRCException(rssi); + + /* length, data ..., rssi, status, checksum -- 4 bytes extra */ + switch (bytes.length) { + case AltosLib.ao_telemetry_standard_len + 4: + int type = AltosLib.uint8(bytes, 4 + 1); /* - * The packet format is a simple hex dump of the raw telemetry frame. - * It starts with 'TELEM', then contains hex digits with a checksum as the last - * byte on the line. - * - * Version 4 is a replacement with consistent syntax. Each telemetry line - * contains a sequence of space-separated names and values, the values are - * either integers or strings. The names are all unique. All values are - * optional - * - * VERSION 4 c KD7SQG n 236 f 18 r -25 s pad t 513 r_a 15756 r_b 26444 r_t 20944 - * r_v 26640 r_d 512 r_m 208 c_a 15775 c_b 26439 c_p 15749 c_m 16281 a_a 15764 - * a_s 0 a_b 26439 g_s u g_n 0 s_n 0 - * - * VERSION 4 c KD7SQG n 19 f 0 r -23 s pad t 513 r_b 26372 r_t 21292 r_v 26788 - * r_d 136 r_m 140 c_b 26370 k_h 0 k_s 0 k_a 0 - * - * General header fields - * - * Name Value - * - * VERSION Telemetry version number (4 or more). Must be first. - * c Callsign (string, no spaces allowed) - * n Flight unit serial number (integer) - * f Flight number (integer) - * r Packet RSSI value (integer) - * s Flight computer state (string, no spaces allowed) - * t Flight computer clock (integer in centiseconds) - * - * Version 3 is Version 2 with fixed RSSI numbers -- the radio reports - * in 1/2dB increments while this protocol provides only integers. So, - * the syntax didn't change just the interpretation of the RSSI - * values. - * - * Version 2 of the telemetry data stream is a bit of a mess, with no - * consistent formatting. In particular, the GPS data is formatted for - * viewing instead of parsing. However, the key feature is that every - * telemetry line contains all of the information necessary to - * describe the current rocket state, including the calibration values - * for accelerometer and barometer. - * - * GPS unlocked: - * - * VERSION 2 CALL KB0G SERIAL 51 FLIGHT 2 RSSI -68 STATUS ff STATE pad 1001 \ - * a: 16032 p: 21232 t: 20284 v: 25160 d: 204 m: 204 fa: 16038 ga: 16032 fv: 0 \ - * fp: 21232 gp: 21230 a+: 16049 a-: 16304 GPS 0 sat unlocked SAT 1 15 30 - * - * GPS locked: - * - * VERSION 2 CALL KB0G SERIAL 51 FLIGHT 2 RSSI -71 STATUS ff STATE pad 2504 \ - * a: 16028 p: 21220 t: 20360 v: 25004 d: 208 m: 200 fa: 16031 ga: 16032 fv: 330 \ - * fp: 21231 gp: 21230 a+: 16049 a-: 16304 \ - * GPS 9 sat 2010-02-13 17:16:51 35°20.0803'N 106°45.2235'W 1790m \ - * 0.00m/s(H) 0° 0.00m/s(V) 1.0(hdop) 0(herr) 0(verr) \ - * SAT 10 29 30 24 28 5 25 21 20 15 33 1 23 30 24 18 26 10 29 2 26 - * - */ + switch (type) { + case packet_type_TM_sensor: + case packet_type_Tm_sensor: + case packet_type_Tn_sensor: + telem = new AltosTelemetrySensor(bytes); + break; + case packet_type_configuration: + telem = new AltosTelemetryConfiguration(bytes); + break; + case packet_type_location: + telem = new AltosTelemetryLocation(bytes); + break; + case packet_type_satellite: + telem = new AltosTelemetrySatellite(bytes); + break; + case packet_type_companion: + telem = new AltosTelemetryCompanion(bytes); + break; + case packet_type_MM_sensor: + telem = new AltosTelemetryMegaSensor(bytes); + break; + case packet_type_MM_data: + telem = new AltosTelemetryMegaData(bytes); + break; + default: + telem = new AltosTelemetryRaw(bytes); + break; + } +*/ + break; + case AltosLib.ao_telemetry_0_9_len + 4: + telem = new AltosTelemetryLegacy(bytes); + break; + case AltosLib.ao_telemetry_0_8_len + 4: + telem = new AltosTelemetryLegacy(bytes); + break; + default: + throw new ParseException(String.format("Invalid packet length %d", bytes.length), 0); + } + if (telem != null) { + telem.received_time = System.currentTimeMillis(); + telem.rssi = rssi; + telem.status = status; + } + return telem; + } + + public static AltosTelemetry parse(String line) throws ParseException, AltosCRCException { + String[] word = line.split("\\s+"); + int i =0; + + if (word[i].equals("CRC") && word[i+1].equals("INVALID")) { + i += 2; + AltosParse.word(word[i++], "RSSI"); + throw new AltosCRCException(AltosParse.parse_int(word[i++])); + } + + AltosTelemetry telem; -public abstract class AltosTelemetry extends AltosRecord { - - /* - * General header fields - * - * Name Value - * - * VERSION Telemetry version number (4 or more). Must be first. - * c Callsign (string, no spaces allowed) - * n Flight unit serial number (integer) - * f Flight number (integer) - * r Packet RSSI value (integer) - * s Flight computer state (string, no spaces allowed) - * t Flight computer clock (integer in centiseconds) - */ - - final static String AO_TELEM_VERSION = "VERSION"; - final static String AO_TELEM_CALL = "c"; - final static String AO_TELEM_SERIAL = "n"; - final static String AO_TELEM_FLIGHT = "f"; - final static String AO_TELEM_RSSI = "r"; - final static String AO_TELEM_STATE = "s"; - final static String AO_TELEM_TICK = "t"; - - /* - * Raw sensor values - * - * Name Value - * r_a Accelerometer reading (integer) - * r_b Barometer reading (integer) - * r_t Thermometer reading (integer) - * r_v Battery reading (integer) - * r_d Drogue continuity (integer) - * r_m Main continuity (integer) - */ - - final static String AO_TELEM_RAW_ACCEL = "r_a"; - final static String AO_TELEM_RAW_BARO = "r_b"; - final static String AO_TELEM_RAW_THERMO = "r_t"; - final static String AO_TELEM_RAW_BATT = "r_v"; - final static String AO_TELEM_RAW_DROGUE = "r_d"; - final static String AO_TELEM_RAW_MAIN = "r_m"; - - /* - * Sensor calibration values - * - * Name Value - * c_a Ground accelerometer reading (integer) - * c_b Ground barometer reading (integer) - * c_p Accelerometer reading for +1g - * c_m Accelerometer reading for -1g - */ - - final static String AO_TELEM_CAL_ACCEL_GROUND = "c_a"; - final static String AO_TELEM_CAL_BARO_GROUND = "c_b"; - final static String AO_TELEM_CAL_ACCEL_PLUS = "c_p"; - final static String AO_TELEM_CAL_ACCEL_MINUS = "c_m"; - - /* - * Kalman state values - * - * Name Value - * k_h Height above pad (integer, meters) - * k_s Vertical speeed (integer, m/s * 16) - * k_a Vertical acceleration (integer, m/s² * 16) - */ - - final static String AO_TELEM_KALMAN_HEIGHT = "k_h"; - final static String AO_TELEM_KALMAN_SPEED = "k_s"; - final static String AO_TELEM_KALMAN_ACCEL = "k_a"; - - /* - * Ad-hoc flight values - * - * Name Value - * a_a Acceleration (integer, sensor units) - * a_s Speed (integer, integrated acceleration value) - * a_b Barometer reading (integer, sensor units) - */ - - final static String AO_TELEM_ADHOC_ACCEL = "a_a"; - final static String AO_TELEM_ADHOC_SPEED = "a_s"; - final static String AO_TELEM_ADHOC_BARO = "a_b"; - - /* - * GPS values - * - * Name Value - * g_s GPS state (string): - * l locked - * u unlocked - * e error (missing or broken) - * g_n Number of sats used in solution - * g_ns Latitude (degrees * 10e7) - * g_ew Longitude (degrees * 10e7) - * g_a Altitude (integer meters) - * g_Y GPS year (integer) - * g_M GPS month (integer - 1-12) - * g_D GPS day (integer - 1-31) - * g_h GPS hour (integer - 0-23) - * g_m GPS minute (integer - 0-59) - * g_s GPS second (integer - 0-59) - * g_v GPS vertical speed (integer, cm/sec) - * g_s GPS horizontal speed (integer, cm/sec) - * g_c GPS course (integer, 0-359) - * g_hd GPS hdop (integer * 10) - * g_vd GPS vdop (integer * 10) - * g_he GPS h error (integer) - * g_ve GPS v error (integer) - */ - - final static String AO_TELEM_GPS_STATE = "g"; - final static String AO_TELEM_GPS_STATE_LOCKED = "l"; - final static String AO_TELEM_GPS_STATE_UNLOCKED = "u"; - final static String AO_TELEM_GPS_STATE_ERROR = "e"; - final static String AO_TELEM_GPS_NUM_SAT = "g_n"; - final static String AO_TELEM_GPS_LATITUDE = "g_ns"; - final static String AO_TELEM_GPS_LONGITUDE = "g_ew"; - final static String AO_TELEM_GPS_ALTITUDE = "g_a"; - final static String AO_TELEM_GPS_YEAR = "g_Y"; - final static String AO_TELEM_GPS_MONTH = "g_M"; - final static String AO_TELEM_GPS_DAY = "g_D"; - final static String AO_TELEM_GPS_HOUR = "g_h"; - final static String AO_TELEM_GPS_MINUTE = "g_m"; - final static String AO_TELEM_GPS_SECOND = "g_s"; - final static String AO_TELEM_GPS_VERTICAL_SPEED = "g_v"; - final static String AO_TELEM_GPS_HORIZONTAL_SPEED = "g_g"; - final static String AO_TELEM_GPS_COURSE = "g_c"; - final static String AO_TELEM_GPS_HDOP = "g_hd"; - final static String AO_TELEM_GPS_VDOP = "g_vd"; - final static String AO_TELEM_GPS_HERROR = "g_he"; - final static String AO_TELEM_GPS_VERROR = "g_ve"; - - /* - * GPS satellite values - * - * Name Value - * s_n Number of satellites reported (integer) - * s_v0 Space vehicle ID (integer) for report 0 - * s_c0 C/N0 number (integer) for report 0 - * s_v1 Space vehicle ID (integer) for report 1 - * s_c1 C/N0 number (integer) for report 1 - * ... - */ - - final static String AO_TELEM_SAT_NUM = "s_n"; - final static String AO_TELEM_SAT_SVID = "s_v"; - final static String AO_TELEM_SAT_C_N_0 = "s_c"; - - static public AltosRecord parse(String line, AltosRecord previous) throws ParseException, AltosCRCException { - AltosTelemetryRecord r = AltosTelemetryRecord.parse(line); - - return r.update_state(previous); + if (word[i].equals("TELEM")) { + telem = parse_hex(word[i+1]); + } else { + telem = new AltosTelemetryLegacy(line); + } + return telem; } } diff --git a/altoslib/AltosTelemetryFile.java b/altoslib/AltosTelemetryFile.java new file mode 100644 index 00000000..9e992576 --- /dev/null +++ b/altoslib/AltosTelemetryFile.java @@ -0,0 +1,94 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * 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. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +package org.altusmetrum.altoslib_1; + +import java.io.*; +import java.util.*; +import java.text.*; + +class AltosTelemetryIterator implements Iterator<AltosState> { + AltosState state; + Iterator<AltosTelemetry> telems; + AltosTelemetry next; + boolean seen; + + public boolean hasNext() { + return !seen || telems.hasNext(); + } + + public AltosState next() { + if (seen) { + AltosState n = state.clone(); + AltosTelemetry t = telems.next(); + + t.update_state(n); + state = n; + } + seen = true; + return state; + } + + public void remove () { + } + + public AltosTelemetryIterator(AltosState start, Iterator<AltosTelemetry> telems) { + this.state = start; + this.telems = telems; + this.seen = false; + } +} + +public class AltosTelemetryFile extends AltosStateIterable { + + AltosTelemetryIterable telems; + AltosState start; + + public void write_comments(PrintStream out) { + } + + public void write(PrintStream out) { + + } + + public AltosTelemetryFile(FileInputStream input) { + telems = new AltosTelemetryIterable(input); + start = new AltosState(); + + /* Find boost tick */ + AltosState state = start.clone(); + + for (AltosTelemetry telem : telems) { + telem.update_state(state); + if (state.state >= AltosLib.ao_flight_boost) { + start.set_boost_tick(state.tick); + break; + } + } + } + + public Iterator<AltosState> iterator() { + AltosState state = start.clone(); + Iterator<AltosTelemetry> i = telems.iterator(); + + while (i.hasNext() && !state.valid()) { + AltosTelemetry t = i.next(); + t.update_state(state); + } + return new AltosTelemetryIterator(state, i); + } +}
\ No newline at end of file diff --git a/altoslib/AltosTelemetryIterable.java b/altoslib/AltosTelemetryIterable.java index 57033638..b7489f77 100644 --- a/altoslib/AltosTelemetryIterable.java +++ b/altoslib/AltosTelemetryIterable.java @@ -21,27 +21,15 @@ import java.io.*; import java.util.*; import java.text.*; -public class AltosTelemetryIterable extends AltosRecordIterable { - TreeSet<AltosRecord> records; +public class AltosTelemetryIterable implements Iterable<AltosTelemetry> { + LinkedList<AltosTelemetry> telems; - public Iterator<AltosRecord> iterator () { - return records.iterator(); + public Iterator<AltosTelemetry> iterator () { + return telems.iterator(); } - boolean has_gps = false; - boolean has_accel = false; - boolean has_ignite = false; - public boolean has_gps() { return has_gps; } - public boolean has_accel() { return has_accel; } - public boolean has_ignite() { return has_ignite; }; - public AltosTelemetryIterable (FileInputStream input) { - boolean saw_boost = false; - int current_tick = 0; - int boost_tick = 0; - - AltosRecord previous = null; - records = new TreeSet<AltosRecord> (); + telems = new LinkedList<AltosTelemetry> (); try { for (;;) { @@ -50,32 +38,10 @@ public class AltosTelemetryIterable extends AltosRecordIterable { break; } try { - AltosRecord record = AltosTelemetry.parse(line, previous); - if (record == null) + AltosTelemetry telem = AltosTelemetry.parse(line); + if (telem == null) break; - if (records.isEmpty()) { - current_tick = record.tick; - } else { - int tick = record.tick; - while (tick < current_tick - 0x1000) - tick += 0x10000; - current_tick = tick; - record.tick = current_tick; - } - if (!saw_boost && record.state >= AltosLib.ao_flight_boost) - { - saw_boost = true; - boost_tick = record.tick; - } - if (record.acceleration() != AltosRecord.MISSING) - has_accel = true; - if (record.gps != null) - has_gps = true; - if (record.main_voltage() != AltosRecord.MISSING) - has_ignite = true; - if (previous != null && previous.tick != record.tick) - records.add(previous); - previous = record; + telems.add(telem); } catch (ParseException pe) { System.out.printf("parse exception %s\n", pe.getMessage()); } catch (AltosCRCException ce) { @@ -84,26 +50,5 @@ public class AltosTelemetryIterable extends AltosRecordIterable { } catch (IOException io) { System.out.printf("io exception\n"); } - - if (previous != null) - records.add(previous); - - /* Adjust all tick counts to match expected eeprom values, - * which starts with a 16-bit tick count 16 samples before boost - */ - - int tick_adjust = (boost_tick - 16) & 0xffff0000; - for (AltosRecord r : this) - r.tick -= tick_adjust; - boost_tick -= tick_adjust; - - /* adjust all tick counts to be relative to boost time */ - for (AltosRecord r : this) - r.time = (r.tick - boost_tick) / 100.0; - - try { - input.close(); - } catch (IOException ie) { - } } } diff --git a/altoslib/AltosTelemetryLegacy.java b/altoslib/AltosTelemetryLegacy.java new file mode 100644 index 00000000..45e5c315 --- /dev/null +++ b/altoslib/AltosTelemetryLegacy.java @@ -0,0 +1,556 @@ +/* + * Copyright © 2010 Keith Packard <keithp@keithp.com> + * + * 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. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +package org.altusmetrum.altoslib_1; + +import java.text.*; + +/* + * Telemetry data contents + */ + + +/* + * The packet format is a simple hex dump of the raw telemetry frame. + * It starts with 'TELEM', then contains hex digits with a checksum as the last + * byte on the line. + * + * Version 4 is a replacement with consistent syntax. Each telemetry line + * contains a sequence of space-separated names and values, the values are + * either integers or strings. The names are all unique. All values are + * optional + * + * VERSION 4 c KD7SQG n 236 f 18 r -25 s pad t 513 r_a 15756 r_b 26444 r_t 20944 + * r_v 26640 r_d 512 r_m 208 c_a 15775 c_b 26439 c_p 15749 c_m 16281 a_a 15764 + * a_s 0 a_b 26439 g_s u g_n 0 s_n 0 + * + * VERSION 4 c KD7SQG n 19 f 0 r -23 s pad t 513 r_b 26372 r_t 21292 r_v 26788 + * r_d 136 r_m 140 c_b 26370 k_h 0 k_s 0 k_a 0 + * + * General header fields + * + * Name Value + * + * VERSION Telemetry version number (4 or more). Must be first. + * c Callsign (string, no spaces allowed) + * n Flight unit serial number (integer) + * f Flight number (integer) + * r Packet RSSI value (integer) + * s Flight computer state (string, no spaces allowed) + * t Flight computer clock (integer in centiseconds) + * + * Version 3 is Version 2 with fixed RSSI numbers -- the radio reports + * in 1/2dB increments while this protocol provides only integers. So, + * the syntax didn't change just the interpretation of the RSSI + * values. + * + * Version 2 of the telemetry data stream is a bit of a mess, with no + * consistent formatting. In particular, the GPS data is formatted for + * viewing instead of parsing. However, the key feature is that every + * telemetry line contains all of the information necessary to + * describe the current rocket state, including the calibration values + * for accelerometer and barometer. + * + * GPS unlocked: + * + * VERSION 2 CALL KB0G SERIAL 51 FLIGHT 2 RSSI -68 STATUS ff STATE pad 1001 \ + * a: 16032 p: 21232 t: 20284 v: 25160 d: 204 m: 204 fa: 16038 ga: 16032 fv: 0 \ + * fp: 21232 gp: 21230 a+: 16049 a-: 16304 GPS 0 sat unlocked SAT 1 15 30 + * + * GPS locked: + * + * VERSION 2 CALL KB0G SERIAL 51 FLIGHT 2 RSSI -71 STATUS ff STATE pad 2504 \ + * a: 16028 p: 21220 t: 20360 v: 25004 d: 208 m: 200 fa: 16031 ga: 16032 fv: 330 \ + * fp: 21231 gp: 21230 a+: 16049 a-: 16304 \ + * GPS 9 sat 2010-02-13 17:16:51 35°20.0803'N 106°45.2235'W 1790m \ + * 0.00m/s(H) 0° 0.00m/s(V) 1.0(hdop) 0(herr) 0(verr) \ + * SAT 10 29 30 24 28 5 25 21 20 15 33 1 23 30 24 18 26 10 29 2 26 + * + */ + +public class AltosTelemetryLegacy extends AltosTelemetry { + /* + * General header fields + * + * Name Value + * + * VERSION Telemetry version number (4 or more). Must be first. + * c Callsign (string, no spaces allowed) + * n Flight unit serial number (integer) + * f Flight number (integer) + * r Packet RSSI value (integer) + * s Flight computer state (string, no spaces allowed) + * t Flight computer clock (integer in centiseconds) + */ + + final static String AO_TELEM_VERSION = "VERSION"; + final static String AO_TELEM_CALL = "c"; + final static String AO_TELEM_SERIAL = "n"; + final static String AO_TELEM_FLIGHT = "f"; + final static String AO_TELEM_RSSI = "r"; + final static String AO_TELEM_STATE = "s"; + final static String AO_TELEM_TICK = "t"; + + /* + * Raw sensor values + * + * Name Value + * r_a Accelerometer reading (integer) + * r_b Barometer reading (integer) + * r_t Thermometer reading (integer) + * r_v Battery reading (integer) + * r_d Drogue continuity (integer) + * r_m Main continuity (integer) + */ + + final static String AO_TELEM_RAW_ACCEL = "r_a"; + final static String AO_TELEM_RAW_BARO = "r_b"; + final static String AO_TELEM_RAW_THERMO = "r_t"; + final static String AO_TELEM_RAW_BATT = "r_v"; + final static String AO_TELEM_RAW_DROGUE = "r_d"; + final static String AO_TELEM_RAW_MAIN = "r_m"; + + /* + * Sensor calibration values + * + * Name Value + * c_a Ground accelerometer reading (integer) + * c_b Ground barometer reading (integer) + * c_p Accelerometer reading for +1g + * c_m Accelerometer reading for -1g + */ + + final static String AO_TELEM_CAL_ACCEL_GROUND = "c_a"; + final static String AO_TELEM_CAL_BARO_GROUND = "c_b"; + final static String AO_TELEM_CAL_ACCEL_PLUS = "c_p"; + final static String AO_TELEM_CAL_ACCEL_MINUS = "c_m"; + + /* + * Kalman state values + * + * Name Value + * k_h Height above pad (integer, meters) + * k_s Vertical speeed (integer, m/s * 16) + * k_a Vertical acceleration (integer, m/s² * 16) + */ + + final static String AO_TELEM_KALMAN_HEIGHT = "k_h"; + final static String AO_TELEM_KALMAN_SPEED = "k_s"; + final static String AO_TELEM_KALMAN_ACCEL = "k_a"; + + /* + * Ad-hoc flight values + * + * Name Value + * a_a Acceleration (integer, sensor units) + * a_s Speed (integer, integrated acceleration value) + * a_b Barometer reading (integer, sensor units) + */ + + final static String AO_TELEM_ADHOC_ACCEL = "a_a"; + final static String AO_TELEM_ADHOC_SPEED = "a_s"; + final static String AO_TELEM_ADHOC_BARO = "a_b"; + + /* + * GPS values + * + * Name Value + * g_s GPS state (string): + * l locked + * u unlocked + * e error (missing or broken) + * g_n Number of sats used in solution + * g_ns Latitude (degrees * 10e7) + * g_ew Longitude (degrees * 10e7) + * g_a Altitude (integer meters) + * g_Y GPS year (integer) + * g_M GPS month (integer - 1-12) + * g_D GPS day (integer - 1-31) + * g_h GPS hour (integer - 0-23) + * g_m GPS minute (integer - 0-59) + * g_s GPS second (integer - 0-59) + * g_v GPS vertical speed (integer, cm/sec) + * g_s GPS horizontal speed (integer, cm/sec) + * g_c GPS course (integer, 0-359) + * g_hd GPS hdop (integer * 10) + * g_vd GPS vdop (integer * 10) + * g_he GPS h error (integer) + * g_ve GPS v error (integer) + */ + + final static String AO_TELEM_GPS_STATE = "g"; + final static String AO_TELEM_GPS_STATE_LOCKED = "l"; + final static String AO_TELEM_GPS_STATE_UNLOCKED = "u"; + final static String AO_TELEM_GPS_STATE_ERROR = "e"; + final static String AO_TELEM_GPS_NUM_SAT = "g_n"; + final static String AO_TELEM_GPS_LATITUDE = "g_ns"; + final static String AO_TELEM_GPS_LONGITUDE = "g_ew"; + final static String AO_TELEM_GPS_ALTITUDE = "g_a"; + final static String AO_TELEM_GPS_YEAR = "g_Y"; + final static String AO_TELEM_GPS_MONTH = "g_M"; + final static String AO_TELEM_GPS_DAY = "g_D"; + final static String AO_TELEM_GPS_HOUR = "g_h"; + final static String AO_TELEM_GPS_MINUTE = "g_m"; + final static String AO_TELEM_GPS_SECOND = "g_s"; + final static String AO_TELEM_GPS_VERTICAL_SPEED = "g_v"; + final static String AO_TELEM_GPS_HORIZONTAL_SPEED = "g_g"; + final static String AO_TELEM_GPS_COURSE = "g_c"; + final static String AO_TELEM_GPS_HDOP = "g_hd"; + final static String AO_TELEM_GPS_VDOP = "g_vd"; + final static String AO_TELEM_GPS_HERROR = "g_he"; + final static String AO_TELEM_GPS_VERROR = "g_ve"; + + /* + * GPS satellite values + * + * Name Value + * s_n Number of satellites reported (integer) + * s_v0 Space vehicle ID (integer) for report 0 + * s_c0 C/N0 number (integer) for report 0 + * s_v1 Space vehicle ID (integer) for report 1 + * s_c1 C/N0 number (integer) for report 1 + * ... + */ + + final static String AO_TELEM_SAT_NUM = "s_n"; + final static String AO_TELEM_SAT_SVID = "s_v"; + final static String AO_TELEM_SAT_C_N_0 = "s_c"; + + public int version; + public String callsign; + public int flight; + public int state; + + public AltosGPS gps; + public int gps_sequence; + + /* Telemetry sources have these values recorded from the flight computer */ + public double kalman_height; + public double kalman_speed; + public double kalman_acceleration; + + /* Sensor values */ + public int accel; + public int pres; + public int temp; + public int batt; + public int apogee; + public int main; + + public int ground_accel; + public int ground_pres; + public int accel_plus_g; + public int accel_minus_g; + + public int flight_accel; + public int flight_vel; + public int flight_pres; + + private void parse_v4(String[] words, int i) throws ParseException { + AltosTelemetryMap map = new AltosTelemetryMap(words, i); + + callsign = map.get_string(AO_TELEM_CALL, "N0CALL"); + serial = map.get_int(AO_TELEM_SERIAL, AltosRecord.MISSING); + flight = map.get_int(AO_TELEM_FLIGHT, AltosRecord.MISSING); + rssi = map.get_int(AO_TELEM_RSSI, AltosRecord.MISSING); + state = AltosLib.state(map.get_string(AO_TELEM_STATE, "invalid")); + tick = map.get_int(AO_TELEM_TICK, 0); + + /* raw sensor values */ + accel = map.get_int(AO_TELEM_RAW_ACCEL, AltosRecord.MISSING); + pres = map.get_int(AO_TELEM_RAW_BARO, AltosRecord.MISSING); + temp = map.get_int(AO_TELEM_RAW_THERMO, AltosRecord.MISSING); + batt = map.get_int(AO_TELEM_RAW_BATT, AltosRecord.MISSING); + apogee = map.get_int(AO_TELEM_RAW_DROGUE, AltosRecord.MISSING); + main = map.get_int(AO_TELEM_RAW_MAIN, AltosRecord.MISSING); + + /* sensor calibration information */ + ground_accel = map.get_int(AO_TELEM_CAL_ACCEL_GROUND, AltosRecord.MISSING); + ground_pres = map.get_int(AO_TELEM_CAL_BARO_GROUND, AltosRecord.MISSING); + accel_plus_g = map.get_int(AO_TELEM_CAL_ACCEL_PLUS, AltosRecord.MISSING); + accel_minus_g = map.get_int(AO_TELEM_CAL_ACCEL_MINUS, AltosRecord.MISSING); + + /* flight computer values */ + kalman_acceleration = map.get_double(AO_TELEM_KALMAN_ACCEL, AltosRecord.MISSING, 1/16.0); + kalman_speed = map.get_double(AO_TELEM_KALMAN_SPEED, AltosRecord.MISSING, 1/16.0); + kalman_height = map.get_int(AO_TELEM_KALMAN_HEIGHT, AltosRecord.MISSING); + + flight_accel = map.get_int(AO_TELEM_ADHOC_ACCEL, AltosRecord.MISSING); + flight_vel = map.get_int(AO_TELEM_ADHOC_SPEED, AltosRecord.MISSING); + flight_pres = map.get_int(AO_TELEM_ADHOC_BARO, AltosRecord.MISSING); + + if (map.has(AO_TELEM_GPS_STATE)) + gps = new AltosGPS(map); + else + gps = null; + } + + private void parse_legacy(String[] words, int i) throws ParseException { + + AltosParse.word (words[i++], "CALL"); + callsign = words[i++]; + + AltosParse.word (words[i++], "SERIAL"); + serial = AltosParse.parse_int(words[i++]); + + if (version >= 2) { + AltosParse.word (words[i++], "FLIGHT"); + flight = AltosParse.parse_int(words[i++]); + } else + flight = 0; + + AltosParse.word(words[i++], "RSSI"); + rssi = AltosParse.parse_int(words[i++]); + + /* Older telemetry data had mis-computed RSSI value */ + if (version <= 2) + rssi = (rssi + 74) / 2 - 74; + + AltosParse.word(words[i++], "STATUS"); + status = AltosParse.parse_hex(words[i++]); + + AltosParse.word(words[i++], "STATE"); + state = AltosLib.state(words[i++]); + + tick = AltosParse.parse_int(words[i++]); + + AltosParse.word(words[i++], "a:"); + accel = AltosParse.parse_int(words[i++]); + + AltosParse.word(words[i++], "p:"); + pres = AltosParse.parse_int(words[i++]); + + AltosParse.word(words[i++], "t:"); + temp = AltosParse.parse_int(words[i++]); + + AltosParse.word(words[i++], "v:"); + batt = AltosParse.parse_int(words[i++]); + + AltosParse.word(words[i++], "d:"); + apogee = AltosParse.parse_int(words[i++]); + + AltosParse.word(words[i++], "m:"); + main = AltosParse.parse_int(words[i++]); + + AltosParse.word(words[i++], "fa:"); + flight_accel = AltosParse.parse_int(words[i++]); + + AltosParse.word(words[i++], "ga:"); + ground_accel = AltosParse.parse_int(words[i++]); + + AltosParse.word(words[i++], "fv:"); + flight_vel = AltosParse.parse_int(words[i++]); + + AltosParse.word(words[i++], "fp:"); + flight_pres = AltosParse.parse_int(words[i++]); + + /* Old TeleDongle code with kalman-reporting TeleMetrum code */ + if ((flight_vel & 0xffff0000) == 0x80000000) { + kalman_speed = ((short) flight_vel) / 16.0; + kalman_acceleration = flight_accel / 16.0; + kalman_height = flight_pres; + flight_vel = AltosRecord.MISSING; + flight_pres = AltosRecord.MISSING; + flight_accel = AltosRecord.MISSING; + } else { + kalman_speed = AltosRecord.MISSING; + kalman_acceleration = AltosRecord.MISSING; + kalman_height = AltosRecord.MISSING; + } + + AltosParse.word(words[i++], "gp:"); + ground_pres = AltosParse.parse_int(words[i++]); + + if (version >= 1) { + AltosParse.word(words[i++], "a+:"); + accel_plus_g = AltosParse.parse_int(words[i++]); + + AltosParse.word(words[i++], "a-:"); + accel_minus_g = AltosParse.parse_int(words[i++]); + } else { + accel_plus_g = ground_accel; + accel_minus_g = ground_accel + 530; + } + + gps = new AltosGPS(words, i, version); + gps_sequence++; + } + + public AltosTelemetryLegacy(String line) throws ParseException, AltosCRCException { + String[] words = line.split("\\s+"); + int i = 0; + + if (words[i].equals("CRC") && words[i+1].equals("INVALID")) { + i += 2; + AltosParse.word(words[i++], "RSSI"); + rssi = AltosParse.parse_int(words[i++]); + throw new AltosCRCException(rssi); + } + if (words[i].equals("CALL")) { + version = 0; + } else { + AltosParse.word (words[i++], "VERSION"); + version = AltosParse.parse_int(words[i++]); + } + + if (version < 4) + parse_legacy(words, i); + else + parse_v4(words, i); + } + + /* + * Given a hex dump of a legacy telemetry line, construct an AltosRecordTM from that + */ + + int[] bytes; + int adjust; + + /* + private int int8(int i) { + return AltosLib.int8(bytes, i + 1 + adjust); + } + */ + private int uint8(int i) { + return AltosLib.uint8(bytes, i + 1 + adjust); + } + private int int16(int i) { + return AltosLib.int16(bytes, i + 1 + adjust); + } + private int uint16(int i) { + return AltosLib.uint16(bytes, i + 1 + adjust); + } + private int uint32(int i) { + return AltosLib.uint32(bytes, i + 1 + adjust); + } + private String string(int i, int l) { + return AltosLib.string(bytes, i + 1 + adjust, l); + } + + static final int AO_GPS_NUM_SAT_MASK = (0xf << 0); + static final int AO_GPS_NUM_SAT_SHIFT = (0); + + 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_COURSE_VALID = (1 << 7); + + public AltosTelemetryLegacy(int[] in_bytes) { + bytes = in_bytes; + version = 4; + adjust = 0; + + if (bytes.length == AltosLib.ao_telemetry_0_8_len + 4) { + serial = uint8(0); + adjust = -1; + } else + serial = uint16(0); + + callsign = string(62, 8); + flight = uint16(2); + state = uint8(4); + tick = uint16(21); + accel = int16(23); + pres = int16(25); + temp = int16(27); + batt = int16(29); + apogee = int16(31); + main = int16(33); + + ground_accel = int16(7); + ground_pres = int16(15); + accel_plus_g = int16(17); + accel_minus_g = int16(19); + + if (uint16(11) == 0x8000) { + kalman_acceleration = int16(5); + kalman_speed = int16(9); + kalman_height = int16(13); + flight_accel = AltosRecord.MISSING; + flight_vel = AltosRecord.MISSING; + flight_pres = AltosRecord.MISSING; + } else { + flight_accel = int16(5); + flight_vel = uint32(9); + flight_pres = int16(13); + kalman_acceleration = AltosRecord.MISSING; + kalman_speed = AltosRecord.MISSING; + kalman_height = AltosRecord.MISSING; + } + + gps = null; + + int gps_flags = uint8(41); + + if ((gps_flags & (AO_GPS_VALID|AO_GPS_RUNNING)) != 0) { + gps = new AltosGPS(); + gps_sequence++; + + gps.nsat = (gps_flags & AO_GPS_NUM_SAT_MASK); + gps.locked = (gps_flags & AO_GPS_VALID) != 0; + gps.connected = true; + gps.lat = uint32(42) / 1.0e7; + gps.lon = uint32(46) / 1.0e7; + gps.alt = int16(50); + gps.ground_speed = uint16(52) / 100.0; + gps.course = uint8(54) * 2; + gps.hdop = uint8(55) / 5.0; + gps.h_error = uint16(58); + gps.v_error = uint16(60); + + int n_tracking_reported = uint8(70); + if (n_tracking_reported > 12) + n_tracking_reported = 12; + int n_tracking_actual = 0; + for (int i = 0; i < n_tracking_reported; i++) { + if (uint8(71 + i*2) != 0) + n_tracking_actual++; + } + if (n_tracking_actual > 0) { + gps.cc_gps_sat = new AltosGPSSat[n_tracking_actual]; + + n_tracking_actual = 0; + for (int i = 0; i < n_tracking_reported; i++) { + int svid = uint8(71 + i*2); + int c_n0 = uint8(72 + i*2); + if (svid != 0) + gps.cc_gps_sat[n_tracking_actual++] = new AltosGPSSat(svid, c_n0); + } + } + } + } + + public void update_state(AltosState state) { + state.set_tick(tick); + state.set_state(this.state); + state.set_flight(flight); + state.set_serial(serial); + state.set_rssi(rssi, status); + + state.set_pressure(AltosConvert.barometer_to_pressure(pres)); + state.set_accel_g(accel_plus_g, accel_minus_g); + state.set_accel(accel); + if (kalman_height != AltosRecord.MISSING) + state.set_kalman(kalman_height, kalman_speed, kalman_acceleration); + state.set_temperature(AltosEepromTM.thermometer_to_temperature(temp)); + state.set_battery_voltage(AltosConvert.cc_battery_to_voltage(batt)); + state.set_apogee_voltage(AltosConvert.cc_ignitor_to_voltage(apogee)); + state.set_main_voltage(AltosConvert.cc_ignitor_to_voltage(main)); + if (gps != null) + state.set_gps(gps, gps_sequence); + } +} diff --git a/altoslib/AltosTelemetryReader.java b/altoslib/AltosTelemetryReader.java index b4293c73..b1cc009c 100644 --- a/altoslib/AltosTelemetryReader.java +++ b/altoslib/AltosTelemetryReader.java @@ -27,16 +27,21 @@ public class AltosTelemetryReader extends AltosFlightReader { AltosRecord previous; double frequency; int telemetry; + AltosState state = null; LinkedBlockingQueue<AltosLine> telem; - public AltosRecord read() throws InterruptedException, ParseException, AltosCRCException, IOException { + public AltosState read() throws InterruptedException, ParseException, AltosCRCException, IOException { AltosLine l = telem.take(); if (l.line == null) throw new IOException("IO error"); - AltosRecord next = AltosTelemetry.parse(l.line, previous); - previous = next; - return next; + AltosTelemetry telem = AltosTelemetryLegacy.parse(l.line); + if (state == null) + state = new AltosState(); + else + state = state.clone(); + telem.update_state(state); + return state; } public void flush() { diff --git a/altoslib/AltosTelemetryRecord.java b/altoslib/AltosTelemetryRecord.java index fdc3c88e..a744e61a 100644 --- a/altoslib/AltosTelemetryRecord.java +++ b/altoslib/AltosTelemetryRecord.java @@ -44,6 +44,7 @@ public abstract class AltosTelemetryRecord { final static int packet_type_companion = 0x07; final static int packet_type_MM_sensor = 0x08; final static int packet_type_MM_data = 0x09; + final static int packet_type_Mini = 0x10; static AltosTelemetryRecord parse_hex(String hex) throws ParseException, AltosCRCException { AltosTelemetryRecord r; diff --git a/altoslib/AltosTelemetryRecordMetrumData.java b/altoslib/AltosTelemetryRecordMetrumData.java new file mode 100644 index 00000000..70179b28 --- /dev/null +++ b/altoslib/AltosTelemetryRecordMetrumData.java @@ -0,0 +1,54 @@ +/* + * Copyright © 2011 Keith Packard <keithp@keithp.com> + * + * 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. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +package org.altusmetrum.altoslib_1; + + +public class AltosTelemetryRecordMetrumData extends AltosTelemetryRecordRaw { + + int ground_pres; + int ground_accel; + int accel_plus_g; + int accel_minus_g; + + public AltosTelemetryRecordMetrumData(int[] in_bytes, int rssi) { + super(in_bytes, rssi); + + ground_pres = int32(8); + ground_accel = int16(12); + accel_plus_g = int16(14); + accel_minus_g = int16(16); + } + + public AltosRecord update_state(AltosRecord previous) { + AltosRecord n = super.update_state(previous); + + AltosRecordTM2 next; + if (!(n instanceof AltosRecordTM2)) { + next = new AltosRecordTM2(n); + } else { + next = (AltosRecordTM2) n; + } + + next.ground_accel = ground_accel; + next.ground_pres = ground_pres; + next.accel_plus_g = accel_plus_g; + next.accel_minus_g = accel_minus_g; + + return next; + } +} diff --git a/altoslib/AltosTelemetryRecordMetrumSensor.java b/altoslib/AltosTelemetryRecordMetrumSensor.java new file mode 100644 index 00000000..e41242c5 --- /dev/null +++ b/altoslib/AltosTelemetryRecordMetrumSensor.java @@ -0,0 +1,81 @@ +/* + * Copyright © 2011 Keith Packard <keithp@keithp.com> + * + * 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. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +package org.altusmetrum.altoslib_1; + + +public class AltosTelemetryRecordMetrumSensor extends AltosTelemetryRecordRaw { + int state; + + int accel; + int pres; + int temp; + + int acceleration; + int speed; + int height; + + int v_batt; + int sense_a; + int sense_m; + + public AltosTelemetryRecordMetrumSensor(int[] in_bytes, int rssi) { + super(in_bytes, rssi); + + state = int8(5); + accel = int16(6); + pres = int32(8); + temp = int16(12); + + acceleration = int16(14); + speed = int16(16); + height = int16(18); + + v_batt = int16(20); + sense_a = int16(22); + sense_m = int16(24); + } + + public AltosRecord update_state(AltosRecord previous) { + AltosRecord n = super.update_state(previous); + + AltosRecordTM2 next; + if (!(n instanceof AltosRecordTM2)) { + next = new AltosRecordTM2(n); + } else { + next = (AltosRecordTM2) n; + } + + next.state = state; + + next.accel = accel; + next.pres = pres; + next.temp = temp; + + next.kalman_acceleration = acceleration / 16.0; + next.kalman_speed = speed / 16.0; + next.kalman_height = height; + + next.v_batt = v_batt; + next.sense_a = sense_a; + next.sense_m = sense_m; + + next.seen |= AltosRecord.seen_sensor | AltosRecord.seen_temp_volt;; + + return next; + } +} diff --git a/altoslib/AltosTelemetryRecordMini.java b/altoslib/AltosTelemetryRecordMini.java new file mode 100644 index 00000000..75a66c16 --- /dev/null +++ b/altoslib/AltosTelemetryRecordMini.java @@ -0,0 +1,82 @@ +/* + * Copyright © 2011 Keith Packard <keithp@keithp.com> + * + * 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. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +package org.altusmetrum.altoslib_1; + + +public class AltosTelemetryRecordMini extends AltosTelemetryRecordRaw { + int state; + + int accel; + int pres; + int temp; + + int v_batt; + int sense_a; + int sense_m; + + int acceleration; + int speed; + int height; + + int ground_pres; + + public AltosTelemetryRecordMini(int[] in_bytes, int rssi) { + super(in_bytes, rssi); + + state = int8(5); + v_batt = int16(6); + sense_a = int16(8); + sense_m = int16(10); + + pres = int32(12); + temp = int16(16); + + acceleration = int16(18); + speed = int16(20); + height = int16(22); + + ground_pres = int32(24); + } + + public AltosRecord update_state(AltosRecord previous) { + AltosRecord n = super.update_state(previous); + + AltosRecordMini next; + if (!(n instanceof AltosRecordMini)) { + next = new AltosRecordMini(n); + } else { + next = (AltosRecordMini) n; + } + + next.pres = pres; + next.temp = temp; + + next.sense_a = sense_a; + next.sense_m = sense_m; + + next.ground_pres = ground_pres; + next.flight_accel = acceleration; + next.flight_vel = speed; + next.flight_height = height; + next.flight_pres = pres; + + next.seen |= AltosRecord.seen_sensor; + + return next; + } +} diff --git a/altoslib/Makefile.am b/altoslib/Makefile.am index 8c1cf2ad..59e0ec1c 100644 --- a/altoslib/Makefile.am +++ b/altoslib/Makefile.am @@ -17,7 +17,11 @@ altoslib_JAVA = \ AltosConvert.java \ AltosCRCException.java \ AltosDebug.java \ + AltosEeprom.java \ AltosEepromChunk.java \ + AltosEepromFile.java \ + AltosEepromTM.java \ + AltosEepromHeader.java \ AltosEepromIterable.java \ AltosEepromLog.java \ AltosEepromMega.java \ @@ -25,7 +29,7 @@ altoslib_JAVA = \ AltosEepromRecord.java \ AltosEepromTeleScience.java \ AltosEepromMini.java \ - AltosEepromMiniIterable.java \ + AltosEepromOldIterable.java \ AltosFile.java \ AltosFlash.java \ AltosFlashListener.java \ @@ -65,21 +69,14 @@ altoslib_JAVA = \ AltosSensorMM.java \ AltosSensorTM.java \ AltosState.java \ + AltosStateIterable.java \ + AltosStateUpdate.java \ AltosTelemetry.java \ + AltosTelemetryFile.java \ AltosTelemetryIterable.java \ + AltosTelemetryLegacy.java \ AltosTelemetryMap.java \ AltosTelemetryReader.java \ - AltosTelemetryRecordCompanion.java \ - AltosTelemetryRecordConfiguration.java \ - AltosTelemetryRecordGeneral.java \ - AltosTelemetryRecord.java \ - AltosTelemetryRecordLegacy.java \ - AltosTelemetryRecordLocation.java \ - AltosTelemetryRecordRaw.java \ - AltosTelemetryRecordSatellite.java \ - AltosTelemetryRecordSensor.java \ - AltosTelemetryRecordMegaSensor.java \ - AltosTelemetryRecordMegaData.java \ AltosUnitsListener.java \ AltosMs5607.java \ AltosIMU.java \ |