doc: update copyright year assertions
[fw/altos] / 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_bt_3_battery(int raw) {
274                 if (raw == AltosLib.MISSING)
275                         return AltosLib.MISSING;
276                 return 3.3 * mega_adc(raw) * (5.1 + 10.0) / 10.0;
277         }
278
279         static double easy_timer_voltage(int sensor) {
280                 return 3.3 * mega_adc(sensor) * (100.0 + 27.0) / 27.0;
281         }
282
283         static double easy_mini_2_adc(double raw) {
284                 return raw / 4095.0;
285         }
286
287         static double easy_mini_1_adc(double raw) {
288                 return raw / 32767.0;
289         }
290
291         static double easy_mini_1_voltage(int sensor, int serial) {
292                 double  supply = 3.3;
293                 double  diode_offset = 0.0;
294
295                 /* early prototypes had a 3.0V regulator */
296                 if (serial < 1000)
297                         supply = 3.0;
298
299                 /* Purple v1.0 boards had the sensor after the
300                  * blocking diode, which drops about 150mV
301                  */
302                 if (serial < 1665)
303                         diode_offset = 0.150;
304
305                 return easy_mini_1_adc(sensor) * supply * 127/27 + diode_offset;
306         }
307
308         static double easy_mini_2_voltage(int sensor) {
309                 double  supply = 3.3;
310
311                 return easy_mini_2_adc(sensor) * supply * 127/27;
312         }
313
314         static double motor_pressure(double voltage) {
315                 double  base = 0.5;
316                 double  max = 4.5;
317                 double  full_scale_pressure = psi_to_pa(1600);
318
319                 if (voltage < base)
320                         voltage = base;
321                 if (voltage > max)
322                         voltage = max;
323                 return (voltage - base) / (max - base) * full_scale_pressure;
324         }
325
326         static double easy_motor_2_motor_pressure(int sensor, double ground_sensor) {
327                 double  supply = 3.3;
328                 double  ground_voltage = easy_mini_2_adc(ground_sensor) * supply * 15.6 / 10.0;
329                 double  voltage = easy_mini_2_adc(sensor) * supply * 15.6 / 10.0;
330
331                 return motor_pressure(voltage) - motor_pressure(ground_voltage);
332         }
333
334         public static double radio_to_frequency(int freq, int setting, int cal, int channel) {
335                 double  f;
336
337                 if (freq > 0)
338                         f = freq / 1000.0;
339                 else {
340                         if (setting <= 0)
341                                 setting = cal;
342                         f = 434.550 * setting / cal;
343                         /* Round to nearest 50KHz */
344                         f = Math.floor (20.0 * f + 0.5) / 20.0;
345                 }
346                 return f + channel * 0.100;
347         }
348
349         public static int radio_frequency_to_setting(double frequency, int cal) {
350                 double  set = frequency / 434.550 * cal;
351
352                 return (int) Math.floor (set + 0.5);
353         }
354
355         public static int radio_frequency_to_channel(double frequency) {
356                 int     channel = (int) Math.floor ((frequency - 434.550) / 0.100 + 0.5);
357
358                 if (channel < 0)
359                         channel = 0;
360                 if (channel > 9)
361                         channel = 9;
362                 return channel;
363         }
364
365         public static double radio_channel_to_frequency(int channel) {
366                 return 434.550 + channel * 0.100;
367         }
368
369         public static int telem_to_rssi(int telem) {
370                 return telem / 2 - 74;
371         }
372
373         public static int[] ParseHex(String line) {
374                 String[] tokens = line.split("\\s+");
375                 int[] array = new int[tokens.length];
376
377                 for (int i = 0; i < tokens.length; i++)
378                         try {
379                                 array[i] = Integer.parseInt(tokens[i], 16);
380                         } catch (NumberFormatException ne) {
381                                 return null;
382                         }
383                 return array;
384         }
385
386         public static double meters_to_feet(double meters) {
387                 return meters * (100 / (2.54 * 12));
388         }
389
390         public static double feet_to_meters(double feet) {
391                 return feet * 12 * 2.54 / 100.0;
392         }
393
394         public static double meters_to_miles(double meters) {
395                 return meters_to_feet(meters) / 5280;
396         }
397
398         public static double miles_to_meters(double miles) {
399                 return feet_to_meters(miles * 5280);
400         }
401
402         public static double meters_to_mph(double mps) {
403                 return meters_to_miles(mps) * 3600;
404         }
405
406         public static double mph_to_meters(double mps) {
407                 return miles_to_meters(mps) / 3600;
408         }
409
410         public static double mps_to_fps(double mps) {
411                 return meters_to_miles(mps) * 5280;
412         }
413
414         public static double fps_to_mps(double mps) {
415                 return miles_to_meters(mps) / 5280;
416         }
417
418         public static double meters_to_mach(double meters) {
419                 return meters / 343;            /* something close to mach at usual rocket sites */
420         }
421
422         public static double meters_to_g(double meters) {
423                 return meters / 9.80665;
424         }
425
426         public static double c_to_f(double c) {
427                 return c * 9/5 + 32;
428         }
429
430         public static double f_to_c(double c) {
431                 return (c - 32) * 5/9;
432         }
433
434         public static double psi_to_pa(double psi) {
435                 return psi * 6894.76;
436         }
437
438         public static double pa_to_psi(double pa) {
439                 return pa / 6894.76;
440         }
441
442         public static double n_to_lb(double n) {
443                 return n * 0.22480894;
444         }
445
446         public static double lb_to_n(double lb) {
447                 return lb / 0.22480894;
448         }
449
450         public static double acceleration_from_sensor(double sensor, double plus_g, double minus_g, double ground) {
451
452                 if (sensor == AltosLib.MISSING)
453                         return AltosLib.MISSING;
454
455                 if (plus_g == AltosLib.MISSING || minus_g == AltosLib.MISSING)
456                         return AltosLib.MISSING;
457
458                 if (ground == AltosLib.MISSING)
459                         ground = plus_g;
460
461                 double counts_per_g = (plus_g - minus_g) / 2.0;
462                 double counts_per_mss = counts_per_g / gravity;
463
464                 if (counts_per_mss == 0)
465                         return AltosLib.MISSING;
466
467                 return (sensor - ground) / counts_per_mss;
468         }
469
470         public static boolean imperial_units = false;
471
472         public static AltosDistance distance = new AltosDistance();
473
474         public static AltosHeight height = new AltosHeight();
475
476         public static AltosPressure pressure = new AltosPressure();
477
478         public static AltosForce force = new AltosForce();
479
480         public static AltosSpeed speed = new AltosSpeed();
481
482         public static AltosAccel accel = new AltosAccel();
483
484         public static AltosTemperature temperature = new AltosTemperature();
485
486         public static AltosOrient orient = new AltosOrient();
487
488         public static AltosVoltage voltage = new AltosVoltage();
489
490         public static AltosLatitude latitude = new AltosLatitude();
491
492         public static AltosLongitude longitude = new AltosLongitude();
493
494         public static AltosRotationRate rotation_rate = new AltosRotationRate();
495
496         public static AltosStateName state_name = new AltosStateName();
497
498         public static AltosPyroName pyro_name = new AltosPyroName();
499
500         public static AltosUnits magnetic_field = new AltosGauss();
501
502         public static String show_gs(String format, double a) {
503                 a = meters_to_g(a);
504                 format = format.concat(" g");
505                 return String.format(format, a);
506         }
507
508         public static String say_gs(double a) {
509                 return String.format("%6.0 gees", meters_to_g(a));
510         }
511
512         public static int checksum(int[] data, int start, int length) {
513                 int     csum = 0x5a;
514                 for (int i = 0; i < length; i++)
515                         csum += data[i + start];
516                 return csum & 0xff;
517         }
518
519         public static int checksum(List<Byte> data, int start, int length) {
520                 int     csum = 0x5a;
521                 for (int i = 0; i < length; i++)
522                         csum += data.get(i+start);
523                 return csum & 0xff;
524         }
525
526         public static double beep_value_to_freq(int value) {
527                 if (value == 0)
528                         return 4000;
529                 return 1.0/2.0 * (24.0e6/32.0) / (double) value;
530         }
531
532         public static int beep_freq_to_value(double freq) {
533                 if (freq == 0)
534                         return 94;
535                 return (int) Math.floor (1.0/2.0 * (24.0e6/32.0) / freq + 0.5);
536         }
537
538         public static final int BEARING_LONG = 0;
539         public static final int BEARING_SHORT = 1;
540         public static final int BEARING_VOICE = 2;
541
542         public static String bearing_to_words(int length, double bearing) {
543                 String [][] bearing_string = {
544                         {
545                                 "North", "North North East", "North East", "East North East",
546                                 "East", "East South East", "South East", "South South East",
547                                 "South", "South South West", "South West", "West South West",
548                                 "West", "West North West", "North West", "North North West"
549                         }, {
550                                 "N", "NNE", "NE", "ENE",
551                                 "E", "ESE", "SE", "SSE",
552                                 "S", "SSW", "SW", "WSW",
553                                 "W", "WNW", "NW", "NNW"
554                         }, {
555                                 "north", "nor nor east", "north east", "east nor east",
556                                 "east", "east sow east", "south east", "sow sow east",
557                                 "south", "sow sow west", "south west", "west sow west",
558                                 "west", "west nor west", "north west", "nor nor west "
559                         }
560                 };
561                 return bearing_string[length][(int)((bearing / 90 * 8 + 1) / 2)%16];
562         }
563 }