X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=altoslib%2FAltosGPS.java;h=371fd7bf1418f638f9d0494c393df46e30e1692e;hp=eb384e4d964d5adbc0e11dc68658d0bdc3492fe7;hb=b1a90adac9f6e2a609ce1ccd6749462bb5c9adbe;hpb=de8d9c5630ae46378c50faf97f7d2e97fe139e30 diff --git a/altoslib/AltosGPS.java b/altoslib/AltosGPS.java index eb384e4d..371fd7bf 100644 --- a/altoslib/AltosGPS.java +++ b/altoslib/AltosGPS.java @@ -15,13 +15,15 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. */ -package org.altusmetrum.altoslib_1; +package org.altusmetrum.altoslib_11; import java.text.*; +import java.util.concurrent.*; +import java.io.*; -public class AltosGPS implements Cloneable { +public class AltosGPS implements Cloneable, AltosHashable { - public final static int MISSING = AltosRecord.MISSING; + public final static int MISSING = AltosLib.MISSING; public int nsat; public boolean locked; @@ -39,10 +41,11 @@ public class AltosGPS implements Cloneable { public double ground_speed; /* m/s */ public int course; /* degrees */ public double climb_rate; /* m/s */ + public double pdop; /* unitless */ public double hdop; /* unitless */ public double vdop; /* unitless */ - public int h_error; /* m */ - public int v_error; /* m */ + public double h_error; /* m */ + public double v_error; /* m */ public AltosGPSSat[] cc_gps_sat; /* tracking data */ @@ -65,40 +68,41 @@ public class AltosGPS implements Cloneable { } public void ClearGPSTime() { - year = month = day = 0; - hour = minute = second = 0; + year = month = day = AltosLib.MISSING; + hour = minute = second = AltosLib.MISSING; } 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); - - 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); - - ground_speed = map.get_double(AltosTelemetry.AO_TELEM_GPS_HORIZONTAL_SPEED, - AltosRecord.MISSING, 1/100.0); - course = map.get_int(AltosTelemetry.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)) { + month = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_MONTH, MISSING); + day = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_DAY, MISSING); + + 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(AltosTelemetryLegacy.AO_TELEM_GPS_HORIZONTAL_SPEED, + AltosLib.MISSING, 1/100.0); + course = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_COURSE, + AltosLib.MISSING); + pdop = map.get_double(AltosTelemetryLegacy.AO_TELEM_GPS_PDOP, MISSING, 1.0); + 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 { @@ -107,6 +111,62 @@ public class AltosGPS implements Cloneable { } } + public boolean parse_string (String line, boolean says_done) { + String[] bits = line.split("\\s+"); + if (bits.length == 0) + return false; + if (line.startsWith("Date:")) { + if (bits.length < 2) + return false; + String[] d = bits[1].split("/"); + if (d.length < 3) + return false; + year = Integer.parseInt(d[0]) + 2000; + month = Integer.parseInt(d[1]); + day = Integer.parseInt(d[2]); + } else if (line.startsWith("Time:")) { + if (bits.length < 2) + return false; + String[] d = bits[1].split(":"); + if (d.length < 3) + return false; + hour = Integer.parseInt(d[0]); + minute = Integer.parseInt(d[1]); + second = Integer.parseInt(d[2]); + } else if (line.startsWith("Lat/Lon:")) { + if (bits.length < 3) + return false; + lat = Integer.parseInt(bits[1]) * 1.0e-7; + lon = Integer.parseInt(bits[2]) * 1.0e-7; + } else if (line.startsWith("Alt:")) { + if (bits.length < 2) + return false; + alt = Integer.parseInt(bits[1]); + } else if (line.startsWith("Flags:")) { + if (bits.length < 2) + return false; + int status = Integer.decode(bits[1]); + connected = (status & AltosLib.AO_GPS_RUNNING) != 0; + locked = (status & AltosLib.AO_GPS_VALID) != 0; + if (!says_done) + return false; + } else if (line.startsWith("Sats:")) { + if (bits.length < 2) + return false; + nsat = Integer.parseInt(bits[1]); + cc_gps_sat = new AltosGPSSat[nsat]; + for (int i = 0; i < nsat; i++) { + int svid = Integer.parseInt(bits[2+i*2]); + int cc_n0 = Integer.parseInt(bits[3+i*2]); + cc_gps_sat[i] = new AltosGPSSat(svid, cc_n0); + } + } else if (line.startsWith("done")) { + return false; + } else + return false; + return true; + } + public AltosGPS(String[] words, int i, int version) throws ParseException { AltosParse.word(words[i++], "GPS"); nsat = AltosParse.parse_int(words[i++]); @@ -135,10 +195,10 @@ public class AltosGPS implements Cloneable { lon = AltosParse.parse_coord(words[i++]); alt = AltosParse.parse_int(words[i++]); if (version > 1 || (i < words.length && !words[i].equals("SAT"))) { - ground_speed = AltosParse.parse_double(AltosParse.strip_suffix(words[i++], "m/s(H)")); + ground_speed = AltosParse.parse_double_net(AltosParse.strip_suffix(words[i++], "m/s(H)")); course = AltosParse.parse_int(words[i++]); - climb_rate = AltosParse.parse_double(AltosParse.strip_suffix(words[i++], "m/s(V)")); - hdop = AltosParse.parse_double(AltosParse.strip_suffix(words[i++], "(hdop)")); + climb_rate = AltosParse.parse_double_net(AltosParse.strip_suffix(words[i++], "m/s(V)")); + hdop = AltosParse.parse_double_net(AltosParse.strip_suffix(words[i++], "(hdop)")); h_error = AltosParse.parse_int(words[i++]); v_error = AltosParse.parse_int(words[i++]); } @@ -211,11 +271,26 @@ public class AltosGPS implements Cloneable { cc_gps_sat[cc_gps_sat.length - 1] = sat; } - public AltosGPS() { + private void init() { + lat = AltosLib.MISSING; + lon = AltosLib.MISSING; + alt = AltosLib.MISSING; + ground_speed = AltosLib.MISSING; + course = AltosLib.MISSING; + climb_rate = AltosLib.MISSING; + pdop = AltosLib.MISSING; + hdop = AltosLib.MISSING; + vdop = AltosLib.MISSING; + h_error = AltosLib.MISSING; + v_error = AltosLib.MISSING; ClearGPSTime(); cc_gps_sat = null; } + public AltosGPS() { + init(); + } + public AltosGPS clone() { AltosGPS g = new AltosGPS(); @@ -235,7 +310,9 @@ public class AltosGPS implements Cloneable { g.ground_speed = ground_speed; /* m/s */ g.course = course; /* degrees */ g.climb_rate = climb_rate; /* m/s */ - g.hdop = hdop; /* unitless? */ + g.pdop = pdop; /* unitless */ + g.hdop = hdop; /* unitless */ + g.vdop = vdop; /* unitless */ g.h_error = h_error; /* m */ g.v_error = v_error; /* m */ @@ -267,9 +344,11 @@ public class AltosGPS implements Cloneable { ground_speed = old.ground_speed; /* m/s */ course = old.course; /* degrees */ climb_rate = old.climb_rate; /* m/s */ + pdop = old.pdop; /* unitless? */ hdop = old.hdop; /* unitless? */ - h_error = old.h_error; /* m */ - v_error = old.v_error; /* m */ + vdop = old.vdop; /* unitless? */ + h_error = old.h_error; /* m */ + v_error = old.v_error; /* m */ if (old.cc_gps_sat != null) { cc_gps_sat = new AltosGPSSat[old.cc_gps_sat.length]; @@ -280,8 +359,94 @@ public class AltosGPS implements Cloneable { } } } else { - ClearGPSTime(); - cc_gps_sat = null; + init(); } } + + static public void update_state(AltosState state, AltosLink link, AltosConfigData config_data) throws InterruptedException { + try { + AltosGPS gps = new AltosGPS(link, config_data); + + if (gps != null) { + state.set_gps(gps, state.gps_sequence++); + return; + } + } catch (TimeoutException te) { + } + state.set_gps(null, 0); + } + + public AltosGPS (AltosLink link, AltosConfigData config_data) throws TimeoutException, InterruptedException { + boolean says_done = config_data.compare_version("1.0") >= 0; + init(); + link.printf("g\n"); + for (;;) { + String line = link.get_reply_no_dialog(5000); + if (line == null) + throw new TimeoutException(); + if (!parse_string(line, says_done)) + break; + } + } + + public AltosHashSet hashSet() { + AltosHashSet h = new AltosHashSet(); + + h.putInt("nsat", nsat); + h.putBoolean("locked", locked); + h.putBoolean("connected", connected); + h.putDouble("lat", lat); + h.putDouble("lon", lon); + h.putDouble("alt", alt); + h.putInt("year", year); + h.putInt("month", month); + h.putInt("day", day); + h.putInt("hour", hour); + h.putInt("minute", minute); + h.putInt("second", second); + + h.putDouble("ground_speed", ground_speed); + h.putInt("course", course); + h.putDouble("climb_rate", climb_rate); + h.putDouble("pdop", pdop); + h.putDouble("hdop", hdop); + h.putDouble("vdop", vdop); + h.putDouble("h_error", h_error); + h.putDouble("v_error", v_error); + h.putString("cc_gps_sat", AltosGPSSat.toString(cc_gps_sat)); + return h; + } + + public AltosGPS(AltosHashSet h) { + init(); + nsat = h.getInt("nsat", nsat); + locked = h.getBoolean("locked", locked); + connected = h.getBoolean("connected", connected); + lat = h.getDouble("lat", lat); + lon = h.getDouble("lon", lon); + alt = h.getDouble("alt", alt); + year = h.getInt("year", year); + month = h.getInt("month", month); + day = h.getInt("day", day); + hour = h.getInt("hour", hour); + minute = h.getInt("minute", minute); + second = h.getInt("second", second); + + ground_speed = h.getDouble("ground_speed", ground_speed); + course = h.getInt("course", course); + climb_rate = h.getDouble("climb_rate", climb_rate); + pdop = h.getDouble("pdop", pdop); + hdop = h.getDouble("hdop", hdop); + vdop = h.getDouble("vdop", vdop); + h_error = h.getDouble("h_error", h_error); + v_error = h.getDouble("v_error", v_error); + cc_gps_sat = AltosGPSSat.array(h.getString("cc_gps_sat", null)); + } + + public static AltosGPS fromHashSet(AltosHashSet h, AltosGPS def) { + if (h == null) + return def; + + return new AltosGPS(h); + } }