git.gag.com Git - fw/altos/blob - altoslib/AltosState.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  * Track flight state from telemetry or eeprom data stream
  21  */
  22
  23 package org.altusmetrum.altoslib_11;
  24
  25 import java.io.*;
  26
  27 public class AltosState extends AltosDataListener {
  28
  29         public static final int set_position = 1;
  30         public static final int set_gps = 2;
  31         public static final int set_data = 4;
  32
  33         public int set;
  34
  35         static final double filter_len = 2.0;
  36         static final double ascent_filter_len = 0.5;
  37         static final double descent_filter_len = 5.0;
  38
  39         /* derived data */
  40
  41         public long     received_time;
  42
  43         public int      rssi;
  44         public int      status;
  45
  46         class AltosValue {
  47                 double  value;
  48                 double  prev_value;
  49                 private double  max_value;
  50                 private double  set_time;
  51                 private double  prev_set_time;
  52
  53                 boolean can_max() { return true; }
  54
  55                 void set(double new_value, double time) {
  56                         if (new_value != AltosLib.MISSING) {
  57                                 value = new_value;
  58                                 if (can_max() && (max_value == AltosLib.MISSING || value > max_value))
  59                                         max_value = value;
  60                                 set_time = time;
  61                         }
  62                 }
  63
  64                 void set_filtered(double new_value, double time) {
  65                         if (prev_value != AltosLib.MISSING) {
  66                                 double f = 1/Math.exp((time - prev_set_time) / filter_len);
  67                                 new_value = f * new_value + (1-f) * prev_value;
  68                         }
  69                         set(new_value, time);
  70                 }
  71
  72                 double value() {
  73                         return value;
  74                 }
  75
  76                 double max() {
  77                         return max_value;
  78                 }
  79
  80                 double prev() {
  81                         return prev_value;
  82                 }
  83
  84                 double change() {
  85                         if (value != AltosLib.MISSING && prev_value != AltosLib.MISSING)
  86                                 return value - prev_value;
  87                         return AltosLib.MISSING;
  88                 }
  89
  90                 double rate() {
  91                         double c = change();
  92                         double t = set_time - prev_set_time;
  93
  94                         if (c != AltosLib.MISSING && t != 0)
  95                                 return c / t;
  96                         return AltosLib.MISSING;
  97                 }
  98
  99                 double integrate() {
 100                         if (value == AltosLib.MISSING)
 101                                 return AltosLib.MISSING;
 102                         if (prev_value == AltosLib.MISSING)
 103                                 return AltosLib.MISSING;
 104
 105                         return (value + prev_value) / 2 * (set_time - prev_set_time);
 106                 }
 107
 108                 double time() {
 109                         return set_time;
 110                 }
 111
 112                 void set_derivative(AltosValue in) {
 113                         double  n = in.rate();
 114
 115                         if (n == AltosLib.MISSING)
 116                                 return;
 117
 118                         double  p = prev_value;
 119                         double  pt = prev_set_time;
 120
 121                         if (p == AltosLib.MISSING) {
 122                                 p = 0;
 123                                 pt = in.time() - 0.01;
 124                         }
 125
 126                         /* Clip changes to reduce noise */
 127                         double  ddt = in.time() - pt;
 128                         double  ddv = (n - p) / ddt;
 129
 130                         final double max = 100000;
 131
 132                         /* 100gs */
 133                         if (Math.abs(ddv) > max) {
 134                                 if (n > p)
 135                                         n = p + ddt * max;
 136                                 else
 137                                         n = p - ddt * max;
 138                         }
 139
 140                         double filter_len;
 141
 142                         if (ascent)
 143                                 filter_len = ascent_filter_len;
 144                         else
 145                                 filter_len = descent_filter_len;
 146
 147                         double f = 1/Math.exp(ddt/ filter_len);
 148                         n = p * f + n * (1-f);
 149
 150                         set(n, in.time());
 151                 }
 152
 153                 void set_integral(AltosValue in) {
 154                         double  change = in.integrate();
 155
 156                         if (change != AltosLib.MISSING) {
 157                                 double  prev = prev_value;
 158                                 if (prev == AltosLib.MISSING)
 159                                         prev = 0;
 160                                 set(prev + change, in.time());
 161                         }
 162                 }
 163
 164                 void copy(AltosValue old) {
 165                         value = old.value;
 166                         set_time = old.set_time;
 167                         prev_value = old.value;
 168                         prev_set_time = old.set_time;
 169                         max_value = old.max_value;
 170                 }
 171
 172                 void finish_update() {
 173                         prev_value = value;
 174                         prev_set_time = set_time;
 175                 }
 176
 177                 AltosValue() {
 178                         value = AltosLib.MISSING;
 179                         prev_value = AltosLib.MISSING;
 180                         max_value = AltosLib.MISSING;
 181                 }
 182
 183         }
 184
 185         class AltosCValue {
 186
 187                 class AltosIValue extends AltosValue {
 188                         boolean can_max() {
 189                                 return c_can_max();
 190                         }
 191
 192                         AltosIValue() {
 193                                 super();
 194                         }
 195                 };
 196
 197                 public AltosIValue      measured;
 198                 public AltosIValue      computed;
 199
 200                 boolean can_max() { return true; }
 201
 202                 boolean c_can_max() { return can_max(); }
 203
 204                 double value() {
 205                         double v = measured.value();
 206                         if (v != AltosLib.MISSING)
 207                                 return v;
 208                         return computed.value();
 209                 }
 210
 211                 boolean is_measured() {
 212                         return measured.value() != AltosLib.MISSING;
 213                 }
 214
 215                 double max() {
 216                         double m = measured.max();
 217
 218                         if (m != AltosLib.MISSING)
 219                                 return m;
 220                         return computed.max();
 221                 }
 222
 223                 double prev_value() {
 224                         if (measured.value != AltosLib.MISSING && measured.prev_value != AltosLib.MISSING)
 225                                 return measured.prev_value;
 226                         return computed.prev_value;
 227                 }
 228
 229                 AltosValue altos_value() {
 230                         if (measured.value() != AltosLib.MISSING)
 231                                 return measured;
 232                         return computed;
 233                 }
 234
 235                 double change() {
 236                         double c = measured.change();
 237                         if (c == AltosLib.MISSING)
 238                                 c = computed.change();
 239                         return c;
 240                 }
 241
 242                 double rate() {
 243                         double r = measured.rate();
 244                         if (r == AltosLib.MISSING)
 245                                 r = computed.rate();
 246                         return r;
 247                 }
 248
 249                 void set_measured(double new_value, double time) {
 250                         measured.set(new_value, time);
 251                 }
 252
 253                 void set_computed(double new_value, double time) {
 254                         computed.set(new_value, time);
 255                 }
 256
 257                 void set_derivative(AltosValue in) {
 258                         computed.set_derivative(in);
 259                 }
 260
 261                 void set_derivative(AltosCValue in) {
 262                         set_derivative(in.altos_value());
 263                 }
 264
 265                 void set_integral(AltosValue in) {
 266                         computed.set_integral(in);
 267                 }
 268
 269                 void set_integral(AltosCValue in) {
 270                         set_integral(in.altos_value());
 271                 }
 272
 273                 void copy(AltosCValue old) {
 274                         measured.copy(old.measured);
 275                         computed.copy(old.computed);
 276                 }
 277
 278                 void finish_update() {
 279                         measured.finish_update();
 280                         computed.finish_update();
 281                 }
 282
 283                 public AltosCValue() {
 284                         measured = new AltosIValue();
 285                         computed = new AltosIValue();
 286                 }
 287         }
 288
 289         public boolean  landed;
 290         public boolean  ascent; /* going up? */
 291         public boolean  boost;  /* under power */
 292
 293         private double pressure_to_altitude(double p) {
 294                 if (p == AltosLib.MISSING)
 295                         return AltosLib.MISSING;
 296                 return AltosConvert.pressure_to_altitude(p);
 297         }
 298
 299         private AltosCValue ground_altitude;
 300
 301         public double ground_altitude() {
 302                 return ground_altitude.value();
 303         }
 304
 305         public void set_ground_altitude(double a) {
 306                 ground_altitude.set_measured(a, time);
 307         }
 308
 309         class AltosGpsGroundAltitude extends AltosValue {
 310                 void set(double a, double t) {
 311                         super.set(a, t);
 312                         pad_alt = value();
 313                         gps_altitude.set_gps_height();
 314                 }
 315
 316                 void set_filtered(double a, double t) {
 317                         super.set_filtered(a, t);
 318                         pad_alt = value();
 319                         gps_altitude.set_gps_height();
 320                 }
 321
 322                 AltosGpsGroundAltitude() {
 323                         super();
 324                 }
 325         }
 326
 327         private AltosGpsGroundAltitude gps_ground_altitude;
 328
 329         public double gps_ground_altitude() {
 330                 return gps_ground_altitude.value();
 331         }
 332
 333         public void set_gps_ground_altitude(double a) {
 334                 gps_ground_altitude.set(a, time);
 335         }
 336
 337         class AltosGroundPressure extends AltosCValue {
 338                 void set_filtered(double p, double time) {
 339                         computed.set_filtered(p, time);
 340                         if (!is_measured())
 341                                 ground_altitude.set_computed(pressure_to_altitude(computed.value()), time);
 342                 }
 343
 344                 void set_measured(double p, double time) {
 345                         super.set_measured(p, time);
 346                         ground_altitude.set_computed(pressure_to_altitude(p), time);
 347                 }
 348
 349                 AltosGroundPressure () {
 350                         super();
 351                 }
 352         }
 353
 354         private AltosGroundPressure ground_pressure;
 355
 356         public double ground_pressure() {
 357                 return ground_pressure.value();
 358         }
 359
 360         public void set_ground_pressure (double pressure) {
 361                 ground_pressure.set_measured(pressure, time);
 362         }
 363
 364         class AltosAltitude extends AltosCValue {
 365
 366                 private void set_speed(AltosValue v) {
 367                         if (!acceleration.is_measured() || !ascent)
 368                                 speed.set_derivative(this);
 369                 }
 370
 371                 void set_computed(double a, double time) {
 372                         super.set_computed(a,time);
 373                         set_speed(computed);
 374                         set |= set_position;
 375                 }
 376
 377                 void set_measured(double a, double time) {
 378                         super.set_measured(a,time);
 379                         set_speed(measured);
 380                         set |= set_position;
 381                 }
 382
 383                 AltosAltitude() {
 384                         super();
 385                 }
 386         }
 387
 388         private AltosAltitude   altitude;
 389
 390         class AltosGpsAltitude extends AltosValue {
 391
 392                 private void set_gps_height() {
 393                         double  a = value();
 394                         double  g = gps_ground_altitude.value();
 395
 396                         if (a != AltosLib.MISSING && g != AltosLib.MISSING)
 397                                 gps_height = a - g;
 398                         else
 399                                 gps_height = AltosLib.MISSING;
 400                 }
 401
 402                 void set(double a, double t) {
 403                         super.set(a, t);
 404                         set_gps_height();
 405                 }
 406
 407                 AltosGpsAltitude() {
 408                         super();
 409                 }
 410         }
 411
 412         private AltosGpsAltitude        gps_altitude;
 413
 414         private AltosValue              gps_ground_speed;
 415         private AltosValue              gps_ascent_rate;
 416         private AltosValue              gps_course;
 417         private AltosValue              gps_speed;
 418
 419         public double altitude() {
 420                 double a = altitude.value();
 421                 if (a != AltosLib.MISSING)
 422                         return a;
 423                 return gps_altitude.value();
 424         }
 425
 426         public double max_altitude() {
 427                 double a = altitude.max();
 428                 if (a != AltosLib.MISSING)
 429                         return a;
 430                 return gps_altitude.max();
 431         }
 432
 433         public void set_altitude(double new_altitude) {
 434                 double old_altitude = altitude.value();
 435                 if (old_altitude != AltosLib.MISSING) {
 436                         while (old_altitude - new_altitude > 32000)
 437                                 new_altitude += 65536.0;
 438                 }
 439                 altitude.set_measured(new_altitude, time);
 440         }
 441
 442         public double gps_altitude() {
 443                 return gps_altitude.value();
 444         }
 445
 446         public double max_gps_altitude() {
 447                 return gps_altitude.max();
 448         }
 449
 450         public void set_gps_altitude(double new_gps_altitude) {
 451                 gps_altitude.set(new_gps_altitude, time);
 452         }
 453
 454         public double gps_ground_speed() {
 455                 return gps_ground_speed.value();
 456         }
 457
 458         public double max_gps_ground_speed() {
 459                 return gps_ground_speed.max();
 460         }
 461
 462         public double gps_ascent_rate() {
 463                 return gps_ascent_rate.value();
 464         }
 465
 466         public double max_gps_ascent_rate() {
 467                 return gps_ascent_rate.max();
 468         }
 469
 470         public double gps_course() {
 471                 return gps_course.value();
 472         }
 473
 474         public double gps_speed() {
 475                 return gps_speed.value();
 476         }
 477
 478         public double max_gps_speed() {
 479                 return gps_speed.max();
 480         }
 481
 482         class AltosPressure extends AltosValue {
 483                 void set(double p, double time) {
 484                         super.set(p, time);
 485                         if (state == AltosLib.ao_flight_pad)
 486                                 ground_pressure.set_filtered(p, time);
 487                         double a = pressure_to_altitude(p);
 488                         altitude.set_computed(a, time);
 489                 }
 490
 491                 AltosPressure() {
 492                         super();
 493                 }
 494         }
 495
 496         private AltosPressure   pressure;
 497
 498         public double pressure() {
 499                 return pressure.value();
 500         }
 501
 502         public void set_pressure(double p) {
 503                 pressure.set(p, time);
 504         }
 505
 506         public void set_thrust(double N) {
 507         }
 508
 509         public double baro_height() {
 510                 double a = altitude();
 511                 double g = ground_altitude();
 512                 if (a != AltosLib.MISSING && g != AltosLib.MISSING)
 513                         return a - g;
 514                 return AltosLib.MISSING;
 515         }
 516
 517         public double height() {
 518                 double k = kalman_height.value();
 519                 if (k != AltosLib.MISSING)
 520                         return k;
 521
 522                 double b = baro_height();
 523                 if (b != AltosLib.MISSING)
 524                         return b;
 525
 526                 return gps_height();
 527         }
 528
 529         public double max_height() {
 530                 double  k = kalman_height.max();
 531                 if (k != AltosLib.MISSING)
 532                         return k;
 533
 534                 double a = altitude.max();
 535                 double g = ground_altitude();
 536                 if (a != AltosLib.MISSING && g != AltosLib.MISSING)
 537                         return a - g;
 538                 return max_gps_height();
 539         }
 540
 541         public double gps_height() {
 542                 double a = gps_altitude();
 543                 double g = gps_ground_altitude();
 544
 545                 if (a != AltosLib.MISSING && g != AltosLib.MISSING)
 546                         return a - g;
 547                 return AltosLib.MISSING;
 548         }
 549
 550         public double max_gps_height() {
 551                 double a = gps_altitude.max();
 552                 double g = gps_ground_altitude();
 553
 554                 if (a != AltosLib.MISSING && g != AltosLib.MISSING)
 555                         return a - g;
 556                 return AltosLib.MISSING;
 557         }
 558
 559         class AltosSpeed extends AltosCValue {
 560
 561                 boolean can_max() {
 562                         return state < AltosLib.ao_flight_fast || state == AltosLib.ao_flight_stateless;
 563                 }
 564
 565                 void set_accel() {
 566                         acceleration.set_derivative(this);
 567                 }
 568
 569                 void set_derivative(AltosCValue in) {
 570                         super.set_derivative(in);
 571                         set_accel();
 572                 }
 573
 574                 void set_computed(double new_value, double time) {
 575                         super.set_computed(new_value, time);
 576                         set_accel();
 577                 }
 578
 579                 void set_measured(double new_value, double time) {
 580                         super.set_measured(new_value, time);
 581                         set_accel();
 582                 }
 583
 584                 AltosSpeed() {
 585                         super();
 586                 }
 587         }
 588
 589         private AltosSpeed speed;
 590
 591         public double speed() {
 592                 double v = kalman_speed.value();
 593                 if (v != AltosLib.MISSING)
 594                         return v;
 595                 v = speed.value();
 596                 if (v != AltosLib.MISSING)
 597                         return v;
 598                 v = gps_speed();
 599                 if (v != AltosLib.MISSING)
 600                         return v;
 601                 return AltosLib.MISSING;
 602         }
 603
 604         public double max_speed() {
 605                 double v = kalman_speed.max();
 606                 if (v != AltosLib.MISSING)
 607                         return v;
 608                 v = speed.max();
 609                 if (v != AltosLib.MISSING)
 610                         return v;
 611                 v = max_gps_speed();
 612                 if (v != AltosLib.MISSING)
 613                         return v;
 614                 return AltosLib.MISSING;
 615         }
 616
 617         class AltosAccel extends AltosCValue {
 618
 619                 boolean can_max() {
 620                         return state < AltosLib.ao_flight_fast || state == AltosLib.ao_flight_stateless;
 621                 }
 622
 623                 void set_measured(double a, double time) {
 624                         super.set_measured(a, time);
 625                         if (ascent)
 626                                 speed.set_integral(this.measured);
 627                 }
 628
 629                 AltosAccel() {
 630                         super();
 631                 }
 632         }
 633
 634         AltosAccel acceleration;
 635
 636         public double acceleration() {
 637                 return acceleration.value();
 638         }
 639
 640         public double max_acceleration() {
 641                 return acceleration.max();
 642         }
 643
 644         public AltosCValue      orient;
 645
 646         public void set_orient(double new_orient) {
 647                 orient.set_measured(new_orient, time);
 648         }
 649
 650         public double orient() {
 651                 return orient.value();
 652         }
 653
 654         public double max_orient() {
 655                 return orient.max();
 656         }
 657
 658         public AltosValue       kalman_height, kalman_speed, kalman_acceleration;
 659
 660         public void set_kalman(double height, double speed, double acceleration) {
 661                 double old_height = kalman_height.value();
 662                 if (old_height != AltosLib.MISSING) {
 663                         while (old_height - height > 32000)
 664                                 height += 65536;
 665                 }
 666                 kalman_height.set(height, time);
 667                 kalman_speed.set(speed, time);
 668                 kalman_acceleration.set(acceleration, time);
 669         }
 670
 671         public double   battery_voltage;
 672         public double   pyro_voltage;
 673         public double   temperature;
 674         public double   apogee_voltage;
 675         public double   main_voltage;
 676
 677         public double   igniter_voltage[];
 678
 679         public AltosGPS gps;
 680         public boolean  gps_pending;
 681
 682         public AltosIMU imu;
 683         public AltosMag mag;
 684
 685         public static final int MIN_PAD_SAMPLES = 10;
 686
 687         public int      npad;
 688         public int      gps_waiting;
 689         public boolean  gps_ready;
 690
 691         public int      ngps;
 692
 693         public AltosGreatCircle from_pad;
 694         public double   elevation;      /* from pad */
 695         public double   range;          /* total distance */
 696
 697         public double   gps_height;
 698
 699         public double pad_lat, pad_lon, pad_alt;
 700
 701         public int      speak_tick;
 702         public double   speak_altitude;
 703
 704         public double   ground_accel;
 705
 706         public AltosCompanion   companion;
 707
 708         public int      pyro_fired;
 709
 710         public void set_npad(int npad) {
 711                 this.npad = npad;
 712                 gps_waiting = MIN_PAD_SAMPLES - npad;
 713                 if (this.gps_waiting < 0)
 714                         gps_waiting = 0;
 715                 gps_ready = gps_waiting == 0;
 716         }
 717
 718         public void init() {
 719                 set = 0;
 720
 721                 received_time = System.currentTimeMillis();
 722                 time = AltosLib.MISSING;
 723                 state = AltosLib.ao_flight_invalid;
 724                 landed = false;
 725                 boost = false;
 726                 rssi = AltosLib.MISSING;
 727                 status = 0;
 728
 729                 ground_altitude = new AltosCValue();
 730                 ground_pressure = new AltosGroundPressure();
 731                 altitude = new AltosAltitude();
 732                 pressure = new AltosPressure();
 733                 speed = new AltosSpeed();
 734                 acceleration = new AltosAccel();
 735                 orient = new AltosCValue();
 736
 737                 temperature = AltosLib.MISSING;
 738                 battery_voltage = AltosLib.MISSING;
 739                 pyro_voltage = AltosLib.MISSING;
 740                 apogee_voltage = AltosLib.MISSING;
 741                 main_voltage = AltosLib.MISSING;
 742                 igniter_voltage = null;
 743
 744                 kalman_height = new AltosValue();
 745                 kalman_speed = new AltosValue();
 746                 kalman_acceleration = new AltosValue();
 747
 748                 gps = null;
 749                 gps_pending = false;
 750
 751                 imu = null;
 752                 last_imu_time = AltosLib.MISSING;
 753                 rotation = null;
 754                 ground_rotation = null;
 755
 756                 mag = null;
 757
 758                 accel_ground_along = AltosLib.MISSING;
 759                 accel_ground_across = AltosLib.MISSING;
 760                 accel_ground_through = AltosLib.MISSING;
 761
 762                 pad_orientation = AltosLib.MISSING;
 763
 764                 set_npad(0);
 765                 ngps = 0;
 766
 767                 from_pad = null;
 768                 elevation = AltosLib.MISSING;
 769                 range = AltosLib.MISSING;
 770                 gps_height = AltosLib.MISSING;
 771
 772                 pad_lat = AltosLib.MISSING;
 773                 pad_lon = AltosLib.MISSING;
 774                 pad_alt = AltosLib.MISSING;
 775
 776                 gps_altitude = new AltosGpsAltitude();
 777                 gps_ground_altitude = new AltosGpsGroundAltitude();
 778                 gps_ground_speed = new AltosValue();
 779                 gps_speed = new AltosValue();
 780                 gps_ascent_rate = new AltosValue();
 781                 gps_course = new AltosValue();
 782
 783                 speak_tick = AltosLib.MISSING;
 784                 speak_altitude = AltosLib.MISSING;
 785
 786                 ground_accel = AltosLib.MISSING;
 787
 788                 companion = null;
 789
 790                 pyro_fired = 0;
 791         }
 792
 793         void finish_update() {
 794                 ground_altitude.finish_update();
 795                 altitude.finish_update();
 796                 pressure.finish_update();
 797                 speed.finish_update();
 798                 acceleration.finish_update();
 799                 orient.finish_update();
 800
 801                 kalman_height.finish_update();
 802                 kalman_speed.finish_update();
 803                 kalman_acceleration.finish_update();
 804         }
 805
 806         void update_time() {
 807         }
 808
 809         void update_gps() {
 810                 elevation = AltosLib.MISSING;
 811                 range = AltosLib.MISSING;
 812
 813                 if (gps == null)
 814                         return;
 815
 816                 if (gps.locked && gps.nsat >= 4) {
 817                         /* Track consecutive 'good' gps reports, waiting for 10 of them */
 818                         if (state == AltosLib.ao_flight_pad || state == AltosLib.ao_flight_stateless) {
 819                                 set_npad(npad+1);
 820                                 if (pad_lat != AltosLib.MISSING && (npad < 10 || state == AltosLib.ao_flight_pad)) {
 821                                         pad_lat = (pad_lat * 31 + gps.lat) / 32;
 822                                         pad_lon = (pad_lon * 31 + gps.lon) / 32;
 823                                         gps_ground_altitude.set_filtered(gps.alt, time);
 824                                 }
 825                         }
 826                         if (pad_lat == AltosLib.MISSING) {
 827                                 pad_lat = gps.lat;
 828                                 pad_lon = gps.lon;
 829                                 gps_ground_altitude.set(gps.alt, time);
 830                         }
 831                         gps_altitude.set(gps.alt, time);
 832                         if (gps.climb_rate != AltosLib.MISSING)
 833                                 gps_ascent_rate.set(gps.climb_rate, time);
 834                         if (gps.ground_speed != AltosLib.MISSING)
 835                                 gps_ground_speed.set(gps.ground_speed, time);
 836                         if (gps.climb_rate != AltosLib.MISSING && gps.ground_speed != AltosLib.MISSING)
 837                                 gps_speed.set(Math.sqrt(gps.ground_speed * gps.ground_speed +
 838                                                         gps.climb_rate * gps.climb_rate), time);
 839                         if (gps.course != AltosLib.MISSING)
 840                                 gps_course.set(gps.course, time);
 841                 }
 842                 if (gps.lat != 0 && gps.lon != 0 &&
 843                     pad_lat != AltosLib.MISSING &&
 844                     pad_lon != AltosLib.MISSING)
 845                 {
 846                         double h = height();
 847
 848                         if (h == AltosLib.MISSING)
 849                                 h = 0;
 850                         from_pad = new AltosGreatCircle(pad_lat, pad_lon, 0, gps.lat, gps.lon, h);
 851                         elevation = from_pad.elevation;
 852                         range = from_pad.range;
 853                 }
 854         }
 855
 856         public String state_name() {
 857                 return AltosLib.state_name(state);
 858         }
 859
 860         public void set_state(int state) {
 861                 if (state != AltosLib.ao_flight_invalid) {
 862                         this.state = state;
 863                         ascent = (AltosLib.ao_flight_boost <= state &&
 864                                   state <= AltosLib.ao_flight_coast);
 865                         boost = (AltosLib.ao_flight_boost == state);
 866                 }
 867         }
 868
 869         public int state() {
 870                 return state;
 871         }
 872
 873         private void re_init() {
 874                 init();
 875         }
 876
 877         public int rssi() {
 878                 if (rssi == AltosLib.MISSING)
 879                         return 0;
 880                 return rssi;
 881         }
 882
 883         public void set_rssi(int rssi, int status) {
 884                 if (rssi != AltosLib.MISSING) {
 885                         this.rssi = rssi;
 886                         this.status = status;
 887                 }
 888         }
 889
 890         public void set_received_time(long ms) {
 891                 received_time = ms;
 892         }
 893
 894         public void set_gps(AltosGPS gps) {
 895                 if (gps != null) {
 896                         this.gps = gps;
 897                         update_gps();
 898                         set |= set_gps;
 899                 }
 900         }
 901
 902
 903         public AltosRotation    rotation;
 904         public AltosRotation    ground_rotation;
 905
 906         public int pad_orientation;
 907
 908         public double   accel_ground_along, accel_ground_across, accel_ground_through;
 909
 910         void update_pad_rotation() {
 911                 if (pad_orientation != AltosLib.MISSING && accel_ground_along != AltosLib.MISSING) {
 912                         rotation = new AltosRotation(AltosIMU.convert_accel(accel_ground_across - cal_data.accel_zero_across),
 913                                                      AltosIMU.convert_accel(accel_ground_through - cal_data.accel_zero_through),
 914                                                      AltosIMU.convert_accel(accel_ground_along - cal_data.accel_zero_along),
 915                                                      pad_orientation);
 916                         ground_rotation = rotation;
 917                         orient.set_computed(rotation.tilt(), time);
 918                 }
 919         }
 920
 921         public void set_accel_ground(double ground_along, double ground_across, double ground_through) {
 922                 accel_ground_along = ground_along;
 923                 accel_ground_across = ground_across;
 924                 accel_ground_through = ground_through;
 925                 update_pad_rotation();
 926         }
 927
 928         public void set_pad_orientation(int pad_orientation) {
 929                 this.pad_orientation = pad_orientation;
 930                 update_pad_rotation();
 931         }
 932
 933         public double   last_imu_time;
 934
 935         private double radians(double degrees) {
 936                 if (degrees == AltosLib.MISSING)
 937                         return AltosLib.MISSING;
 938                 return degrees * Math.PI / 180.0;
 939         }
 940
 941         private void update_orient() {
 942                 if (last_imu_time != AltosLib.MISSING) {
 943                         double  t = time - last_imu_time;
 944
 945                         double  pitch = radians(gyro_pitch());
 946                         double  yaw = radians(gyro_yaw());
 947                         double  roll = radians(gyro_roll());
 948
 949                         if (t > 0 & pitch != AltosLib.MISSING && rotation != null) {
 950                                 rotation.rotate(t, pitch, yaw, roll);
 951                                 orient.set_computed(rotation.tilt(), time);
 952                         }
 953                 }
 954                 last_imu_time = time;
 955         }
 956
 957         private double  gyro_roll, gyro_pitch, gyro_yaw;
 958
 959         public void set_gyro(double roll, double pitch, double yaw) {
 960                 gyro_roll = roll;
 961                 gyro_pitch = roll;
 962                 gyro_roll = roll;
 963                 update_orient();
 964         }
 965
 966         private double accel_along, accel_across, accel_through;
 967
 968         public void set_accel(double along, double across, double through) {
 969                 accel_along = along;
 970                 accel_across = across;
 971                 accel_through = through;
 972                 update_orient();
 973         }
 974
 975         public double accel_along() {
 976                 return accel_along;
 977         }
 978
 979         public double accel_across() {
 980                 return accel_across;
 981         }
 982
 983         public double accel_through() {
 984                 return accel_through;
 985         }
 986
 987         public double gyro_roll() {
 988                 return gyro_roll;
 989         }
 990
 991         public double gyro_pitch() {
 992                 return gyro_pitch;
 993         }
 994
 995         public double gyro_yaw() {
 996                 return gyro_yaw;
 997         }
 998
 999         private double mag_along, mag_across, mag_through;
1000
1001         public void set_mag(double along, double across, double through) {
1002                 mag_along = along;
1003                 mag_across = across;
1004                 mag_through = through;
1005         }
1006
1007         public double mag_along() {
1008                 return mag_along;
1009         }
1010
1011         public double mag_across() {
1012                 if (mag != null)
1013                         return AltosMag.convert_gauss(mag.across);
1014                 return AltosLib.MISSING;
1015         }
1016
1017         public double mag_through() {
1018                 if (mag != null)
1019                         return AltosMag.convert_gauss(mag.through);
1020                 return AltosLib.MISSING;
1021         }
1022
1023         public void set_companion(AltosCompanion companion) {
1024                 this.companion = companion;
1025         }
1026
1027         public void set_acceleration(double acceleration) {
1028                 if (acceleration != AltosLib.MISSING) {
1029                         this.acceleration.set_measured(acceleration, time);
1030                         set |= set_data;
1031                 }
1032         }
1033
1034         public void set_temperature(double temperature) {
1035                 if (temperature != AltosLib.MISSING) {
1036                         this.temperature = temperature;
1037                         set |= set_data;
1038                 }
1039         }
1040
1041         public void set_battery_voltage(double battery_voltage) {
1042                 if (battery_voltage != AltosLib.MISSING) {
1043                         this.battery_voltage = battery_voltage;
1044                         set |= set_data;
1045                 }
1046         }
1047
1048         public void set_pyro_voltage(double pyro_voltage) {
1049                 if (pyro_voltage != AltosLib.MISSING) {
1050                         this.pyro_voltage = pyro_voltage;
1051                         set |= set_data;
1052                 }
1053         }
1054
1055         public void set_apogee_voltage(double apogee_voltage) {
1056                 if (apogee_voltage != AltosLib.MISSING) {
1057                         this.apogee_voltage = apogee_voltage;
1058                         set |= set_data;
1059                 }
1060         }
1061
1062         public void set_main_voltage(double main_voltage) {
1063                 if (main_voltage != AltosLib.MISSING) {
1064                         this.main_voltage = main_voltage;
1065                         set |= set_data;
1066                 }
1067         }
1068
1069         public void set_igniter_voltage(double[] voltage) {
1070                 this.igniter_voltage = voltage;
1071         }
1072
1073         public void set_pyro_fired(int fired) {
1074                 this.pyro_fired = fired;
1075         }
1076
1077         public AltosState() {
1078                 Thread.dumpStack();
1079                 init();
1080         }
1081
1082         public AltosState (AltosCalData cal_data) {
1083                 super(cal_data);
1084                 if (cal_data == null)
1085                         Thread.dumpStack();
1086                 init();
1087         }
1088 }