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