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(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                         System.out.println("Error parsing GPS date " + date + " got " + ymd.length);
  47                         throw new ParseException(date, 0);
  48                 }
  49                 year = parse_int(ymd[0]);
  50                 month = parse_int(ymd[1]);
  51                 day = parse_int(ymd[2]);
  52
  53                 String[] hms = time.split(":");
  54                 if (hms.length != 3) {
  55                         System.out.println("Error parsing GPS time " + time + " got " + hms.length);
  56                         throw new ParseException(time, 0);
  57                 }
  58                 hour = parse_int(hms[0]);
  59                 minute = parse_int(hms[1]);
  60                 second = parse_int(hms[2]);
  61         }
  62
  63         public AltosGPSTime() {
  64                 year = month = day = 0;
  65                 hour = minute = second = 0;
  66         }
  67 };
  68
  69 class AltosGPS {
  70         int     nsat;
  71         int     gps_locked;
  72         int     gps_connected;
  73         AltosGPSTime gps_time;
  74         double  lat;            /* degrees (+N -S) */
  75         double  lon;            /* degrees (+E -W) */
  76         int     alt;            /* m */
  77
  78         int     gps_extended;   /* has extra data */
  79         double  ground_speed;   /* m/s */
  80         int     course;         /* degrees */
  81         double  climb_rate;     /* m/s */
  82         double  hdop;           /* unitless? */
  83         int     h_error;        /* m */
  84         int     v_error;        /* m */
  85
  86 }
  87
  88 class AltosGPSSat {
  89         int     svid;
  90         int     c_n0;
  91 }
  92
  93 class AltosGPSTracking {
  94         int                     channels;
  95         AltosGPSSat[]           cc_gps_sat;
  96 }
  97
  98 public class AltosTelemetry {
  99         int     version;
 100         String  callsign;
 101         int     serial;
 102         int     flight;
 103         int     rssi;
 104         int     status;
 105         String  state;
 106         int     tick;
 107         int     accel;
 108         int     pres;
 109         int     temp;
 110         int     batt;
 111         int     drogue;
 112         int     main;
 113         int     flight_accel;
 114         int     ground_accel;
 115         int     flight_vel;
 116         int     flight_pres;
 117         int     ground_pres;
 118         int     accel_plus_g;
 119         int     accel_minus_g;
 120         AltosGPS        gps;
 121         AltosGPSTracking        gps_tracking;
 122
 123         int parse_int(String v) throws ParseException {
 124                 try {
 125                         return Integer.parseInt(v);
 126                 } catch (NumberFormatException e) {
 127                         System.out.println("error parsing int " + v);
 128                         throw new ParseException(v, 0);
 129                 }
 130         }
 131
 132         int parse_hex(String v) throws ParseException {
 133                 try {
 134                         return Integer.parseInt(v, 16);
 135                 } catch (NumberFormatException e) {
 136                         System.out.println("error parsing hex " + v);
 137                         throw new ParseException(v, 0);
 138                 }
 139         }
 140
 141         double parse_double(String v) throws ParseException {
 142                 try {
 143                         return Double.parseDouble(v);
 144                 } catch (NumberFormatException e) {
 145                         System.out.println("error parsing double " + v);
 146                         throw new ParseException(v, 0);
 147                 }
 148         }
 149
 150         double parse_coord(String coord) throws ParseException {
 151                 String[]        dsf = coord.split("\\D+");
 152
 153                 if (dsf.length != 3) {
 154                         System.out.println("error parsing coord " + coord);
 155                         throw new ParseException(coord, 0);
 156                 }
 157                 int deg = parse_int(dsf[0]);
 158                 int min = parse_int(dsf[1]);
 159                 int frac = parse_int(dsf[2]);
 160
 161                 double r = deg + (min + frac / 10000.0) / 60.0;
 162                 if (coord.endsWith("S") || coord.endsWith("W"))
 163                         r = -r;
 164                 return r;
 165         }
 166
 167         String strip_suffix(String v, String suffix) {
 168                 if (v.endsWith(suffix))
 169                         return v.substring(0, v.length() - suffix.length());
 170                 return v;
 171         }
 172
 173         void word(String v, String m) throws ParseException {
 174                 if (!v.equals(m)) {
 175                         System.out.println("error matching '" + v + "' '" + m + "'");
 176                         throw new ParseException(v, 0);
 177                 }
 178         }
 179
 180         public AltosTelemetry(String line) throws ParseException {
 181                 String[] words = line.split("\\s+");
 182
 183                 int     i = 0;
 184
 185                 word (words[i++], "VERSION");
 186                 version = parse_int(words[i++]);
 187
 188                 word (words[i++], "CALL");
 189                 callsign = words[i++];
 190
 191                 word (words[i++], "SERIAL");
 192                 serial = parse_int(words[i++]);
 193
 194                 word (words[i++], "FLIGHT");
 195                 flight = parse_int(words[i++]);
 196
 197                 word(words[i++], "RSSI");
 198                 rssi = parse_int(words[i++]);
 199
 200                 word(words[i++], "STATUS");
 201                 status = parse_hex(words[i++]);
 202
 203                 word(words[i++], "STATE");
 204                 state = words[i++];
 205
 206                 tick = parse_int(words[i++]);
 207
 208                 word(words[i++], "a:");
 209                 accel = parse_int(words[i++]);
 210
 211                 word(words[i++], "p:");
 212                 pres = parse_int(words[i++]);
 213
 214                 word(words[i++], "t:");
 215                 temp = parse_int(words[i++]);
 216
 217                 word(words[i++], "v:");
 218                 batt = parse_int(words[i++]);
 219
 220                 word(words[i++], "d:");
 221                 drogue = parse_int(words[i++]);
 222
 223                 word(words[i++], "m:");
 224                 main = parse_int(words[i++]);
 225
 226                 word(words[i++], "fa:");
 227                 flight_accel = parse_int(words[i++]);
 228
 229                 word(words[i++], "ga:");
 230                 ground_accel = parse_int(words[i++]);
 231
 232                 word(words[i++], "fv:");
 233                 flight_vel = parse_int(words[i++]);
 234
 235                 word(words[i++], "fp:");
 236                 flight_pres = parse_int(words[i++]);
 237
 238                 word(words[i++], "gp:");
 239                 ground_pres = parse_int(words[i++]);
 240
 241                 word(words[i++], "a+:");
 242                 accel_plus_g = parse_int(words[i++]);
 243
 244                 word(words[i++], "a-:");
 245                 accel_minus_g = parse_int(words[i++]);
 246
 247                 word(words[i++], "GPS");
 248                 gps = new AltosGPS();
 249                 gps.nsat = parse_int(words[i++]);
 250                 word(words[i++], "sat");
 251
 252                 gps.gps_connected = 0;
 253                 gps.gps_locked = 0;
 254                 gps.lat = gps.lon = 0;
 255                 gps.alt = 0;
 256                 if ((words[i]).equals("unlocked")) {
 257                         gps.gps_connected = 1;
 258                         gps.gps_time = new AltosGPSTime();
 259                         i++;
 260                 } else if (words.length >= 40) {
 261                         gps.gps_locked = 1;
 262                         gps.gps_connected = 1;
 263
 264                         gps.gps_time = new AltosGPSTime(words[i], words[i+1]); i += 2;
 265                         gps.lat = parse_coord(words[i++]);
 266                         gps.lon = parse_coord(words[i++]);
 267                         gps.alt = parse_int(strip_suffix(words[i++], "m"));
 268                         gps.ground_speed = parse_double(strip_suffix(words[i++], "m/s(H)"));
 269                         gps.course = parse_int(strip_suffix(words[i++], "Â°"));
 270                         gps.climb_rate = parse_double(strip_suffix(words[i++], "m/s(V)"));
 271                         gps.hdop = parse_double(strip_suffix(words[i++], "(hdop)"));
 272                         gps.h_error = parse_int(strip_suffix(words[i++], "(herr)"));
 273                         gps.v_error = parse_int(strip_suffix(words[i++], "(verr)"));
 274                 } else {
 275                         gps.gps_time = new AltosGPSTime();
 276                 }
 277                 word(words[i++], "SAT");
 278                 gps_tracking = new AltosGPSTracking();
 279                 gps_tracking.channels = parse_int(words[i++]);
 280                 gps_tracking.cc_gps_sat = new AltosGPSSat[gps_tracking.channels];
 281                 for (int chan = 0; chan < gps_tracking.channels; chan++) {
 282                         gps_tracking.cc_gps_sat[chan] = new AltosGPSSat();
 283                         gps_tracking.cc_gps_sat[chan].svid = parse_int(words[i++]);
 284                         gps_tracking.cc_gps_sat[chan].c_n0 = parse_int(words[i++]);
 285                 }
 286         }
 287 }