git.gag.com Git - fw/altos/blob - altoslib/AltosGPS.java

   1 /*
   2  * Copyright © 2010 Keith Packard <keithp@keithp.com>
   3  *
   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.
   7  *
   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.
  12  *
  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.
  16  */
  17
  18 package org.altusmetrum.altoslib_5;
  19
  20 import java.text.*;
  21 import java.util.concurrent.*;
  22
  23 public class AltosGPS implements Cloneable {
  24
  25         public final static int MISSING = AltosLib.MISSING;
  26
  27         public int      nsat;
  28         public boolean  locked;
  29         public boolean  connected;
  30         public double   lat;            /* degrees (+N -S) */
  31         public double   lon;            /* degrees (+E -W) */
  32         public double   alt;            /* m */
  33         public int      year;
  34         public int      month;
  35         public int      day;
  36         public int      hour;
  37         public int      minute;
  38         public int      second;
  39
  40         public double   ground_speed;   /* m/s */
  41         public int      course;         /* degrees */
  42         public double   climb_rate;     /* m/s */
  43         public double   pdop;           /* unitless */
  44         public double   hdop;           /* unitless */
  45         public double   vdop;           /* unitless */
  46         public double   h_error;        /* m */
  47         public double   v_error;        /* m */
  48
  49         public AltosGPSSat[] cc_gps_sat;        /* tracking data */
  50
  51         public void ParseGPSDate(String date) throws ParseException {
  52                 String[] ymd = date.split("-");
  53                 if (ymd.length != 3)
  54                         throw new ParseException("error parsing GPS date " + date + " got " + ymd.length, 0);
  55                 year = AltosParse.parse_int(ymd[0]);
  56                 month = AltosParse.parse_int(ymd[1]);
  57                 day = AltosParse.parse_int(ymd[2]);
  58         }
  59
  60         public void ParseGPSTime(String time) throws ParseException {
  61                 String[] hms = time.split(":");
  62                 if (hms.length != 3)
  63                         throw new ParseException("Error parsing GPS time " + time + " got " + hms.length, 0);
  64                 hour = AltosParse.parse_int(hms[0]);
  65                 minute = AltosParse.parse_int(hms[1]);
  66                 second = AltosParse.parse_int(hms[2]);
  67         }
  68
  69         public void ClearGPSTime() {
  70                 year = month = day = AltosLib.MISSING;
  71                 hour = minute = second = AltosLib.MISSING;
  72         }
  73
  74         public AltosGPS(AltosTelemetryMap map) throws ParseException {
  75                 String  state = map.get_string(AltosTelemetryLegacy.AO_TELEM_GPS_STATE,
  76                                                AltosTelemetryLegacy.AO_TELEM_GPS_STATE_ERROR);
  77
  78                 nsat = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_NUM_SAT, 0);
  79                 if (state.equals(AltosTelemetryLegacy.AO_TELEM_GPS_STATE_LOCKED)) {
  80                         connected = true;
  81                         locked = true;
  82                         lat = map.get_double(AltosTelemetryLegacy.AO_TELEM_GPS_LATITUDE, MISSING, 1.0e-7);
  83                         lon = map.get_double(AltosTelemetryLegacy.AO_TELEM_GPS_LONGITUDE, MISSING, 1.0e-7);
  84                         alt = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_ALTITUDE, MISSING);
  85                         year = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_YEAR, MISSING);
  86                         if (year != MISSING)
  87                                 year += 2000;
  88                         month = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_MONTH, MISSING);
  89                         day = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_DAY, MISSING);
  90
  91                         hour = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_HOUR, 0);
  92                         minute = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_MINUTE, 0);
  93                         second = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_SECOND, 0);
  94
  95                         ground_speed = map.get_double(AltosTelemetryLegacy.AO_TELEM_GPS_HORIZONTAL_SPEED,
  96                                                       AltosLib.MISSING, 1/100.0);
  97                         course = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_COURSE,
  98                                              AltosLib.MISSING);
  99                         pdop = map.get_double(AltosTelemetryLegacy.AO_TELEM_GPS_PDOP, MISSING, 1.0);
 100                         hdop = map.get_double(AltosTelemetryLegacy.AO_TELEM_GPS_HDOP, MISSING, 1.0);
 101                         vdop = map.get_double(AltosTelemetryLegacy.AO_TELEM_GPS_VDOP, MISSING, 1.0);
 102                         h_error = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_HERROR, MISSING);
 103                         v_error = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_VERROR, MISSING);
 104                 } else if (state.equals(AltosTelemetryLegacy.AO_TELEM_GPS_STATE_UNLOCKED)) {
 105                         connected = true;
 106                         locked = false;
 107                 } else {
 108                         connected = false;
 109                         locked = false;
 110                 }
 111         }
 112
 113         public boolean parse_string (String line, boolean says_done) {
 114                 String[] bits = line.split("\\s+");
 115                 if (bits.length == 0)
 116                         return false;
 117                 if (line.startsWith("Date:")) {
 118                         if (bits.length < 2)
 119                                 return false;
 120                         String[] d = bits[1].split("/");
 121                         if (d.length < 3)
 122                                 return false;
 123                         year = Integer.parseInt(d[0]) + 2000;
 124                         month = Integer.parseInt(d[1]);
 125                         day = Integer.parseInt(d[2]);
 126                 } else if (line.startsWith("Time:")) {
 127                         if (bits.length < 2)
 128                                 return false;
 129                         String[] d = bits[1].split(":");
 130                         if (d.length < 3)
 131                                 return false;
 132                         hour = Integer.parseInt(d[0]);
 133                         minute = Integer.parseInt(d[1]);
 134                         second = Integer.parseInt(d[2]);
 135                 } else if (line.startsWith("Lat/Lon:")) {
 136                         if (bits.length < 3)
 137                                 return false;
 138                         lat = Integer.parseInt(bits[1]) * 1.0e-7;
 139                         lon = Integer.parseInt(bits[2]) * 1.0e-7;
 140                 } else if (line.startsWith("Alt:")) {
 141                         if (bits.length < 2)
 142                                 return false;
 143                         alt = Integer.parseInt(bits[1]);
 144                 } else if (line.startsWith("Flags:")) {
 145                         if (bits.length < 2)
 146                                 return false;
 147                         int status = Integer.decode(bits[1]);
 148                         connected = (status & AltosLib.AO_GPS_RUNNING) != 0;
 149                         locked = (status & AltosLib.AO_GPS_VALID) != 0;
 150                         if (!says_done)
 151                                 return false;
 152                 } else if (line.startsWith("Sats:")) {
 153                         if (bits.length < 2)
 154                                 return false;
 155                         nsat = Integer.parseInt(bits[1]);
 156                         cc_gps_sat = new AltosGPSSat[nsat];
 157                         for (int i = 0; i < nsat; i++) {
 158                                 int     svid = Integer.parseInt(bits[2+i*2]);
 159                                 int     cc_n0 = Integer.parseInt(bits[3+i*2]);
 160                                 cc_gps_sat[i] = new AltosGPSSat(svid, cc_n0);
 161                         }
 162                 } else if (line.startsWith("done")) {
 163                         return false;
 164                 } else
 165                         return false;
 166                 return true;
 167         }
 168
 169         public AltosGPS(String[] words, int i, int version) throws ParseException {
 170                 AltosParse.word(words[i++], "GPS");
 171                 nsat = AltosParse.parse_int(words[i++]);
 172                 AltosParse.word(words[i++], "sat");
 173
 174                 connected = false;
 175                 locked = false;
 176                 lat = lon = 0;
 177                 alt = 0;
 178                 ClearGPSTime();
 179                 if ((words[i]).equals("unlocked")) {
 180                         connected = true;
 181                         i++;
 182                 } else if ((words[i]).equals("not-connected")) {
 183                         i++;
 184                 } else if (words.length >= 40) {
 185                         locked = true;
 186                         connected = true;
 187
 188                         if (version > 1)
 189                                 ParseGPSDate(words[i++]);
 190                         else
 191                                 year = month = day = 0;
 192                         ParseGPSTime(words[i++]);
 193                         lat = AltosParse.parse_coord(words[i++]);
 194                         lon = AltosParse.parse_coord(words[i++]);
 195                         alt = AltosParse.parse_int(words[i++]);
 196                         if (version > 1 || (i < words.length && !words[i].equals("SAT"))) {
 197                                 ground_speed = AltosParse.parse_double(AltosParse.strip_suffix(words[i++], "m/s(H)"));
 198                                 course = AltosParse.parse_int(words[i++]);
 199                                 climb_rate = AltosParse.parse_double(AltosParse.strip_suffix(words[i++], "m/s(V)"));
 200                                 hdop = AltosParse.parse_double(AltosParse.strip_suffix(words[i++], "(hdop)"));
 201                                 h_error = AltosParse.parse_int(words[i++]);
 202                                 v_error = AltosParse.parse_int(words[i++]);
 203                         }
 204                 } else {
 205                         i++;
 206                 }
 207                 if (i < words.length) {
 208                         AltosParse.word(words[i++], "SAT");
 209                         int tracking_channels = 0;
 210                         if (words[i].equals("not-connected"))
 211                                 tracking_channels = 0;
 212                         else
 213                                 tracking_channels = AltosParse.parse_int(words[i]);
 214                         i++;
 215                         cc_gps_sat = new AltosGPSSat[tracking_channels];
 216                         for (int chan = 0; chan < tracking_channels; chan++) {
 217                                 cc_gps_sat[chan] = new AltosGPSSat();
 218                                 cc_gps_sat[chan].svid = AltosParse.parse_int(words[i++]);
 219                                 /* Older versions included SiRF status bits */
 220                                 if (version < 2)
 221                                         i++;
 222                                 cc_gps_sat[chan].c_n0 = AltosParse.parse_int(words[i++]);
 223                         }
 224                 } else
 225                         cc_gps_sat = new AltosGPSSat[0];
 226         }
 227
 228         public void set_latitude(int in_lat) {
 229                 lat = in_lat / 10.0e7;
 230         }
 231
 232         public void set_longitude(int in_lon) {
 233                 lon = in_lon / 10.0e7;
 234         }
 235
 236         public void set_time(int in_hour, int in_minute, int in_second) {
 237                 hour = in_hour;
 238                 minute = in_minute;
 239                 second = in_second;
 240         }
 241
 242         public void set_date(int in_year, int in_month, int in_day) {
 243                 year = in_year;
 244                 month = in_month;
 245                 day = in_day;
 246         }
 247
 248         /*
 249         public void set_flags(int in_flags) {
 250                 flags = in_flags;
 251         }
 252         */
 253
 254         public void set_altitude(int in_altitude) {
 255                 alt = in_altitude;
 256         }
 257
 258         public void add_sat(int svid, int c_n0) {
 259                 if (cc_gps_sat == null) {
 260                         cc_gps_sat = new AltosGPSSat[1];
 261                 } else {
 262                         AltosGPSSat[] new_gps_sat = new AltosGPSSat[cc_gps_sat.length + 1];
 263                         for (int i = 0; i < cc_gps_sat.length; i++)
 264                                 new_gps_sat[i] = cc_gps_sat[i];
 265                         cc_gps_sat = new_gps_sat;
 266                 }
 267                 AltosGPSSat     sat = new AltosGPSSat();
 268                 sat.svid = svid;
 269                 sat.c_n0 = c_n0;
 270                 cc_gps_sat[cc_gps_sat.length - 1] = sat;
 271         }
 272
 273         private void init() {
 274                 lat = AltosLib.MISSING;
 275                 lon = AltosLib.MISSING;
 276                 alt = AltosLib.MISSING;
 277                 ground_speed = AltosLib.MISSING;
 278                 course = AltosLib.MISSING;
 279                 climb_rate = AltosLib.MISSING;
 280                 pdop = AltosLib.MISSING;
 281                 hdop = AltosLib.MISSING;
 282                 vdop = AltosLib.MISSING;
 283                 h_error = AltosLib.MISSING;
 284                 v_error = AltosLib.MISSING;
 285                 ClearGPSTime();
 286                 cc_gps_sat = null;
 287         }
 288
 289         public AltosGPS() {
 290                 init();
 291         }
 292
 293         public AltosGPS clone() {
 294                 AltosGPS        g = new AltosGPS();
 295
 296                 g.nsat = nsat;
 297                 g.locked = locked;
 298                 g.connected = connected;
 299                 g.lat = lat;            /* degrees (+N -S) */
 300                 g.lon = lon;            /* degrees (+E -W) */
 301                 g.alt = alt;            /* m */
 302                 g.year = year;
 303                 g.month = month;
 304                 g.day = day;
 305                 g.hour = hour;
 306                 g.minute = minute;
 307                 g.second = second;
 308
 309                 g.ground_speed = ground_speed;  /* m/s */
 310                 g.course = course;              /* degrees */
 311                 g.climb_rate = climb_rate;      /* m/s */
 312                 g.pdop = pdop;          /* unitless */
 313                 g.hdop = hdop;          /* unitless */
 314                 g.vdop = vdop;          /* unitless */
 315                 g.h_error = h_error;    /* m */
 316                 g.v_error = v_error;    /* m */
 317
 318                 if (cc_gps_sat != null) {
 319                         g.cc_gps_sat = new AltosGPSSat[cc_gps_sat.length];
 320                         for (int i = 0; i < cc_gps_sat.length; i++) {
 321                                 g.cc_gps_sat[i] = new AltosGPSSat(cc_gps_sat[i].svid,
 322                                                                   cc_gps_sat[i].c_n0);
 323                         }
 324                 }
 325                 return g;
 326         }
 327
 328         public AltosGPS(AltosGPS old) {
 329                 if (old != null) {
 330                         nsat = old.nsat;
 331                         locked = old.locked;
 332                         connected = old.connected;
 333                         lat = old.lat;          /* degrees (+N -S) */
 334                         lon = old.lon;          /* degrees (+E -W) */
 335                         alt = old.alt;          /* m */
 336                         year = old.year;
 337                         month = old.month;
 338                         day = old.day;
 339                         hour = old.hour;
 340                         minute = old.minute;
 341                         second = old.second;
 342
 343                         ground_speed = old.ground_speed;        /* m/s */
 344                         course = old.course;            /* degrees */
 345                         climb_rate = old.climb_rate;    /* m/s */
 346                         pdop = old.pdop;                /* unitless? */
 347                         hdop = old.hdop;                /* unitless? */
 348                         vdop = old.vdop;                /* unitless? */
 349                         h_error = old.h_error;          /* m */
 350                         v_error = old.v_error;          /* m */
 351
 352                         if (old.cc_gps_sat != null) {
 353                                 cc_gps_sat = new AltosGPSSat[old.cc_gps_sat.length];
 354                                 for (int i = 0; i < old.cc_gps_sat.length; i++) {
 355                                         cc_gps_sat[i] = new AltosGPSSat();
 356                                         cc_gps_sat[i].svid = old.cc_gps_sat[i].svid;
 357                                         cc_gps_sat[i].c_n0 = old.cc_gps_sat[i].c_n0;
 358                                 }
 359                         }
 360                 } else {
 361                         init();
 362                 }
 363         }
 364
 365         static public void update_state(AltosState state, AltosLink link, AltosConfigData config_data) throws InterruptedException {
 366                 try {
 367                         AltosGPS        gps = new AltosGPS(link, config_data);
 368
 369                         if (gps != null) {
 370                                 state.set_gps(gps, state.gps_sequence++);
 371                                 return;
 372                         }
 373                 } catch (TimeoutException te) {
 374                 }
 375                 state.set_gps(null, 0);
 376         }
 377
 378         public AltosGPS (AltosLink link, AltosConfigData config_data) throws TimeoutException, InterruptedException {
 379                 boolean says_done = config_data.compare_version("1.0") >= 0;
 380                 link.printf("g\n");
 381                 for (;;) {
 382                         String line = link.get_reply_no_dialog(5000);
 383                         if (line == null)
 384                                 throw new TimeoutException();
 385                         if (!parse_string(line, says_done))
 386                                 break;
 387                 }
 388         }
 389 }