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; either version 2 of the License, or
   7  * (at your option) any later version.
   8  *
   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.
  13  *
  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.
  17  */
  18
  19 package org.altusmetrum.altoslib_14;
  20
  21 import java.text.*;
  22 import java.util.concurrent.*;
  23 import java.io.*;
  24 import java.time.*;
  25
  26 public class AltosGPS implements Cloneable {
  27
  28         public final static int MISSING = AltosLib.MISSING;
  29
  30         public int      nsat;
  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 */
  36         public int      year;
  37         public int      month;
  38         public int      day;
  39         public int      hour;
  40         public int      minute;
  41         public int      second;
  42
  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 */
  51
  52         public AltosGPSSat[] cc_gps_sat;        /* tracking data */
  53
  54         public void ParseGPSDate(String date) throws ParseException {
  55                 String[] ymd = date.split("-");
  56                 if (ymd.length != 3)
  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]);
  61         }
  62
  63         public void ParseGPSTime(String time) throws ParseException {
  64                 String[] hms = time.split(":");
  65                 if (hms.length != 3)
  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]);
  70         }
  71
  72         public void ClearGPSTime() {
  73                 year = month = day = AltosLib.MISSING;
  74                 hour = minute = second = AltosLib.MISSING;
  75         }
  76
  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();
  93         }
  94
  95         public  AltosLatLon lat_lon() {
  96                 return new AltosLatLon(lat, lon);
  97         }
  98
  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);
 102
 103                 nsat = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_NUM_SAT, 0);
 104                 if (state.equals(AltosTelemetryLegacy.AO_TELEM_GPS_STATE_LOCKED)) {
 105                         connected = true;
 106                         locked = true;
 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);
 111                         if (year != MISSING)
 112                                 year += 2000;
 113                         month = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_MONTH, MISSING);
 114                         day = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_DAY, MISSING);
 115
 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);
 119
 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,
 123                                              AltosLib.MISSING);
 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)) {
 130                         connected = true;
 131                         locked = false;
 132                 } else {
 133                         connected = false;
 134                         locked = false;
 135                 }
 136         }
 137
 138         public boolean parse_string (String line, boolean says_done) {
 139                 String[] bits = line.split("\\s+");
 140                 if (bits.length == 0)
 141                         return false;
 142                 if (line.startsWith("Date:")) {
 143                         if (bits.length < 2)
 144                                 return false;
 145                         String[] d = bits[1].split("/");
 146                         if (d.length < 3)
 147                                 return false;
 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:")) {
 152                         if (bits.length < 2)
 153                                 return false;
 154                         String[] d = bits[1].split(":");
 155                         if (d.length < 3)
 156                                 return false;
 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:")) {
 161                         if (bits.length < 3)
 162                                 return false;
 163                         lat = Integer.parseInt(bits[1]) * 1.0e-7;
 164                         lon = Integer.parseInt(bits[2]) * 1.0e-7;
 165                 } else if (line.startsWith("Alt:")) {
 166                         if (bits.length < 2)
 167                                 return false;
 168                         alt = Integer.parseInt(bits[1]);
 169                 } else if (line.startsWith("Flags:")) {
 170                         if (bits.length < 2)
 171                                 return false;
 172                         int status = Integer.decode(bits[1]);
 173                         connected = (status & AltosLib.AO_GPS_RUNNING) != 0;
 174                         locked = (status & AltosLib.AO_GPS_VALID) != 0;
 175                         if (!says_done)
 176                                 return false;
 177                 } else if (line.startsWith("Sats:")) {
 178                         if (bits.length < 2)
 179                                 return false;
 180                         nsat = Integer.parseInt(bits[1]);
 181                         cc_gps_sat = new AltosGPSSat[nsat];
 182                         for (int i = 0; i < nsat; i++) {
 183                                 int     svid = Integer.parseInt(bits[2+i*2]);
 184                                 int     cc_n0 = Integer.parseInt(bits[3+i*2]);
 185                                 cc_gps_sat[i] = new AltosGPSSat(svid, cc_n0);
 186                         }
 187                 } else if (line.startsWith("done")) {
 188                         return false;
 189                 } else
 190                         return false;
 191                 return true;
 192         }
 193
 194         public AltosGPS(String[] words, int i, int version) throws ParseException {
 195                 AltosParse.word(words[i++], "GPS");
 196                 nsat = AltosParse.parse_int(words[i++]);
 197                 AltosParse.word(words[i++], "sat");
 198
 199                 connected = false;
 200                 locked = false;
 201                 lat = lon = 0;
 202                 alt = 0;
 203                 ClearGPSTime();
 204                 if ((words[i]).equals("unlocked")) {
 205                         connected = true;
 206                         i++;
 207                 } else if ((words[i]).equals("not-connected")) {
 208                         i++;
 209                 } else if (words.length >= 40) {
 210                         locked = true;
 211                         connected = true;
 212
 213                         if (version > 1)
 214                                 ParseGPSDate(words[i++]);
 215                         else
 216                                 year = month = day = 0;
 217                         ParseGPSTime(words[i++]);
 218                         lat = AltosParse.parse_coord(words[i++]);
 219                         lon = AltosParse.parse_coord(words[i++]);
 220                         alt = AltosParse.parse_int(words[i++]);
 221                         if (version > 1 || (i < words.length && !words[i].equals("SAT"))) {
 222                                 ground_speed = AltosParse.parse_double_net(AltosParse.strip_suffix(words[i++], "m/s(H)"));
 223                                 course = AltosParse.parse_int(words[i++]);
 224                                 climb_rate = AltosParse.parse_double_net(AltosParse.strip_suffix(words[i++], "m/s(V)"));
 225                                 hdop = AltosParse.parse_double_net(AltosParse.strip_suffix(words[i++], "(hdop)"));
 226                                 h_error = AltosParse.parse_int(words[i++]);
 227                                 v_error = AltosParse.parse_int(words[i++]);
 228                         }
 229                 } else {
 230                         i++;
 231                 }
 232                 if (i < words.length) {
 233                         AltosParse.word(words[i++], "SAT");
 234                         int tracking_channels = 0;
 235                         if (words[i].equals("not-connected"))
 236                                 tracking_channels = 0;
 237                         else
 238                                 tracking_channels = AltosParse.parse_int(words[i]);
 239                         i++;
 240                         cc_gps_sat = new AltosGPSSat[tracking_channels];
 241                         for (int chan = 0; chan < tracking_channels; chan++) {
 242                                 cc_gps_sat[chan] = new AltosGPSSat();
 243                                 cc_gps_sat[chan].svid = AltosParse.parse_int(words[i++]);
 244                                 /* Older versions included SiRF status bits */
 245                                 if (version < 2)
 246                                         i++;
 247                                 cc_gps_sat[chan].c_n0 = AltosParse.parse_int(words[i++]);
 248                         }
 249                 } else
 250                         cc_gps_sat = new AltosGPSSat[0];
 251         }
 252
 253         public void set_latitude(int in_lat) {
 254                 lat = in_lat / 10.0e7;
 255         }
 256
 257         public void set_longitude(int in_lon) {
 258                 lon = in_lon / 10.0e7;
 259         }
 260
 261         public void set_time(int in_hour, int in_minute, int in_second) {
 262                 hour = in_hour;
 263                 minute = in_minute;
 264                 second = in_second;
 265         }
 266
 267         public void set_date(int in_year, int in_month, int in_day) {
 268                 year = in_year;
 269                 month = in_month;
 270                 day = in_day;
 271         }
 272
 273         /*
 274         public void set_flags(int in_flags) {
 275                 flags = in_flags;
 276         }
 277         */
 278
 279         public void set_altitude(int in_altitude) {
 280                 alt = in_altitude;
 281         }
 282
 283         public void add_sat(int svid, int c_n0) {
 284                 if (cc_gps_sat == null) {
 285                         cc_gps_sat = new AltosGPSSat[1];
 286                 } else {
 287                         AltosGPSSat[] new_gps_sat = new AltosGPSSat[cc_gps_sat.length + 1];
 288                         for (int i = 0; i < cc_gps_sat.length; i++)
 289                                 new_gps_sat[i] = cc_gps_sat[i];
 290                         cc_gps_sat = new_gps_sat;
 291                 }
 292                 AltosGPSSat     sat = new AltosGPSSat();
 293                 sat.svid = svid;
 294                 sat.c_n0 = c_n0;
 295                 cc_gps_sat[cc_gps_sat.length - 1] = sat;
 296         }
 297
 298         private void init() {
 299                 lat = AltosLib.MISSING;
 300                 lon = AltosLib.MISSING;
 301                 alt = AltosLib.MISSING;
 302                 ground_speed = AltosLib.MISSING;
 303                 course = AltosLib.MISSING;
 304                 climb_rate = AltosLib.MISSING;
 305                 pdop = AltosLib.MISSING;
 306                 hdop = AltosLib.MISSING;
 307                 vdop = AltosLib.MISSING;
 308                 h_error = AltosLib.MISSING;
 309                 v_error = AltosLib.MISSING;
 310                 ClearGPSTime();
 311                 cc_gps_sat = null;
 312         }
 313
 314         public AltosGPS() {
 315                 init();
 316         }
 317
 318         public AltosGPS clone() {
 319                 AltosGPS        g = new AltosGPS();
 320
 321                 g.nsat = nsat;
 322                 g.locked = locked;
 323                 g.connected = connected;
 324                 g.lat = lat;            /* degrees (+N -S) */
 325                 g.lon = lon;            /* degrees (+E -W) */
 326                 g.alt = alt;            /* m */
 327                 g.year = year;
 328                 g.month = month;
 329                 g.day = day;
 330                 g.hour = hour;
 331                 g.minute = minute;
 332                 g.second = second;
 333
 334                 g.ground_speed = ground_speed;  /* m/s */
 335                 g.course = course;              /* degrees */
 336                 g.climb_rate = climb_rate;      /* m/s */
 337                 g.pdop = pdop;          /* unitless */
 338                 g.hdop = hdop;          /* unitless */
 339                 g.vdop = vdop;          /* unitless */
 340                 g.h_error = h_error;    /* m */
 341                 g.v_error = v_error;    /* m */
 342
 343                 if (cc_gps_sat != null) {
 344                         g.cc_gps_sat = new AltosGPSSat[cc_gps_sat.length];
 345                         for (int i = 0; i < cc_gps_sat.length; i++) {
 346                                 g.cc_gps_sat[i] = new AltosGPSSat(cc_gps_sat[i].svid,
 347                                                                   cc_gps_sat[i].c_n0);
 348                         }
 349                 }
 350                 return g;
 351         }
 352
 353         public AltosGPS(AltosGPS old) {
 354                 if (old != null) {
 355                         nsat = old.nsat;
 356                         locked = old.locked;
 357                         connected = old.connected;
 358                         lat = old.lat;          /* degrees (+N -S) */
 359                         lon = old.lon;          /* degrees (+E -W) */
 360                         alt = old.alt;          /* m */
 361                         year = old.year;
 362                         month = old.month;
 363                         day = old.day;
 364                         hour = old.hour;
 365                         minute = old.minute;
 366                         second = old.second;
 367
 368                         ground_speed = old.ground_speed;        /* m/s */
 369                         course = old.course;            /* degrees */
 370                         climb_rate = old.climb_rate;    /* m/s */
 371                         pdop = old.pdop;                /* unitless? */
 372                         hdop = old.hdop;                /* unitless? */
 373                         vdop = old.vdop;                /* unitless? */
 374                         h_error = old.h_error;          /* m */
 375                         v_error = old.v_error;          /* m */
 376
 377                         if (old.cc_gps_sat != null) {
 378                                 cc_gps_sat = new AltosGPSSat[old.cc_gps_sat.length];
 379                                 for (int i = 0; i < old.cc_gps_sat.length; i++) {
 380                                         cc_gps_sat[i] = new AltosGPSSat();
 381                                         cc_gps_sat[i].svid = old.cc_gps_sat[i].svid;
 382                                         cc_gps_sat[i].c_n0 = old.cc_gps_sat[i].c_n0;
 383                                 }
 384                         }
 385                 } else {
 386                         init();
 387                 }
 388         }
 389
 390         static public void provide_data(AltosDataListener listener, AltosLink link) throws InterruptedException {
 391                 try {
 392                         AltosGPS gps = new AltosGPS(link, link.config_data());
 393                         if (gps != null)
 394                                 listener.set_gps(gps, true, true);
 395                 } catch (TimeoutException te) {
 396                 }
 397         }
 398
 399         public AltosGPS (AltosLink link, AltosConfigData config_data) throws TimeoutException, InterruptedException {
 400                 boolean says_done = config_data.compare_version("1.0") >= 0;
 401                 init();
 402                 link.printf("g\n");
 403                 for (;;) {
 404                         String line = link.get_reply_no_dialog(5000);
 405                         if (line == null)
 406                                 throw new TimeoutException();
 407                         if (!parse_string(line, says_done))
 408                                 break;
 409                 }
 410         }
 411 }