From: Keith Packard Date: Tue, 9 May 2017 07:19:05 +0000 (-0700) Subject: altoslib: Add new eeprom management code X-Git-Tag: 1.8~132 X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=commitdiff_plain;h=e67a5c6ffa7174d66e985483fab4bf52ccaea5ca altoslib: Add new eeprom management code Generic .eeprom file parsing, simpler per-type eeprom data extraction. Signed-off-by: Keith Packard --- diff --git a/altoslib/AltosEepromNew.java b/altoslib/AltosEepromNew.java new file mode 100644 index 00000000..c8f44509 --- /dev/null +++ b/altoslib/AltosEepromNew.java @@ -0,0 +1,275 @@ +/* + * Copyright © 2017 Keith Packard + * + * 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, either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + */ + +package org.altusmetrum.altoslib_11; + +import java.util.*; +import java.io.*; + +public class AltosEepromNew { + + private AltosJson config; + ArrayList data; + private AltosConfigData config_data; + + /* + * Public accessor APIs + */ + public int data8(int offset) { + return ((int) data.get(offset)) & 0xff; + } + + public int data16(int offset) { + return data8(offset) | (data8(offset+1) << 8); + } + + public int data24(int offset) { + return (data8(offset) | + (data8(offset+1) << 8) | + (data8(offset+2) << 16)); + } + + public int data32(int offset) { + return (data8(offset) | + (data8(offset+1) << 8) | + (data8(offset+2) << 16) | + (data8(offset+3) << 24)); + } + + public int size() { + return data.size(); + } + + public AltosConfigData config_data() { + if (config_data == null) { + config_data = (AltosConfigData) config.make(AltosConfigData.class); + if (config_data == null) + config_data = new AltosConfigData(); + + if (config_data.log_format == AltosLib.MISSING) { + if (config_data.product != null) { + if (config_data.product.startsWith("TeleMetrum")) + config_data.log_format = AltosLib.AO_LOG_FORMAT_FULL; + else if (config_data.product.startsWith("TeleMini")) + config_data.log_format = AltosLib.AO_LOG_FORMAT_TINY; + } + } + } + return config_data; + } + + public void reset_config_data() { + config_data = null; + } + + private void write_config(Writer w) throws IOException { + config.write(w, 0, true); + w.append('\n'); + } + + /* + * Private I/O APIs + */ + private void write_data(Writer w) throws IOException { + PrintWriter pw = new PrintWriter(w); + + for (int i = 0; i < data.size(); i++) { + if (i > 0) { + if ((i & 0x1f) == 0) + pw.printf("\n"); + else + pw.printf(" "); + } + pw.printf("%02x", data.get(i)); + } + w.append('\n'); + } + + private boolean read_config(Reader r) throws IOException { + config = AltosJson.fromReader(r); + if (config == null) + return false; + return true; + } + + static private byte[] byte_list_to_array(List bytes) { + byte[] data = new byte[bytes.size()]; + int i = 0; + + for (Byte b : bytes) { + data[i++] = b; + } + return data; + } + + private boolean read_data(Reader r) throws IOException { + BufferedReader br = new BufferedReader(r); + String s; + + data = new ArrayList(); + while ((s = br.readLine()) != null) { + String[] tokens = s.split("\\s+"); + + for (int i = 0; i < tokens.length; i++) { + try { + data.add((byte) AltosLib.fromhex(tokens[i])); + } catch (NumberFormatException e) { + throw new IOException(e.toString()); + } + } + } + return true; + } + + private boolean read_old_config(BufferedReader r) throws IOException { + AltosConfigData cfg = new AltosConfigData(); + for (;;) { + boolean done = false; + + /* The data starts with an upper case F character followed by a space */ + r.mark(2); + int first = r.read(); + if (first == 'F') { + int second = r.read(); + if (second == ' ') + done = true; + } + r.reset(); + if (done) + break; + + String line = r.readLine(); + if (line == null) + return false; + cfg.parse_line(line); + } + config = new AltosJson(cfg); + return true; + } + + private boolean read_old_data(BufferedReader r) throws IOException { + String line; + + data = new ArrayList(); + while ((line = r.readLine()) != null) { + String[] tokens = line.split("\\s+"); + + /* Make sure there's at least a type and time */ + if (tokens.length < 2) + break; + + /* packet type */ + if (tokens[0].length() != 1) + break; + int start = data.size(); + + if (config_data().log_format != AltosLib.AO_LOG_FORMAT_TINY) { + data.add((byte) tokens[0].codePointAt(0)); + + int time = AltosLib.fromhex(tokens[1]); + + data.add((byte) 0); + data.add((byte) (time & 0xff)); + data.add((byte) (time >> 8)); + } + if (tokens.length == 4) { + /* Handle ancient log files */ + if (config_data().log_format == AltosLib.AO_LOG_FORMAT_TINY) { + /* + * Ancient TeleMini log files stored "extra" data to pretend + * that it was a TeleMetrum device. Throw that away and + * just save the actual log data. + */ + int a = AltosLib.fromhex(tokens[2]); + int b = AltosLib.fromhex(tokens[3]); + if (a != 0) + b = 0x8000 | a; + data.add((byte) (b & 0xff)); + data.add((byte) ((b >> 8))); + } else { + for (int i = 2; i < tokens.length; i++) { + int v = AltosLib.fromhex(tokens[i]); + data.add((byte) (v & 0xff)); + data.add((byte) ((v >> 8))); + } + /* Re-compute the checksum byte */ + data.set(start + 1, (byte) (256 - AltosConvert.checksum(data, start, data.size() - start))); + } + } else { + for (int i = 2; i < tokens.length; i++) + data.add((byte) AltosLib.fromhex(tokens[i])); + /* Re-compute the checksum byte */ + data.set(start + 1, (byte) (256 - AltosConvert.checksum(data, start, data.size() - start))); + } + } + return true; + } + + private void read(Reader r) throws IOException { + BufferedReader br = new BufferedReader(r); + + br.mark(1); + int c = br.read(); + br.reset(); + + if (c == '{') { + if (!read_config(br)) + throw new IOException("failed to read config"); + if (!read_data(br)) + throw new IOException("failed to read data"); + } else { + if (!read_old_config(br)) + throw new IOException("failed to read old config"); + if (!read_old_data(br)) + throw new IOException("failed to read old data"); + } + } + + /* + * Public APIs for I/O + */ + public void write(Writer w) throws IOException { + write_config(w); + write_data(w); + } + + public String toString() { + try { + Writer w = new StringWriter(); + + write(w); + return w.toString(); + } catch (Exception e) { + return null; + } + } + + public void print() throws IOException { + System.out.printf("%s", toString()); + } + + /* + * Constructors + */ + public AltosEepromNew(Reader r) throws IOException { + read(r); + } + + public AltosEepromNew(String s) throws IOException { + read(new StringReader(s)); + } + + public AltosEepromNew() { + } +} diff --git a/altoslib/AltosEepromRecord.java b/altoslib/AltosEepromRecord.java new file mode 100644 index 00000000..efcca857 --- /dev/null +++ b/altoslib/AltosEepromRecord.java @@ -0,0 +1,95 @@ +/* + * Copyright © 2017 Keith Packard + * + * 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, either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + */ + +package org.altusmetrum.altoslib_11; + + +public abstract class AltosEepromRecord implements Comparable { + + AltosEepromNew eeprom; + + int wide_tick; + + final int start; + final int length; + + public final static int header_length = 4; + + public int cmd() { + return eeprom.data8(start); + } + + public int tick() { + return eeprom.data16(start+2); + } + + public int data8(int i) { + i += start + header_length; + return eeprom.data8(i); + } + + public int data16(int i) { + return ((data8(i) | (data8(i+1) << 8)) << 16) >> 16; + } + + public int data24(int i) { + return data8(i) | (data8(i+1) << 8) | (data8(i+2) << 16); + } + + public int data32(int i) { + return data8(i) | (data8(i+1) << 8) | (data8(i+2) << 16) | (data8(i+3) << 24); + } + + public boolean valid() { + return AltosConvert.checksum(eeprom.data, start, length) == 0; + } + + private int cmdi() { + if (cmd() == AltosLib.AO_LOG_FLIGHT) + return 0; + return 1; + } + + public int compareTo(AltosEepromRecord o) { + int cmd_diff = cmdi() - o.cmdi(); + + if (cmd_diff != 0) + return cmd_diff; + + int tick_diff = tick() - o.tick(); + + if (tick_diff != 0) + return tick_diff; + return start - o.start; + } + + public void update_state(AltosState state) { + if (cmd() == AltosLib.AO_LOG_FLIGHT) + state.set_boost_tick(tick()); + else + state.set_tick(tick()); + } + + public boolean hasNext() { + return start + length * 2 < eeprom.data.size(); + } + + public abstract AltosEepromRecord next(); + + public AltosEepromRecord(AltosEepromNew eeprom, int start, int length) { + this.eeprom = eeprom; + this.start = start; + this.length = length; + } +} diff --git a/altoslib/AltosEepromRecordFireTwo.java b/altoslib/AltosEepromRecordFireTwo.java new file mode 100644 index 00000000..9b92f111 --- /dev/null +++ b/altoslib/AltosEepromRecordFireTwo.java @@ -0,0 +1,105 @@ +/* + * Copyright © 2017 Keith Packard + * + * 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; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * 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_11; + +import java.io.*; +import java.util.*; +import java.text.*; + +public class AltosEepromRecordFireTwo extends AltosEepromRecord { + public static final int record_length = 32; + + /* AO_LOG_FLIGHT elements */ + public int flight() { return data16(0); } + public int idle_pres() { return data16(2); } + public int idle_thrust() { return data16(4); } + + /* AO_LOG_STATE elements */ + public int state() { return data16(0); } + public int reason() { return data16(2); } + + /* AO_LOG_SENSOR elements */ + public int pres() { return data16(0); } + public int thrust() { return data16(2); } + public int temp(int i) { return data16(4+i*2); } + + private static final double r_above = 5600.0; + private static final double r_below = 10000.0; + private static final double v_adc = 3.3; + + private static double firetwo_adc(int raw) { + return raw / 4095.0; + } + + public static double adc_to_pa(int adc) { + + /* raw adc to processor voltage, then back through the + * voltage divider to the sensor voltage + */ + + double v = firetwo_adc(adc) * v_adc * (r_above + r_below) / r_below; + + /* Bound to ranges provided in sensor */ + if (v < 0.5) v = 0.5; + if (v > 4.5) v = 4.5; + + double psi = (v - 0.5) / 4.0 * 1600.0; + return AltosConvert.psi_to_pa(psi); + } + + public static double adc_to_n(int adc) { + double v = firetwo_adc(adc); + + /* this is a total guess */ + return AltosConvert.lb_to_n(v * 298 * 9.807); + } + + public void update_state(AltosState state) { + super.update_state(state); + + switch (cmd()) { + case AltosLib.AO_LOG_FLIGHT: + state.set_flight(flight()); + state.set_ground_pressure(adc_to_pa(idle_pres())); + state.set_accel_g(0, -1); + break; + case AltosLib.AO_LOG_STATE: + state.set_state(state()); + break; + case AltosLib.AO_LOG_SENSOR: + state.set_pressure(adc_to_pa(pres())); + state.set_accel(adc_to_n(thrust())); + break; + } + } + + public AltosEepromRecord next() { + if (start + length + length < eeprom.data.size()) + return new AltosEepromRecordFireTwo(eeprom, start + length); + return null; + } + + public AltosEepromRecordFireTwo(AltosEepromNew eeprom, int start) { + super(eeprom, start, record_length); + } + + public AltosEepromRecordFireTwo(AltosEepromNew eeprom) { + this(eeprom, 0); + } +} diff --git a/altoslib/AltosEepromRecordFull.java b/altoslib/AltosEepromRecordFull.java new file mode 100644 index 00000000..d240da28 --- /dev/null +++ b/altoslib/AltosEepromRecordFull.java @@ -0,0 +1,123 @@ +/* + * Copyright © 2017 Keith Packard + * + * 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, either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + */ + +package org.altusmetrum.altoslib_11; + +public class AltosEepromRecordFull extends AltosEepromRecord { + public static final int record_length = 8; + + public static final int max_sat = 12; + + public void update_state(AltosState state) { + super.update_state(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(data16(0)); + state.set_flight(data16(2)); + break; + case AltosLib.AO_LOG_SENSOR: + state.set_accel(data16(0)); + state.set_pressure(AltosConvert.barometer_to_pressure(data16(2))); + break; + case AltosLib.AO_LOG_PRESSURE: + state.set_pressure(AltosConvert.barometer_to_pressure(data16(2))); + break; + case AltosLib.AO_LOG_TEMP_VOLT: + state.set_temperature(AltosConvert.thermometer_to_temperature(data16(0))); + state.set_battery_voltage(AltosConvert.cc_battery_to_voltage(data16(2))); + break; + case AltosLib.AO_LOG_DEPLOY: + state.set_apogee_voltage(AltosConvert.cc_ignitor_to_voltage(data16(0))); + state.set_main_voltage(AltosConvert.cc_ignitor_to_voltage(data16(2))); + break; + case AltosLib.AO_LOG_STATE: + state.set_state(data16(0)); + break; + case AltosLib.AO_LOG_GPS_TIME: + gps = state.make_temp_gps(false); + + gps.hour = data8(0); + gps.minute = data8(1); + gps.second = data8(2); + + int flags = data8(3); + + 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(false); + + int lat32 = data32(0); + gps.lat = (double) lat32 / 1e7; + break; + case AltosLib.AO_LOG_GPS_LON: + gps = state.make_temp_gps(false); + + int lon32 = data32(0); + gps.lon = (double) lon32 / 1e7; + break; + case AltosLib.AO_LOG_GPS_ALT: + gps = state.make_temp_gps(false); + gps.alt = data16(0); + break; + case AltosLib.AO_LOG_GPS_SAT: + gps = state.make_temp_gps(true); + int svid = data16(0); + int c_n0 = data16(3); + gps.add_sat(svid, c_n0); + break; + case AltosLib.AO_LOG_GPS_DATE: + gps = state.make_temp_gps(false); + gps.year = data8(0) + 2000; + gps.month = data8(1); + gps.day = data8(2); + break; + } + } + + public AltosEepromRecord next() { + if (start + length + length < eeprom.data.size()) + return new AltosEepromRecordFull(eeprom, start + length); + return null; + } + + public AltosEepromRecordFull(AltosEepromNew eeprom, int start) { + super(eeprom, start, record_length); + } + + public AltosEepromRecordFull(AltosEepromNew eeprom) { + this(eeprom, 0); + } +} diff --git a/altoslib/AltosEepromRecordGps.java b/altoslib/AltosEepromRecordGps.java new file mode 100644 index 00000000..d547ef02 --- /dev/null +++ b/altoslib/AltosEepromRecordGps.java @@ -0,0 +1,160 @@ +/* + * Copyright © 2017 Keith Packard + * + * 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; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * 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_11; + +import java.io.*; +import java.util.*; +import java.text.*; + +public class AltosEepromRecordGps extends AltosEepromRecord { + public static final int record_length = 32; + + /* AO_LOG_FLIGHT elements */ + public int flight() { return data16(0); } + public int start_altitude() { return data16(2); } + public int start_latitude() { return data32(4); } + public int start_longitude() { return data32(8); } + + /* AO_LOG_GPS_TIME elements */ + public int latitude() { return data32(0); } + public int longitude() { return data32(4); } + public int altitude_low() { return data16(8); } + public int hour() { return data8(10); } + public int minute() { return data8(11); } + public int second() { return data8(12); } + public int flags() { return data8(13); } + public int year() { return data8(14); } + public int month() { return data8(15); } + public int day() { return data8(16); } + public int course() { return data8(17); } + public int ground_speed() { return data16(18); } + public int climb_rate() { return data16(20); } + public int pdop() { return data8(22); } + public int hdop() { return data8(23); } + public int vdop() { return data8(24); } + public int mode() { return data8(25); } + public int altitude_high() { return data16(26); } + + private int seconds() { + switch (cmd()) { + case AltosLib.AO_LOG_GPS_TIME: + return second() + 60 * (minute() + 60 * (hour() + 24 * (day() + 31 * month()))); + default: + return 0; + } + } + + public int compareTo(AltosEepromRecord o) { + AltosEepromRecordGps og = (AltosEepromRecordGps) o; + + int seconds_diff = seconds() - og.seconds(); + + if (seconds_diff != 0) + return seconds_diff; + + return start - o.start; + } + + public void update_state(AltosState state) { + super.update_state(state); + + AltosGPS gps; + + /* Flush any pending RecordGps 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: + if (state.flight == AltosLib.MISSING) { + state.set_boost_tick(tick()); + state.set_flight(flight()); + } + /* no place to log start lat/lon yet */ + break; + case AltosLib.AO_LOG_GPS_TIME: + gps = state.make_temp_gps(false); + gps.lat = latitude() / 1e7; + gps.lon = longitude() / 1e7; + if (eeprom.config_data().altitude_32 == 1) + gps.alt = (altitude_low() & 0xffff) | (altitude_high() << 16); + else + gps.alt = altitude_low(); + + gps.hour = hour(); + gps.minute = minute(); + gps.second = second(); + + int flags = flags(); + + 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; + + gps.year = 2000 + year(); + gps.month = month(); + gps.day = day(); + gps.ground_speed = ground_speed() * 1.0e-2; + gps.course = course() * 2; + gps.climb_rate = climb_rate() * 1.0e-2; + if (eeprom.config_data().compare_version("1.4.9") >= 0) { + gps.pdop = pdop() / 10.0; + gps.hdop = hdop() / 10.0; + gps.vdop = vdop() / 10.0; + } else { + gps.pdop = pdop() / 100.0; + if (gps.pdop < 0.8) + gps.pdop += 2.56; + gps.hdop = hdop() / 100.0; + if (gps.hdop < 0.8) + gps.hdop += 2.56; + gps.vdop = vdop() / 100.0; + if (gps.vdop < 0.8) + gps.vdop += 2.56; + } + break; + } + } + + public AltosEepromRecord next() { + if (start + length + length < eeprom.data.size()) + return new AltosEepromRecordGps(eeprom, start + length); + return null; + } + + public AltosEepromRecordGps(AltosEepromNew eeprom, int start) { + super(eeprom, start, record_length); + } + + public AltosEepromRecordGps(AltosEepromNew eeprom) { + this(eeprom, 0); + } +} diff --git a/altoslib/AltosEepromRecordMega.java b/altoslib/AltosEepromRecordMega.java new file mode 100644 index 00000000..167f666b --- /dev/null +++ b/altoslib/AltosEepromRecordMega.java @@ -0,0 +1,253 @@ +/* + * Copyright © 2017 Keith Packard + * + * 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, either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + */ + +package org.altusmetrum.altoslib_11; + +public class AltosEepromRecordMega extends AltosEepromRecord { + public static final int record_length = 32; + + public static final int max_sat = 12; + + private int log_format; + + /* AO_LOG_FLIGHT elements */ + private int flight() { return data16(0); } + private int ground_accel() { return data16(2); } + private int ground_pres() { return data32(4); } + private int ground_accel_along() { return data16(8); } + private int ground_accel_across() { return data16(10); } + private int ground_accel_through() { return data16(12); } + private int ground_roll() { + switch (log_format) { + case AltosLib.AO_LOG_FORMAT_TELEMEGA: + return data32(16); + case AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD: + return data16(14); + default: + return AltosLib.MISSING; + } + } + private int ground_pitch() { + switch (log_format) { + case AltosLib.AO_LOG_FORMAT_TELEMEGA: + return data32(20); + case AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD: + return data16(16); + default: + return AltosLib.MISSING; + } + } + private int ground_yaw() { + switch (log_format) { + case AltosLib.AO_LOG_FORMAT_TELEMEGA: + return data32(24); + case AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD: + return data16(18); + default: + return AltosLib.MISSING; + } + } + + /* AO_LOG_STATE elements */ + private int state() { return data16(0); } + private int reason() { return data16(2); } + + /* AO_LOG_SENSOR elements */ + private int pres() { return data32(0); } + private int temp() { return data32(4); } + private int accel_x() { return data16(8); } + private int accel_y() { return data16(10); } + private int accel_z() { return data16(12); } + private int gyro_x() { return data16(14); } + private int gyro_y() { return data16(16); } + private int gyro_z() { return data16(18); } + private int mag_x() { return data16(20); } + private int mag_y() { return data16(22); } + private int mag_z() { return data16(24); } + private int accel() { return data16(26); } + + /* AO_LOG_TEMP_VOLT elements */ + private int v_batt() { return data16(0); } + private int v_pbatt() { return data16(2); } + private int nsense() { return data16(4); } + private int sense(int i) { return data16(6 + i * 2); } + private int pyro() { return data16(26); } + + /* AO_LOG_GPS_TIME elements */ + private int latitude() { return data32(0); } + private int longitude() { return data32(4); } + private int altitude_low() { return data16(8); } + private int hour() { return data8(10); } + private int minute() { return data8(11); } + private int second() { return data8(12); } + private int flags() { return data8(13); } + private int year() { return data8(14); } + private int month() { return data8(15); } + private int day() { return data8(16); } + private int course() { return data8(17); } + private int ground_speed() { return data16(18); } + private int climb_rate() { return data16(20); } + private int pdop() { return data8(22); } + private int hdop() { return data8(23); } + private int vdop() { return data8(24); } + private int mode() { return data8(25); } + private int altitude_high() { return data16(26); } + + /* AO_LOG_GPS_SAT elements */ + private int nsat() { return data16(0); } + private int svid(int n) { return data8(2 + n * 2); } + private int c_n(int n) { return data8(2 + n * 2 + 1); } + + public void update_state(AltosState state) { + super.update_state(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_flight(flight()); + state.set_ground_accel(ground_accel()); + state.set_ground_pressure(ground_pres()); + state.set_accel_ground(ground_accel_along(), + ground_accel_across(), + ground_accel_through()); + state.set_gyro_zero(ground_roll() / 512.0, + ground_pitch() / 512.0, + ground_yaw() / 512.0); + break; + case AltosLib.AO_LOG_STATE: + state.set_state(state()); + break; + case AltosLib.AO_LOG_SENSOR: + state.set_ms5607(pres(), temp()); + + AltosIMU imu = new AltosIMU(accel_y(), /* along */ + accel_x(), /* across */ + accel_z(), /* through */ + gyro_y(), /* roll */ + gyro_x(), /* pitch */ + gyro_z()); /* yaw */ + + if (log_format == AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD) + state.check_imu_wrap(imu); + + state.set_imu(imu); + + state.set_mag(new AltosMag(mag_x(), + mag_y(), + mag_z())); + + state.set_accel(accel()); + + break; + case AltosLib.AO_LOG_TEMP_VOLT: + state.set_battery_voltage(AltosConvert.mega_battery_voltage(v_batt())); + state.set_pyro_voltage(AltosConvert.mega_pyro_voltage(v_pbatt())); + + int nsense = nsense(); + + state.set_apogee_voltage(AltosConvert.mega_pyro_voltage(sense(nsense-2))); + state.set_main_voltage(AltosConvert.mega_pyro_voltage(sense(nsense-1))); + + double voltages[] = new double[nsense-2]; + for (int i = 0; i < nsense-2; i++) + voltages[i] = AltosConvert.mega_pyro_voltage(sense(i)); + + state.set_ignitor_voltage(voltages); + state.set_pyro_fired(pyro()); + break; + case AltosLib.AO_LOG_GPS_TIME: + gps = state.make_temp_gps(false); + gps.lat = latitude() / 1e7; + gps.lon = longitude() / 1e7; + + if (state.altitude_32()) + gps.alt = (altitude_low() & 0xffff) | (altitude_high() << 16); + else + gps.alt = altitude_low(); + + gps.hour = hour(); + gps.minute = minute(); + gps.second = second(); + + int flags = flags(); + + 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; + + gps.year = 2000 + year(); + gps.month = month(); + gps.day = day(); + gps.ground_speed = ground_speed() * 1.0e-2; + gps.course = course() * 2; + gps.climb_rate = climb_rate() * 1.0e-2; + if (state.compare_version("1.4.9") >= 0) { + gps.pdop = pdop() / 10.0; + gps.hdop = hdop() / 10.0; + gps.vdop = vdop() / 10.0; + } else { + gps.pdop = pdop() / 100.0; + if (gps.pdop < 0.8) + gps.pdop += 2.56; + gps.hdop = hdop() / 100.0; + if (gps.hdop < 0.8) + gps.hdop += 2.56; + gps.vdop = vdop() / 100.0; + if (gps.vdop < 0.8) + gps.vdop += 2.56; + } + break; + case AltosLib.AO_LOG_GPS_SAT: + gps = state.make_temp_gps(true); + + int n = nsat(); + if (n > max_sat) + n = max_sat; + for (int i = 0; i < n; i++) + gps.add_sat(svid(i), c_n(i)); + break; + } + } + + public AltosEepromRecord next() { + if (start + length + length < eeprom.data.size()) + return new AltosEepromRecordMega(eeprom, start + length); + return null; + } + + public AltosEepromRecordMega(AltosEepromNew eeprom, int start) { + super(eeprom, start, record_length); + log_format = eeprom.config_data().log_format; + } + + public AltosEepromRecordMega(AltosEepromNew eeprom) { + this(eeprom, 0); + } +} diff --git a/altoslib/AltosEepromRecordMetrum.java b/altoslib/AltosEepromRecordMetrum.java new file mode 100644 index 00000000..df4d23a2 --- /dev/null +++ b/altoslib/AltosEepromRecordMetrum.java @@ -0,0 +1,161 @@ +/* + * Copyright © 2017 Keith Packard + * + * 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; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * 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_11; + +public class AltosEepromRecordMetrum extends AltosEepromRecord { + public static final int record_length = 16; + + public int record_length() { return record_length; } + + /* 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_low() { return data16(8); } + public int altitude_high() { return data16(10); } + + /* 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); } + public int pdop() { return data8(7); } + + /* AO_LOG_GPS_SAT elements */ + public int nsat() { 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 void update_state(AltosState state) { + super.update_state(state); + + AltosGPS gps; + + /* Flush any pending GPS changes */ + if (state.gps_pending) { + switch (cmd()) { + case AltosLib.AO_LOG_GPS_POS: + 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_flight(flight()); + state.set_ground_accel(ground_accel()); + 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_accel(accel()); + + break; + case AltosLib.AO_LOG_TEMP_VOLT: + state.set_battery_voltage(AltosConvert.mega_battery_voltage(v_batt())); + + state.set_apogee_voltage(AltosConvert.mega_pyro_voltage(sense_a())); + state.set_main_voltage(AltosConvert.mega_pyro_voltage(sense_m())); + + break; + case AltosLib.AO_LOG_GPS_POS: + gps = state.make_temp_gps(false); + gps.lat = latitude() / 1e7; + gps.lon = longitude() / 1e7; + if (state.altitude_32()) + gps.alt = (altitude_low() & 0xffff) | (altitude_high() << 16); + else + gps.alt = altitude_low(); + break; + case AltosLib.AO_LOG_GPS_TIME: + gps = state.make_temp_gps(false); + + gps.hour = hour(); + gps.minute = minute(); + gps.second = second(); + + int flags = flags(); + + 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; + + gps.year = 2000 + year(); + gps.month = month(); + gps.day = day(); + gps.pdop = pdop() / 10.0; + break; + case AltosLib.AO_LOG_GPS_SAT: + gps = state.make_temp_gps(true); + + int n = nsat(); + for (int i = 0; i < n; i++) + gps.add_sat(svid(i), c_n(i)); + break; + } + } + + public AltosEepromRecord next() { + if (start + length + length < eeprom.data.size()) + return new AltosEepromRecordMetrum(eeprom, start + length); + return null; + } + + public AltosEepromRecordMetrum(AltosEepromNew eeprom, int start) { + super(eeprom, start, record_length); + } + + public AltosEepromRecordMetrum(AltosEepromNew eeprom) { + this(eeprom, 0); + } +} diff --git a/altoslib/AltosEepromRecordMini.java b/altoslib/AltosEepromRecordMini.java new file mode 100644 index 00000000..4c5a88bf --- /dev/null +++ b/altoslib/AltosEepromRecordMini.java @@ -0,0 +1,98 @@ +/* + * Copyright © 2017 Keith Packard + * + * 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; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * 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_11; + +public class AltosEepromRecordMini extends AltosEepromRecord { + public static final int record_length = 16; + + /* AO_LOG_FLIGHT elements */ + public int flight() { return data16(0); } + public int ground_pres() { return data32(4); } + + /* AO_LOG_STATE elements */ + public int state() { return data16(0); } + public int reason() { return data16(2); } + + /* AO_LOG_SENSOR elements */ + public int pres() { return data24(0); } + public int temp() { return data24(3); } + public int sense_a() { return data16(6); } + public int sense_m() { return data16(8); } + public int v_batt() { return data16(10); } + + private int log_format() { + return eeprom.config_data().log_format; + } + + private double battery_voltage(int sensor) { + int log_format = log_format(); + if (log_format == AltosLib.AO_LOG_FORMAT_EASYMINI) + return AltosConvert.easy_mini_voltage(sensor, eeprom.config_data().serial); + if (log_format == AltosLib.AO_LOG_FORMAT_TELEMINI2) + return AltosConvert.tele_mini_2_voltage(sensor); + if (log_format == AltosLib.AO_LOG_FORMAT_TELEMINI3) + return AltosConvert.tele_mini_3_battery_voltage(sensor); + return -1; + } + + private double pyro_voltage(int sensor) { + int log_format = log_format(); + if (log_format == AltosLib.AO_LOG_FORMAT_EASYMINI) + return AltosConvert.easy_mini_voltage(sensor, eeprom.config_data().serial); + if (log_format == AltosLib.AO_LOG_FORMAT_TELEMINI2) + return AltosConvert.tele_mini_2_voltage(sensor); + if (log_format == AltosLib.AO_LOG_FORMAT_TELEMINI3) + return AltosConvert.tele_mini_3_pyro_voltage(sensor); + return -1; + } + + public void update_state(AltosState state) { + super.update_state(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(pyro_voltage(sense_a())); + state.set_main_voltage(pyro_voltage(sense_m())); + state.set_battery_voltage(battery_voltage(v_batt())); + break; + } + } + + public AltosEepromRecord next() { + if (start + length + length < eeprom.data.size()) + return new AltosEepromRecordMini(eeprom, start + length); + return null; + } + + public AltosEepromRecordMini(AltosEepromNew eeprom, int start) { + super(eeprom, start, record_length); + } + + public AltosEepromRecordMini(AltosEepromNew eeprom) { + this(eeprom, 0); + } +} diff --git a/altoslib/AltosEepromRecordSet.java b/altoslib/AltosEepromRecordSet.java new file mode 100644 index 00000000..000d9c02 --- /dev/null +++ b/altoslib/AltosEepromRecordSet.java @@ -0,0 +1,121 @@ +/* + * Copyright © 2017 Keith Packard + * + * 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, either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + */ + +package org.altusmetrum.altoslib_11; + +import java.io.*; +import java.util.*; + +public class AltosEepromRecordSet implements Iterable { + TreeSet ordered; + AltosState start_state; + + class RecordIterator implements Iterator { + Iterator riterator; + AltosState state; + boolean started; + + public boolean hasNext() { + return state == null || riterator.hasNext(); + } + + public AltosState next() { + if (state == null) + state = start_state.clone(); + else { + state = state.clone(); + AltosEepromRecord r = riterator.next(); + r.update_state(state); + } + return state; + } + + public RecordIterator() { + riterator = ordered.iterator(); + state = null; + } + } + + public Iterator iterator() { + return new RecordIterator(); + } + + public AltosEepromRecordSet(AltosEepromNew eeprom) { + AltosConfigData config_data = eeprom.config_data(); + + AltosEepromRecord record = null; + + switch (config_data.log_format) { + case AltosLib.AO_LOG_FORMAT_FULL: + record = new AltosEepromRecordFull(eeprom); + break; + case AltosLib.AO_LOG_FORMAT_TINY: + record = new AltosEepromRecordTiny(eeprom); + break; + case AltosLib.AO_LOG_FORMAT_TELEMETRY: + case AltosLib.AO_LOG_FORMAT_TELESCIENCE: + case AltosLib.AO_LOG_FORMAT_TELEMEGA: + case AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD: + record = new AltosEepromRecordMega(eeprom); + break; + case AltosLib.AO_LOG_FORMAT_TELEMETRUM: + record = new AltosEepromRecordMetrum(eeprom); + break; + case AltosLib.AO_LOG_FORMAT_TELEMINI2: + case AltosLib.AO_LOG_FORMAT_TELEMINI3: + case AltosLib.AO_LOG_FORMAT_EASYMINI: + record = new AltosEepromRecordMini(eeprom); + break; + case AltosLib.AO_LOG_FORMAT_TELEGPS: + record = new AltosEepromRecordGps(eeprom); + break; + case AltosLib.AO_LOG_FORMAT_TELEFIRETWO: + record = new AltosEepromRecordFireTwo(eeprom); + break; + } + + if (record == null) { + System.out.printf("failed to parse log format %d\n", config_data.log_format); + return; + } + ordered = new TreeSet(); + int tick = 0; + boolean first = true; + + start_state = new AltosState(); + start_state.set_config_data(record.eeprom.config_data()); + + for (;;) { + int t = record.tick(); + + if (first) { + tick = t; + first = false; + } else { + while (t < tick - 32767) + t += 65536; + tick = t; + } + record.wide_tick = tick; + ordered.add(record); + if (!record.hasNext()) + break; + record = record.next(); + } + } + + public AltosEepromRecordSet(Reader input) throws IOException { + this(new AltosEepromNew(input)); + } +} diff --git a/altoslib/AltosEepromRecordTiny.java b/altoslib/AltosEepromRecordTiny.java new file mode 100644 index 00000000..6c04bfee --- /dev/null +++ b/altoslib/AltosEepromRecordTiny.java @@ -0,0 +1,81 @@ +/* + * Copyright © 2017 Keith Packard + * + * 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, either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + */ + +package org.altusmetrum.altoslib_11; + +public class AltosEepromRecordTiny extends AltosEepromRecord { + public static final int record_length = 2; + + private int value() { + return eeprom.data16(start); + } + + public int cmd() { + if (start == 0) + return AltosLib.AO_LOG_FLIGHT; + if ((value() & 0x8000) != 0) + return AltosLib.AO_LOG_STATE; + return AltosLib.AO_LOG_SENSOR; + } + + public int tick() { + int tick = 0; + int step = 10; + for (int i = 2; i < start; i += 2) + { + int v = eeprom.data16(i); + + if ((v & 0x8000) != 0) { + if ((v & 0x7fff) >= AltosLib.ao_flight_drogue) + step = 100; + } else { + tick += step; + } + } + return tick; + } + + public void update_state(AltosState state) { + int value = data16(-header_length); + + state.set_tick(tick()); + switch (cmd()) { + case AltosLib.AO_LOG_FLIGHT: + state.set_state(AltosLib.ao_flight_pad); + state.set_flight(value); + state.set_boost_tick(0); + break; + case AltosLib.AO_LOG_STATE: + state.set_state(value & 0x7fff); + break; + case AltosLib.AO_LOG_SENSOR: + state.set_pressure(AltosConvert.barometer_to_pressure(value)); + break; + } + } + + public AltosEepromRecord next() { + if (start + record_length * 2 < eeprom.data.size()) + return new AltosEepromRecordTiny(eeprom, start + record_length); + return null; + } + + public AltosEepromRecordTiny(AltosEepromNew eeprom, int start) { + super(eeprom, start, record_length); + } + + public AltosEepromRecordTiny(AltosEepromNew eeprom) { + this(eeprom, 0); + } +} diff --git a/altoslib/Makefile.am b/altoslib/Makefile.am index 26159421..7c55a421 100644 --- a/altoslib/Makefile.am +++ b/altoslib/Makefile.am @@ -34,6 +34,16 @@ altoslib_JAVA = \ AltosCRCException.java \ AltosCSV.java \ AltosDebug.java \ + AltosEepromNew.java \ + AltosEepromRecord.java \ + AltosEepromRecordFull.java \ + AltosEepromRecordTiny.java \ + AltosEepromRecordMega.java \ + AltosEepromRecordMetrum.java \ + AltosEepromRecordMini.java \ + AltosEepromRecordGps.java \ + AltosEepromRecordFireTwo.java \ + AltosEepromRecordSet.java \ AltosEeprom.java \ AltosEepromChunk.java \ AltosEepromDownload.java \