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