git.gag.com Git - fw/altos/blob - ao-tools/altosui/AltosTelemetry.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 altosui;
  19
  20 import java.lang.*;
  21 import java.text.*;
  22
  23 /*
  24  * Telemetry data contents
  25  */
  26
  27 class AltosGPSTime {
  28         int year;
  29         int month;
  30         int day;
  31         int hour;
  32         int minute;
  33         int second;
  34
  35         int parse_int(String v) throws ParseException {
  36                 try {
  37                         return Integer.parseInt(v);
  38                 } catch (NumberFormatException e) {
  39                         throw new ParseException("error parsing GPS value " + v, 0);
  40                 }
  41         }
  42
  43         public AltosGPSTime(String date, String time) throws ParseException {
  44                 String[] ymd = date.split("-");
  45                 if (ymd.length != 3)
  46                         throw new ParseException("error parsing GPS date " + date + " got " + ymd.length, 0);
  47                 year = parse_int(ymd[0]);
  48                 month = parse_int(ymd[1]);
  49                 day = parse_int(ymd[2]);
  50
  51                 String[] hms = time.split(":");
  52                 if (hms.length != 3)
  53                         throw new ParseException("Error parsing GPS time " + time + " got " + hms.length, 0);
  54                 hour = parse_int(hms[0]);
  55                 minute = parse_int(hms[1]);
  56                 second = parse_int(hms[2]);
  57         }
  58
  59         public AltosGPSTime() {
  60                 year = month = day = 0;
  61                 hour = minute = second = 0;
  62         }
  63 };
  64
  65 class AltosGPS {
  66         int     nsat;
  67         int     gps_locked;
  68         int     gps_connected;
  69         AltosGPSTime gps_time;
  70         double  lat;            /* degrees (+N -S) */
  71         double  lon;            /* degrees (+E -W) */
  72         int     alt;            /* m */
  73
  74         int     gps_extended;   /* has extra data */
  75         double  ground_speed;   /* m/s */
  76         int     course;         /* degrees */
  77         double  climb_rate;     /* m/s */
  78         double  hdop;           /* unitless? */
  79         int     h_error;        /* m */
  80         int     v_error;        /* m */
  81
  82 }
  83
  84 class AltosGPSSat {
  85         int     svid;
  86         int     c_n0;
  87 }
  88
  89 class AltosGPSTracking {
  90         int                     channels;
  91         AltosGPSSat[]           cc_gps_sat;
  92 }
  93
  94 /*
  95  * The telemetry data stream is a bit of a mess at present, with no consistent
  96  * formatting. In particular, the GPS data is formatted for viewing instead of parsing.
  97  * However, the key feature is that every telemetry line contains all of the information
  98  * necessary to describe the current rocket state, including the calibration values
  99  * for accelerometer and barometer.
 100  *
 101  * GPS unlocked:
 102  *
 103  * VERSION 2 CALL KB0G SERIAL  51 FLIGHT     2 RSSI  -68 STATUS ff STATE     pad  1001 \
 104  *    a: 16032 p: 21232 t: 20284 v: 25160 d:   204 m:   204 fa: 16038 ga: 16032 fv:       0 \
 105  *    fp: 21232 gp: 21230 a+: 16049 a-: 16304 GPS  0 sat unlocked SAT 1   15  30
 106  *
 107  * GPS locked:
 108  *
 109  * VERSION 2 CALL KB0G SERIAL  51 FLIGHT     2 RSSI  -71 STATUS ff STATE     pad  2504 \
 110  *     a: 16028 p: 21220 t: 20360 v: 25004 d:   208 m:   200 fa: 16031 ga: 16032 fv:     330 \
 111  *     fp: 21231 gp: 21230 a+: 16049 a-: 16304 \
 112  *     GPS  9 sat 2010-02-13 17:16:51 35°20.0803'N 106°45.2235'W  1790m  \
 113  *     0.00m/s(H) 0°     0.00m/s(V) 1.0(hdop)     0(herr)     0(verr) \
 114  *     SAT 10   29  30  24  28   5  25  21  20  15  33   1  23  30  24  18  26  10  29   2  26
 115  */
 116
 117 public class AltosTelemetry {
 118         int     version;
 119         String  callsign;
 120         int     serial;
 121         int     flight;
 122         int     rssi;
 123         int     status;
 124         String  state;
 125         int     tick;
 126         int     accel;
 127         int     pres;
 128         int     temp;
 129         int     batt;
 130         int     drogue;
 131         int     main;
 132         int     flight_accel;
 133         int     ground_accel;
 134         int     flight_vel;
 135         int     flight_pres;
 136         int     ground_pres;
 137         int     accel_plus_g;
 138         int     accel_minus_g;
 139         AltosGPS        gps;
 140         AltosGPSTracking        gps_tracking;
 141
 142         int parse_int(String v) throws ParseException {
 143                 try {
 144                         return Integer.parseInt(v);
 145                 } catch (NumberFormatException e) {
 146                         throw new ParseException("error parsing int " + v, 0);
 147                 }
 148         }
 149
 150         int parse_hex(String v) throws ParseException {
 151                 try {
 152                         return Integer.parseInt(v, 16);
 153                 } catch (NumberFormatException e) {
 154                         throw new ParseException("error parsing hex " + v, 0);
 155                 }
 156         }
 157
 158         double parse_double(String v) throws ParseException {
 159                 try {
 160                         return Double.parseDouble(v);
 161                 } catch (NumberFormatException e) {
 162                         throw new ParseException("error parsing double " + v, 0);
 163                 }
 164         }
 165
 166         double parse_coord(String coord) throws ParseException {
 167                 String[]        dsf = coord.split("\\D+");
 168
 169                 if (dsf.length != 3) {
 170                         throw new ParseException("error parsing coord " + coord, 0);
 171                 }
 172                 int deg = parse_int(dsf[0]);
 173                 int min = parse_int(dsf[1]);
 174                 int frac = parse_int(dsf[2]);
 175
 176                 double r = deg + (min + frac / 10000.0) / 60.0;
 177                 if (coord.endsWith("S") || coord.endsWith("W"))
 178                         r = -r;
 179                 return r;
 180         }
 181
 182         String strip_suffix(String v, String suffix) {
 183                 if (v.endsWith(suffix))
 184                         return v.substring(0, v.length() - suffix.length());
 185                 return v;
 186         }
 187
 188         void word(String v, String m) throws ParseException {
 189                 if (!v.equals(m)) {
 190                         throw new ParseException("error matching '" + v + "' '" + m + "'", 0);
 191                 }
 192         }
 193
 194         public AltosTelemetry(String line) throws ParseException {
 195                 String[] words = line.split("\\s+");
 196
 197                 int     i = 0;
 198
 199                 word (words[i++], "VERSION");
 200                 version = parse_int(words[i++]);
 201
 202                 word (words[i++], "CALL");
 203                 callsign = words[i++];
 204
 205                 word (words[i++], "SERIAL");
 206                 serial = parse_int(words[i++]);
 207
 208                 word (words[i++], "FLIGHT");
 209                 flight = parse_int(words[i++]);
 210
 211                 word(words[i++], "RSSI");
 212                 rssi = parse_int(words[i++]);
 213
 214                 word(words[i++], "STATUS");
 215                 status = parse_hex(words[i++]);
 216
 217                 word(words[i++], "STATE");
 218                 state = words[i++];
 219
 220                 tick = parse_int(words[i++]);
 221
 222                 word(words[i++], "a:");
 223                 accel = parse_int(words[i++]);
 224
 225                 word(words[i++], "p:");
 226                 pres = parse_int(words[i++]);
 227
 228                 word(words[i++], "t:");
 229                 temp = parse_int(words[i++]);
 230
 231                 word(words[i++], "v:");
 232                 batt = parse_int(words[i++]);
 233
 234                 word(words[i++], "d:");
 235                 drogue = parse_int(words[i++]);
 236
 237                 word(words[i++], "m:");
 238                 main = parse_int(words[i++]);
 239
 240                 word(words[i++], "fa:");
 241                 flight_accel = parse_int(words[i++]);
 242
 243                 word(words[i++], "ga:");
 244                 ground_accel = parse_int(words[i++]);
 245
 246                 word(words[i++], "fv:");
 247                 flight_vel = parse_int(words[i++]);
 248
 249                 word(words[i++], "fp:");
 250                 flight_pres = parse_int(words[i++]);
 251
 252                 word(words[i++], "gp:");
 253                 ground_pres = parse_int(words[i++]);
 254
 255                 word(words[i++], "a+:");
 256                 accel_plus_g = parse_int(words[i++]);
 257
 258                 word(words[i++], "a-:");
 259                 accel_minus_g = parse_int(words[i++]);
 260
 261                 word(words[i++], "GPS");
 262                 gps = new AltosGPS();
 263                 gps.nsat = parse_int(words[i++]);
 264                 word(words[i++], "sat");
 265
 266                 gps.gps_connected = 0;
 267                 gps.gps_locked = 0;
 268                 gps.lat = gps.lon = 0;
 269                 gps.alt = 0;
 270                 if ((words[i]).equals("unlocked")) {
 271                         gps.gps_connected = 1;
 272                         gps.gps_time = new AltosGPSTime();
 273                         i++;
 274                 } else if (words.length >= 40) {
 275                         gps.gps_locked = 1;
 276                         gps.gps_connected = 1;
 277
 278                         gps.gps_time = new AltosGPSTime(words[i], words[i+1]); i += 2;
 279                         gps.lat = parse_coord(words[i++]);
 280                         gps.lon = parse_coord(words[i++]);
 281                         gps.alt = parse_int(strip_suffix(words[i++], "m"));
 282                         gps.ground_speed = parse_double(strip_suffix(words[i++], "m/s(H)"));
 283                         gps.course = parse_int(strip_suffix(words[i++], "Â°"));
 284                         gps.climb_rate = parse_double(strip_suffix(words[i++], "m/s(V)"));
 285                         gps.hdop = parse_double(strip_suffix(words[i++], "(hdop)"));
 286                         gps.h_error = parse_int(strip_suffix(words[i++], "(herr)"));
 287                         gps.v_error = parse_int(strip_suffix(words[i++], "(verr)"));
 288                 } else {
 289                         gps.gps_time = new AltosGPSTime();
 290                 }
 291                 word(words[i++], "SAT");
 292                 gps_tracking = new AltosGPSTracking();
 293                 gps_tracking.channels = parse_int(words[i++]);
 294                 gps_tracking.cc_gps_sat = new AltosGPSSat[gps_tracking.channels];
 295                 for (int chan = 0; chan < gps_tracking.channels; chan++) {
 296                         gps_tracking.cc_gps_sat[chan] = new AltosGPSSat();
 297                         gps_tracking.cc_gps_sat[chan].svid = parse_int(words[i++]);
 298                         gps_tracking.cc_gps_sat[chan].c_n0 = parse_int(words[i++]);
 299                 }
 300         }
 301 }