2 * Copyright © 2010 Keith Packard <keithp@keithp.com>
4 * This program is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation; either version 2 of the License, or
7 * (at your option) any later version.
9 * This program is distributed in the hope that it will be useful, but
10 * WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
12 * General Public License for more details.
14 * You should have received a copy of the GNU General Public License along
15 * with this program; if not, write to the Free Software Foundation, Inc.,
16 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
19 package org.altusmetrum.altoslib_14;
22 import java.util.concurrent.*;
26 public class AltosGPS implements Cloneable {
28 public final static int MISSING = AltosLib.MISSING;
31 public boolean locked;
32 public boolean connected;
33 public double lat; /* degrees (+N -S) */
34 public double lon; /* degrees (+E -W) */
35 public double alt; /* m */
43 public double ground_speed; /* m/s */
44 public int course; /* degrees */
45 public double climb_rate; /* m/s */
46 public double pdop; /* unitless */
47 public double hdop; /* unitless */
48 public double vdop; /* unitless */
49 public double h_error; /* m */
50 public double v_error; /* m */
52 public AltosGPSSat[] cc_gps_sat; /* tracking data */
54 public void ParseGPSDate(String date) throws ParseException {
55 String[] ymd = date.split("-");
57 throw new ParseException("error parsing GPS date " + date + " got " + ymd.length, 0);
58 year = AltosParse.parse_int(ymd[0]);
59 month = AltosParse.parse_int(ymd[1]);
60 day = AltosParse.parse_int(ymd[2]);
63 public void ParseGPSTime(String time) throws ParseException {
64 String[] hms = time.split(":");
66 throw new ParseException("Error parsing GPS time " + time + " got " + hms.length, 0);
67 hour = AltosParse.parse_int(hms[0]);
68 minute = AltosParse.parse_int(hms[1]);
69 second = AltosParse.parse_int(hms[2]);
72 public void ClearGPSTime() {
73 year = month = day = AltosLib.MISSING;
74 hour = minute = second = AltosLib.MISSING;
77 /* Return time since epoc in seconds */
78 public long seconds() {
79 if (year == AltosLib.MISSING)
80 return AltosLib.MISSING;
81 if (month == AltosLib.MISSING)
82 return AltosLib.MISSING;
83 if (day == AltosLib.MISSING)
84 return AltosLib.MISSING;
85 if (hour == AltosLib.MISSING)
86 return AltosLib.MISSING;
87 if (minute == AltosLib.MISSING)
88 return AltosLib.MISSING;
89 if (second == AltosLib.MISSING)
90 return AltosLib.MISSING;
91 OffsetDateTime odt = OffsetDateTime.of(year, month, day, hour, minute, second, 0, ZoneOffset.UTC);
92 return odt.toEpochSecond();
95 public AltosLatLon lat_lon() {
96 return new AltosLatLon(lat, lon);
99 public AltosGPS(AltosTelemetryMap map) throws ParseException {
100 String state = map.get_string(AltosTelemetryLegacy.AO_TELEM_GPS_STATE,
101 AltosTelemetryLegacy.AO_TELEM_GPS_STATE_ERROR);
103 nsat = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_NUM_SAT, 0);
104 if (state.equals(AltosTelemetryLegacy.AO_TELEM_GPS_STATE_LOCKED)) {
107 lat = map.get_double(AltosTelemetryLegacy.AO_TELEM_GPS_LATITUDE, MISSING, 1.0e-7);
108 lon = map.get_double(AltosTelemetryLegacy.AO_TELEM_GPS_LONGITUDE, MISSING, 1.0e-7);
109 alt = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_ALTITUDE, MISSING);
110 year = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_YEAR, MISSING);
113 month = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_MONTH, MISSING);
114 day = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_DAY, MISSING);
116 hour = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_HOUR, 0);
117 minute = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_MINUTE, 0);
118 second = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_SECOND, 0);
120 ground_speed = map.get_double(AltosTelemetryLegacy.AO_TELEM_GPS_HORIZONTAL_SPEED,
121 AltosLib.MISSING, 1/100.0);
122 course = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_COURSE,
124 pdop = map.get_double(AltosTelemetryLegacy.AO_TELEM_GPS_PDOP, MISSING, 1.0);
125 hdop = map.get_double(AltosTelemetryLegacy.AO_TELEM_GPS_HDOP, MISSING, 1.0);
126 vdop = map.get_double(AltosTelemetryLegacy.AO_TELEM_GPS_VDOP, MISSING, 1.0);
127 h_error = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_HERROR, MISSING);
128 v_error = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_VERROR, MISSING);
129 } else if (state.equals(AltosTelemetryLegacy.AO_TELEM_GPS_STATE_UNLOCKED)) {
138 public boolean parse_string (String line, boolean says_done) {
139 String[] bits = line.split("\\s+");
140 if (bits.length == 0)
142 if (line.startsWith("Date:")) {
145 String[] d = bits[1].split("/");
148 year = Integer.parseInt(d[0]) + 2000;
149 month = Integer.parseInt(d[1]);
150 day = Integer.parseInt(d[2]);
151 } else if (line.startsWith("Time:")) {
154 String[] d = bits[1].split(":");
157 hour = Integer.parseInt(d[0]);
158 minute = Integer.parseInt(d[1]);
159 second = Integer.parseInt(d[2]);
160 } else if (line.startsWith("Lat/Lon:")) {
163 lat = Integer.parseInt(bits[1]) * 1.0e-7;
164 lon = Integer.parseInt(bits[2]) * 1.0e-7;
165 } else if (line.startsWith("Alt:")) {
168 alt = Integer.parseInt(bits[1]);
169 } else if (line.startsWith("Pdop/Hdop/Vdop:")) {
172 pdop = Integer.parseInt(bits[1]) / 10.0;
173 hdop = Integer.parseInt(bits[2]) / 10.0;
174 vdop = Integer.parseInt(bits[3]) / 10.0;
175 } else if (line.startsWith("Ground Speed/Climb Rate/Course:")) {
178 ground_speed = Integer.parseInt(bits[3]) * 1.0e-2;
179 climb_rate = Integer.parseInt(bits[4]) * 1.0e-2;
180 course = Integer.parseInt(bits[5]) * 2;
181 } else if (line.startsWith("Flags:")) {
184 int status = Integer.decode(bits[1]);
185 connected = (status & AltosLib.AO_GPS_RUNNING) != 0;
186 locked = (status & AltosLib.AO_GPS_VALID) != 0;
187 nsat = (status >> AltosLib.AO_GPS_NUM_SAT_SHIFT) & AltosLib.AO_GPS_NUM_SAT_MASK;
190 } else if (line.startsWith("Sats:")) {
193 int nsvs = Integer.parseInt(bits[1]);
194 cc_gps_sat = new AltosGPSSat[nsvs];
195 for (int i = 0; i < nsvs; i++) {
196 int svid = Integer.parseInt(bits[2+i*2]);
197 int cc_n0 = Integer.parseInt(bits[3+i*2]);
198 cc_gps_sat[i] = new AltosGPSSat(svid, cc_n0);
200 } else if (line.startsWith("done")) {
207 public AltosGPS(String[] words, int i, int version) throws ParseException {
208 AltosParse.word(words[i++], "GPS");
209 nsat = AltosParse.parse_int(words[i++]);
210 AltosParse.word(words[i++], "sat");
217 if ((words[i]).equals("unlocked")) {
220 } else if ((words[i]).equals("not-connected")) {
222 } else if (words.length >= 40) {
227 ParseGPSDate(words[i++]);
229 year = month = day = 0;
230 ParseGPSTime(words[i++]);
231 lat = AltosParse.parse_coord(words[i++]);
232 lon = AltosParse.parse_coord(words[i++]);
233 alt = AltosParse.parse_int(words[i++]);
234 if (version > 1 || (i < words.length && !words[i].equals("SAT"))) {
235 ground_speed = AltosParse.parse_double_net(AltosParse.strip_suffix(words[i++], "m/s(H)"));
236 course = AltosParse.parse_int(words[i++]);
237 climb_rate = AltosParse.parse_double_net(AltosParse.strip_suffix(words[i++], "m/s(V)"));
238 hdop = AltosParse.parse_double_net(AltosParse.strip_suffix(words[i++], "(hdop)"));
239 h_error = AltosParse.parse_int(words[i++]);
240 v_error = AltosParse.parse_int(words[i++]);
245 if (i < words.length) {
246 AltosParse.word(words[i++], "SAT");
247 int tracking_channels = 0;
248 if (words[i].equals("not-connected"))
249 tracking_channels = 0;
251 tracking_channels = AltosParse.parse_int(words[i]);
253 cc_gps_sat = new AltosGPSSat[tracking_channels];
254 for (int chan = 0; chan < tracking_channels; chan++) {
255 cc_gps_sat[chan] = new AltosGPSSat();
256 cc_gps_sat[chan].svid = AltosParse.parse_int(words[i++]);
257 /* Older versions included SiRF status bits */
260 cc_gps_sat[chan].c_n0 = AltosParse.parse_int(words[i++]);
263 cc_gps_sat = new AltosGPSSat[0];
266 public void set_latitude(int in_lat) {
267 lat = in_lat / 10.0e7;
270 public void set_longitude(int in_lon) {
271 lon = in_lon / 10.0e7;
274 public void set_time(int in_hour, int in_minute, int in_second) {
280 public void set_date(int in_year, int in_month, int in_day) {
287 public void set_flags(int in_flags) {
292 public void set_altitude(int in_altitude) {
296 public void add_sat(int svid, int c_n0) {
297 if (cc_gps_sat == null) {
298 cc_gps_sat = new AltosGPSSat[1];
300 AltosGPSSat[] new_gps_sat = new AltosGPSSat[cc_gps_sat.length + 1];
301 for (int i = 0; i < cc_gps_sat.length; i++)
302 new_gps_sat[i] = cc_gps_sat[i];
303 cc_gps_sat = new_gps_sat;
305 AltosGPSSat sat = new AltosGPSSat();
308 cc_gps_sat[cc_gps_sat.length - 1] = sat;
311 private void init() {
312 lat = AltosLib.MISSING;
313 lon = AltosLib.MISSING;
314 alt = AltosLib.MISSING;
315 ground_speed = AltosLib.MISSING;
316 course = AltosLib.MISSING;
317 climb_rate = AltosLib.MISSING;
318 pdop = AltosLib.MISSING;
319 hdop = AltosLib.MISSING;
320 vdop = AltosLib.MISSING;
321 h_error = AltosLib.MISSING;
322 v_error = AltosLib.MISSING;
331 public AltosGPS clone() {
332 AltosGPS g = new AltosGPS();
336 g.connected = connected;
337 g.lat = lat; /* degrees (+N -S) */
338 g.lon = lon; /* degrees (+E -W) */
347 g.ground_speed = ground_speed; /* m/s */
348 g.course = course; /* degrees */
349 g.climb_rate = climb_rate; /* m/s */
350 g.pdop = pdop; /* unitless */
351 g.hdop = hdop; /* unitless */
352 g.vdop = vdop; /* unitless */
353 g.h_error = h_error; /* m */
354 g.v_error = v_error; /* m */
356 if (cc_gps_sat != null) {
357 g.cc_gps_sat = new AltosGPSSat[cc_gps_sat.length];
358 for (int i = 0; i < cc_gps_sat.length; i++) {
359 g.cc_gps_sat[i] = new AltosGPSSat(cc_gps_sat[i].svid,
366 public AltosGPS(AltosGPS old) {
370 connected = old.connected;
371 lat = old.lat; /* degrees (+N -S) */
372 lon = old.lon; /* degrees (+E -W) */
373 alt = old.alt; /* m */
381 ground_speed = old.ground_speed; /* m/s */
382 course = old.course; /* degrees */
383 climb_rate = old.climb_rate; /* m/s */
384 pdop = old.pdop; /* unitless? */
385 hdop = old.hdop; /* unitless? */
386 vdop = old.vdop; /* unitless? */
387 h_error = old.h_error; /* m */
388 v_error = old.v_error; /* m */
390 if (old.cc_gps_sat != null) {
391 cc_gps_sat = new AltosGPSSat[old.cc_gps_sat.length];
392 for (int i = 0; i < old.cc_gps_sat.length; i++) {
393 cc_gps_sat[i] = new AltosGPSSat();
394 cc_gps_sat[i].svid = old.cc_gps_sat[i].svid;
395 cc_gps_sat[i].c_n0 = old.cc_gps_sat[i].c_n0;
403 static public void provide_data(AltosDataListener listener, AltosLink link) throws InterruptedException {
405 AltosGPS gps = new AltosGPS(link, link.config_data());
407 listener.set_gps(gps, true, true);
408 } catch (TimeoutException te) {
412 public AltosGPS (AltosLink link, AltosConfigData config_data) throws TimeoutException, InterruptedException {
413 boolean says_done = config_data.compare_version("1.0") >= 0;
417 String line = link.get_reply_no_dialog(5000);
419 throw new TimeoutException();
420 if (!parse_string(line, says_done))