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
 114
 115         class FileEndedException extends Exception {
 116         }
 117
 118         class NonHexcharException extends Exception {
 119         }
 120
 121         class InvalidCrcException extends Exception {
 122         }
 123
 124         private int getc(InputStream f) throws IOException, FileEndedException {
 125                 int     c = f.read();
 126
 127                 if (c == -1)
 128                         throw new FileEndedException();
 129                 bytes.add(c);
 130                 return c;
 131         }
 132
 133         private int get_nonwhite(InputStream f) throws IOException, FileEndedException {
 134                 int     c;
 135
 136                 for (;;) {
 137                         c = getc(f);
 138                         if (!Character.isWhitespace(c))
 139                                 return c;
 140                 }
 141         }
 142
 143         private int get_hexc(InputStream f) throws IOException, FileEndedException, NonHexcharException {
 144                 int     c = get_nonwhite(f);
 145
 146                 if ('0' <= c && c <= '9')
 147                         return c - '0';
 148                 if ('a' <= c && c <= 'f')
 149                         return c - 'a' + 10;
 150                 if ('A' <= c && c <= 'F')
 151                         return c - 'A' + 10;
 152                 throw new NonHexcharException();
 153         }
 154
 155         private static final int POLY = 0x8408;
 156
 157         private int log_crc(int crc, int b) {
 158                 int     i;
 159
 160                 for (i = 0; i < 8; i++) {
 161                         if (((crc & 0x0001) ^ (b & 0x0001)) != 0)
 162                                 crc = (crc >> 1) ^ POLY;
 163                         else
 164                                 crc = crc >> 1;
 165                         b >>= 1;
 166                 }
 167                 return crc & 0xffff;
 168         }
 169
 170         int     file_crc;
 171
 172         private int get_hex(InputStream f) throws IOException, FileEndedException, NonHexcharException {
 173                 int     a = get_hexc(f);
 174                 int     b = get_hexc(f);
 175
 176                 int h = (a << 4) + b;
 177
 178                 file_crc = log_crc(file_crc, h);
 179                 return h;
 180         }
 181
 182         private boolean find_header(InputStream f) throws IOException {
 183                 try {
 184                         for (;;) {
 185                                 if (get_nonwhite(f) == 'M' && get_nonwhite(f) == 'P')
 186                                         return true;
 187                         }
 188                 } catch (FileEndedException fe) {
 189                         return false;
 190                 }
 191         }
 192
 193         private int get_32(InputStream f)  throws IOException, FileEndedException, NonHexcharException {
 194                 int     v = 0;
 195                 for (int i = 0; i < 4; i++) {
 196                         v += get_hex(f) << (i * 8);
 197                 }
 198                 return v;
 199         }
 200
 201         private int get_16(InputStream f) throws IOException, FileEndedException, NonHexcharException {
 202                 int     v = 0;
 203                 for (int i = 0; i < 2; i++) {
 204                         v += get_hex(f) << (i * 8);
 205                 }
 206                 return v;
 207         }
 208
 209         private int swap16(int i) {
 210                 return ((i << 8) & 0xff00) | ((i >> 8) & 0xff);
 211         }
 212
 213         public boolean  crc_valid;
 214
 215         int mix_in (int high, int low) {
 216                 return  high - (high & 0xffff) + low;
 217         }
 218
 219         boolean closer (int target, int a, int b) {
 220                 return Math.abs (target - a) < Math.abs(target - b);
 221         }
 222
 223         public double altitude(int i) {
 224                 return AltosConvert.pressure_to_altitude(pressures[i]);
 225         }
 226
 227         public Iterable<Double> heights() {
 228                 return new MicroHeightIterable(this);
 229         }
 230
 231         public Iterable<Double> speeds() {
 232                 return new MicroSpeedIterable(this);
 233         }
 234
 235         public Iterable<Double> accels() {
 236                 return new MicroAccelIterable(this);
 237         }
 238
 239         int fact(int n) {
 240                 if (n == 0)
 241                         return 1;
 242                 return n * fact(n-1);
 243         }
 244
 245         int choose(int n, int k) {
 246                 return fact(n) / (fact(k) * fact(n-k));
 247         }
 248
 249
 250         public double avg_altitude(int center, int dist) {
 251                 int     start = center - dist;
 252                 int     stop = center + dist;
 253
 254                 if (start < 0)
 255                         start = 0;
 256                 if (stop >= pressures.length)
 257                         stop = pressures.length - 1;
 258
 259                 double  sum = 0;
 260                 double  div = 0;
 261
 262                 int     n = dist * 2;
 263
 264                 for (int i = start; i <= stop; i++) {
 265                         int     k = i - (center - dist);
 266                         int     c = choose (n, k);
 267
 268                         sum += c * pressures[i];
 269                         div += c;
 270                 }
 271
 272                 double pres = sum / div;
 273
 274                 double alt = AltosConvert.pressure_to_altitude(pres);
 275                 return alt;
 276         }
 277
 278         public double height(int i) {
 279                 return altitude(i) - ground_altitude;
 280         }
 281
 282         static final int speed_avg = 3;
 283         static final int accel_avg = 5;
 284
 285         private double avg_speed(int center, int dist) {
 286                 if (center == 0)
 287                         return 0;
 288
 289                 double ai = avg_altitude(center, dist);
 290                 double aj = avg_altitude(center - 1, dist);
 291                 double s = (ai - aj) / time_step;
 292
 293                 return s;
 294         }
 295
 296         public double speed(int i) {
 297                 return avg_speed(i, speed_avg);
 298         }
 299
 300         public double acceleration(int i) {
 301                 if (i == 0)
 302                         return 0;
 303                 return (avg_speed(i, accel_avg) - avg_speed(i-1, accel_avg)) / time_step;
 304         }
 305
 306         public double time(int i) {
 307                 return i * time_step;
 308         }
 309
 310         public void save (OutputStream f) throws IOException {
 311                 for (int c : bytes)
 312                         f.write(c);
 313         }
 314
 315         public MicroData (InputStream f) throws IOException {
 316                 bytes = new ArrayList<Integer>();
 317                 if (!find_header(f))
 318                         throw new IOException();
 319                 try {
 320                         file_crc = 0xffff;
 321                         ground_pressure = get_32(f);
 322                         min_pressure = get_32(f);
 323                         int nsamples = get_16(f);
 324                         pressures = new int[nsamples + 1];
 325
 326                         ground_altitude = AltosConvert.pressure_to_altitude(ground_pressure);
 327                         int cur = ground_pressure;
 328                         pressures[0] = cur;
 329                         for (int i = 0; i < nsamples; i++) {
 330                                 int     k = get_16(f);
 331                                 int     same = mix_in(cur, k);
 332                                 int     up = mix_in(cur + 0x10000, k);
 333                                 int     down = mix_in(cur - 0x10000, k);
 334
 335                                 if (closer (cur, same, up)) {
 336                                         if (closer (cur, same, down))
 337                                                 cur = same;
 338                                         else
 339                                                 cur = down;
 340                                 } else {
 341                                         if (closer (cur, up, down))
 342                                                 cur = up;
 343                                         else
 344                                                 cur = down;
 345                                 }
 346
 347                                 pressures[i+1] = cur;
 348                         }
 349
 350                         int current_crc = swap16(~file_crc & 0xffff);
 351                         int crc = get_16(f);
 352
 353                         crc_valid = crc == current_crc;
 354
 355                         time_step = 0.192;
 356                 } catch (FileEndedException fe) {
 357                         throw new IOException();
 358                 } catch (NonHexcharException ne) {
 359                         throw new IOException();
 360                 }
 361         }
 362
 363         public MicroData() {
 364                 ground_pressure = 101000;
 365                 min_pressure = 101000;
 366                 pressures = new int[1];
 367                 pressures[0] = 101000;
 368         }
 369
 370 }