From fcea12ac416b1eab11e9e8aae801358574308f73 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 7 Jun 2014 11:46:32 -0700 Subject: [PATCH] altoslib: Add TeleGPS log parsing code Signed-off-by: Keith Packard --- altoslib/AltosEepromChunk.java | 5 +- altoslib/AltosEepromFile.java | 3 + altoslib/AltosEepromGPS.java | 159 +++++++++++++++++++++++++++++++++ altoslib/AltosLib.java | 1 + altoslib/Makefile.am | 1 + 5 files changed, 168 insertions(+), 1 deletion(-) create mode 100644 altoslib/AltosEepromGPS.java diff --git a/altoslib/AltosEepromChunk.java b/altoslib/AltosEepromChunk.java index 32de96bc..91eebc5a 100644 --- a/altoslib/AltosEepromChunk.java +++ b/altoslib/AltosEepromChunk.java @@ -84,6 +84,9 @@ public class AltosEepromChunk { case AltosLib.AO_LOG_FORMAT_EASYMINI: eeprom = new AltosEepromMini(this, offset); break; + case AltosLib.AO_LOG_FORMAT_TELEGPS: + eeprom = new AltosEepromGPS(this, offset); + break; default: throw new ParseException("unknown eeprom format " + log_format, 0); } @@ -125,4 +128,4 @@ public class AltosEepromChunk { } } } -} \ No newline at end of file +} diff --git a/altoslib/AltosEepromFile.java b/altoslib/AltosEepromFile.java index 1664dc95..f59585f8 100644 --- a/altoslib/AltosEepromFile.java +++ b/altoslib/AltosEepromFile.java @@ -102,6 +102,9 @@ public class AltosEepromFile extends AltosStateIterable { case AltosLib.AO_LOG_FORMAT_EASYMINI: body = new AltosEepromIterable(AltosEepromMini.read(input)); break; + case AltosLib.AO_LOG_FORMAT_TELEGPS: + body = new AltosEepromIterable(AltosEepromGPS.read(input)); + break; default: body = new AltosEepromIterable(new LinkedList()); break; diff --git a/altoslib/AltosEepromGPS.java b/altoslib/AltosEepromGPS.java new file mode 100644 index 00000000..33172eed --- /dev/null +++ b/altoslib/AltosEepromGPS.java @@ -0,0 +1,159 @@ +/* + * Copyright © 2014 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; 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_4; + +import java.io.*; +import java.util.*; +import java.text.*; + +public class AltosEepromGPS extends AltosEeprom { + public static final int record_length = 32; + + public static final int max_sat = 12; + + public int record_length() { return record_length; } + + /* 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() { 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 state() { return data8(26); } + + /* AO_LOG_GPS_SAT elements */ + public int nsat() { return data16(0); } + public int svid(int n) { return data8(2 + n * 2); } + public int c_n(int n) { return data8(2 + n * 2 + 1); } + + public AltosEepromGPS (AltosEepromChunk chunk, int start) throws ParseException { + parse_chunk(chunk, start); + } + + 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_boost_tick(tick); + state.set_flight(flight()); + /* no place to log start lat/lon yet */ + break; + case AltosLib.AO_LOG_GPS_TIME: + state.set_tick(tick); + state.set_state(state()); + gps = state.make_temp_gps(false); + gps.lat = latitude() / 1e7; + gps.lon = longitude() / 1e7; + gps.alt = altitude(); + + 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; + gps.hdop = hdop(); + gps.vdop = vdop(); + break; + case AltosLib.AO_LOG_GPS_SAT: + state.set_tick(tick); + 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 AltosEepromGPS (String line) { + parse_string(line); + } + + static public LinkedList read(FileInputStream input) { + LinkedList tgpss = new LinkedList(); + + for (;;) { + try { + String line = AltosLib.gets(input); + if (line == null) + break; + try { + AltosEepromGPS tgps = new AltosEepromGPS(line); + if (tgps.cmd != AltosLib.AO_LOG_INVALID) + tgpss.add(tgps); + } catch (Exception e) { + System.out.printf ("exception\n"); + } + } catch (IOException ie) { + break; + } + } + + return tgpss; + } +} diff --git a/altoslib/AltosLib.java b/altoslib/AltosLib.java index 3aef077a..7080ebfe 100644 --- a/altoslib/AltosLib.java +++ b/altoslib/AltosLib.java @@ -265,6 +265,7 @@ public class AltosLib { 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_TELEGPS = 9; public static final int AO_LOG_FORMAT_NONE = 127; public static boolean isspace(int c) { diff --git a/altoslib/Makefile.am b/altoslib/Makefile.am index 0f1d7a47..1086f858 100644 --- a/altoslib/Makefile.am +++ b/altoslib/Makefile.am @@ -47,6 +47,7 @@ altoslib_JAVA = \ AltosEepromMega.java \ AltosEepromMetrum2.java \ AltosEepromMini.java \ + AltosEepromGPS.java \ AltosEepromMonitor.java \ AltosFile.java \ AltosFlash.java \ -- 2.30.2