git.gag.com Git - debian/openrocket/blob - core/src-extra/altimeter/Alt15K.java

   1 package altimeter;
   2
   3 import gnu.io.CommPortIdentifier;
   4 import gnu.io.PortInUseException;
   5 import gnu.io.SerialPort;
   6 import gnu.io.UnsupportedCommOperationException;
   7
   8 import java.io.FileNotFoundException;
   9 import java.io.IOException;
  10 import java.io.InputStream;
  11 import java.io.OutputStream;
  12 import java.io.PrintStream;
  13 import java.nio.charset.Charset;
  14 import java.text.DateFormat;
  15 import java.util.ArrayList;
  16 import java.util.Date;
  17 import java.util.Enumeration;
  18 import java.util.TimeZone;
  19
  20 /**
  21  * Class to interface the PerfectFlite Alt15K/WD altimeter.
  22  *
  23  * Also includes a main method that retrieves all flight profiles and saves them to files.
  24  *
  25  * @author Sampo Niskanen <sampo.niskanen@iki.fi>
  26  */
  27
  28 public class Alt15K {
  29         public static final int TIMEOUT = 500;
  30         public static final int RWDELAY = 5;
  31
  32         private static final boolean DEBUG = false;
  33
  34         private static final Charset CHARSET = Charset.forName("ISO-8859-1");
  35
  36         private final CommPortIdentifier portID;
  37         private SerialPort port = null;
  38         private InputStream is = null;
  39         private OutputStream os = null;
  40
  41
  42
  43         @SuppressWarnings("unchecked")
  44         public static String[] getNames() {
  45                 ArrayList<String> list = new ArrayList<String>();;
  46
  47                 Enumeration pids = CommPortIdentifier.getPortIdentifiers();
  48
  49                 while (pids.hasMoreElements()) {
  50                     CommPortIdentifier pid = (CommPortIdentifier) pids.nextElement();
  51
  52                     if (pid.getPortType() == CommPortIdentifier.PORT_SERIAL)
  53                         list.add(pid.getName());
  54                 }
  55                 return list.toArray(new String[0]);
  56         }
  57
  58
  59
  60         @SuppressWarnings("unchecked")
  61         public Alt15K(String name) throws IOException {
  62                 CommPortIdentifier pID = null;
  63
  64                 Enumeration portIdentifiers = CommPortIdentifier.getPortIdentifiers();
  65                 while (portIdentifiers.hasMoreElements()) {
  66                     CommPortIdentifier pid = (CommPortIdentifier) portIdentifiers.nextElement();
  67
  68                     if(pid.getPortType() == CommPortIdentifier.PORT_SERIAL &&
  69                        pid.getName().equals(name)) {
  70                         pID = pid;
  71                         break;
  72                     }
  73                 }
  74
  75                 if (pID==null) {
  76                         throw new IOException("Port '"+name+"' not found.");
  77                 }
  78                 this.portID = pID;
  79         }
  80
  81
  82         /**
  83          * Get altimeter flight data.  The flight profile is chosen by the parameter n,
  84          * 0 = latest flight, 1 = second latest, etc.
  85          *
  86          * @param n  Which flight profile to use (0=newest, 1=second newest, etc)
  87          * @return   The altimeter flight data
  88          * @throws IOException                  in case of IOException
  89          * @throws PortInUseException   in case of PortInUseException
  90          */
  91         public AltData getData(int n) throws IOException, PortInUseException {
  92                 AltData alt = new AltData();
  93                 ArrayList<Integer> data = new ArrayList<Integer>();
  94                 byte[] buf;
  95                 byte[] buf2 = new byte[0];
  96                 boolean identical = false;  // Whether identical lines have been read
  97
  98                 if (DEBUG)
  99                         System.out.println("  Retrieving altimeter data n="+n);
 100
 101                 try {
 102                         open();
 103
 104                         // Get version and position data
 105                         byte[] ver = getVersionData();
 106                         alt.setVersion(new byte[] { ver[0],ver[1] });
 107
 108                         // Calculate the position requested
 109                         if (n > 2)
 110                                 n = 2;
 111                         int position = ver[2] - n;
 112                         while (position < 0)
 113                                 position += 3;
 114
 115                         if (DEBUG)
 116                                 System.out.println("  Requesting data from position "+position);
 117
 118                         // Request the data
 119                         write("D");
 120                         write((byte)position);
 121                         write("PS");
 122
 123                         sleep();
 124
 125                         // Read preliminary data
 126                         buf = read(4);
 127                         int msl_level = combine(buf[0],buf[1]);
 128                         int datacount = combine(buf[2],buf[3]);
 129
 130                         if (DEBUG)
 131                                 System.out.println("  Preliminary data msl="+msl_level+" count="+datacount);
 132
 133                         alt.setMslLevel(msl_level-6000);
 134                         alt.setDataSamples(datacount);
 135
 136                         if (DEBUG)
 137                                 System.out.println("  Retrieving "+datacount+" samples");
 138
 139                         long t = System.currentTimeMillis();
 140
 141                         int count = 0;
 142                         while (count < datacount) {
 143                                 sleep();
 144                                 write("G");
 145                                 sleep();
 146                                 buf = read(17);
 147
 148                                 if (buf.length == 17) {
 149                                         // Checksum = sum of all bytes + 1
 150                                         // (signedness does not change the result)
 151                                         byte checksum = 1;
 152                                         for (int i=0; i<16; i++)
 153                                                 checksum += buf[i];
 154                                         if (checksum != buf[16]) {
 155                                                 printBytes("ERROR: Checksum fail on data (computed="+checksum+
 156                                                                 " orig="+buf[16]+")",buf);
 157                                                 System.out.println("Ignoring error");
 158                                         }
 159                                 } else {
 160                                         System.err.println("ERROR:  Only "+buf.length+" bytes read, should be 17");
 161                                 }
 162
 163                                 for (int i=0; i<buf.length-1; i+=2) {
 164                                         data.add(combine(buf[i],buf[i+1]));
 165                                         count++;
 166                                 }
 167
 168                                 /*
 169                                  * Check whether the data is identical to the previous data batch.  If reading
 170                                  * too fast, the data seems to become duplicated in the transfer.  We need to check
 171                                  * whether this has happened by attempting to read more data than is normally
 172                                  * available.
 173                                  */
 174                                 int c, l=Math.min(buf.length, buf2.length);
 175                                 for (c=0; c<l; c++) {
 176                                         if (buf[c] != buf2[c])
 177                                                 break;
 178                                 }
 179                                 if (c==l && buf.length == buf2.length)
 180                                         identical = true;
 181                                 buf2 = buf.clone();
 182                         }
 183
 184                         if (DEBUG)
 185                                 System.out.println("  Retrieved "+data.size()+" samples in "+
 186                                                 (System.currentTimeMillis()-t)+" ms");
 187
 188
 189                         // In case of identical lines, check for more data.  This would mean that the
 190                         // transfer was corrupted.
 191                         if (identical) {
 192                                 System.err.println("WARNING:  Duplicate data detected, possible error");
 193                         }
 194
 195                         // Test for more data
 196                         if (DEBUG)
 197                                 System.out.println("  Testing for more data");
 198                         sleep();
 199                         write("G");
 200                         sleep();
 201                         buf = read(17);
 202                         if (buf.length > 0) {
 203                                 System.err.println("ERROR: Data available after transfer! (length="+buf.length+")");
 204                         }
 205
 206
 207
 208
 209
 210
 211                         // Create an int[] array and set it
 212                         int[] d = new int[data.size()];
 213                         for (int i=0; i<d.length; i++)
 214                                 d[i] = data.get(i);
 215                         alt.setData(d);
 216
 217                 //  Catch all exceptions, close the port and re-throw the exception
 218                 } catch (PortInUseException e) {
 219                         close();
 220                         throw e;
 221                 } catch (IOException e) {
 222                         close();
 223                         throw e;
 224                 } catch (UnsupportedCommOperationException e) {
 225                         close();
 226                         throw new RuntimeException("Required function of RxTx library not supported",e);
 227                 } catch (RuntimeException e) {
 228                         // Catch-all for all other types of exceptions
 229                         close();
 230                         throw e;
 231                 }
 232
 233                 close();
 234                 return alt;
 235         }
 236
 237
 238
 239
 240         private byte[] getVersionData() throws PortInUseException, IOException,
 241                                                                                    UnsupportedCommOperationException {
 242                 byte[] ver = new byte[3];
 243                 byte[] buf;
 244
 245                 if (DEBUG)
 246                         System.out.println("  Retrieving altimeter version information");
 247
 248                 // Signal to altimeter we are here
 249                 write((byte)0);
 250                 sleep(15);  // Sleep for 15ms, data is incoming at 10 samples/sec
 251
 252                 // Get altimeter version, skip zeros
 253                 write("PV");
 254                 sleep();
 255                 buf = readSkipZero(2);
 256                 sleep();
 257                 if (buf.length != 2) {
 258                         close();
 259                         throw new IOException("Communication with altimeter failed.");
 260                 }
 261                 ver[0] = buf[0];
 262                 ver[1] = buf[1];
 263
 264                 // Get position of newest data
 265                 write("M");
 266                 sleep();
 267                 buf = read(1);
 268                 if (buf.length != 1) {
 269                         close();
 270                         throw new IOException("Communication with altimeter failed.");
 271                 }
 272                 ver[2] = buf[0];
 273
 274                 if (DEBUG)
 275                         System.out.println("  Received version info "+ver[0]+"."+ver[1]+", position "+ver[2]);
 276
 277                 return ver;
 278         }
 279
 280
 281         /**
 282          * Delay the communication by a small delay (RWDELAY ms).
 283          */
 284         private void sleep() {
 285                 sleep(RWDELAY);
 286         }
 287
 288         /**
 289          * Sleep for the given amount of milliseconds.
 290          */
 291         private void sleep(int n) {
 292                 try {
 293                         Thread.sleep(n);
 294                 } catch (InterruptedException ignore) { }
 295         }
 296
 297
 298         private void open()
 299         throws PortInUseException, IOException, UnsupportedCommOperationException {
 300                 if (port != null) {
 301                         System.err.println("ERROR: open() called with port="+port);
 302                         Thread.dumpStack();
 303                         close();
 304                 }
 305
 306                 if (DEBUG) {
 307                         System.out.println("  Opening port...");
 308                 }
 309
 310                 port = (SerialPort)portID.open("OpenRocket",1000);
 311
 312                 port.setSerialPortParams(9600, SerialPort.DATABITS_8, SerialPort.STOPBITS_1,
 313                                 SerialPort.PARITY_NONE);
 314
 315                 port.setInputBufferSize(1);
 316                 port.setOutputBufferSize(1);
 317
 318                 port.enableReceiveTimeout(TIMEOUT);
 319
 320                 is = port.getInputStream();
 321                 os = port.getOutputStream();
 322         }
 323
 324
 325         private byte[] readSkipZero(int n) throws IOException, UnsupportedCommOperationException {
 326                 long t = System.currentTimeMillis() + TIMEOUT*2;
 327
 328                 if (DEBUG) {
 329                         System.out.println("    readSkipZero "+n+" bytes");
 330                 }
 331
 332                 while (System.currentTimeMillis() < t) {
 333                         byte[] buf = read(n);
 334                         if (DEBUG)
 335                                 printBytes("      Received",buf);
 336
 337                         if (buf.length == 0)  // No data available
 338                                 return buf;
 339
 340                         // Skip zeros
 341                         int i;
 342                         for (i=0; i<buf.length; i++)
 343                                 if (buf[i] != 0)
 344                                         break;
 345
 346                         if (i==0)   // No zeros to skip
 347                                 return buf;
 348
 349                         if (i < buf.length) {
 350                                 // Partially read
 351                                 int count = buf.length-i;  // No. of data bytes
 352                                 byte[] array = new byte[n];
 353                                 System.arraycopy(buf, i, array, 0, count);
 354                                 buf = read(n-count);
 355                                 if (DEBUG)
 356                                         printBytes("      Received (partial)",buf);
 357                                 System.arraycopy(buf, 0, array, count, buf.length);
 358
 359                                 if (DEBUG)
 360                                         printBytes("    Returning",array);
 361                                 return array;
 362                         }
 363                 }
 364
 365                 if (DEBUG)
 366                         System.out.println("  No data read, returning empty");
 367                 return new byte[0];  // no data, only zeros
 368         }
 369
 370
 371         private byte[] read(int n) throws IOException, UnsupportedCommOperationException {
 372                 byte[] bytes = new byte[n];
 373
 374                 port.enableReceiveThreshold(n);
 375
 376                 long t = System.currentTimeMillis() + TIMEOUT;
 377                 int count = 0;
 378
 379                 if (DEBUG)
 380                         System.out.println("    Reading "+n+" bytes");
 381
 382                 while (count < n && System.currentTimeMillis() < t) {
 383                         byte[] buf = new byte[n-count];
 384                         int c = is.read(buf);
 385                         System.arraycopy(buf, 0, bytes, count, c);
 386                         count += c;
 387                 }
 388
 389                 byte[] array = new byte[count];
 390                 System.arraycopy(bytes, 0, array, 0, count);
 391
 392                 if (DEBUG)
 393                         printBytes("    Returning",array);
 394
 395                 return array;
 396         }
 397
 398         private void write(String s) throws IOException {
 399                 write(s.getBytes(CHARSET));
 400         }
 401
 402         private void write(byte ... bytes) throws IOException {
 403                 if (DEBUG)
 404                         printBytes("    Writing",bytes);
 405                 os.write(bytes);
 406         }
 407
 408         private void close() {
 409                 if (DEBUG)
 410                         System.out.println("  Closing port");
 411
 412                 SerialPort p = port;
 413                 port = null;
 414                 is = null;
 415                 os = null;
 416                 if (p != null)
 417                         p.close();
 418         }
 419
 420
 421
 422
 423
 424         public static void main(String[] arg) {
 425
 426                 if (arg.length != 1) {
 427                         System.err.println("Usage:  java Alt15K <basename>");
 428                         System.err.println("Files will be saved <basename>-old.log, -med and -new");
 429                         return;
 430                 }
 431
 432
 433                 String device = null;
 434                 String[] devices = Alt15K.getNames();
 435                 for (int i=0; i<devices.length; i++) {
 436                         if (devices[i].matches(".*USB.*")) {
 437                                 device = devices[i];
 438                                 break;
 439                         }
 440                 }
 441                 if (device == null) {
 442                         System.out.println("Device not found.");
 443                         return;
 444                 }
 445
 446
 447                 System.out.println("Selected device "+device);
 448
 449                 AltData alt = null;
 450                 String file;
 451                 try {
 452                         Alt15K p = new Alt15K(device);
 453
 454                         System.out.println("Retrieving newest data...");
 455                         alt = p.getData(0);
 456                         System.out.println("Apogee at "+alt.getApogee()+" feet");
 457
 458                         file = arg[0]+"-new.log";
 459                         System.out.println("Saving data to "+file+"...");
 460                         savefile(file,alt);
 461
 462
 463                         System.out.println("Retrieving medium data...");
 464                         alt = p.getData(1);
 465                         System.out.println("Apogee at "+alt.getApogee()+" feet");
 466
 467                         file = arg[0]+"-med.log";
 468                         System.out.println("Saving data to "+file+"...");
 469                         savefile(file,alt);
 470
 471
 472                         System.out.println("Retrieving oldest data...");
 473                         alt = p.getData(2);
 474                         System.out.println("Apogee at "+alt.getApogee()+" feet");
 475
 476                         file = arg[0]+"-old.log";
 477                         System.out.println("Saving data to "+file+"...");
 478                         savefile(file,alt);
 479
 480                 } catch (IOException e) {
 481                         e.printStackTrace();
 482                 } catch (PortInUseException e) {
 483                         e.printStackTrace();
 484                 }
 485
 486 //              System.out.println(alt);
 487 //              alt.printData();
 488
 489         }
 490
 491
 492         static private void savefile(String file, AltData data) throws FileNotFoundException {
 493
 494                 PrintStream output = new PrintStream(file);
 495
 496                 // WTF is this so difficult?!?
 497                 DateFormat fmt = DateFormat.getDateTimeInstance(DateFormat.MEDIUM, DateFormat.MEDIUM);
 498                 TimeZone tz=TimeZone.getTimeZone("GMT+3");
 499                 fmt.setTimeZone(tz);
 500
 501                 output.println("# Alt15K data, file "+file);
 502                 output.println("# Data retrieved at: "+fmt.format(new Date()));
 503                 output.println("# Values are in feet above launch level");
 504                 output.println("# ");
 505                 output.println("# Apogee = "+data.getApogee());
 506                 output.println("# MSL level = "+data.getMslLevel());
 507                 output.println("# Data count = "+data.getDataSamples());
 508
 509                 byte[] b = data.getVersion();
 510                 String s="";
 511                 for (int i=0; i<b.length; i++) {
 512                         if (s.equals(""))
 513                                 s = ""+((int)b[i]);
 514                         else
 515                                 s = s+"."+((int)b[i]);
 516                 }
 517                 output.println("# Altimeter version = " + s);
 518
 519                 int[] values = data.getData();
 520                 for (int i=0; i < values.length; i++) {
 521                         output.println(""+values[i]);
 522                 }
 523
 524                 output.close();
 525         }
 526
 527
 528         static private void printBytes(String str, byte[] b) {
 529                 printBytes(str, b,b.length);
 530         }
 531
 532         static private void printBytes(String str, byte[] b, int n) {
 533                 String s;
 534                 s = str+" "+n+" bytes:";
 535                 for (int i=0; i<n; i++) {
 536                         s += " "+unsign(b[i]);
 537                 }
 538                 System.out.println(s);
 539         }
 540
 541         static private int unsign(byte b) {
 542                 if (b >= 0)
 543                         return b;
 544                 else
 545                         return 256 + b;
 546         }
 547
 548         @SuppressWarnings("unused")
 549         static private int combine(int a, int b) {
 550                 return 256*a + b;
 551         }
 552
 553         static private int combine(byte a, byte b) {
 554                 int val = 256*unsign(a)+unsign(b);
 555                 if (val <= 32767)
 556                         return val;
 557                 else
 558                         return val-65536;
 559
 560         }
 561
 562 }