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 MAXIMUM_ALTITUDE            = 100000.0;
  53         private static final double MINIMUM_PRESSURE            = 0.023439;
  54         private static final double LAYER0_BASE_TEMPERATURE     = 288.15;
  55         private static final double LAYER0_BASE_PRESSURE        = 101325;
  56
  57         /* lapse rate and base altitude for each layer in the atmosphere */
  58         private static final double[] lapse_rate = {
  59                 -0.0065, 0.0, 0.001, 0.0028, 0.0, -0.0028, -0.002, 0,
  60         };
  61
  62         private static final double[] base_altitude = {
  63                 0, 11000, 20000, 32000, 47000, 51000, 71000, 84852,
  64         };
  65
  66         private static final int NUMBER_OF_LAYERS               = base_altitude.length;
  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 = 0;
 132                 double base_temperature = 0;
 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                 double delta_z; /* difference between two altitudes */
 139
 140                 if (pressure < MINIMUM_PRESSURE) /* FIX ME: use sensor data to improve model */
 141                         pressure = MINIMUM_PRESSURE;
 142
 143                 /* calculate the base temperature and pressure for the atmospheric layer
 144                    associated with the inputted pressure. */
 145                 for (layer_number = 0; layer_number < NUMBER_OF_LAYERS - 1; layer_number++) {
 146                         base_pressure = next_base_pressure;
 147                         base_temperature = next_base_temperature;
 148                         delta_z = base_altitude[layer_number + 1] - base_altitude[layer_number];
 149                         if (lapse_rate[layer_number] == 0.0) {
 150                                 exponent = GRAVITATIONAL_ACCELERATION * delta_z
 151                                         / AIR_GAS_CONSTANT / base_temperature;
 152                                 next_base_pressure *= Math.exp(exponent);
 153                         }
 154                         else {
 155                                 base = (lapse_rate[layer_number] * delta_z / base_temperature) + 1.0;
 156                                 exponent = GRAVITATIONAL_ACCELERATION /
 157                                         (AIR_GAS_CONSTANT * lapse_rate[layer_number]);
 158                                 next_base_pressure *= Math.pow(base, exponent);
 159                         }
 160                         next_base_temperature += delta_z * lapse_rate[layer_number];
 161                         if (pressure >= next_base_pressure)
 162                                 break;
 163                 }
 164
 165                 /* calculate the altitude associated with the inputted pressure */
 166                 if (lapse_rate[layer_number] == 0.0) {
 167                         coefficient = (AIR_GAS_CONSTANT / GRAVITATIONAL_ACCELERATION)
 168                                 * base_temperature;
 169                         altitude = base_altitude[layer_number]
 170                                 + coefficient * Math.log(pressure / base_pressure);
 171                 }
 172                 else {
 173                         base = pressure / base_pressure;
 174                         exponent = AIR_GAS_CONSTANT * lapse_rate[layer_number]
 175                                 / GRAVITATIONAL_ACCELERATION;
 176                         coefficient = base_temperature / lapse_rate[layer_number];
 177                         altitude = base_altitude[layer_number]
 178                                 + coefficient * (Math.pow(base, exponent) - 1);
 179                 }
 180
 181                 if (altitude > MAXIMUM_ALTITUDE)
 182                         altitude = MAXIMUM_ALTITUDE;
 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 double stm_adc(int raw) {
 228                 return raw / 4095.0;
 229         }
 230
 231         static public double easy_timer_battery_voltage(int v_batt) {
 232                 if (v_batt != AltosLib.MISSING)
 233                         return 3.3 * stm_adc(v_batt) * (5.6 + 10.0) / 10.0;
 234                 return AltosLib.MISSING;
 235         }
 236
 237         static double easy_timer_pyro_voltage_15v(int raw) {
 238                 if (raw != AltosLib.MISSING)
 239                         return 3.3 * stm_adc(raw) * (100.0 + 27.0) / 27.0;
 240                 return AltosLib.MISSING;
 241         }
 242
 243         static public double metrum_battery_voltage(int v_batt) {
 244                 if (v_batt != AltosLib.MISSING)
 245                         return 3.3 * stm_adc(v_batt) * (5.6 + 10.0) / 10.0;
 246                 return AltosLib.MISSING;
 247         }
 248
 249         static double metrum_pyro_voltage(int raw) {
 250                 if (raw != AltosLib.MISSING)
 251                         return 3.3 * stm_adc(raw) * (100.0 + 27.0) / 27.0;
 252                 return AltosLib.MISSING;
 253         }
 254
 255         static public double mega_battery_voltage(int v_batt) {
 256                 if (v_batt != AltosLib.MISSING)
 257                         return 3.3 * stm_adc(v_batt) * (5.6 + 10.0) / 10.0;
 258                 return AltosLib.MISSING;
 259         }
 260
 261         static double mega_pyro_voltage_15v(int raw) {
 262                 if (raw != AltosLib.MISSING)
 263                         return 3.3 * stm_adc(raw) * (100.0 + 27.0) / 27.0;
 264                 return AltosLib.MISSING;
 265         }
 266
 267         static double mega_pyro_voltage_30v(int raw) {
 268                 if (raw != AltosLib.MISSING)
 269                         return 3.3 * stm_adc(raw) * (100.0 + 12.0) / 12.0;
 270                 return AltosLib.MISSING;
 271         }
 272
 273         static double tele_mini_3_adc(int raw) {
 274                 return raw / 4095.0;
 275         }
 276
 277         static public double tele_mini_3_battery_voltage(int v_batt) {
 278                 if (v_batt != AltosLib.MISSING)
 279                         return 3.3 * tele_mini_3_adc(v_batt) * (5.6 + 10.0) / 10.0;
 280                 return AltosLib.MISSING;
 281         }
 282
 283         static double tele_mini_3_pyro_voltage(int raw) {
 284                 if (raw != AltosLib.MISSING)
 285                         return 3.3 * tele_mini_3_adc(raw) * (100.0 + 27.0) / 27.0;
 286                 return AltosLib.MISSING;
 287         }
 288
 289         static double tele_mini_2_voltage(int sensor) {
 290                 double  supply = 3.3;
 291
 292                 return sensor / 32767.0 * supply * 127/27;
 293         }
 294
 295         static double tele_gps_1_voltage(int sensor) {
 296                 double  supply = 3.3;
 297
 298                 return sensor / 32767.0 * supply * (5.6 + 10.0) / 10.0;
 299         }
 300
 301         static double tele_gps_2_voltage(int sensor) {
 302                 double  supply = 3.3;
 303
 304                 return sensor / 4095.0 * supply * (5.6 + 10.0) / 10.0;
 305         }
 306
 307         static double tele_gps_3_voltage(int sensor) {
 308                 double  supply = 3.3;
 309
 310                 return sensor / 32767.0 * supply * (5.6 + 10.0) / 10.0;
 311         }
 312
 313         /* STM32F042 */
 314         static double tele_gps_4_voltage(int sensor) {
 315                 double  supply = 3.3;
 316
 317                 return sensor / 4095.0 * supply * (5.6 + 10.0) / 10.0;
 318         }
 319
 320         static double tele_bt_3_battery(int raw) {
 321                 if (raw == AltosLib.MISSING)
 322                         return AltosLib.MISSING;
 323                 return 3.3 * mega_adc(raw) * (5.1 + 10.0) / 10.0;
 324         }
 325
 326         static double easy_timer_voltage(int sensor) {
 327                 return 3.3 * mega_adc(sensor) * (100.0 + 27.0) / 27.0;
 328         }
 329
 330         static double easy_mini_2_adc(double raw) {
 331                 return raw / 4095.0;
 332         }
 333
 334         static double easy_mini_1_adc(double raw) {
 335                 return raw / 32767.0;
 336         }
 337
 338         static double easy_mini_1_voltage(int sensor, int serial) {
 339                 double  supply = 3.3;
 340                 double  diode_offset = 0.0;
 341
 342                 /* early prototypes had a 3.0V regulator */
 343                 if (serial < 1000)
 344                         supply = 3.0;
 345
 346                 /* Purple v1.0 boards had the sensor after the
 347                  * blocking diode, which drops about 150mV
 348                  */
 349                 if (serial < 1665)
 350                         diode_offset = 0.150;
 351
 352                 return easy_mini_1_adc(sensor) * supply * 127/27 + diode_offset;
 353         }
 354
 355         static double easy_mini_2_voltage(int sensor) {
 356                 double  supply = 3.3;
 357
 358                 return easy_mini_2_adc(sensor) * supply * 127/27;
 359         }
 360
 361         static double easy_mini_3_voltage(int sensor) {
 362                 return easy_mini_1_voltage(sensor, 10000);
 363         }
 364
 365         static double motor_pressure(double voltage) {
 366                 double  base = 0.5;
 367                 double  max = 4.5;
 368                 double  full_scale_pressure = psi_to_pa(1600);
 369
 370                 if (voltage < base)
 371                         voltage = base;
 372                 if (voltage > max)
 373                         voltage = max;
 374                 return (voltage - base) / (max - base) * full_scale_pressure;
 375         }
 376
 377         static double easy_motor_3_adc(double raw) {
 378                 return raw / 32767.0;
 379         }
 380
 381         static double easy_motor_3_voltage(int sensor) {
 382                 double  supply = 3.3;
 383
 384                 return easy_motor_3_adc(sensor) * supply * 15.6 / 10.0;
 385         }
 386
 387         static double easy_motor_2_motor_pressure(int sensor, double ground_sensor) {
 388                 double  supply = 3.3;
 389                 double  ground_voltage = easy_mini_2_adc(ground_sensor) * supply * 15.6 / 10.0;
 390                 double  voltage = easy_mini_2_adc(sensor) * supply * 15.6 / 10.0;
 391
 392                 return motor_pressure(voltage) - motor_pressure(ground_voltage);
 393         }
 394
 395         static double easy_motor_3_motor_pressure(int sensor, double ground_sensor) {
 396                 double  supply = 3.3;
 397                 double  ground_voltage = easy_motor_3_adc(ground_sensor) * supply * 15.6 / 10.0;
 398                 double  voltage = easy_motor_3_adc(sensor) * supply * 15.6 / 10.0;
 399
 400                 return motor_pressure(voltage) - motor_pressure(ground_voltage);
 401         }
 402
 403         public static double radio_to_frequency(int freq, int setting, int cal, int channel) {
 404                 double  f;
 405
 406                 if (freq > 0)
 407                         f = freq / 1000.0;
 408                 else {
 409                         if (setting <= 0)
 410                                 setting = cal;
 411                         f = 434.550 * setting / cal;
 412                         /* Round to nearest 50KHz */
 413                         f = Math.floor (20.0 * f + 0.5) / 20.0;
 414                 }
 415                 return f + channel * 0.100;
 416         }
 417
 418         public static int radio_frequency_to_setting(double frequency, int cal) {
 419                 double  set = frequency / 434.550 * cal;
 420
 421                 return (int) Math.floor (set + 0.5);
 422         }
 423
 424         public static int radio_frequency_to_channel(double frequency) {
 425                 int     channel = (int) Math.floor ((frequency - 434.550) / 0.100 + 0.5);
 426
 427                 if (channel < 0)
 428                         channel = 0;
 429                 if (channel > 9)
 430                         channel = 9;
 431                 return channel;
 432         }
 433
 434         public static double radio_channel_to_frequency(int channel) {
 435                 return 434.550 + channel * 0.100;
 436         }
 437
 438         public static int telem_to_rssi(int telem) {
 439                 return telem / 2 - 74;
 440         }
 441
 442         public static int[] ParseHex(String line) {
 443                 String[] tokens = line.split("\\s+");
 444                 int[] array = new int[tokens.length];
 445
 446                 for (int i = 0; i < tokens.length; i++)
 447                         try {
 448                                 array[i] = Integer.parseInt(tokens[i], 16);
 449                         } catch (NumberFormatException ne) {
 450                                 return null;
 451                         }
 452                 return array;
 453         }
 454
 455         public static double meters_to_feet(double meters) {
 456                 return meters * (100 / (2.54 * 12));
 457         }
 458
 459         public static double feet_to_meters(double feet) {
 460                 return feet * 12 * 2.54 / 100.0;
 461         }
 462
 463         public static double meters_to_miles(double meters) {
 464                 return meters_to_feet(meters) / 5280;
 465         }
 466
 467         public static double miles_to_meters(double miles) {
 468                 return feet_to_meters(miles * 5280);
 469         }
 470
 471         public static double meters_to_mph(double mps) {
 472                 return meters_to_miles(mps) * 3600;
 473         }
 474
 475         public static double mph_to_meters(double mps) {
 476                 return miles_to_meters(mps) / 3600;
 477         }
 478
 479         public static double mps_to_fps(double mps) {
 480                 return meters_to_miles(mps) * 5280;
 481         }
 482
 483         public static double fps_to_mps(double mps) {
 484                 return miles_to_meters(mps) / 5280;
 485         }
 486
 487         public static double meters_to_mach(double meters) {
 488                 return meters / 343;            /* something close to mach at usual rocket sites */
 489         }
 490
 491         public static double meters_to_g(double meters) {
 492                 return meters / 9.80665;
 493         }
 494
 495         public static double c_to_f(double c) {
 496                 return c * 9/5 + 32;
 497         }
 498
 499         public static double f_to_c(double c) {
 500                 return (c - 32) * 5/9;
 501         }
 502
 503         public static double psi_to_pa(double psi) {
 504                 return psi * 6894.76;
 505         }
 506
 507         public static double pa_to_psi(double pa) {
 508                 return pa / 6894.76;
 509         }
 510
 511         public static double n_to_lb(double n) {
 512                 return n * 0.22480894;
 513         }
 514
 515         public static double lb_to_n(double lb) {
 516                 return lb / 0.22480894;
 517         }
 518
 519         public static double acceleration_from_sensor(double sensor, double plus_g, double minus_g, double ground) {
 520
 521                 if (sensor == AltosLib.MISSING)
 522                         return AltosLib.MISSING;
 523
 524                 if (plus_g == AltosLib.MISSING || minus_g == AltosLib.MISSING)
 525                         return AltosLib.MISSING;
 526
 527                 if (ground == AltosLib.MISSING)
 528                         ground = plus_g;
 529
 530                 double counts_per_g = (plus_g - minus_g) / 2.0;
 531                 double counts_per_mss = counts_per_g / gravity;
 532
 533                 if (counts_per_mss == 0)
 534                         return AltosLib.MISSING;
 535
 536                 return (sensor - ground) / counts_per_mss;
 537         }
 538
 539         public static boolean imperial_units = false;
 540
 541         public static AltosDistance distance = new AltosDistance();
 542
 543         public static AltosHeight height = new AltosHeight();
 544
 545         public static AltosPressure pressure = new AltosPressure();
 546
 547         public static AltosForce force = new AltosForce();
 548
 549         public static AltosSpeed speed = new AltosSpeed();
 550
 551         public static AltosAccel accel = new AltosAccel();
 552
 553         public static AltosTemperature temperature = new AltosTemperature();
 554
 555         public static AltosOrient orient = new AltosOrient();
 556
 557         public static AltosVoltage voltage = new AltosVoltage();
 558
 559         public static AltosLatitude latitude = new AltosLatitude();
 560
 561         public static AltosLongitude longitude = new AltosLongitude();
 562
 563         public static AltosRotationRate rotation_rate = new AltosRotationRate();
 564
 565         public static AltosStateName state_name = new AltosStateName();
 566
 567         public static AltosPyroName pyro_name = new AltosPyroName();
 568
 569         public static AltosUnits magnetic_field = new AltosGauss();
 570
 571         public static String show_gs(String format, double a) {
 572                 a = meters_to_g(a);
 573                 format = format.concat(" g");
 574                 return String.format(format, a);
 575         }
 576
 577         public static String say_gs(double a) {
 578                 return String.format("%6.0 gees", meters_to_g(a));
 579         }
 580
 581         public static int checksum(int[] data, int start, int length) {
 582                 int     csum = 0x5a;
 583                 for (int i = 0; i < length; i++)
 584                         csum += data[i + start];
 585                 return csum & 0xff;
 586         }
 587
 588         public static int checksum(List<Byte> data, int start, int length) {
 589                 int     csum = 0x5a;
 590                 for (int i = 0; i < length; i++)
 591                         csum += data.get(i+start);
 592                 return csum & 0xff;
 593         }
 594
 595         public static double beep_value_to_freq(int value) {
 596                 if (value == 0)
 597                         return 4000;
 598                 return 1.0/2.0 * (24.0e6/32.0) / (double) value;
 599         }
 600
 601         public static int beep_freq_to_value(double freq) {
 602                 if (freq == 0)
 603                         return 0;
 604                 return (int) Math.floor (1.0/2.0 * (24.0e6/32.0) / freq + 0.5);
 605         }
 606
 607         public static final int BEARING_LONG = 0;
 608         public static final int BEARING_SHORT = 1;
 609         public static final int BEARING_VOICE = 2;
 610
 611         public static String bearing_to_words(int length, double bearing) {
 612                 String [][] bearing_string = {
 613                         {
 614                                 "North", "North North East", "North East", "East North East",
 615                                 "East", "East South East", "South East", "South South East",
 616                                 "South", "South South West", "South West", "West South West",
 617                                 "West", "West North West", "North West", "North North West"
 618                         }, {
 619                                 "N", "NNE", "NE", "ENE",
 620                                 "E", "ESE", "SE", "SSE",
 621                                 "S", "SSW", "SW", "WSW",
 622                                 "W", "WNW", "NW", "NNW"
 623                         }, {
 624                                 "north", "nor nor east", "north east", "east nor east",
 625                                 "east", "east sow east", "south east", "sow sow east",
 626                                 "south", "sow sow west", "south west", "west sow west",
 627                                 "west", "west nor west", "north west", "nor nor west "
 628                         }
 629                 };
 630                 return bearing_string[length][(int)((bearing / 90 * 8 + 1) / 2)%16];
 631         }
 632 }