git.gag.com Git - debian/openrocket/blob - core/src-extra/altimeter/RotationLogger.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.FileInputStream;
   9 import java.io.FileOutputStream;
  10 import java.io.IOException;
  11 import java.io.InputStream;
  12 import java.io.OutputStream;
  13 import java.util.ArrayList;
  14 import java.util.Enumeration;
  15
  16 /**
  17  * Class to interface the PerfectFlite Alt15K/WD altimeter.
  18  *
  19  * Also includes a main method that retrieves all flight profiles and saves them to files.
  20  *
  21  * @author Sampo Niskanen <sampo.niskanen@iki.fi>
  22  */
  23
  24 public class RotationLogger {
  25         private static final boolean DEBUG = false;
  26
  27         private static final int BYTES = 65536;
  28
  29
  30         private final CommPortIdentifier portID;
  31         private SerialPort port = null;
  32         private InputStream is = null;
  33         private OutputStream os = null;
  34
  35
  36
  37         @SuppressWarnings("unchecked")
  38         public static String[] getNames() {
  39                 ArrayList<String> list = new ArrayList<String>();;
  40
  41                 Enumeration pids = CommPortIdentifier.getPortIdentifiers();
  42
  43                 while (pids.hasMoreElements()) {
  44                     CommPortIdentifier pid = (CommPortIdentifier) pids.nextElement();
  45
  46                     if (pid.getPortType() == CommPortIdentifier.PORT_SERIAL)
  47                         list.add(pid.getName());
  48                 }
  49                 return list.toArray(new String[0]);
  50         }
  51
  52
  53
  54
  55
  56         @SuppressWarnings("unchecked")
  57         public RotationLogger(String name) throws IOException {
  58                 CommPortIdentifier portID = null;
  59
  60                 Enumeration portIdentifiers = CommPortIdentifier.getPortIdentifiers();
  61                 while (portIdentifiers.hasMoreElements()) {
  62                     CommPortIdentifier pid = (CommPortIdentifier) portIdentifiers.nextElement();
  63
  64                     if(pid.getPortType() == CommPortIdentifier.PORT_SERIAL &&
  65                        pid.getName().equals(name)) {
  66                         portID = pid;
  67                         break;
  68                     }
  69                 }
  70
  71                 if (portID==null) {
  72                         throw new IOException("Port '"+name+"' not found.");
  73                 }
  74                 this.portID = portID;
  75         }
  76
  77
  78
  79
  80
  81
  82         public void readData() throws IOException, PortInUseException {
  83                 int c;
  84
  85                 int[] data = new int[BYTES];
  86
  87                 FileOutputStream rawdump = null;
  88
  89
  90                 try {
  91                         open();
  92
  93                         System.err.println("Sending dump mode command...");
  94
  95                         for (int i=0; i<16; i++) {
  96                                 os.write('D');
  97                                 try {
  98                                         Thread.sleep(10);
  99                                 } catch (InterruptedException ignore) { }
 100                         }
 101
 102                         System.err.println("Waiting for response...");
 103                         while (true) {
 104                                 c = is.read();
 105                                 if (c == 'K') {
 106                                         break;
 107                                 } else {
 108                                         System.err.printf("Received spurious c=%d\n",c);
 109                                 }
 110                         }
 111
 112                         System.err.println("Received response.");
 113
 114
 115
 116                         System.err.println("Opening 'rawdump'...");
 117                         rawdump = new FileOutputStream("rawdump");
 118
 119
 120
 121                         System.err.println("Performing dump...");
 122
 123                         os.write('A');
 124
 125                         byte[] buffer = new byte[1024];
 126                         int printCount = 0;
 127                         for (int count=0; count < BYTES; ) {
 128                                 if ((BYTES-count) < buffer.length) {
 129                                         buffer = new byte[BYTES-count];
 130                                 }
 131
 132                                 int n = is.read(buffer);
 133                                 if (n < 0) {
 134                                         System.err.println("Error condition, n="+n);
 135                                         return;
 136                                 }
 137
 138                                 rawdump.write(buffer, 0, n);
 139
 140                                 for (int i=0; i<n; i++) {
 141                                         data[count+i] = unsign(buffer[i]);
 142                                 }
 143                                 count += n;
 144                                 if (count - printCount > 1024) {
 145                                         System.err.println("Read "+count+" bytes...");
 146                                         printCount = count;
 147                                 }
 148                         }
 149
 150
 151                         System.err.println("Verifying checksum...");
 152                         int reported = is.read();
 153
 154                         byte computed = 0;
 155                         for (int i=0; i < data.length; i++) {
 156                                 computed += data[i];
 157                         }
 158                         if (computed == reported) {
 159                                 System.err.println("Checksum ok ("+computed+")");
 160                         } else {
 161                                 System.err.println("Error in checksum, computed="+computed+
 162                                                 " reported="+reported);
 163                         }
 164
 165                         System.err.println("Communication done.");
 166
 167                 } catch (UnsupportedCommOperationException e) {
 168                         // TODO Auto-generated catch block
 169                         e.printStackTrace();
 170                 } finally {
 171                         close();
 172                         if (rawdump != null)
 173                                 rawdump.close();
 174                 }
 175
 176                 convertData(data);
 177
 178         }
 179
 180
 181
 182         ////////////  Data interpretation   //////////////
 183
 184
 185         private static void convertData(int[] data) {
 186
 187                 System.err.println("Converting data...");
 188
 189                 int lastBuffer = data[0xffff];
 190                 if (lastBuffer < 0 || lastBuffer > 3) {
 191                         System.err.println("Illegal last accessed buffer: "+lastBuffer);
 192                         return;
 193                 }
 194                 System.err.println("Last used buffer: "+lastBuffer);
 195
 196                 for (int i=4; i>0; i--) {
 197                         int n = (lastBuffer + i) % 4;
 198                         int bufNumber = 4-i;
 199
 200                         convertBuffer(data, n * (BYTES/4), bufNumber);
 201                 }
 202
 203         }
 204
 205
 206         private static void convertBuffer(int[] data, int position, int bufNumber) {
 207                 int startPosition;
 208
 209                 startPosition = data[position + 0xfd] << 8 + data[position+0xfe];
 210
 211                 // 50 samples per 128 bytes
 212                 int startTime = (startPosition -position) * 50 / 128;
 213
 214                 System.err.println("  Buffer "+ bufNumber + " (at position "+position+")...");
 215                 System.err.println("  Start position "+startPosition+" time "+startTime);
 216
 217                 System.out.println("# Buffer "+bufNumber);
 218                 System.out.println("# Start position t="+startTime);
 219
 220
 221                 int t = 0;
 222                 for (int page = 0; page < 128; page++) {
 223                         int pageStart = position + page * 128;
 224
 225                         if (pageStart == startPosition) {
 226                                 System.out.println("# ---clip---");
 227                         }
 228
 229                         for (int i=0; i<125; i += 5) {
 230                                 int sample1, sample2;
 231
 232                                 int start = pageStart + i;
 233 //                              System.err.println("page="+page+" i="+i+
 234 //                                              " position="+position+" pageStart="+pageStart+" start="+start);
 235
 236                                 sample1 = (data[start] << 2) + (data[start+1] >> 6);
 237                                 sample2 = ((data[start+1] & 0x3f) << 4) + (data[start+2] >> 4);
 238                                 System.out.printf("%d  %4d  %4d %4d\n", bufNumber, t, sample1, sample2);
 239                                 t++;
 240
 241                                 sample1 = ((data[start+2] & 0x0f) << 6) + (data[start+3] >> 2);
 242                                 sample2 = ((data[start+3] & 3) << 8) + data[start+4];
 243                                 System.out.printf("%d  %4d  %4d %4d\n", bufNumber, t, sample1, sample2);
 244                                 t++;
 245                         }
 246                 }
 247         }
 248
 249
 250
 251         private void open() throws PortInUseException, IOException,
 252                         UnsupportedCommOperationException {
 253
 254                 if (port != null) {
 255                         System.err.println("ERROR: open() called with port="+port);
 256                         Thread.dumpStack();
 257                         close();
 258                 }
 259
 260                 if (DEBUG) {
 261                         System.err.println("  Opening port...");
 262                 }
 263
 264                 port = (SerialPort)portID.open("OpenRocket",1000);
 265
 266                 port.setSerialPortParams(9600, SerialPort.DATABITS_8, SerialPort.STOPBITS_1,
 267                                 SerialPort.PARITY_NONE);
 268
 269                 port.setInputBufferSize(1);
 270                 port.setOutputBufferSize(1);
 271
 272                 is = port.getInputStream();
 273                 os = port.getOutputStream();
 274         }
 275
 276
 277         private void close() {
 278                 if (DEBUG)
 279                         System.err.println("  Closing port");
 280
 281                 SerialPort p = port;
 282                 port = null;
 283                 is = null;
 284                 if (p != null)
 285                         p.close();
 286         }
 287
 288
 289
 290         private static int unsign(byte b) {
 291                 if (b >= 0)
 292                         return b;
 293                 else
 294                 return 256 + b;
 295         }
 296
 297
 298
 299
 300         public static void main(String[] arg) throws Exception {
 301
 302                 if (arg.length > 2) {
 303                         System.err.println("Illegal arguments.");
 304                         return;
 305                 }
 306                 if (arg.length == 1) {
 307                         FileInputStream is = new FileInputStream(arg[0]);
 308                         byte[] buffer = new byte[BYTES];
 309                         int n = is.read(buffer);
 310                         if (n != BYTES) {
 311                                 System.err.println("Could read only "+n+" bytes");
 312                                 return;
 313                         }
 314
 315                         int[] data = new int[BYTES];
 316                         for (int i=0; i<BYTES; i++) {
 317                                 data[i] = unsign(buffer[i]);
 318                         }
 319
 320                         int checksum=0;
 321                         for (int i=0; i<BYTES; i++) {
 322                                 checksum += data[i];
 323                         }
 324                         checksum = checksum%256;
 325                         System.err.println("Checksum: "+checksum);
 326
 327                         convertData(data);
 328                         return;
 329                 }
 330
 331
 332                 String device = null;
 333                 String[] devices = RotationLogger.getNames();
 334                 for (int i=0; i<devices.length; i++) {
 335                         if (devices[i].matches(".*USB.*")) {
 336                                 device = devices[i];
 337                                 break;
 338                         }
 339                 }
 340                 if (device == null) {
 341                         System.err.println("Device not found.");
 342                         return;
 343                 }
 344
 345
 346                 System.err.println("Selected device "+device);
 347
 348
 349                 RotationLogger p = new RotationLogger(device);
 350
 351                 p.readData();
 352
 353         }
 354
 355
 356 }