git.gag.com Git - fw/altos/blob - micropeak/MicroData.java

   1 /*
   2  * Copyright © 2012 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.micropeak;
  19
  20 import java.lang.*;
  21 import java.io.*;
  22 import java.util.*;
  23 import org.altusmetrum.altoslib_1.*;
  24
  25 class MicroIterator implements Iterator<MicroDataPoint> {
  26         int             i;
  27         MicroData       data;
  28
  29         public boolean hasNext() {
  30                 return i < data.pressures.length;
  31         }
  32
  33         public MicroDataPoint next() {
  34                 return new MicroDataPoint(data, i++);
  35         }
  36
  37         public MicroIterator (MicroData data) {
  38                 this.data = data;
  39                 i = 0;
  40         }
  41
  42         public void remove() {
  43         }
  44 }
  45
  46 class MicroIterable implements Iterable<MicroDataPoint> {
  47
  48         MicroData       data;
  49
  50         public Iterator<MicroDataPoint> iterator() {
  51                 return new MicroIterator(data);
  52         }
  53
  54         public MicroIterable(MicroData data) {
  55                 this.data = data;
  56         }
  57 }
  58
  59 public class MicroData {
  60         public int              ground_pressure;
  61         public int              min_pressure;
  62         public int[]            pressures;
  63         private double          time_step;
  64         private double          ground_altitude;
  65         private ArrayList<Integer>      bytes;
  66         String                  name;
  67
  68
  69         class FileEndedException extends Exception {
  70         }
  71
  72         class NonHexcharException extends Exception {
  73         }
  74
  75         class InvalidCrcException extends Exception {
  76         }
  77
  78         private int getc(InputStream f) throws IOException, FileEndedException {
  79                 int     c = f.read();
  80
  81                 if (c == -1)
  82                         throw new FileEndedException();
  83                 bytes.add(c);
  84                 return c;
  85         }
  86
  87         private int get_nonwhite(InputStream f) throws IOException, FileEndedException {
  88                 int     c;
  89
  90                 for (;;) {
  91                         c = getc(f);
  92                         if (!Character.isWhitespace(c))
  93                                 return c;
  94                 }
  95         }
  96
  97         private int get_hexc(InputStream f) throws IOException, FileEndedException, NonHexcharException {
  98                 int     c = get_nonwhite(f);
  99
 100                 if ('0' <= c && c <= '9')
 101                         return c - '0';
 102                 if ('a' <= c && c <= 'f')
 103                         return c - 'a' + 10;
 104                 if ('A' <= c && c <= 'F')
 105                         return c - 'A' + 10;
 106                 throw new NonHexcharException();
 107         }
 108
 109         private static final int POLY = 0x8408;
 110
 111         private int log_crc(int crc, int b) {
 112                 int     i;
 113
 114                 for (i = 0; i < 8; i++) {
 115                         if (((crc & 0x0001) ^ (b & 0x0001)) != 0)
 116                                 crc = (crc >> 1) ^ POLY;
 117                         else
 118                                 crc = crc >> 1;
 119                         b >>= 1;
 120                 }
 121                 return crc & 0xffff;
 122         }
 123
 124         int     file_crc;
 125
 126         private int get_hex(InputStream f) throws IOException, FileEndedException, NonHexcharException {
 127                 int     a = get_hexc(f);
 128                 int     b = get_hexc(f);
 129
 130                 int h = (a << 4) + b;
 131
 132                 file_crc = log_crc(file_crc, h);
 133                 return h;
 134         }
 135
 136         private boolean find_header(InputStream f) throws IOException {
 137                 try {
 138                         for (;;) {
 139                                 if (get_nonwhite(f) == 'M' && get_nonwhite(f) == 'P')
 140                                         return true;
 141                         }
 142                 } catch (FileEndedException fe) {
 143                         return false;
 144                 }
 145         }
 146
 147         private int get_32(InputStream f)  throws IOException, FileEndedException, NonHexcharException {
 148                 int     v = 0;
 149                 for (int i = 0; i < 4; i++) {
 150                         v += get_hex(f) << (i * 8);
 151                 }
 152                 return v;
 153         }
 154
 155         private int get_16(InputStream f) throws IOException, FileEndedException, NonHexcharException {
 156                 int     v = 0;
 157                 for (int i = 0; i < 2; i++) {
 158                         v += get_hex(f) << (i * 8);
 159                 }
 160                 return v;
 161         }
 162
 163         private int swap16(int i) {
 164                 return ((i << 8) & 0xff00) | ((i >> 8) & 0xff);
 165         }
 166
 167         public boolean  crc_valid;
 168
 169         int mix_in (int high, int low) {
 170                 return  high - (high & 0xffff) + low;
 171         }
 172
 173         boolean closer (int target, int a, int b) {
 174                 return Math.abs (target - a) < Math.abs(target - b);
 175         }
 176
 177         public double altitude(int i) {
 178                 return AltosConvert.pressure_to_altitude(pressures[i]);
 179         }
 180
 181         public Iterable<MicroDataPoint> points() {
 182                 return new MicroIterable(this);
 183         }
 184
 185         int fact(int n) {
 186                 if (n == 0)
 187                         return 1;
 188                 return n * fact(n-1);
 189         }
 190
 191         int choose(int n, int k) {
 192                 return fact(n) / (fact(k) * fact(n-k));
 193         }
 194
 195
 196         public double avg_altitude(int center, int dist) {
 197                 int     start = center - dist;
 198                 int     stop = center + dist;
 199
 200                 if (start < 0)
 201                         start = 0;
 202                 if (stop >= pressures.length)
 203                         stop = pressures.length - 1;
 204
 205                 double  sum = 0;
 206                 double  div = 0;
 207
 208                 int     n = dist * 2;
 209
 210                 for (int i = start; i <= stop; i++) {
 211                         int     k = i - (center - dist);
 212                         int     c = choose (n, k);
 213
 214                         sum += c * pressures[i];
 215                         div += c;
 216                 }
 217
 218                 double pres = sum / div;
 219
 220                 double alt = AltosConvert.pressure_to_altitude(pres);
 221                 return alt;
 222         }
 223
 224         public double pressure(int i) {
 225                 return pressures[i];
 226         }
 227
 228         public double height(int i) {
 229                 return altitude(i) - ground_altitude;
 230         }
 231
 232         public double apogee_pressure() {
 233                 return min_pressure;
 234         }
 235
 236         public double apogee_altitude() {
 237                 return AltosConvert.pressure_to_altitude(apogee_pressure());
 238         }
 239
 240         public double apogee_height() {
 241                 return apogee_altitude() - ground_altitude;
 242         }
 243
 244         static final int speed_avg = 3;
 245         static final int accel_avg = 5;
 246
 247         private double avg_speed(int center, int dist) {
 248                 if (center == 0)
 249                         return 0;
 250
 251                 double ai = avg_altitude(center, dist);
 252                 double aj = avg_altitude(center - 1, dist);
 253                 double s = (ai - aj) / time_step;
 254
 255                 return s;
 256         }
 257
 258         public double speed(int i) {
 259                 return avg_speed(i, speed_avg);
 260         }
 261
 262         public double acceleration(int i) {
 263                 if (i == 0)
 264                         return 0;
 265                 return (avg_speed(i, accel_avg) - avg_speed(i-1, accel_avg)) / time_step;
 266         }
 267
 268         public double time(int i) {
 269                 return i * time_step;
 270         }
 271
 272         public void save (OutputStream f) throws IOException {
 273                 for (int c : bytes)
 274                         f.write(c);
 275                 f.write('\n');
 276         }
 277
 278         public void export (Writer f) throws IOException {
 279                 PrintWriter     pw = new PrintWriter(f);
 280                 pw.printf("  Time, Press(Pa), Height(m), Height(f), Speed(m/s), Speed(mph), Speed(mach), Accel(m/s²), Accel(ft/s²),  Accel(g)\n");
 281                 for (MicroDataPoint point : points()) {
 282                         pw.printf("%6.3f,%10.0f,%10.1f,%10.1f,%11.2f,%11.2f,%12.4f,%12.2f,%13.2f,%10.4f\n",
 283                                   point.time,
 284                                   point.pressure,
 285                                   point.height,
 286                                   AltosConvert.meters_to_feet(point.height),
 287                                   point.speed,
 288                                   AltosConvert.meters_to_mph(point.speed),
 289                                   AltosConvert.meters_to_mach(point.speed),
 290                                   point.accel,
 291                                   AltosConvert.meters_to_feet(point.accel),
 292                                   AltosConvert.meters_to_g(point.accel));
 293                 }
 294         }
 295
 296         public void set_name(String name) {
 297                 this.name = name;
 298         }
 299
 300         public MicroData (InputStream f, String name) throws IOException, InterruptedException {
 301                 this.name = name;
 302                 bytes = new ArrayList<Integer>();
 303                 if (!find_header(f))
 304                         throw new IOException("No MicroPeak data header found");
 305                 try {
 306                         file_crc = 0xffff;
 307                         ground_pressure = get_32(f);
 308                         min_pressure = get_32(f);
 309                         int nsamples = get_16(f);
 310                         pressures = new int[nsamples + 1];
 311
 312                         ground_altitude = AltosConvert.pressure_to_altitude(ground_pressure);
 313                         int cur = ground_pressure;
 314                         pressures[0] = cur;
 315                         for (int i = 0; i < nsamples; i++) {
 316                                 int     k = get_16(f);
 317                                 int     same = mix_in(cur, k);
 318                                 int     up = mix_in(cur + 0x10000, k);
 319                                 int     down = mix_in(cur - 0x10000, k);
 320
 321                                 if (closer (cur, same, up)) {
 322                                         if (closer (cur, same, down))
 323                                                 cur = same;
 324                                         else
 325                                                 cur = down;
 326                                 } else {
 327                                         if (closer (cur, up, down))
 328                                                 cur = up;
 329                                         else
 330                                                 cur = down;
 331                                 }
 332
 333                                 pressures[i+1] = cur;
 334                         }
 335
 336                         int current_crc = swap16(~file_crc & 0xffff);
 337                         int crc = get_16(f);
 338
 339                         crc_valid = crc == current_crc;
 340
 341                         time_step = 0.192;
 342                 } catch (FileEndedException fe) {
 343                         throw new IOException("File Ended Unexpectedly");
 344                 } catch (NonHexcharException ne) {
 345                         throw new IOException("Non hexadecimal character found");
 346                 }
 347         }
 348
 349         public MicroData() {
 350                 ground_pressure = 101000;
 351                 min_pressure = 101000;
 352                 pressures = new int[1];
 353                 pressures[0] = 101000;
 354         }
 355
 356 }