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; version 2 of the License.
8 * This program is distributed in the hope that it will be useful, but
9 * WITHOUT ANY WARRANTY; without even the implied warranty of
10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
11 * General Public License for more details.
13 * You should have received a copy of the GNU General Public License along
14 * with this program; if not, write to the Free Software Foundation, Inc.,
15 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
18 package org.altusmetrum.altoslib_10;
21 import java.util.concurrent.*;
24 public class AltosGPS implements Cloneable, Serializable {
26 public final static int MISSING = AltosLib.MISSING;
29 public boolean locked;
30 public boolean connected;
31 public double lat; /* degrees (+N -S) */
32 public double lon; /* degrees (+E -W) */
33 public double alt; /* m */
41 public double ground_speed; /* m/s */
42 public int course; /* degrees */
43 public double climb_rate; /* m/s */
44 public double pdop; /* unitless */
45 public double hdop; /* unitless */
46 public double vdop; /* unitless */
47 public double h_error; /* m */
48 public double v_error; /* m */
50 public AltosGPSSat[] cc_gps_sat; /* tracking data */
52 public void ParseGPSDate(String date) throws ParseException {
53 String[] ymd = date.split("-");
55 throw new ParseException("error parsing GPS date " + date + " got " + ymd.length, 0);
56 year = AltosParse.parse_int(ymd[0]);
57 month = AltosParse.parse_int(ymd[1]);
58 day = AltosParse.parse_int(ymd[2]);
61 public void ParseGPSTime(String time) throws ParseException {
62 String[] hms = time.split(":");
64 throw new ParseException("Error parsing GPS time " + time + " got " + hms.length, 0);
65 hour = AltosParse.parse_int(hms[0]);
66 minute = AltosParse.parse_int(hms[1]);
67 second = AltosParse.parse_int(hms[2]);
70 public void ClearGPSTime() {
71 year = month = day = AltosLib.MISSING;
72 hour = minute = second = AltosLib.MISSING;
75 public AltosGPS(AltosTelemetryMap map) throws ParseException {
76 String state = map.get_string(AltosTelemetryLegacy.AO_TELEM_GPS_STATE,
77 AltosTelemetryLegacy.AO_TELEM_GPS_STATE_ERROR);
79 nsat = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_NUM_SAT, 0);
80 if (state.equals(AltosTelemetryLegacy.AO_TELEM_GPS_STATE_LOCKED)) {
83 lat = map.get_double(AltosTelemetryLegacy.AO_TELEM_GPS_LATITUDE, MISSING, 1.0e-7);
84 lon = map.get_double(AltosTelemetryLegacy.AO_TELEM_GPS_LONGITUDE, MISSING, 1.0e-7);
85 alt = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_ALTITUDE, MISSING);
86 year = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_YEAR, MISSING);
89 month = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_MONTH, MISSING);
90 day = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_DAY, MISSING);
92 hour = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_HOUR, 0);
93 minute = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_MINUTE, 0);
94 second = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_SECOND, 0);
96 ground_speed = map.get_double(AltosTelemetryLegacy.AO_TELEM_GPS_HORIZONTAL_SPEED,
97 AltosLib.MISSING, 1/100.0);
98 course = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_COURSE,
100 pdop = map.get_double(AltosTelemetryLegacy.AO_TELEM_GPS_PDOP, MISSING, 1.0);
101 hdop = map.get_double(AltosTelemetryLegacy.AO_TELEM_GPS_HDOP, MISSING, 1.0);
102 vdop = map.get_double(AltosTelemetryLegacy.AO_TELEM_GPS_VDOP, MISSING, 1.0);
103 h_error = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_HERROR, MISSING);
104 v_error = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_VERROR, MISSING);
105 } else if (state.equals(AltosTelemetryLegacy.AO_TELEM_GPS_STATE_UNLOCKED)) {
114 public boolean parse_string (String line, boolean says_done) {
115 String[] bits = line.split("\\s+");
116 if (bits.length == 0)
118 if (line.startsWith("Date:")) {
121 String[] d = bits[1].split("/");
124 year = Integer.parseInt(d[0]) + 2000;
125 month = Integer.parseInt(d[1]);
126 day = Integer.parseInt(d[2]);
127 } else if (line.startsWith("Time:")) {
130 String[] d = bits[1].split(":");
133 hour = Integer.parseInt(d[0]);
134 minute = Integer.parseInt(d[1]);
135 second = Integer.parseInt(d[2]);
136 } else if (line.startsWith("Lat/Lon:")) {
139 lat = Integer.parseInt(bits[1]) * 1.0e-7;
140 lon = Integer.parseInt(bits[2]) * 1.0e-7;
141 } else if (line.startsWith("Alt:")) {
144 alt = Integer.parseInt(bits[1]);
145 } else if (line.startsWith("Flags:")) {
148 int status = Integer.decode(bits[1]);
149 connected = (status & AltosLib.AO_GPS_RUNNING) != 0;
150 locked = (status & AltosLib.AO_GPS_VALID) != 0;
153 } else if (line.startsWith("Sats:")) {
156 nsat = Integer.parseInt(bits[1]);
157 cc_gps_sat = new AltosGPSSat[nsat];
158 for (int i = 0; i < nsat; i++) {
159 int svid = Integer.parseInt(bits[2+i*2]);
160 int cc_n0 = Integer.parseInt(bits[3+i*2]);
161 cc_gps_sat[i] = new AltosGPSSat(svid, cc_n0);
163 } else if (line.startsWith("done")) {
170 public AltosGPS(String[] words, int i, int version) throws ParseException {
171 AltosParse.word(words[i++], "GPS");
172 nsat = AltosParse.parse_int(words[i++]);
173 AltosParse.word(words[i++], "sat");
180 if ((words[i]).equals("unlocked")) {
183 } else if ((words[i]).equals("not-connected")) {
185 } else if (words.length >= 40) {
190 ParseGPSDate(words[i++]);
192 year = month = day = 0;
193 ParseGPSTime(words[i++]);
194 lat = AltosParse.parse_coord(words[i++]);
195 lon = AltosParse.parse_coord(words[i++]);
196 alt = AltosParse.parse_int(words[i++]);
197 if (version > 1 || (i < words.length && !words[i].equals("SAT"))) {
198 ground_speed = AltosParse.parse_double_net(AltosParse.strip_suffix(words[i++], "m/s(H)"));
199 course = AltosParse.parse_int(words[i++]);
200 climb_rate = AltosParse.parse_double_net(AltosParse.strip_suffix(words[i++], "m/s(V)"));
201 hdop = AltosParse.parse_double_net(AltosParse.strip_suffix(words[i++], "(hdop)"));
202 h_error = AltosParse.parse_int(words[i++]);
203 v_error = AltosParse.parse_int(words[i++]);
208 if (i < words.length) {
209 AltosParse.word(words[i++], "SAT");
210 int tracking_channels = 0;
211 if (words[i].equals("not-connected"))
212 tracking_channels = 0;
214 tracking_channels = AltosParse.parse_int(words[i]);
216 cc_gps_sat = new AltosGPSSat[tracking_channels];
217 for (int chan = 0; chan < tracking_channels; chan++) {
218 cc_gps_sat[chan] = new AltosGPSSat();
219 cc_gps_sat[chan].svid = AltosParse.parse_int(words[i++]);
220 /* Older versions included SiRF status bits */
223 cc_gps_sat[chan].c_n0 = AltosParse.parse_int(words[i++]);
226 cc_gps_sat = new AltosGPSSat[0];
229 public void set_latitude(int in_lat) {
230 lat = in_lat / 10.0e7;
233 public void set_longitude(int in_lon) {
234 lon = in_lon / 10.0e7;
237 public void set_time(int in_hour, int in_minute, int in_second) {
243 public void set_date(int in_year, int in_month, int in_day) {
250 public void set_flags(int in_flags) {
255 public void set_altitude(int in_altitude) {
259 public void add_sat(int svid, int c_n0) {
260 if (cc_gps_sat == null) {
261 cc_gps_sat = new AltosGPSSat[1];
263 AltosGPSSat[] new_gps_sat = new AltosGPSSat[cc_gps_sat.length + 1];
264 for (int i = 0; i < cc_gps_sat.length; i++)
265 new_gps_sat[i] = cc_gps_sat[i];
266 cc_gps_sat = new_gps_sat;
268 AltosGPSSat sat = new AltosGPSSat();
271 cc_gps_sat[cc_gps_sat.length - 1] = sat;
274 private void init() {
275 lat = AltosLib.MISSING;
276 lon = AltosLib.MISSING;
277 alt = AltosLib.MISSING;
278 ground_speed = AltosLib.MISSING;
279 course = AltosLib.MISSING;
280 climb_rate = AltosLib.MISSING;
281 pdop = AltosLib.MISSING;
282 hdop = AltosLib.MISSING;
283 vdop = AltosLib.MISSING;
284 h_error = AltosLib.MISSING;
285 v_error = AltosLib.MISSING;
294 public AltosGPS clone() {
295 AltosGPS g = new AltosGPS();
299 g.connected = connected;
300 g.lat = lat; /* degrees (+N -S) */
301 g.lon = lon; /* degrees (+E -W) */
310 g.ground_speed = ground_speed; /* m/s */
311 g.course = course; /* degrees */
312 g.climb_rate = climb_rate; /* m/s */
313 g.pdop = pdop; /* unitless */
314 g.hdop = hdop; /* unitless */
315 g.vdop = vdop; /* unitless */
316 g.h_error = h_error; /* m */
317 g.v_error = v_error; /* m */
319 if (cc_gps_sat != null) {
320 g.cc_gps_sat = new AltosGPSSat[cc_gps_sat.length];
321 for (int i = 0; i < cc_gps_sat.length; i++) {
322 g.cc_gps_sat[i] = new AltosGPSSat(cc_gps_sat[i].svid,
329 public AltosGPS(AltosGPS old) {
333 connected = old.connected;
334 lat = old.lat; /* degrees (+N -S) */
335 lon = old.lon; /* degrees (+E -W) */
336 alt = old.alt; /* m */
344 ground_speed = old.ground_speed; /* m/s */
345 course = old.course; /* degrees */
346 climb_rate = old.climb_rate; /* m/s */
347 pdop = old.pdop; /* unitless? */
348 hdop = old.hdop; /* unitless? */
349 vdop = old.vdop; /* unitless? */
350 h_error = old.h_error; /* m */
351 v_error = old.v_error; /* m */
353 if (old.cc_gps_sat != null) {
354 cc_gps_sat = new AltosGPSSat[old.cc_gps_sat.length];
355 for (int i = 0; i < old.cc_gps_sat.length; i++) {
356 cc_gps_sat[i] = new AltosGPSSat();
357 cc_gps_sat[i].svid = old.cc_gps_sat[i].svid;
358 cc_gps_sat[i].c_n0 = old.cc_gps_sat[i].c_n0;
366 static public void update_state(AltosState state, AltosLink link, AltosConfigData config_data) throws InterruptedException {
368 AltosGPS gps = new AltosGPS(link, config_data);
371 state.set_gps(gps, state.gps_sequence++);
374 } catch (TimeoutException te) {
376 state.set_gps(null, 0);
379 public AltosGPS (AltosLink link, AltosConfigData config_data) throws TimeoutException, InterruptedException {
380 boolean says_done = config_data.compare_version("1.0") >= 0;
384 String line = link.get_reply_no_dialog(5000);
386 throw new TimeoutException();
387 if (!parse_string(line, says_done))