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 public class MicroData {
  26         public int              ground_pressure;
  27         public int              min_pressure;
  28         public int[]            pressures;
  29         private double          time_step;
  30         private double          ground_altitude;
  31         private ArrayList<Integer>      bytes;
  32
  33
  34         class FileEndedException extends Exception {
  35         }
  36
  37         class NonHexcharException extends Exception {
  38         }
  39
  40         class InvalidCrcException extends Exception {
  41         }
  42
  43         private int getc(InputStream f) throws IOException, FileEndedException {
  44                 int     c = f.read();
  45
  46                 if (c == -1)
  47                         throw new FileEndedException();
  48                 bytes.add(c);
  49                 return c;
  50         }
  51
  52         private int get_nonwhite(InputStream f) throws IOException, FileEndedException {
  53                 int     c;
  54
  55                 for (;;) {
  56                         c = getc(f);
  57                         if (!Character.isWhitespace(c))
  58                                 return c;
  59                 }
  60         }
  61
  62         private int get_hexc(InputStream f) throws IOException, FileEndedException, NonHexcharException {
  63                 int     c = get_nonwhite(f);
  64
  65                 if ('0' <= c && c <= '9')
  66                         return c - '0';
  67                 if ('a' <= c && c <= 'f')
  68                         return c - 'a' + 10;
  69                 if ('A' <= c && c <= 'F')
  70                         return c - 'A' + 10;
  71                 throw new NonHexcharException();
  72         }
  73
  74         private static final int POLY = 0x8408;
  75
  76         private int log_crc(int crc, int b) {
  77                 int     i;
  78
  79                 for (i = 0; i < 8; i++) {
  80                         if (((crc & 0x0001) ^ (b & 0x0001)) != 0)
  81                                 crc = (crc >> 1) ^ POLY;
  82                         else
  83                                 crc = crc >> 1;
  84                         b >>= 1;
  85                 }
  86                 return crc & 0xffff;
  87         }
  88
  89         int     file_crc;
  90
  91         private int get_hex(InputStream f) throws IOException, FileEndedException, NonHexcharException {
  92                 int     a = get_hexc(f);
  93                 int     b = get_hexc(f);
  94
  95                 int h = (a << 4) + b;
  96
  97                 file_crc = log_crc(file_crc, h);
  98                 return h;
  99         }
 100
 101         private boolean find_header(InputStream f) throws IOException {
 102                 try {
 103                         for (;;) {
 104                                 if (get_nonwhite(f) == 'M' && get_nonwhite(f) == 'P')
 105                                         return true;
 106                         }
 107                 } catch (FileEndedException fe) {
 108                         return false;
 109                 }
 110         }
 111
 112         private int get_32(InputStream f)  throws IOException, FileEndedException, NonHexcharException {
 113                 int     v = 0;
 114                 for (int i = 0; i < 4; i++) {
 115                         v += get_hex(f) << (i * 8);
 116                 }
 117                 return v;
 118         }
 119
 120         private int get_16(InputStream f) throws IOException, FileEndedException, NonHexcharException {
 121                 int     v = 0;
 122                 for (int i = 0; i < 2; i++) {
 123                         v += get_hex(f) << (i * 8);
 124                 }
 125                 return v;
 126         }
 127
 128         private int swap16(int i) {
 129                 return ((i << 8) & 0xff00) | ((i >> 8) & 0xff);
 130         }
 131
 132         public boolean  crc_valid;
 133
 134         int mix_in (int high, int low) {
 135                 return  high - (high & 0xffff) + low;
 136         }
 137
 138         boolean closer (int target, int a, int b) {
 139                 return Math.abs (target - a) < Math.abs(target - b);
 140         }
 141
 142         public double altitude(int i) {
 143                 return AltosConvert.pressure_to_altitude(pressures[i]);
 144         }
 145
 146         int fact(int n) {
 147                 if (n == 0)
 148                         return 1;
 149                 return n * fact(n-1);
 150         }
 151
 152         int choose(int n, int k) {
 153                 return fact(n) / (fact(k) * fact(n-k));
 154         }
 155
 156
 157         public double avg_altitude(int center, int dist) {
 158                 int     start = center - dist;
 159                 int     stop = center + dist;
 160
 161                 if (start < 0)
 162                         start = 0;
 163                 if (stop >= pressures.length)
 164                         stop = pressures.length - 1;
 165
 166                 double  sum = 0;
 167                 double  div = 0;
 168
 169                 int     n = dist * 2;
 170
 171                 for (int i = start; i <= stop; i++) {
 172                         int     k = i - (center - dist);
 173                         int     c = choose (n, k);
 174
 175                         sum += c * pressures[i];
 176                         div += c;
 177                 }
 178
 179                 double pres = sum / div;
 180
 181                 double alt = AltosConvert.pressure_to_altitude(pres);
 182                 return alt;
 183         }
 184
 185         public double height(int i) {
 186                 return altitude(i) - ground_altitude;
 187         }
 188
 189         static final int speed_avg = 3;
 190         static final int accel_avg = 5;
 191
 192         private double avg_speed(int center, int dist) {
 193                 if (center == 0)
 194                         return 0;
 195
 196                 double ai = avg_altitude(center, dist);
 197                 double aj = avg_altitude(center - 1, dist);
 198                 double s = (ai - aj) / time_step;
 199
 200                 return s;
 201         }
 202
 203         public double speed(int i) {
 204                 return avg_speed(i, speed_avg);
 205         }
 206
 207         public double acceleration(int i) {
 208                 if (i == 0)
 209                         return 0;
 210                 return (avg_speed(i, accel_avg) - avg_speed(i-1, accel_avg)) / time_step;
 211         }
 212
 213         public double time(int i) {
 214                 return i * time_step;
 215         }
 216
 217         public void save (OutputStream f) throws IOException {
 218                 for (int c : bytes)
 219                         f.write(c);
 220         }
 221
 222         public MicroData (InputStream f) throws IOException {
 223                 bytes = new ArrayList<Integer>();
 224                 if (!find_header(f))
 225                         throw new IOException();
 226                 try {
 227                         file_crc = 0xffff;
 228                         ground_pressure = get_32(f);
 229                         min_pressure = get_32(f);
 230                         int nsamples = get_16(f);
 231                         pressures = new int[nsamples + 1];
 232
 233                         ground_altitude = AltosConvert.pressure_to_altitude(ground_pressure);
 234                         int cur = ground_pressure;
 235                         pressures[0] = cur;
 236                         for (int i = 0; i < nsamples; i++) {
 237                                 int     k = get_16(f);
 238                                 int     same = mix_in(cur, k);
 239                                 int     up = mix_in(cur + 0x10000, k);
 240                                 int     down = mix_in(cur - 0x10000, k);
 241
 242                                 if (closer (cur, same, up)) {
 243                                         if (closer (cur, same, down))
 244                                                 cur = same;
 245                                         else
 246                                                 cur = down;
 247                                 } else {
 248                                         if (closer (cur, up, down))
 249                                                 cur = up;
 250                                         else
 251                                                 cur = down;
 252                                 }
 253
 254                                 pressures[i+1] = cur;
 255                         }
 256
 257                         int current_crc = swap16(~file_crc & 0xffff);
 258                         int crc = get_16(f);
 259
 260                         crc_valid = crc == current_crc;
 261
 262                         time_step = 0.192;
 263                 } catch (FileEndedException fe) {
 264                         throw new IOException();
 265                 } catch (NonHexcharException ne) {
 266                         throw new IOException();
 267                 }
 268         }
 269
 270 }