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_1;
  19
  20 import java.text.*;
  21
  22 public class AltosGPS implements Cloneable {
  23
  24         public final static int MISSING = AltosRecord.MISSING;
  25
  26         public int      nsat;
  27         public boolean  locked;
  28         public boolean  connected;
  29         public double   lat;            /* degrees (+N -S) */
  30         public double   lon;            /* degrees (+E -W) */
  31         public int      alt;            /* m */
  32         public int      year;
  33         public int      month;
  34         public int      day;
  35         public int      hour;
  36         public int      minute;
  37         public int      second;
  38
  39         public double   ground_speed;   /* m/s */
  40         public int      course;         /* degrees */
  41         public double   climb_rate;     /* m/s */
  42         public double   hdop;           /* unitless */
  43         public double   vdop;           /* unitless */
  44         public int      h_error;        /* m */
  45         public int      v_error;        /* m */
  46
  47         public AltosGPSSat[] cc_gps_sat;        /* tracking data */
  48
  49         public void ParseGPSDate(String date) throws ParseException {
  50                 String[] ymd = date.split("-");
  51                 if (ymd.length != 3)
  52                         throw new ParseException("error parsing GPS date " + date + " got " + ymd.length, 0);
  53                 year = AltosParse.parse_int(ymd[0]);
  54                 month = AltosParse.parse_int(ymd[1]);
  55                 day = AltosParse.parse_int(ymd[2]);
  56         }
  57
  58         public void ParseGPSTime(String time) throws ParseException {
  59                 String[] hms = time.split(":");
  60                 if (hms.length != 3)
  61                         throw new ParseException("Error parsing GPS time " + time + " got " + hms.length, 0);
  62                 hour = AltosParse.parse_int(hms[0]);
  63                 minute = AltosParse.parse_int(hms[1]);
  64                 second = AltosParse.parse_int(hms[2]);
  65         }
  66
  67         public void ClearGPSTime() {
  68                 year = month = day = 0;
  69                 hour = minute = second = 0;
  70         }
  71
  72         public AltosGPS(AltosTelemetryMap map) throws ParseException {
  73                 String  state = map.get_string(AltosTelemetry.AO_TELEM_GPS_STATE,
  74                                                AltosTelemetry.AO_TELEM_GPS_STATE_ERROR);
  75
  76                 nsat = map.get_int(AltosTelemetry.AO_TELEM_GPS_NUM_SAT, 0);
  77                 if (state.equals(AltosTelemetry.AO_TELEM_GPS_STATE_LOCKED)) {
  78                         connected = true;
  79                         locked = true;
  80                         lat = map.get_double(AltosTelemetry.AO_TELEM_GPS_LATITUDE, MISSING, 1.0e-7);
  81                         lon = map.get_double(AltosTelemetry.AO_TELEM_GPS_LONGITUDE, MISSING, 1.0e-7);
  82                         alt = map.get_int(AltosTelemetry.AO_TELEM_GPS_ALTITUDE, MISSING);
  83                         year = map.get_int(AltosTelemetry.AO_TELEM_GPS_YEAR, MISSING);
  84                         if (year != MISSING)
  85                                 year += 2000;
  86                         month = map.get_int(AltosTelemetry.AO_TELEM_GPS_MONTH, MISSING);
  87                         day = map.get_int(AltosTelemetry.AO_TELEM_GPS_DAY, MISSING);
  88
  89                         hour = map.get_int(AltosTelemetry.AO_TELEM_GPS_HOUR, 0);
  90                         minute = map.get_int(AltosTelemetry.AO_TELEM_GPS_MINUTE, 0);
  91                         second = map.get_int(AltosTelemetry.AO_TELEM_GPS_SECOND, 0);
  92
  93                         ground_speed = map.get_double(AltosTelemetry.AO_TELEM_GPS_HORIZONTAL_SPEED,
  94                                                       AltosRecord.MISSING, 1/100.0);
  95                         course = map.get_int(AltosTelemetry.AO_TELEM_GPS_COURSE,
  96                                              AltosRecord.MISSING);
  97                         hdop = map.get_double(AltosTelemetry.AO_TELEM_GPS_HDOP, MISSING, 1.0);
  98                         vdop = map.get_double(AltosTelemetry.AO_TELEM_GPS_VDOP, MISSING, 1.0);
  99                         h_error = map.get_int(AltosTelemetry.AO_TELEM_GPS_HERROR, MISSING);
 100                         v_error = map.get_int(AltosTelemetry.AO_TELEM_GPS_VERROR, MISSING);
 101                 } else if (state.equals(AltosTelemetry.AO_TELEM_GPS_STATE_UNLOCKED)) {
 102                         connected = true;
 103                         locked = false;
 104                 } else {
 105                         connected = false;
 106                         locked = false;
 107                 }
 108         }
 109
 110         public AltosGPS(String[] words, int i, int version) throws ParseException {
 111                 AltosParse.word(words[i++], "GPS");
 112                 nsat = AltosParse.parse_int(words[i++]);
 113                 AltosParse.word(words[i++], "sat");
 114
 115                 connected = false;
 116                 locked = false;
 117                 lat = lon = 0;
 118                 alt = 0;
 119                 ClearGPSTime();
 120                 if ((words[i]).equals("unlocked")) {
 121                         connected = true;
 122                         i++;
 123                 } else if ((words[i]).equals("not-connected")) {
 124                         i++;
 125                 } else if (words.length >= 40) {
 126                         locked = true;
 127                         connected = true;
 128
 129                         if (version > 1)
 130                                 ParseGPSDate(words[i++]);
 131                         else
 132                                 year = month = day = 0;
 133                         ParseGPSTime(words[i++]);
 134                         lat = AltosParse.parse_coord(words[i++]);
 135                         lon = AltosParse.parse_coord(words[i++]);
 136                         alt = AltosParse.parse_int(words[i++]);
 137                         if (version > 1 || (i < words.length && !words[i].equals("SAT"))) {
 138                                 ground_speed = AltosParse.parse_double(AltosParse.strip_suffix(words[i++], "m/s(H)"));
 139                                 course = AltosParse.parse_int(words[i++]);
 140                                 climb_rate = AltosParse.parse_double(AltosParse.strip_suffix(words[i++], "m/s(V)"));
 141                                 hdop = AltosParse.parse_double(AltosParse.strip_suffix(words[i++], "(hdop)"));
 142                                 h_error = AltosParse.parse_int(words[i++]);
 143                                 v_error = AltosParse.parse_int(words[i++]);
 144                         }
 145                 } else {
 146                         i++;
 147                 }
 148                 if (i < words.length) {
 149                         AltosParse.word(words[i++], "SAT");
 150                         int tracking_channels = 0;
 151                         if (words[i].equals("not-connected"))
 152                                 tracking_channels = 0;
 153                         else
 154                                 tracking_channels = AltosParse.parse_int(words[i]);
 155                         i++;
 156                         cc_gps_sat = new AltosGPSSat[tracking_channels];
 157                         for (int chan = 0; chan < tracking_channels; chan++) {
 158                                 cc_gps_sat[chan] = new AltosGPSSat();
 159                                 cc_gps_sat[chan].svid = AltosParse.parse_int(words[i++]);
 160                                 /* Older versions included SiRF status bits */
 161                                 if (version < 2)
 162                                         i++;
 163                                 cc_gps_sat[chan].c_n0 = AltosParse.parse_int(words[i++]);
 164                         }
 165                 } else
 166                         cc_gps_sat = new AltosGPSSat[0];
 167         }
 168
 169         public void set_latitude(int in_lat) {
 170                 lat = in_lat / 10.0e7;
 171         }
 172
 173         public void set_longitude(int in_lon) {
 174                 lon = in_lon / 10.0e7;
 175         }
 176
 177         public void set_time(int in_hour, int in_minute, int in_second) {
 178                 hour = in_hour;
 179                 minute = in_minute;
 180                 second = in_second;
 181         }
 182
 183         public void set_date(int in_year, int in_month, int in_day) {
 184                 year = in_year;
 185                 month = in_month;
 186                 day = in_day;
 187         }
 188
 189         /*
 190         public void set_flags(int in_flags) {
 191                 flags = in_flags;
 192         }
 193         */
 194
 195         public void set_altitude(int in_altitude) {
 196                 alt = in_altitude;
 197         }
 198
 199         public void add_sat(int svid, int c_n0) {
 200                 if (cc_gps_sat == null) {
 201                         cc_gps_sat = new AltosGPSSat[1];
 202                 } else {
 203                         AltosGPSSat[] new_gps_sat = new AltosGPSSat[cc_gps_sat.length + 1];
 204                         for (int i = 0; i < cc_gps_sat.length; i++)
 205                                 new_gps_sat[i] = cc_gps_sat[i];
 206                         cc_gps_sat = new_gps_sat;
 207                 }
 208                 AltosGPSSat     sat = new AltosGPSSat();
 209                 sat.svid = svid;
 210                 sat.c_n0 = c_n0;
 211                 cc_gps_sat[cc_gps_sat.length - 1] = sat;
 212         }
 213
 214         public AltosGPS() {
 215                 ClearGPSTime();
 216                 cc_gps_sat = null;
 217         }
 218
 219         public AltosGPS clone() {
 220                 AltosGPS        g = new AltosGPS();
 221
 222                 g.nsat = nsat;
 223                 g.locked = locked;
 224                 g.connected = connected;
 225                 g.lat = lat;            g./* degrees (+N -S) */
 226                 g.lon = lon;            /* degrees (+E -W) */
 227                 g.alt = alt;            /* m */
 228                 g.year = year;
 229                 g.month = month;
 230                 g.day = day;
 231                 g.hour = hour;
 232                 g.minute = minute;
 233                 g.second = second;
 234
 235                 g.ground_speed = ground_speed;  /* m/s */
 236                 g.course = course;              /* degrees */
 237                 g.climb_rate = climb_rate;      /* m/s */
 238                 g.hdop = hdop;          /* unitless? */
 239                 g.h_error = h_error;    /* m */
 240                 g.v_error = v_error;    /* m */
 241
 242                 if (cc_gps_sat != null) {
 243                         g.cc_gps_sat = new AltosGPSSat[cc_gps_sat.length];
 244                         for (int i = 0; i < cc_gps_sat.length; i++) {
 245                                 g.cc_gps_sat[i] = new AltosGPSSat();
 246                                 g.cc_gps_sat[i].svid = cc_gps_sat[i].svid;
 247                                 g.cc_gps_sat[i].c_n0 = cc_gps_sat[i].c_n0;
 248                         }
 249                 }
 250         }
 251
 252         public AltosGPS(AltosGPS old) {
 253                 if (old != null) {
 254                         nsat = old.nsat;
 255                         locked = old.locked;
 256                         connected = old.connected;
 257                         lat = old.lat;          /* degrees (+N -S) */
 258                         lon = old.lon;          /* degrees (+E -W) */
 259                         alt = old.alt;          /* m */
 260                         year = old.year;
 261                         month = old.month;
 262                         day = old.day;
 263                         hour = old.hour;
 264                         minute = old.minute;
 265                         second = old.second;
 266
 267                         ground_speed = old.ground_speed;        /* m/s */
 268                         course = old.course;            /* degrees */
 269                         climb_rate = old.climb_rate;    /* m/s */
 270                         hdop = old.hdop;                /* unitless? */
 271                         h_error = old.h_error;  /* m */
 272                         v_error = old.v_error;  /* m */
 273
 274                         if (old.cc_gps_sat != null) {
 275                                 cc_gps_sat = new AltosGPSSat[old.cc_gps_sat.length];
 276                                 for (int i = 0; i < old.cc_gps_sat.length; i++) {
 277                                         cc_gps_sat[i] = new AltosGPSSat();
 278                                         cc_gps_sat[i].svid = old.cc_gps_sat[i].svid;
 279                                         cc_gps_sat[i].c_n0 = old.cc_gps_sat[i].c_n0;
 280                                 }
 281                         }
 282                 } else {
 283                         ClearGPSTime();
 284                         cc_gps_sat = null;
 285                 }
 286         }
 287 }