git.gag.com Git - fw/altos/blob - altoslib/AltosConvert.java

   1 /*
   2  * Copyright © 2010 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; either version 2 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * This program is distributed in the hope that it will be useful, but
  10  * WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  12  * General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License along
  15  * with this program; if not, write to the Free Software Foundation, Inc.,
  16  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  17  */
  18
  19 /*
  20  * Sensor data conversion functions
  21  */
  22 package org.altusmetrum.altoslib_14;
  23
  24 import java.util.*;
  25
  26 public class AltosConvert {
  27
  28         public static final double gravity = 9.80665;
  29
  30         /*
  31          * Pressure Sensor Model, version 1.1
  32          *
  33          * written by Holly Grimes
  34          *
  35          * Uses the International Standard Atmosphere as described in
  36          *   "A Quick Derivation relating altitude to air pressure" (version 1.03)
  37          *    from the Portland State Aerospace Society, except that the atmosphere
  38          *    is divided into layers with each layer having a different lapse rate.
  39          *
  40          * Lapse rate data for each layer was obtained from Wikipedia on Sept. 1, 2007
  41          *    at site <http://en.wikipedia.org/wiki/International_Standard_Atmosphere
  42          *
  43          * Height measurements use the local tangent plane.  The postive z-direction is up.
  44          *
  45          * All measurements are given in SI units (Kelvin, Pascal, meter, meters/second^2).
  46          *   The lapse rate is given in Kelvin/meter, the gas constant for air is given
  47          *   in Joules/(kilogram-Kelvin).
  48          */
  49
  50         private static final double GRAVITATIONAL_ACCELERATION = -gravity;
  51         private static final double AIR_GAS_CONSTANT            = 287.053;
  52         private static final double NUMBER_OF_LAYERS            = 7;
  53         private static final double MAXIMUM_ALTITUDE            = 84852.0;
  54         private static final double MINIMUM_PRESSURE            = 0.3734;
  55         private static final double LAYER0_BASE_TEMPERATURE     = 288.15;
  56         private static final double LAYER0_BASE_PRESSURE        = 101325;
  57
  58         /* lapse rate and base altitude for each layer in the atmosphere */
  59         private static final double[] lapse_rate = {
  60                 -0.0065, 0.0, 0.001, 0.0028, 0.0, -0.0028, -0.002
  61         };
  62
  63         private static final int[] base_altitude = {
  64                 0, 11000, 20000, 32000, 47000, 51000, 71000
  65         };
  66
  67         /* outputs atmospheric pressure associated with the given altitude.
  68          * altitudes are measured with respect to the mean sea level
  69          */
  70         public static double
  71         altitude_to_pressure(double altitude)
  72         {
  73                 double base_temperature = LAYER0_BASE_TEMPERATURE;
  74                 double base_pressure = LAYER0_BASE_PRESSURE;
  75
  76                 double pressure;
  77                 double base; /* base for function to determine pressure */
  78                 double exponent; /* exponent for function to determine pressure */
  79                 int layer_number; /* identifies layer in the atmosphere */
  80                 double delta_z; /* difference between two altitudes */
  81
  82                 if (altitude > MAXIMUM_ALTITUDE) /* FIX ME: use sensor data to improve model */
  83                         return 0;
  84
  85                 /* calculate the base temperature and pressure for the atmospheric layer
  86                    associated with the inputted altitude */
  87                 for(layer_number = 0; layer_number < NUMBER_OF_LAYERS - 1 && altitude > base_altitude[layer_number + 1]; layer_number++) {
  88                         delta_z = base_altitude[layer_number + 1] - base_altitude[layer_number];
  89                         if (lapse_rate[layer_number] == 0.0) {
  90                                 exponent = GRAVITATIONAL_ACCELERATION * delta_z
  91                                         / AIR_GAS_CONSTANT / base_temperature;
  92                                 base_pressure *= Math.exp(exponent);
  93                         }
  94                         else {
  95                                 base = (lapse_rate[layer_number] * delta_z / base_temperature) + 1.0;
  96                                 exponent = GRAVITATIONAL_ACCELERATION /
  97                                         (AIR_GAS_CONSTANT * lapse_rate[layer_number]);
  98                                 base_pressure *= Math.pow(base, exponent);
  99                         }
 100                         base_temperature += delta_z * lapse_rate[layer_number];
 101                 }
 102
 103                 /* calculate the pressure at the inputted altitude */
 104                 delta_z = altitude - base_altitude[layer_number];
 105                 if (lapse_rate[layer_number] == 0.0) {
 106                         exponent = GRAVITATIONAL_ACCELERATION * delta_z
 107                                 / AIR_GAS_CONSTANT / base_temperature;
 108                         pressure = base_pressure * Math.exp(exponent);
 109                 }
 110                 else {
 111                         base = (lapse_rate[layer_number] * delta_z / base_temperature) + 1.0;
 112                         exponent = GRAVITATIONAL_ACCELERATION /
 113                                 (AIR_GAS_CONSTANT * lapse_rate[layer_number]);
 114                         pressure = base_pressure * Math.pow(base, exponent);
 115                 }
 116
 117                 return pressure;
 118         }
 119
 120
 121 /* outputs the altitude associated with the given pressure. the altitude
 122    returned is measured with respect to the mean sea level */
 123         public static double
 124         pressure_to_altitude(double pressure)
 125         {
 126
 127                 double next_base_temperature = LAYER0_BASE_TEMPERATURE;
 128                 double next_base_pressure = LAYER0_BASE_PRESSURE;
 129
 130                 double altitude;
 131                 double base_pressure;
 132                 double base_temperature;
 133                 double base; /* base for function to determine base pressure of next layer */
 134                 double exponent; /* exponent for function to determine base pressure
 135                                     of next layer */
 136                 double coefficient;
 137                 int layer_number; /* identifies layer in the atmosphere */
 138                 int delta_z; /* difference between two altitudes */
 139
 140                 if (pressure < 0)  /* illegal pressure */
 141                         return -1;
 142                 if (pressure < MINIMUM_PRESSURE) /* FIX ME: use sensor data to improve model */
 143                         return MAXIMUM_ALTITUDE;
 144
 145                 /* calculate the base temperature and pressure for the atmospheric layer
 146                    associated with the inputted pressure. */
 147                 layer_number = -1;
 148                 do {
 149                         layer_number++;
 150                         base_pressure = next_base_pressure;
 151                         base_temperature = next_base_temperature;
 152                         delta_z = base_altitude[layer_number + 1] - base_altitude[layer_number];
 153                         if (lapse_rate[layer_number] == 0.0) {
 154                                 exponent = GRAVITATIONAL_ACCELERATION * delta_z
 155                                         / AIR_GAS_CONSTANT / base_temperature;
 156                                 next_base_pressure *= Math.exp(exponent);
 157                         }
 158                         else {
 159                                 base = (lapse_rate[layer_number] * delta_z / base_temperature) + 1.0;
 160                                 exponent = GRAVITATIONAL_ACCELERATION /
 161                                         (AIR_GAS_CONSTANT * lapse_rate[layer_number]);
 162                                 next_base_pressure *= Math.pow(base, exponent);
 163                         }
 164                         next_base_temperature += delta_z * lapse_rate[layer_number];
 165                 }
 166                 while(layer_number < NUMBER_OF_LAYERS - 1 && pressure < next_base_pressure);
 167
 168                 /* calculate the altitude associated with the inputted pressure */
 169                 if (lapse_rate[layer_number] == 0.0) {
 170                         coefficient = (AIR_GAS_CONSTANT / GRAVITATIONAL_ACCELERATION)
 171                                 * base_temperature;
 172                         altitude = base_altitude[layer_number]
 173                                 + coefficient * Math.log(pressure / base_pressure);
 174                 }
 175                 else {
 176                         base = pressure / base_pressure;
 177                         exponent = AIR_GAS_CONSTANT * lapse_rate[layer_number]
 178                                 / GRAVITATIONAL_ACCELERATION;
 179                         coefficient = base_temperature / lapse_rate[layer_number];
 180                         altitude = base_altitude[layer_number]
 181                                 + coefficient * (Math.pow(base, exponent) - 1);
 182                 }
 183
 184                 return altitude;
 185         }
 186
 187         public static double degrees_to_radians(double degrees) {
 188                 if (degrees == AltosLib.MISSING)
 189                         return AltosLib.MISSING;
 190                 return degrees * (Math.PI / 180.0);
 191         }
 192
 193         public static double radians_to_degrees(double radians) {
 194                 if (radians == AltosLib.MISSING)
 195                         return AltosLib.MISSING;
 196                 return radians * (180.0 / Math.PI);
 197         }
 198
 199         public static double
 200         cc_battery_to_voltage(double battery)
 201         {
 202                 return battery / 32767.0 * 5.0;
 203         }
 204
 205         public static double
 206         cc_igniter_to_voltage(double ignite)
 207         {
 208                 return ignite / 32767 * 15.0;
 209         }
 210
 211         public static double
 212         barometer_to_pressure(double count)
 213         {
 214                 return ((count / 16.0) / 2047.0 + 0.095) / 0.009 * 1000.0;
 215         }
 216
 217         static double
 218         thermometer_to_temperature(double thermo)
 219         {
 220                 return (thermo - 19791.268) / 32728.0 * 1.25 / 0.00247;
 221         }
 222
 223         static double mega_adc(int raw) {
 224                 return raw / 4095.0;
 225         }
 226
 227         static public double mega_battery_voltage(int v_batt) {
 228                 if (v_batt != AltosLib.MISSING)
 229                         return 3.3 * mega_adc(v_batt) * (5.6 + 10.0) / 10.0;
 230                 return AltosLib.MISSING;
 231         }
 232
 233         static double mega_pyro_voltage(int raw) {
 234                 if (raw != AltosLib.MISSING)
 235                         return 3.3 * mega_adc(raw) * (100.0 + 27.0) / 27.0;
 236                 return AltosLib.MISSING;
 237         }
 238
 239         static double tele_mini_3_adc(int raw) {
 240                 return raw / 4095.0;
 241         }
 242
 243         static public double tele_mini_3_battery_voltage(int v_batt) {
 244                 if (v_batt != AltosLib.MISSING)
 245                         return 3.3 * tele_mini_3_adc(v_batt) * (5.6 + 10.0) / 10.0;
 246                 return AltosLib.MISSING;
 247         }
 248
 249         static double tele_mini_3_pyro_voltage(int raw) {
 250                 if (raw != AltosLib.MISSING)
 251                         return 3.3 * tele_mini_3_adc(raw) * (100.0 + 27.0) / 27.0;
 252                 return AltosLib.MISSING;
 253         }
 254
 255         static double tele_mini_2_voltage(int sensor) {
 256                 double  supply = 3.3;
 257
 258                 return sensor / 32767.0 * supply * 127/27;
 259         }
 260
 261         static double tele_gps_1_voltage(int sensor) {
 262                 double  supply = 3.3;
 263
 264                 return sensor / 32767.0 * supply * (5.6 + 10.0) / 10.0;
 265         }
 266
 267         static double tele_gps_2_voltage(int sensor) {
 268                 double  supply = 3.3;
 269
 270                 return sensor / 4095.0 * supply * (5.6 + 10.0) / 10.0;
 271         }
 272
 273         static double tele_gps_3_voltage(int sensor) {
 274                 double  supply = 3.3;
 275
 276                 return sensor / 32767.0 * supply * (5.6 + 10.0) / 10.0;
 277         }
 278
 279         static double tele_bt_3_battery(int raw) {
 280                 if (raw == AltosLib.MISSING)
 281                         return AltosLib.MISSING;
 282                 return 3.3 * mega_adc(raw) * (5.1 + 10.0) / 10.0;
 283         }
 284
 285         static double easy_timer_voltage(int sensor) {
 286                 return 3.3 * mega_adc(sensor) * (100.0 + 27.0) / 27.0;
 287         }
 288
 289         static double easy_mini_2_adc(double raw) {
 290                 return raw / 4095.0;
 291         }
 292
 293         static double easy_mini_1_adc(double raw) {
 294                 return raw / 32767.0;
 295         }
 296
 297         static double easy_mini_1_voltage(int sensor, int serial) {
 298                 double  supply = 3.3;
 299                 double  diode_offset = 0.0;
 300
 301                 /* early prototypes had a 3.0V regulator */
 302                 if (serial < 1000)
 303                         supply = 3.0;
 304
 305                 /* Purple v1.0 boards had the sensor after the
 306                  * blocking diode, which drops about 150mV
 307                  */
 308                 if (serial < 1665)
 309                         diode_offset = 0.150;
 310
 311                 return easy_mini_1_adc(sensor) * supply * 127/27 + diode_offset;
 312         }
 313
 314         static double easy_mini_2_voltage(int sensor) {
 315                 double  supply = 3.3;
 316
 317                 return easy_mini_2_adc(sensor) * supply * 127/27;
 318         }
 319
 320         static double motor_pressure(double voltage) {
 321                 double  base = 0.5;
 322                 double  max = 4.5;
 323                 double  full_scale_pressure = psi_to_pa(1600);
 324
 325                 if (voltage < base)
 326                         voltage = base;
 327                 if (voltage > max)
 328                         voltage = max;
 329                 return (voltage - base) / (max - base) * full_scale_pressure;
 330         }
 331
 332         static double easy_motor_3_adc(double raw) {
 333                 return raw / 32767.0;
 334         }
 335
 336         static double easy_motor_3_voltage(int sensor) {
 337                 double  supply = 3.3;
 338
 339                 return easy_motor_3_adc(sensor) * supply * 15.6 / 10.0;
 340         }
 341
 342         static double easy_motor_2_motor_pressure(int sensor, double ground_sensor) {
 343                 double  supply = 3.3;
 344                 double  ground_voltage = easy_mini_2_adc(ground_sensor) * supply * 15.6 / 10.0;
 345                 double  voltage = easy_mini_2_adc(sensor) * supply * 15.6 / 10.0;
 346
 347                 return motor_pressure(voltage) - motor_pressure(ground_voltage);
 348         }
 349
 350         static double easy_motor_3_motor_pressure(int sensor, double ground_sensor) {
 351                 double  supply = 3.3;
 352                 double  ground_voltage = easy_motor_3_adc(ground_sensor) * supply * 15.6 / 10.0;
 353                 double  voltage = easy_motor_3_adc(sensor) * supply * 15.6 / 10.0;
 354
 355                 return motor_pressure(voltage) - motor_pressure(ground_voltage);
 356         }
 357
 358         public static double radio_to_frequency(int freq, int setting, int cal, int channel) {
 359                 double  f;
 360
 361                 if (freq > 0)
 362                         f = freq / 1000.0;
 363                 else {
 364                         if (setting <= 0)
 365                                 setting = cal;
 366                         f = 434.550 * setting / cal;
 367                         /* Round to nearest 50KHz */
 368                         f = Math.floor (20.0 * f + 0.5) / 20.0;
 369                 }
 370                 return f + channel * 0.100;
 371         }
 372
 373         public static int radio_frequency_to_setting(double frequency, int cal) {
 374                 double  set = frequency / 434.550 * cal;
 375
 376                 return (int) Math.floor (set + 0.5);
 377         }
 378
 379         public static int radio_frequency_to_channel(double frequency) {
 380                 int     channel = (int) Math.floor ((frequency - 434.550) / 0.100 + 0.5);
 381
 382                 if (channel < 0)
 383                         channel = 0;
 384                 if (channel > 9)
 385                         channel = 9;
 386                 return channel;
 387         }
 388
 389         public static double radio_channel_to_frequency(int channel) {
 390                 return 434.550 + channel * 0.100;
 391         }
 392
 393         public static int telem_to_rssi(int telem) {
 394                 return telem / 2 - 74;
 395         }
 396
 397         public static int[] ParseHex(String line) {
 398                 String[] tokens = line.split("\\s+");
 399                 int[] array = new int[tokens.length];
 400
 401                 for (int i = 0; i < tokens.length; i++)
 402                         try {
 403                                 array[i] = Integer.parseInt(tokens[i], 16);
 404                         } catch (NumberFormatException ne) {
 405                                 return null;
 406                         }
 407                 return array;
 408         }
 409
 410         public static double meters_to_feet(double meters) {
 411                 return meters * (100 / (2.54 * 12));
 412         }
 413
 414         public static double feet_to_meters(double feet) {
 415                 return feet * 12 * 2.54 / 100.0;
 416         }
 417
 418         public static double meters_to_miles(double meters) {
 419                 return meters_to_feet(meters) / 5280;
 420         }
 421
 422         public static double miles_to_meters(double miles) {
 423                 return feet_to_meters(miles * 5280);
 424         }
 425
 426         public static double meters_to_mph(double mps) {
 427                 return meters_to_miles(mps) * 3600;
 428         }
 429
 430         public static double mph_to_meters(double mps) {
 431                 return miles_to_meters(mps) / 3600;
 432         }
 433
 434         public static double mps_to_fps(double mps) {
 435                 return meters_to_miles(mps) * 5280;
 436         }
 437
 438         public static double fps_to_mps(double mps) {
 439                 return miles_to_meters(mps) / 5280;
 440         }
 441
 442         public static double meters_to_mach(double meters) {
 443                 return meters / 343;            /* something close to mach at usual rocket sites */
 444         }
 445
 446         public static double meters_to_g(double meters) {
 447                 return meters / 9.80665;
 448         }
 449
 450         public static double c_to_f(double c) {
 451                 return c * 9/5 + 32;
 452         }
 453
 454         public static double f_to_c(double c) {
 455                 return (c - 32) * 5/9;
 456         }
 457
 458         public static double psi_to_pa(double psi) {
 459                 return psi * 6894.76;
 460         }
 461
 462         public static double pa_to_psi(double pa) {
 463                 return pa / 6894.76;
 464         }
 465
 466         public static double n_to_lb(double n) {
 467                 return n * 0.22480894;
 468         }
 469
 470         public static double lb_to_n(double lb) {
 471                 return lb / 0.22480894;
 472         }
 473
 474         public static double acceleration_from_sensor(double sensor, double plus_g, double minus_g, double ground) {
 475
 476                 if (sensor == AltosLib.MISSING)
 477                         return AltosLib.MISSING;
 478
 479                 if (plus_g == AltosLib.MISSING || minus_g == AltosLib.MISSING)
 480                         return AltosLib.MISSING;
 481
 482                 if (ground == AltosLib.MISSING)
 483                         ground = plus_g;
 484
 485                 double counts_per_g = (plus_g - minus_g) / 2.0;
 486                 double counts_per_mss = counts_per_g / gravity;
 487
 488                 if (counts_per_mss == 0)
 489                         return AltosLib.MISSING;
 490
 491                 return (sensor - ground) / counts_per_mss;
 492         }
 493
 494         public static boolean imperial_units = false;
 495
 496         public static AltosDistance distance = new AltosDistance();
 497
 498         public static AltosHeight height = new AltosHeight();
 499
 500         public static AltosPressure pressure = new AltosPressure();
 501
 502         public static AltosForce force = new AltosForce();
 503
 504         public static AltosSpeed speed = new AltosSpeed();
 505
 506         public static AltosAccel accel = new AltosAccel();
 507
 508         public static AltosTemperature temperature = new AltosTemperature();
 509
 510         public static AltosOrient orient = new AltosOrient();
 511
 512         public static AltosVoltage voltage = new AltosVoltage();
 513
 514         public static AltosLatitude latitude = new AltosLatitude();
 515
 516         public static AltosLongitude longitude = new AltosLongitude();
 517
 518         public static AltosRotationRate rotation_rate = new AltosRotationRate();
 519
 520         public static AltosStateName state_name = new AltosStateName();
 521
 522         public static AltosPyroName pyro_name = new AltosPyroName();
 523
 524         public static AltosUnits magnetic_field = new AltosGauss();
 525
 526         public static String show_gs(String format, double a) {
 527                 a = meters_to_g(a);
 528                 format = format.concat(" g");
 529                 return String.format(format, a);
 530         }
 531
 532         public static String say_gs(double a) {
 533                 return String.format("%6.0 gees", meters_to_g(a));
 534         }
 535
 536         public static int checksum(int[] data, int start, int length) {
 537                 int     csum = 0x5a;
 538                 for (int i = 0; i < length; i++)
 539                         csum += data[i + start];
 540                 return csum & 0xff;
 541         }
 542
 543         public static int checksum(List<Byte> data, int start, int length) {
 544                 int     csum = 0x5a;
 545                 for (int i = 0; i < length; i++)
 546                         csum += data.get(i+start);
 547                 return csum & 0xff;
 548         }
 549
 550         public static double beep_value_to_freq(int value) {
 551                 if (value == 0)
 552                         return 4000;
 553                 return 1.0/2.0 * (24.0e6/32.0) / (double) value;
 554         }
 555
 556         public static int beep_freq_to_value(double freq) {
 557                 if (freq == 0)
 558                         return 94;
 559                 return (int) Math.floor (1.0/2.0 * (24.0e6/32.0) / freq + 0.5);
 560         }
 561
 562         public static final int BEARING_LONG = 0;
 563         public static final int BEARING_SHORT = 1;
 564         public static final int BEARING_VOICE = 2;
 565
 566         public static String bearing_to_words(int length, double bearing) {
 567                 String [][] bearing_string = {
 568                         {
 569                                 "North", "North North East", "North East", "East North East",
 570                                 "East", "East South East", "South East", "South South East",
 571                                 "South", "South South West", "South West", "West South West",
 572                                 "West", "West North West", "North West", "North North West"
 573                         }, {
 574                                 "N", "NNE", "NE", "ENE",
 575                                 "E", "ESE", "SE", "SSE",
 576                                 "S", "SSW", "SW", "WSW",
 577                                 "W", "WNW", "NW", "NNW"
 578                         }, {
 579                                 "north", "nor nor east", "north east", "east nor east",
 580                                 "east", "east sow east", "south east", "sow sow east",
 581                                 "south", "sow sow west", "south west", "west sow west",
 582                                 "west", "west nor west", "north west", "nor nor west "
 583                         }
 584                 };
 585                 return bearing_string[length][(int)((bearing / 90 * 8 + 1) / 2)%16];
 586         }
 587 }