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; version 2 of the License.
   7  *
   8  * This program is distributed in the hope that it will be useful, but
   9  * WITHOUT ANY WARRANTY; without even the implied warranty of
  10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  11  * General Public License for more details.
  12  *
  13  * You should have received a copy of the GNU General Public License along
  14  * with this program; if not, write to the Free Software Foundation, Inc.,
  15  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  16  */
  17
  18 /*
  19  * Track flight state from telemetry or eeprom data stream
  20  */
  21
  22 package org.altusmetrum.altoslib_11;
  23
  24 import java.io.*;
  25
  26 public class AltosState implements Cloneable, AltosJsonable {
  27
  28         public static final int set_position = 1;
  29         public static final int set_gps = 2;
  30         public static final int set_data = 4;
  31
  32         public int set;
  33
  34         static final double filter_len = 2.0;
  35         static final double ascent_filter_len = 0.5;
  36         static final double descent_filter_len = 5.0;
  37
  38         /* derived data */
  39
  40         public long     received_time;
  41
  42         public double   time;
  43         public double   prev_time;
  44         public double   time_change;
  45         public int      tick;
  46         private int     prev_tick;
  47         public int      boost_tick;
  48
  49         class AltosValue implements AltosJsonable {
  50                 double  value;
  51                 double  prev_value;
  52                 private double  max_value;
  53                 private double  set_time;
  54                 private double  prev_set_time;
  55
  56                 boolean can_max() { return true; }
  57
  58                 void set(double new_value, double time) {
  59                         if (new_value != AltosLib.MISSING) {
  60                                 value = new_value;
  61                                 if (can_max() && (max_value == AltosLib.MISSING || value > max_value))
  62                                         max_value = value;
  63                                 set_time = time;
  64                         }
  65                 }
  66
  67                 void set_filtered(double new_value, double time) {
  68                         if (prev_value != AltosLib.MISSING) {
  69                                 double f = 1/Math.exp((time - prev_set_time) / filter_len);
  70                                 new_value = f * new_value + (1-f) * prev_value;
  71                         }
  72                         set(new_value, time);
  73                 }
  74
  75                 double value() {
  76                         return value;
  77                 }
  78
  79                 double max() {
  80                         return max_value;
  81                 }
  82
  83                 double prev() {
  84                         return prev_value;
  85                 }
  86
  87                 double change() {
  88                         if (value != AltosLib.MISSING && prev_value != AltosLib.MISSING)
  89                                 return value - prev_value;
  90                         return AltosLib.MISSING;
  91                 }
  92
  93                 double rate() {
  94                         double c = change();
  95                         double t = set_time - prev_set_time;
  96
  97                         if (c != AltosLib.MISSING && t != 0)
  98                                 return c / t;
  99                         return AltosLib.MISSING;
 100                 }
 101
 102                 double integrate() {
 103                         if (value == AltosLib.MISSING)
 104                                 return AltosLib.MISSING;
 105                         if (prev_value == AltosLib.MISSING)
 106                                 return AltosLib.MISSING;
 107
 108                         return (value + prev_value) / 2 * (set_time - prev_set_time);
 109                 }
 110
 111                 double time() {
 112                         return set_time;
 113                 }
 114
 115                 void set_derivative(AltosValue in) {
 116                         double  n = in.rate();
 117
 118                         if (n == AltosLib.MISSING)
 119                                 return;
 120
 121                         double  p = prev_value;
 122                         double  pt = prev_set_time;
 123
 124                         if (p == AltosLib.MISSING) {
 125                                 p = 0;
 126                                 pt = in.time() - 0.01;
 127                         }
 128
 129                         /* Clip changes to reduce noise */
 130                         double  ddt = in.time() - pt;
 131                         double  ddv = (n - p) / ddt;
 132
 133                         final double max = 100000;
 134
 135                         /* 100gs */
 136                         if (Math.abs(ddv) > max) {
 137                                 if (n > p)
 138                                         n = p + ddt * max;
 139                                 else
 140                                         n = p - ddt * max;
 141                         }
 142
 143                         double filter_len;
 144
 145                         if (ascent)
 146                                 filter_len = ascent_filter_len;
 147                         else
 148                                 filter_len = descent_filter_len;
 149
 150                         double f = 1/Math.exp(ddt/ filter_len);
 151                         n = p * f + n * (1-f);
 152
 153                         set(n, in.time());
 154                 }
 155
 156                 void set_integral(AltosValue in) {
 157                         double  change = in.integrate();
 158
 159                         if (change != AltosLib.MISSING) {
 160                                 double  prev = prev_value;
 161                                 if (prev == AltosLib.MISSING)
 162                                         prev = 0;
 163                                 set(prev + change, in.time());
 164                         }
 165                 }
 166
 167                 void copy(AltosValue old) {
 168                         value = old.value;
 169                         set_time = old.set_time;
 170                         prev_value = old.value;
 171                         prev_set_time = old.set_time;
 172                         max_value = old.max_value;
 173                 }
 174
 175                 void finish_update() {
 176                         prev_value = value;
 177                         prev_set_time = set_time;
 178                 }
 179
 180                 public AltosJson json() {
 181                         AltosJson j = new AltosJson();
 182
 183                         j.put("value", value);
 184                         j.put("prev_value", prev_value);
 185                         j.put("max_value", max_value);
 186                         j.put("set_time", set_time);
 187                         j.put("prev_set_time", prev_set_time);
 188                         return j;
 189                 }
 190
 191                 AltosValue(AltosJson j) {
 192                         this();
 193                         if (j != null) {
 194                                 value = j.get_double("value", value);
 195                                 prev_value = j.get_double("prev_value", prev_value);
 196                                 max_value = j.get_double("max_value", max_value);
 197                                 set_time = j.get_double("set_time", 0);
 198                                 prev_set_time = j.get_double("prev_set_time", 0);
 199                         }
 200                 }
 201
 202                 AltosValue() {
 203                         value = AltosLib.MISSING;
 204                         prev_value = AltosLib.MISSING;
 205                         max_value = AltosLib.MISSING;
 206                 }
 207
 208         }
 209
 210         AltosValue AltosValue_fromJson(AltosJson j, AltosValue def) {
 211                 if (j == null)
 212                         return def;
 213                 return new AltosValue(j);
 214         }
 215
 216         class AltosCValue implements AltosJsonable {
 217
 218                 class AltosIValue extends AltosValue implements AltosJsonable {
 219                         boolean can_max() {
 220                                 return c_can_max();
 221                         }
 222
 223                         AltosIValue() {
 224                                 super();
 225                         }
 226
 227                         AltosIValue(AltosJson j) {
 228                                 super(j);
 229                         }
 230                 };
 231
 232                 public AltosIValue      measured;
 233                 public AltosIValue      computed;
 234
 235                 boolean can_max() { return true; }
 236
 237                 boolean c_can_max() { return can_max(); }
 238
 239                 double value() {
 240                         double v = measured.value();
 241                         if (v != AltosLib.MISSING)
 242                                 return v;
 243                         return computed.value();
 244                 }
 245
 246                 boolean is_measured() {
 247                         return measured.value() != AltosLib.MISSING;
 248                 }
 249
 250                 double max() {
 251                         double m = measured.max();
 252
 253                         if (m != AltosLib.MISSING)
 254                                 return m;
 255                         return computed.max();
 256                 }
 257
 258                 double prev_value() {
 259                         if (measured.value != AltosLib.MISSING && measured.prev_value != AltosLib.MISSING)
 260                                 return measured.prev_value;
 261                         return computed.prev_value;
 262                 }
 263
 264                 AltosValue altos_value() {
 265                         if (measured.value() != AltosLib.MISSING)
 266                                 return measured;
 267                         return computed;
 268                 }
 269
 270                 double change() {
 271                         double c = measured.change();
 272                         if (c == AltosLib.MISSING)
 273                                 c = computed.change();
 274                         return c;
 275                 }
 276
 277                 double rate() {
 278                         double r = measured.rate();
 279                         if (r == AltosLib.MISSING)
 280                                 r = computed.rate();
 281                         return r;
 282                 }
 283
 284                 void set_measured(double new_value, double time) {
 285                         measured.set(new_value, time);
 286                 }
 287
 288                 void set_computed(double new_value, double time) {
 289                         computed.set(new_value, time);
 290                 }
 291
 292                 void set_derivative(AltosValue in) {
 293                         computed.set_derivative(in);
 294                 }
 295
 296                 void set_derivative(AltosCValue in) {
 297                         set_derivative(in.altos_value());
 298                 }
 299
 300                 void set_integral(AltosValue in) {
 301                         computed.set_integral(in);
 302                 }
 303
 304                 void set_integral(AltosCValue in) {
 305                         set_integral(in.altos_value());
 306                 }
 307
 308                 void copy(AltosCValue old) {
 309                         measured.copy(old.measured);
 310                         computed.copy(old.computed);
 311                 }
 312
 313                 void finish_update() {
 314                         measured.finish_update();
 315                         computed.finish_update();
 316                 }
 317
 318                 AltosCValue() {
 319                         measured = new AltosIValue();
 320                         computed = new AltosIValue();
 321                 }
 322
 323                 public AltosJson json() {
 324                         AltosJson       j = new AltosJson();
 325
 326                         j.put("measured", measured.json());
 327                         j.put("computed", computed.json());
 328                         return j;
 329                 }
 330
 331                 AltosCValue(AltosJson j) {
 332                         measured = new AltosIValue(j.get("measured"));
 333                         computed = new AltosIValue(j.get("computed"));
 334                 }
 335         }
 336
 337         AltosCValue AltosCValue_fromJson(AltosJson j, AltosCValue def) {
 338                 if (j == null)
 339                         return def;
 340                 return new AltosCValue(j);
 341         }
 342
 343         private int     state;
 344         public int      flight;
 345         public int      serial;
 346         public int      altitude_32;
 347         public int      receiver_serial;
 348         public boolean  landed;
 349         public boolean  ascent; /* going up? */
 350         public boolean  boost;  /* under power */
 351         public int      rssi;
 352         public int      status;
 353         public int      device_type;
 354         public int      config_major;
 355         public int      config_minor;
 356         public int      apogee_delay;
 357         public int      main_deploy;
 358         public int      flight_log_max;
 359
 360         private double pressure_to_altitude(double p) {
 361                 if (p == AltosLib.MISSING)
 362                         return AltosLib.MISSING;
 363                 return AltosConvert.pressure_to_altitude(p);
 364         }
 365
 366         private AltosCValue ground_altitude;
 367
 368         public double ground_altitude() {
 369                 return ground_altitude.value();
 370         }
 371
 372         public void set_ground_altitude(double a) {
 373                 ground_altitude.set_measured(a, time);
 374         }
 375
 376         class AltosGpsGroundAltitude extends AltosValue {
 377                 void set(double a, double t) {
 378                         super.set(a, t);
 379                         pad_alt = value();
 380                         gps_altitude.set_gps_height();
 381                 }
 382
 383                 void set_filtered(double a, double t) {
 384                         super.set_filtered(a, t);
 385                         pad_alt = value();
 386                         gps_altitude.set_gps_height();
 387                 }
 388
 389                 AltosGpsGroundAltitude() {
 390                         super();
 391                 }
 392
 393                 AltosGpsGroundAltitude (AltosJson j) {
 394                         super(j);
 395                 }
 396         }
 397
 398         AltosGpsGroundAltitude AltosGpsGroundAltitude_fromJson(AltosJson j, AltosGpsGroundAltitude def) {
 399                 if (j == null) return def;
 400                 return new AltosGpsGroundAltitude(j);
 401         }
 402
 403         private AltosGpsGroundAltitude gps_ground_altitude;
 404
 405         public double gps_ground_altitude() {
 406                 return gps_ground_altitude.value();
 407         }
 408
 409         public void set_gps_ground_altitude(double a) {
 410                 gps_ground_altitude.set(a, time);
 411         }
 412
 413         class AltosGroundPressure extends AltosCValue {
 414                 void set_filtered(double p, double time) {
 415                         computed.set_filtered(p, time);
 416                         if (!is_measured())
 417                                 ground_altitude.set_computed(pressure_to_altitude(computed.value()), time);
 418                 }
 419
 420                 void set_measured(double p, double time) {
 421                         super.set_measured(p, time);
 422                         ground_altitude.set_computed(pressure_to_altitude(p), time);
 423                 }
 424
 425                 AltosGroundPressure () {
 426                         super();
 427                 }
 428
 429                 AltosGroundPressure (AltosJson j) {
 430                         super(j);
 431                 }
 432         }
 433
 434         AltosGroundPressure AltosGroundPressure_fromJson(AltosJson j, AltosGroundPressure def) {
 435                 if (j == null) return def;
 436                 return new AltosGroundPressure(j);
 437         }
 438
 439         private AltosGroundPressure ground_pressure;
 440
 441         public double ground_pressure() {
 442                 return ground_pressure.value();
 443         }
 444
 445         public void set_ground_pressure (double pressure) {
 446                 ground_pressure.set_measured(pressure, time);
 447         }
 448
 449         class AltosAltitude extends AltosCValue {
 450
 451                 private void set_speed(AltosValue v) {
 452                         if (!acceleration.is_measured() || !ascent)
 453                                 speed.set_derivative(this);
 454                 }
 455
 456                 void set_computed(double a, double time) {
 457                         super.set_computed(a,time);
 458                         set_speed(computed);
 459                         set |= set_position;
 460                 }
 461
 462                 void set_measured(double a, double time) {
 463                         super.set_measured(a,time);
 464                         set_speed(measured);
 465                         set |= set_position;
 466                 }
 467
 468                 AltosAltitude() {
 469                         super();
 470                 }
 471
 472                 AltosAltitude (AltosJson j) {
 473                         super(j);
 474                 }
 475         }
 476
 477         AltosAltitude AltosAltitude_fromJson(AltosJson j, AltosAltitude def) {
 478                 if (j == null) return def;
 479                 return new AltosAltitude(j);
 480         }
 481
 482         private AltosAltitude   altitude;
 483
 484         class AltosGpsAltitude extends AltosValue {
 485
 486                 private void set_gps_height() {
 487                         double  a = value();
 488                         double  g = gps_ground_altitude.value();
 489
 490                         if (a != AltosLib.MISSING && g != AltosLib.MISSING)
 491                                 gps_height = a - g;
 492                         else
 493                                 gps_height = AltosLib.MISSING;
 494                 }
 495
 496                 void set(double a, double t) {
 497                         super.set(a, t);
 498                         set_gps_height();
 499                 }
 500
 501                 AltosGpsAltitude() {
 502                         super();
 503                 }
 504
 505                 AltosGpsAltitude (AltosJson j) {
 506                         super(j);
 507                 }
 508         }
 509
 510         AltosGpsAltitude AltosGpsAltitude_fromJson(AltosJson j, AltosGpsAltitude def) {
 511                 if (j == null) return def;
 512                 return new AltosGpsAltitude(j);
 513         }
 514
 515         private AltosGpsAltitude        gps_altitude;
 516
 517         private AltosValue              gps_ground_speed;
 518         private AltosValue              gps_ascent_rate;
 519         private AltosValue              gps_course;
 520         private AltosValue              gps_speed;
 521
 522         public double altitude() {
 523                 double a = altitude.value();
 524                 if (a != AltosLib.MISSING)
 525                         return a;
 526                 return gps_altitude.value();
 527         }
 528
 529         public double max_altitude() {
 530                 double a = altitude.max();
 531                 if (a != AltosLib.MISSING)
 532                         return a;
 533                 return gps_altitude.max();
 534         }
 535
 536         public void set_altitude(double new_altitude) {
 537                 altitude.set_measured(new_altitude, time);
 538         }
 539
 540         public double gps_altitude() {
 541                 return gps_altitude.value();
 542         }
 543
 544         public double max_gps_altitude() {
 545                 return gps_altitude.max();
 546         }
 547
 548         public void set_gps_altitude(double new_gps_altitude) {
 549                 gps_altitude.set(new_gps_altitude, time);
 550         }
 551
 552         public double gps_ground_speed() {
 553                 return gps_ground_speed.value();
 554         }
 555
 556         public double max_gps_ground_speed() {
 557                 return gps_ground_speed.max();
 558         }
 559
 560         public double gps_ascent_rate() {
 561                 return gps_ascent_rate.value();
 562         }
 563
 564         public double max_gps_ascent_rate() {
 565                 return gps_ascent_rate.max();
 566         }
 567
 568         public double gps_course() {
 569                 return gps_course.value();
 570         }
 571
 572         public double gps_speed() {
 573                 return gps_speed.value();
 574         }
 575
 576         public double max_gps_speed() {
 577                 return gps_speed.max();
 578         }
 579
 580         class AltosPressure extends AltosValue {
 581                 void set(double p, double time) {
 582                         super.set(p, time);
 583                         if (state == AltosLib.ao_flight_pad)
 584                                 ground_pressure.set_filtered(p, time);
 585                         double a = pressure_to_altitude(p);
 586                         altitude.set_computed(a, time);
 587                 }
 588
 589                 AltosPressure() {
 590                         super();
 591                 }
 592
 593                 AltosPressure (AltosJson j) {
 594                         super(j);
 595                 }
 596         }
 597
 598         AltosPressure AltosPressure_fromJson(AltosJson j, AltosPressure def) {
 599                 if (j == null) return def;
 600                 return new AltosPressure(j);
 601         }
 602
 603         private AltosPressure   pressure;
 604
 605         public double pressure() {
 606                 return pressure.value();
 607         }
 608
 609         public void set_pressure(double p) {
 610                 pressure.set(p, time);
 611         }
 612
 613         public double baro_height() {
 614                 double a = altitude();
 615                 double g = ground_altitude();
 616                 if (a != AltosLib.MISSING && g != AltosLib.MISSING)
 617                         return a - g;
 618                 return AltosLib.MISSING;
 619         }
 620
 621         public double height() {
 622                 double k = kalman_height.value();
 623                 if (k != AltosLib.MISSING)
 624                         return k;
 625
 626                 double b = baro_height();
 627                 if (b != AltosLib.MISSING)
 628                         return b;
 629
 630                 return gps_height();
 631         }
 632
 633         public double max_height() {
 634                 double  k = kalman_height.max();
 635                 if (k != AltosLib.MISSING)
 636                         return k;
 637
 638                 double a = altitude.max();
 639                 double g = ground_altitude();
 640                 if (a != AltosLib.MISSING && g != AltosLib.MISSING)
 641                         return a - g;
 642                 return max_gps_height();
 643         }
 644
 645         public double gps_height() {
 646                 double a = gps_altitude();
 647                 double g = gps_ground_altitude();
 648
 649                 if (a != AltosLib.MISSING && g != AltosLib.MISSING)
 650                         return a - g;
 651                 return AltosLib.MISSING;
 652         }
 653
 654         public double max_gps_height() {
 655                 double a = gps_altitude.max();
 656                 double g = gps_ground_altitude();
 657
 658                 if (a != AltosLib.MISSING && g != AltosLib.MISSING)
 659                         return a - g;
 660                 return AltosLib.MISSING;
 661         }
 662
 663         class AltosSpeed extends AltosCValue {
 664
 665                 boolean can_max() {
 666                         return state < AltosLib.ao_flight_fast || state == AltosLib.ao_flight_stateless;
 667                 }
 668
 669                 void set_accel() {
 670                         acceleration.set_derivative(this);
 671                 }
 672
 673                 void set_derivative(AltosCValue in) {
 674                         super.set_derivative(in);
 675                         set_accel();
 676                 }
 677
 678                 void set_computed(double new_value, double time) {
 679                         super.set_computed(new_value, time);
 680                         set_accel();
 681                 }
 682
 683                 void set_measured(double new_value, double time) {
 684                         super.set_measured(new_value, time);
 685                         set_accel();
 686                 }
 687
 688                 AltosSpeed() {
 689                         super();
 690                 }
 691
 692                 AltosSpeed (AltosJson j) {
 693                         super(j);
 694                 }
 695         }
 696
 697         AltosSpeed AltosSpeed_fromJson(AltosJson j, AltosSpeed def) {
 698                 if (j == null) return def;
 699                 return new AltosSpeed(j);
 700         }
 701
 702         private AltosSpeed speed;
 703
 704         public double speed() {
 705                 double v = kalman_speed.value();
 706                 if (v != AltosLib.MISSING)
 707                         return v;
 708                 v = speed.value();
 709                 if (v != AltosLib.MISSING)
 710                         return v;
 711                 v = gps_speed();
 712                 if (v != AltosLib.MISSING)
 713                         return v;
 714                 return AltosLib.MISSING;
 715         }
 716
 717         public double max_speed() {
 718                 double v = kalman_speed.max();
 719                 if (v != AltosLib.MISSING)
 720                         return v;
 721                 v = speed.max();
 722                 if (v != AltosLib.MISSING)
 723                         return v;
 724                 v = max_gps_speed();
 725                 if (v != AltosLib.MISSING)
 726                         return v;
 727                 return AltosLib.MISSING;
 728         }
 729
 730         class AltosAccel extends AltosCValue {
 731
 732                 boolean can_max() {
 733                         return state < AltosLib.ao_flight_fast || state == AltosLib.ao_flight_stateless;
 734                 }
 735
 736                 void set_measured(double a, double time) {
 737                         super.set_measured(a, time);
 738                         if (ascent)
 739                                 speed.set_integral(this.measured);
 740                 }
 741
 742                 AltosAccel() {
 743                         super();
 744                 }
 745
 746                 AltosAccel (AltosJson j) {
 747                         super(j);
 748                 }
 749         }
 750
 751         AltosAccel AltosAccel_fromJson(AltosJson j, AltosAccel def) {
 752                 if (j == null) return def;
 753                 return new AltosAccel(j);
 754         }
 755
 756         AltosAccel acceleration;
 757
 758         public double acceleration() {
 759                 return acceleration.value();
 760         }
 761
 762         public double max_acceleration() {
 763                 return acceleration.max();
 764         }
 765
 766         public AltosCValue      orient;
 767
 768         public void set_orient(double new_orient) {
 769                 orient.set_measured(new_orient, time);
 770         }
 771
 772         public double orient() {
 773                 return orient.value();
 774         }
 775
 776         public double max_orient() {
 777                 return orient.max();
 778         }
 779
 780         public AltosValue       kalman_height, kalman_speed, kalman_acceleration;
 781
 782         public void set_kalman(double height, double speed, double acceleration) {
 783                 kalman_height.set(height, time);
 784                 kalman_speed.set(speed, time);
 785                 kalman_acceleration.set(acceleration, time);
 786         }
 787
 788         public double   battery_voltage;
 789         public double   pyro_voltage;
 790         public double   temperature;
 791         public double   apogee_voltage;
 792         public double   main_voltage;
 793
 794         public double   ignitor_voltage[];
 795
 796         public AltosGPS gps;
 797         public AltosGPS temp_gps;
 798         public int temp_gps_sat_tick;
 799         public boolean  gps_pending;
 800         public int gps_sequence;
 801
 802         public AltosIMU imu;
 803         public AltosMag mag;
 804
 805         public static final int MIN_PAD_SAMPLES = 10;
 806
 807         public int      npad;
 808         public int      gps_waiting;
 809         public boolean  gps_ready;
 810
 811         public int      ngps;
 812
 813         public AltosGreatCircle from_pad;
 814         public double   elevation;      /* from pad */
 815         public double   range;          /* total distance */
 816
 817         public double   gps_height;
 818
 819         public double pad_lat, pad_lon, pad_alt;
 820
 821         public int      speak_tick;
 822         public double   speak_altitude;
 823
 824         public String   callsign;
 825         public String   firmware_version;
 826
 827         public double   accel_plus_g;
 828         public double   accel_minus_g;
 829         public double   accel;
 830         public double   ground_accel;
 831         public double   ground_accel_avg;
 832
 833         public int      log_format;
 834         public int      log_space;
 835         public String   product;
 836
 837         public AltosMs5607      baro;
 838
 839         public AltosCompanion   companion;
 840
 841         public int      pyro_fired;
 842
 843         public void set_npad(int npad) {
 844                 this.npad = npad;
 845                 gps_waiting = MIN_PAD_SAMPLES - npad;
 846                 if (this.gps_waiting < 0)
 847                         gps_waiting = 0;
 848                 gps_ready = gps_waiting == 0;
 849         }
 850
 851         public void init() {
 852                 set = 0;
 853
 854                 received_time = System.currentTimeMillis();
 855                 time = AltosLib.MISSING;
 856                 time_change = AltosLib.MISSING;
 857                 prev_time = AltosLib.MISSING;
 858                 tick = AltosLib.MISSING;
 859                 prev_tick = AltosLib.MISSING;
 860                 boost_tick = AltosLib.MISSING;
 861                 state = AltosLib.ao_flight_invalid;
 862                 flight = AltosLib.MISSING;
 863                 landed = false;
 864                 boost = false;
 865                 rssi = AltosLib.MISSING;
 866                 status = 0;
 867                 device_type = AltosLib.MISSING;
 868                 config_major = AltosLib.MISSING;
 869                 config_minor = AltosLib.MISSING;
 870                 apogee_delay = AltosLib.MISSING;
 871                 main_deploy = AltosLib.MISSING;
 872                 flight_log_max = AltosLib.MISSING;
 873
 874                 ground_altitude = new AltosCValue();
 875                 ground_pressure = new AltosGroundPressure();
 876                 altitude = new AltosAltitude();
 877                 pressure = new AltosPressure();
 878                 speed = new AltosSpeed();
 879                 acceleration = new AltosAccel();
 880                 orient = new AltosCValue();
 881
 882                 temperature = AltosLib.MISSING;
 883                 battery_voltage = AltosLib.MISSING;
 884                 pyro_voltage = AltosLib.MISSING;
 885                 apogee_voltage = AltosLib.MISSING;
 886                 main_voltage = AltosLib.MISSING;
 887                 ignitor_voltage = null;
 888
 889                 kalman_height = new AltosValue();
 890                 kalman_speed = new AltosValue();
 891                 kalman_acceleration = new AltosValue();
 892
 893                 gps = null;
 894                 temp_gps = null;
 895                 temp_gps_sat_tick = 0;
 896                 gps_sequence = 0;
 897                 gps_pending = false;
 898
 899                 imu = null;
 900                 last_imu_time = AltosLib.MISSING;
 901                 rotation = null;
 902                 ground_rotation = null;
 903
 904                 mag = null;
 905                 accel_zero_along = AltosLib.MISSING;
 906                 accel_zero_across = AltosLib.MISSING;
 907                 accel_zero_through = AltosLib.MISSING;
 908
 909                 accel_ground_along = AltosLib.MISSING;
 910                 accel_ground_across = AltosLib.MISSING;
 911                 accel_ground_through = AltosLib.MISSING;
 912
 913                 pad_orientation = AltosLib.MISSING;
 914
 915                 gyro_zero_roll = AltosLib.MISSING;
 916                 gyro_zero_pitch = AltosLib.MISSING;
 917                 gyro_zero_yaw = AltosLib.MISSING;
 918
 919                 set_npad(0);
 920                 ngps = 0;
 921
 922                 from_pad = null;
 923                 elevation = AltosLib.MISSING;
 924                 range = AltosLib.MISSING;
 925                 gps_height = AltosLib.MISSING;
 926
 927                 pad_lat = AltosLib.MISSING;
 928                 pad_lon = AltosLib.MISSING;
 929                 pad_alt = AltosLib.MISSING;
 930
 931                 gps_altitude = new AltosGpsAltitude();
 932                 gps_ground_altitude = new AltosGpsGroundAltitude();
 933                 gps_ground_speed = new AltosValue();
 934                 gps_speed = new AltosValue();
 935                 gps_ascent_rate = new AltosValue();
 936                 gps_course = new AltosValue();
 937
 938                 speak_tick = AltosLib.MISSING;
 939                 speak_altitude = AltosLib.MISSING;
 940
 941                 callsign = null;
 942                 firmware_version = null;
 943
 944                 accel_plus_g = AltosLib.MISSING;
 945                 accel_minus_g = AltosLib.MISSING;
 946                 accel = AltosLib.MISSING;
 947
 948                 ground_accel = AltosLib.MISSING;
 949                 ground_accel_avg = AltosLib.MISSING;
 950
 951                 log_format = AltosLib.MISSING;
 952                 log_space = AltosLib.MISSING;
 953                 product = null;
 954                 serial = AltosLib.MISSING;
 955                 receiver_serial = AltosLib.MISSING;
 956                 altitude_32 = AltosLib.MISSING;
 957
 958                 baro = null;
 959                 companion = null;
 960
 961                 pyro_fired = 0;
 962         }
 963
 964         void finish_update() {
 965                 prev_tick = tick;
 966
 967                 ground_altitude.finish_update();
 968                 altitude.finish_update();
 969                 pressure.finish_update();
 970                 speed.finish_update();
 971                 acceleration.finish_update();
 972                 orient.finish_update();
 973
 974                 kalman_height.finish_update();
 975                 kalman_speed.finish_update();
 976                 kalman_acceleration.finish_update();
 977         }
 978
 979         void copy(AltosState old) {
 980
 981                 if (old == null) {
 982                         init();
 983                         return;
 984                 }
 985
 986                 received_time = old.received_time;
 987                 time = old.time;
 988                 time_change = old.time_change;
 989                 prev_time = old.time;
 990
 991                 tick = old.tick;
 992                 prev_tick = old.tick;
 993                 boost_tick = old.boost_tick;
 994
 995                 state = old.state;
 996                 flight = old.flight;
 997                 landed = old.landed;
 998                 ascent = old.ascent;
 999                 boost = old.boost;
1000                 rssi = old.rssi;
1001                 status = old.status;
1002                 device_type = old.device_type;
1003                 config_major = old.config_major;
1004                 config_minor = old.config_minor;
1005                 apogee_delay = old.apogee_delay;
1006                 main_deploy = old.main_deploy;
1007                 flight_log_max = old.flight_log_max;
1008
1009                 set = 0;
1010
1011                 ground_pressure.copy(old.ground_pressure);
1012                 ground_altitude.copy(old.ground_altitude);
1013                 altitude.copy(old.altitude);
1014                 pressure.copy(old.pressure);
1015                 speed.copy(old.speed);
1016                 acceleration.copy(old.acceleration);
1017                 orient.copy(old.orient);
1018
1019                 battery_voltage = old.battery_voltage;
1020                 pyro_voltage = old.pyro_voltage;
1021                 temperature = old.temperature;
1022                 apogee_voltage = old.apogee_voltage;
1023                 main_voltage = old.main_voltage;
1024                 ignitor_voltage = old.ignitor_voltage;
1025
1026                 kalman_height.copy(old.kalman_height);
1027                 kalman_speed.copy(old.kalman_speed);
1028                 kalman_acceleration.copy(old.kalman_acceleration);
1029
1030                 if (old.gps != null)
1031                         gps = old.gps.clone();
1032                 else
1033                         gps = null;
1034                 if (old.temp_gps != null)
1035                         temp_gps = old.temp_gps.clone();
1036                 else
1037                         temp_gps = null;
1038                 temp_gps_sat_tick = old.temp_gps_sat_tick;
1039                 gps_sequence = old.gps_sequence;
1040                 gps_pending = old.gps_pending;
1041
1042                 if (old.imu != null)
1043                         imu = old.imu.clone();
1044                 else
1045                         imu = null;
1046                 last_imu_time = old.last_imu_time;
1047
1048                 if (old.rotation != null)
1049                         rotation = new AltosRotation (old.rotation);
1050
1051                 if (old.ground_rotation != null) {
1052                         ground_rotation = new AltosRotation(old.ground_rotation);
1053                 }
1054
1055                 accel_zero_along = old.accel_zero_along;
1056                 accel_zero_across = old.accel_zero_across;
1057                 accel_zero_through = old.accel_zero_through;
1058
1059                 accel_ground_along = old.accel_ground_along;
1060                 accel_ground_across = old.accel_ground_across;
1061                 accel_ground_through = old.accel_ground_through;
1062                 pad_orientation = old.pad_orientation;
1063
1064                 gyro_zero_roll = old.gyro_zero_roll;
1065                 gyro_zero_pitch = old.gyro_zero_pitch;
1066                 gyro_zero_yaw = old.gyro_zero_yaw;
1067
1068                 if (old.mag != null)
1069                         mag = old.mag.clone();
1070                 else
1071                         mag = null;
1072
1073                 npad = old.npad;
1074                 gps_waiting = old.gps_waiting;
1075                 gps_ready = old.gps_ready;
1076                 ngps = old.ngps;
1077
1078                 if (old.from_pad != null)
1079                         from_pad = old.from_pad.clone();
1080                 else
1081                         from_pad = null;
1082
1083                 elevation = old.elevation;
1084                 range = old.range;
1085
1086                 gps_height = old.gps_height;
1087
1088                 gps_altitude.copy(old.gps_altitude);
1089                 gps_ground_altitude.copy(old.gps_ground_altitude);
1090                 gps_ground_speed.copy(old.gps_ground_speed);
1091                 gps_ascent_rate.copy(old.gps_ascent_rate);
1092                 gps_course.copy(old.gps_course);
1093                 gps_speed.copy(old.gps_speed);
1094
1095                 pad_lat = old.pad_lat;
1096                 pad_lon = old.pad_lon;
1097                 pad_alt = old.pad_alt;
1098
1099                 speak_tick = old.speak_tick;
1100                 speak_altitude = old.speak_altitude;
1101
1102                 callsign = old.callsign;
1103                 firmware_version = old.firmware_version;
1104
1105                 accel_plus_g = old.accel_plus_g;
1106                 accel_minus_g = old.accel_minus_g;
1107                 accel = old.accel;
1108                 ground_accel = old.ground_accel;
1109                 ground_accel_avg = old.ground_accel_avg;
1110
1111                 log_format = old.log_format;
1112                 log_space = old.log_space;
1113                 product = old.product;
1114                 serial = old.serial;
1115                 receiver_serial = old.receiver_serial;
1116                 altitude_32 = old.altitude_32;
1117
1118                 baro = old.baro;
1119                 companion = old.companion;
1120
1121                 pyro_fired = old.pyro_fired;
1122         }
1123
1124         void update_time() {
1125         }
1126
1127         void update_gps() {
1128                 elevation = AltosLib.MISSING;
1129                 range = AltosLib.MISSING;
1130
1131                 if (gps == null)
1132                         return;
1133
1134                 if (gps.locked && gps.nsat >= 4) {
1135                         /* Track consecutive 'good' gps reports, waiting for 10 of them */
1136                         if (state == AltosLib.ao_flight_pad || state == AltosLib.ao_flight_stateless) {
1137                                 set_npad(npad+1);
1138                                 if (pad_lat != AltosLib.MISSING && (npad < 10 || state == AltosLib.ao_flight_pad)) {
1139                                         pad_lat = (pad_lat * 31 + gps.lat) / 32;
1140                                         pad_lon = (pad_lon * 31 + gps.lon) / 32;
1141                                         gps_ground_altitude.set_filtered(gps.alt, time);
1142                                 }
1143                         }
1144                         if (pad_lat == AltosLib.MISSING) {
1145                                 pad_lat = gps.lat;
1146                                 pad_lon = gps.lon;
1147                                 gps_ground_altitude.set(gps.alt, time);
1148                         }
1149                         gps_altitude.set(gps.alt, time);
1150                         if (gps.climb_rate != AltosLib.MISSING)
1151                                 gps_ascent_rate.set(gps.climb_rate, time);
1152                         if (gps.ground_speed != AltosLib.MISSING)
1153                                 gps_ground_speed.set(gps.ground_speed, time);
1154                         if (gps.climb_rate != AltosLib.MISSING && gps.ground_speed != AltosLib.MISSING)
1155                                 gps_speed.set(Math.sqrt(gps.ground_speed * gps.ground_speed +
1156                                                         gps.climb_rate * gps.climb_rate), time);
1157                         if (gps.course != AltosLib.MISSING)
1158                                 gps_course.set(gps.course, time);
1159                 }
1160                 if (gps.lat != 0 && gps.lon != 0 &&
1161                     pad_lat != AltosLib.MISSING &&
1162                     pad_lon != AltosLib.MISSING)
1163                 {
1164                         double h = height();
1165
1166                         if (h == AltosLib.MISSING)
1167                                 h = 0;
1168                         from_pad = new AltosGreatCircle(pad_lat, pad_lon, 0, gps.lat, gps.lon, h);
1169                         elevation = from_pad.elevation;
1170                         range = from_pad.range;
1171                 }
1172         }
1173
1174         public void set_tick(int new_tick) {
1175                 if (new_tick != AltosLib.MISSING) {
1176                         if (prev_tick != AltosLib.MISSING) {
1177                                 while (new_tick < prev_tick - 1000) {
1178                                         new_tick += 65536;
1179                                 }
1180                         }
1181                         tick = new_tick;
1182                         time = tick / 100.0;
1183                         time_change = time - prev_time;
1184                 }
1185         }
1186
1187         public void set_boost_tick(int boost_tick) {
1188                 if (boost_tick != AltosLib.MISSING)
1189                         this.boost_tick = boost_tick;
1190         }
1191
1192         public String state_name() {
1193                 return AltosLib.state_name(state);
1194         }
1195
1196         public void set_product(String product) {
1197                 this.product = product;
1198         }
1199
1200         public void set_state(int state) {
1201                 if (state != AltosLib.ao_flight_invalid) {
1202                         this.state = state;
1203                         ascent = (AltosLib.ao_flight_boost <= state &&
1204                                   state <= AltosLib.ao_flight_coast);
1205                         boost = (AltosLib.ao_flight_boost == state);
1206                 }
1207         }
1208
1209         public int state() {
1210                 return state;
1211         }
1212
1213         public void set_device_type(int device_type) {
1214                 this.device_type = device_type;
1215                 switch (device_type) {
1216                 case AltosLib.product_telegps:
1217                         this.state = AltosLib.ao_flight_stateless;
1218                         break;
1219                 }
1220         }
1221
1222         public void set_log_format(int log_format) {
1223                 this.log_format = log_format;
1224                 switch (log_format) {
1225                 case AltosLib.AO_LOG_FORMAT_TELEGPS:
1226                         this.state = AltosLib.ao_flight_stateless;
1227                         break;
1228                 }
1229         }
1230
1231         public void set_log_space(int log_space) {
1232                 this.log_space = log_space;
1233         }
1234
1235         public void set_flight_params(int apogee_delay, int main_deploy) {
1236                 this.apogee_delay = apogee_delay;
1237                 this.main_deploy = main_deploy;
1238         }
1239
1240         public void set_config(int major, int minor, int flight_log_max) {
1241                 config_major = major;
1242                 config_minor = minor;
1243                 this.flight_log_max = flight_log_max;
1244         }
1245
1246         public void set_callsign(String callsign) {
1247                 this.callsign = callsign;
1248         }
1249
1250         public void set_firmware_version(String version) {
1251                 firmware_version = version;
1252         }
1253
1254         public int compare_version(String other_version) {
1255                 if (firmware_version == null)
1256                         return AltosLib.MISSING;
1257                 return AltosLib.compare_version(firmware_version, other_version);
1258         }
1259
1260         private void re_init() {
1261                 int bt = boost_tick;
1262                 int rs = receiver_serial;
1263                 init();
1264                 boost_tick = bt;
1265                 receiver_serial = rs;
1266         }
1267
1268         public void set_flight(int flight) {
1269
1270                 /* When the flight changes, reset the state */
1271                 if (flight != AltosLib.MISSING) {
1272                         if (this.flight != AltosLib.MISSING &&
1273                             this.flight != flight) {
1274                                 re_init();
1275                         }
1276                         this.flight = flight;
1277                 }
1278         }
1279
1280         public void set_serial(int serial) {
1281                 /* When the serial changes, reset the state */
1282                 if (serial != AltosLib.MISSING) {
1283                         if (this.serial != AltosLib.MISSING &&
1284                             this.serial != serial) {
1285                                 re_init();
1286                         }
1287                         this.serial = serial;
1288                 }
1289         }
1290
1291         public void set_receiver_serial(int serial) {
1292                 if (serial != AltosLib.MISSING)
1293                         receiver_serial = serial;
1294         }
1295
1296         public boolean altitude_32() {
1297                 return altitude_32 == 1;
1298         }
1299
1300         public void set_altitude_32(int altitude_32) {
1301                 if (altitude_32 != AltosLib.MISSING)
1302                         this.altitude_32 = altitude_32;
1303         }
1304
1305         public int rssi() {
1306                 if (rssi == AltosLib.MISSING)
1307                         return 0;
1308                 return rssi;
1309         }
1310
1311         public void set_rssi(int rssi, int status) {
1312                 if (rssi != AltosLib.MISSING) {
1313                         this.rssi = rssi;
1314                         this.status = status;
1315                 }
1316         }
1317
1318         public void set_received_time(long ms) {
1319                 received_time = ms;
1320         }
1321
1322         public void set_gps(AltosGPS gps, int sequence) {
1323                 if (gps != null) {
1324                         this.gps = gps.clone();
1325                         gps_sequence = sequence;
1326                         update_gps();
1327                         set |= set_gps;
1328                 }
1329         }
1330
1331
1332         public double   accel_zero_along;
1333         public double   accel_zero_across;
1334         public double   accel_zero_through;
1335
1336         public AltosRotation    rotation;
1337         public AltosRotation    ground_rotation;
1338
1339         public void set_accel_zero(double zero_along, double zero_across, double zero_through) {
1340                 if (zero_along != AltosLib.MISSING) {
1341                         accel_zero_along = zero_along;
1342                         accel_zero_across = zero_across;
1343                         accel_zero_through = zero_through;
1344                 }
1345         }
1346
1347         public int pad_orientation;
1348
1349         public double   accel_ground_along, accel_ground_across, accel_ground_through;
1350
1351         void update_pad_rotation() {
1352                 if (pad_orientation != AltosLib.MISSING && accel_ground_along != AltosLib.MISSING) {
1353                         rotation = new AltosRotation(AltosIMU.convert_accel(accel_ground_across - accel_zero_across),
1354                                                      AltosIMU.convert_accel(accel_ground_through - accel_zero_through),
1355                                                      AltosIMU.convert_accel(accel_ground_along - accel_zero_along),
1356                                                      pad_orientation);
1357                         ground_rotation = rotation;
1358                         orient.set_computed(rotation.tilt(), time);
1359                 }
1360         }
1361
1362         public void set_accel_ground(double ground_along, double ground_across, double ground_through) {
1363                 accel_ground_along = ground_along;
1364                 accel_ground_across = ground_across;
1365                 accel_ground_through = ground_through;
1366                 update_pad_rotation();
1367         }
1368
1369         public void set_pad_orientation(int pad_orientation) {
1370                 this.pad_orientation = pad_orientation;
1371                 update_pad_rotation();
1372         }
1373
1374         public double   gyro_zero_roll;
1375         public double   gyro_zero_pitch;
1376         public double   gyro_zero_yaw;
1377
1378         public void set_gyro_zero(double roll, double pitch, double yaw) {
1379                 if (roll != AltosLib.MISSING) {
1380                         gyro_zero_roll = roll;
1381                         gyro_zero_pitch = pitch;
1382                         gyro_zero_yaw = yaw;
1383                 }
1384         }
1385
1386         public double   last_imu_time;
1387
1388         private double radians(double degrees) {
1389                 if (degrees == AltosLib.MISSING)
1390                         return AltosLib.MISSING;
1391                 return degrees * Math.PI / 180.0;
1392         }
1393
1394         private void update_orient() {
1395                 if (last_imu_time != AltosLib.MISSING) {
1396                         double  t = time - last_imu_time;
1397
1398                         double  pitch = radians(gyro_pitch());
1399                         double  yaw = radians(gyro_yaw());
1400                         double  roll = radians(gyro_roll());
1401
1402                         if (t > 0 & pitch != AltosLib.MISSING && rotation != null) {
1403                                 rotation.rotate(t, pitch, yaw, roll);
1404                                 orient.set_computed(rotation.tilt(), time);
1405                         }
1406                 }
1407                 last_imu_time = time;
1408         }
1409
1410         public void set_imu(AltosIMU imu) {
1411                 if (imu != null)
1412                         imu = imu.clone();
1413                 this.imu = imu;
1414                 update_orient();
1415         }
1416
1417         private double gyro_zero_overflow(double first) {
1418                 double v = first / 128.0;
1419                 if (v < 0)
1420                         v = Math.ceil(v);
1421                 else
1422                         v = Math.floor(v);
1423                 return v * 128.0;
1424         }
1425
1426         public void check_imu_wrap(AltosIMU imu) {
1427                 if (this.imu == null) {
1428                         gyro_zero_roll += gyro_zero_overflow(imu.gyro_roll);
1429                         gyro_zero_pitch += gyro_zero_overflow(imu.gyro_pitch);
1430                         gyro_zero_yaw += gyro_zero_overflow(imu.gyro_yaw);
1431                 }
1432         }
1433
1434         public double accel_along() {
1435                 if (imu != null && accel_zero_along != AltosLib.MISSING)
1436                         return AltosIMU.convert_accel(imu.accel_along - accel_zero_along);
1437                 return AltosLib.MISSING;
1438         }
1439
1440         public double accel_across() {
1441                 if (imu != null && accel_zero_across != AltosLib.MISSING)
1442                         return AltosIMU.convert_accel(imu.accel_across - accel_zero_across);
1443                 return AltosLib.MISSING;
1444         }
1445
1446         public double accel_through() {
1447                 if (imu != null && accel_zero_through != AltosLib.MISSING)
1448                         return AltosIMU.convert_accel(imu.accel_through - accel_zero_through);
1449                 return AltosLib.MISSING;
1450         }
1451
1452         public double gyro_roll() {
1453                 if (imu != null && gyro_zero_roll != AltosLib.MISSING) {
1454                         return AltosIMU.convert_gyro(imu.gyro_roll - gyro_zero_roll);
1455                 }
1456                 return AltosLib.MISSING;
1457         }
1458
1459         public double gyro_pitch() {
1460                 if (imu != null && gyro_zero_pitch != AltosLib.MISSING) {
1461                         return AltosIMU.convert_gyro(imu.gyro_pitch - gyro_zero_pitch);
1462                 }
1463                 return AltosLib.MISSING;
1464         }
1465
1466         public double gyro_yaw() {
1467                 if (imu != null && gyro_zero_yaw != AltosLib.MISSING) {
1468                         return AltosIMU.convert_gyro(imu.gyro_yaw - gyro_zero_yaw);
1469                 }
1470                 return AltosLib.MISSING;
1471         }
1472
1473         public void set_mag(AltosMag mag) {
1474                 this.mag = mag.clone();
1475         }
1476
1477         public double mag_along() {
1478                 if (mag != null)
1479                         return AltosMag.convert_gauss(mag.along);
1480                 return AltosLib.MISSING;
1481         }
1482
1483         public double mag_across() {
1484                 if (mag != null)
1485                         return AltosMag.convert_gauss(mag.across);
1486                 return AltosLib.MISSING;
1487         }
1488
1489         public double mag_through() {
1490                 if (mag != null)
1491                         return AltosMag.convert_gauss(mag.through);
1492                 return AltosLib.MISSING;
1493         }
1494
1495         public AltosMs5607 make_baro() {
1496                 if (baro == null)
1497                         baro = new AltosMs5607();
1498                 return baro;
1499         }
1500
1501         public void set_ms5607(AltosMs5607 ms5607) {
1502                 baro = ms5607;
1503
1504                 if (baro != null) {
1505                         set_pressure(baro.pa);
1506                         set_temperature(baro.cc / 100.0);
1507                 }
1508         }
1509
1510         public void set_ms5607(int pres, int temp) {
1511                 if (baro != null) {
1512                         baro.set(pres, temp);
1513
1514                         set_pressure(baro.pa);
1515                         set_temperature(baro.cc / 100.0);
1516                 }
1517         }
1518
1519         public void set_companion(AltosCompanion companion) {
1520                 this.companion = companion;
1521         }
1522
1523         void update_accel() {
1524                 if (accel == AltosLib.MISSING)
1525                         return;
1526                 if (accel_plus_g == AltosLib.MISSING)
1527                         return;
1528                 if (accel_minus_g == AltosLib.MISSING)
1529                         return;
1530
1531                 double counts_per_g = (accel_minus_g - accel_plus_g) / 2.0;
1532                 double counts_per_mss = counts_per_g / 9.80665;
1533                 acceleration.set_measured((accel_plus_g - accel) / counts_per_mss, time);
1534         }
1535
1536         public void set_accel_g(double accel_plus_g, double accel_minus_g) {
1537                 if (accel_plus_g != AltosLib.MISSING) {
1538                         this.accel_plus_g = accel_plus_g;
1539                         this.accel_minus_g = accel_minus_g;
1540                         update_accel();
1541                 }
1542         }
1543
1544         public void set_ground_accel(double ground_accel) {
1545                 if (ground_accel != AltosLib.MISSING)
1546                         this.ground_accel = ground_accel;
1547         }
1548
1549         public void set_accel(double accel) {
1550                 if (accel != AltosLib.MISSING) {
1551                         this.accel = accel;
1552                         if (state == AltosLib.ao_flight_pad) {
1553                                 if (ground_accel_avg == AltosLib.MISSING)
1554                                         ground_accel_avg = accel;
1555                                 else
1556                                         ground_accel_avg = (ground_accel_avg * 7 + accel) / 8;
1557                         }
1558                 }
1559                 update_accel();
1560         }
1561
1562         public void set_temperature(double temperature) {
1563                 if (temperature != AltosLib.MISSING) {
1564                         this.temperature = temperature;
1565                         set |= set_data;
1566                 }
1567         }
1568
1569         public void set_battery_voltage(double battery_voltage) {
1570                 if (battery_voltage != AltosLib.MISSING) {
1571                         this.battery_voltage = battery_voltage;
1572                         set |= set_data;
1573                 }
1574         }
1575
1576         public void set_pyro_voltage(double pyro_voltage) {
1577                 if (pyro_voltage != AltosLib.MISSING) {
1578                         this.pyro_voltage = pyro_voltage;
1579                         set |= set_data;
1580                 }
1581         }
1582
1583         public void set_apogee_voltage(double apogee_voltage) {
1584                 if (apogee_voltage != AltosLib.MISSING) {
1585                         this.apogee_voltage = apogee_voltage;
1586                         set |= set_data;
1587                 }
1588         }
1589
1590         public void set_main_voltage(double main_voltage) {
1591                 if (main_voltage != AltosLib.MISSING) {
1592                         this.main_voltage = main_voltage;
1593                         set |= set_data;
1594                 }
1595         }
1596
1597         public void set_ignitor_voltage(double[] voltage) {
1598                 this.ignitor_voltage = voltage;
1599         }
1600
1601         public void set_pyro_fired(int fired) {
1602                 this.pyro_fired = fired;
1603         }
1604
1605         public double time_since_boost() {
1606                 if (tick == AltosLib.MISSING)
1607                         return 0.0;
1608
1609                 if (boost_tick == AltosLib.MISSING)
1610                         return tick / 100.0;
1611                 return (tick - boost_tick) / 100.0;
1612         }
1613
1614         public boolean valid() {
1615                 return tick != AltosLib.MISSING && serial != AltosLib.MISSING;
1616         }
1617
1618         public AltosGPS make_temp_gps(boolean sats) {
1619                 if (temp_gps == null) {
1620                         temp_gps = new AltosGPS(gps);
1621                 }
1622                 gps_pending = true;
1623                 if (sats) {
1624                         if (tick != temp_gps_sat_tick)
1625                                 temp_gps.cc_gps_sat = null;
1626                         temp_gps_sat_tick = tick;
1627                 }
1628                 return temp_gps;
1629         }
1630
1631         public void set_temp_gps() {
1632                 set_gps(temp_gps, gps_sequence + 1);
1633                 gps_pending = false;
1634                 temp_gps = null;
1635         }
1636
1637         public AltosState clone() {
1638                 AltosState s = new AltosState();
1639                 s.copy(this);
1640
1641                 if (false) {
1642                         AltosJson       json = json();
1643                         String          onetrip = json.toPrettyString();
1644                         AltosJson       back = AltosJson.fromString(onetrip);
1645                         AltosState      tripstate = AltosState.fromJson(back);
1646                         AltosJson       tripjson = tripstate.json();
1647                         String          twotrip = tripjson.toPrettyString();
1648
1649                         if (!onetrip.equals(twotrip)) {
1650                                 System.out.printf("one:\n%s\ntwo:\n%s\n", onetrip, twotrip);
1651                                 System.exit(1);
1652                         }
1653                 }
1654                 return s;
1655         }
1656
1657         public AltosState () {
1658                 init();
1659         }
1660
1661         public AltosJson json() {
1662                 AltosJson       j = new AltosJson();
1663
1664                 j.put("valid", true);
1665                 j.put("set", set);
1666                 j.put("received_time", received_time);
1667                 j.put("time", time);
1668                 j.put("prev_time", prev_time);
1669                 j.put("time_change", time_change);
1670                 j.put("tick", tick);
1671                 j.put("prev_tick", prev_tick);
1672                 j.put("boost_tick", boost_tick);
1673                 j.put("state", state);
1674                 j.put("flight", flight);
1675                 j.put("serial", serial);
1676                 j.put("altitude_32", altitude_32);
1677                 j.put("receiver_serial", receiver_serial);
1678                 j.put("landed", landed);
1679                 j.put("ascent", ascent);
1680                 j.put("boost", boost);
1681                 j.put("rssi", rssi);
1682                 j.put("status", status);
1683                 j.put("device_type", device_type);
1684                 j.put("config_major", config_major);
1685                 j.put("config_minor", config_minor);
1686                 j.put("apogee_delay", apogee_delay);
1687                 j.put("main_deploy", main_deploy);
1688                 j.put("flight_log_max", flight_log_max);
1689                 j.put("ground_altitude", ground_altitude);
1690                 j.put("gps_ground_altitude", gps_ground_altitude);
1691                 j.put("ground_pressure", ground_pressure);
1692                 j.put("altitude", altitude);
1693                 j.put("gps_altitude", gps_altitude);
1694                 j.put("gps_ground_speed", gps_ground_speed);
1695                 j.put("gps_ascent_rate", gps_ascent_rate);
1696                 j.put("gps_course", gps_course);
1697                 j.put("gps_speed", gps_speed);
1698                 j.put("pressure", pressure);
1699                 j.put("speed", speed);
1700                 j.put("acceleration", acceleration);
1701                 j.put("orient", orient);
1702                 j.put("kalman_height", kalman_height);
1703                 j.put("kalman_speed", kalman_speed);
1704                 j.put("kalman_acceleration", kalman_acceleration);
1705
1706                 j.put("battery_voltage",battery_voltage);
1707                 j.put("pyro_voltage",pyro_voltage);
1708                 j.put("temperature",temperature);
1709                 j.put("apogee_voltage",apogee_voltage);
1710                 j.put("main_voltage",main_voltage);
1711                 j.put("ignitor_voltage",ignitor_voltage);
1712                 j.put("gps", gps);
1713                 j.put("temp_gps", temp_gps);
1714                 j.put("temp_gps_sat_tick", temp_gps_sat_tick);
1715                 j.put("gps_pending", gps_pending);
1716                 j.put("gps_sequence", gps_sequence);
1717                 j.put("imu", imu);
1718                 j.put("mag", mag);
1719
1720                 j.put("npad", npad);
1721                 j.put("gps_waiting", gps_waiting);
1722                 j.put("gps_ready", gps_ready);
1723                 j.put("ngps", ngps);
1724                 j.put("from_pad", from_pad);
1725                 j.put("elevation", elevation);
1726                 j.put("range", range);
1727                 j.put("gps_height", gps_height);
1728                 j.put("pad_lat", pad_lat);
1729                 j.put("pad_lon", pad_lon);
1730                 j.put("pad_alt", pad_alt);
1731                 j.put("speak_tick", speak_tick);
1732                 j.put("speak_altitude", speak_altitude);
1733                 j.put("callsign", callsign);
1734                 j.put("firmware_version", firmware_version);
1735                 j.put("accel_plus_g", accel_plus_g);
1736                 j.put("accel_minus_g", accel_minus_g);
1737                 j.put("accel", accel);
1738                 j.put("ground_accel", ground_accel);
1739                 j.put("ground_accel_avg", ground_accel_avg);
1740                 j.put("log_format", log_format);
1741                 j.put("log_space", log_space);
1742                 j.put("product", product);
1743                 j.put("baro", baro);
1744                 j.put("companion", companion);
1745                 j.put("pyro_fired", pyro_fired);
1746                 j.put("accel_zero_along", accel_zero_along);
1747                 j.put("accel_zero_across", accel_zero_across);
1748                 j.put("accel_zero_through", accel_zero_through);
1749
1750                 j.put("rotation", rotation);
1751                 j.put("ground_rotation", ground_rotation);
1752
1753                 j.put("pad_orientation", pad_orientation);
1754
1755                 j.put("accel_ground_along", accel_ground_along);
1756                 j.put("accel_ground_across", accel_ground_across);
1757                 j.put("accel_ground_through", accel_ground_through);
1758
1759                 j.put("gyro_zero_roll", gyro_zero_roll);
1760                 j.put("gyro_zero_pitch", gyro_zero_pitch);
1761                 j.put("gyro_zero_yaw", gyro_zero_yaw);
1762
1763                 j.put("last_imu_time", last_imu_time);
1764                 return j;
1765         }
1766
1767         public AltosState(AltosJson j) {
1768                 this();
1769
1770                 set = j.get_int("set", set);
1771                 received_time = j.get_long("received_time", received_time);
1772                 time = j.get_double("time", time);
1773                 prev_time = j.get_double("prev_time", prev_time);
1774                 time_change = j.get_double("time_change", time_change);
1775                 tick = j.get_int("tick", tick);
1776                 prev_tick = j.get_int("prev_tick", prev_tick);
1777                 boost_tick = j.get_int("boost_tick", boost_tick);
1778                 state = j.get_int("state", state);
1779                 flight = j.get_int("flight", flight);
1780                 serial = j.get_int("serial", serial);
1781                 altitude_32 = j.get_int("altitude_32", altitude_32);
1782                 receiver_serial = j.get_int("receiver_serial", receiver_serial);
1783                 landed = j.get_boolean("landed", landed);
1784                 ascent = j.get_boolean("ascent", ascent);
1785                 boost = j.get_boolean("boost", boost);
1786                 rssi = j.get_int("rssi", rssi);
1787                 status = j.get_int("status", status);
1788                 device_type = j.get_int("device_type", device_type);
1789                 config_major = j.get_int("config_major", config_major);
1790                 config_minor = j.get_int("config_minor", config_minor);
1791                 apogee_delay = j.get_int("apogee_delay", apogee_delay);
1792                 main_deploy = j.get_int("main_deploy", main_deploy);
1793                 flight_log_max = j.get_int("flight_log_max", flight_log_max);
1794                 ground_altitude = AltosCValue_fromJson(j.get("ground_altitude"), ground_altitude);
1795                 gps_ground_altitude = AltosGpsGroundAltitude_fromJson(j.get("gps_ground_altitude"), gps_ground_altitude);
1796                 ground_pressure = AltosGroundPressure_fromJson(j.get("ground_pressure"), ground_pressure);
1797                 altitude = AltosAltitude_fromJson(j.get("altitude"), altitude);
1798                 gps_altitude = AltosGpsAltitude_fromJson(j.get("gps_altitude"), gps_altitude);
1799                 gps_ground_speed = AltosValue_fromJson(j.get("gps_ground_speed"), gps_ground_speed);
1800                 gps_ascent_rate = AltosValue_fromJson(j.get("gps_ascent_rate"), gps_ascent_rate);
1801                 gps_course = AltosValue_fromJson(j.get("gps_course"), gps_course);
1802                 gps_speed = AltosValue_fromJson(j.get("gps_speed"), gps_speed);
1803                 pressure = AltosPressure_fromJson(j.get("pressure"), pressure);
1804                 speed = AltosSpeed_fromJson(j.get("speed"), speed);
1805                 acceleration = AltosAccel_fromJson(j.get("acceleration"), acceleration);
1806                 orient = AltosCValue_fromJson(j.get("orient"), orient);
1807                 kalman_height = AltosValue_fromJson(j.get("kalman_height"), kalman_height);
1808                 kalman_speed = AltosValue_fromJson(j.get("kalman_speed"), kalman_speed);
1809                 kalman_acceleration = AltosValue_fromJson(j.get("kalman_acceleration"), kalman_acceleration);
1810
1811                 battery_voltage = j.get_double("battery_voltage", battery_voltage);
1812                 pyro_voltage = j.get_double("pyro_voltage", pyro_voltage);
1813                 temperature = j.get_double("temperature", temperature);
1814                 apogee_voltage = j.get_double("apogee_voltage", apogee_voltage);
1815                 main_voltage=  j.get_double("main_voltage", main_voltage);
1816                 ignitor_voltage = j.get_double_array("ignitor_voltage", ignitor_voltage);
1817                 gps = AltosGPS.fromJson(j.get("gps"), gps);
1818                 temp_gps = AltosGPS.fromJson(j.get("temp_gps"), temp_gps);
1819                 temp_gps_sat_tick = j.get_int("temp_gps_sat_tick", temp_gps_sat_tick);
1820                 gps_pending = j.get_boolean("gps_pending", gps_pending);
1821                 gps_sequence = j.get_int("gps_sequence", gps_sequence);
1822                 imu = AltosIMU.fromJson(j.get("imu"), imu);
1823                 mag = AltosMag.fromJson(j.get("mag"), mag);
1824
1825                 npad = j.get_int("npad", npad);
1826                 gps_waiting = j.get_int("gps_waiting", gps_waiting);
1827                 gps_ready = j.get_boolean("gps_ready", gps_ready);
1828                 ngps = j.get_int("ngps", ngps);
1829                 from_pad = AltosGreatCircle.fromJson(j.get("from_pad"), from_pad);
1830                 elevation = j.get_double("elevation", elevation);
1831                 range = j.get_double("range", range);
1832                 gps_height = j.get_double("gps_height", gps_height);
1833                 pad_lat = j.get_double("pad_lat", pad_lat);
1834                 pad_lon = j.get_double("pad_lon", pad_lon);
1835                 pad_alt = j.get_double("pad_alt", pad_alt);
1836                 speak_tick = j.get_int("speak_tick", speak_tick);
1837                 speak_altitude = j.get_double("speak_altitude", speak_altitude);
1838                 callsign = j.get_string("callsign", callsign);
1839                 firmware_version = j.get_string("firmware_version", firmware_version);
1840                 accel_plus_g = j.get_double("accel_plus_g", accel_plus_g);
1841                 accel_minus_g = j.get_double("accel_minus_g", accel_minus_g);
1842                 accel = j.get_double("accel", accel);
1843                 ground_accel = j.get_double("ground_accel", ground_accel);
1844                 ground_accel_avg = j.get_double("ground_accel_avg", ground_accel_avg);
1845                 log_format = j.get_int("log_format", log_format);
1846                 log_space = j.get_int("log_space", log_space);
1847                 product = j.get_string("product", product);
1848                 baro = AltosMs5607.fromJson(j.get("baro"), baro);
1849                 companion = AltosCompanion.fromJson(j.get("companion"), companion);
1850                 pyro_fired = j.get_int("pyro_fired", pyro_fired);
1851                 accel_zero_along = j.get_double("accel_zero_along", accel_zero_along);
1852                 accel_zero_across = j.get_double("accel_zero_across", accel_zero_across);
1853                 accel_zero_through = j.get_double("accel_zero_through", accel_zero_through);
1854
1855                 rotation = AltosRotation.fromJson(j.get("rotation"), rotation);
1856                 ground_rotation = AltosRotation.fromJson(j.get("ground_rotation"), ground_rotation);
1857
1858                 pad_orientation = j.get_int("pad_orientation", pad_orientation);
1859
1860                 accel_ground_along = j.get_double("accel_ground_along", accel_ground_along);
1861                 accel_ground_across = j.get_double("accel_ground_across", accel_ground_across);
1862                 accel_ground_through = j.get_double("accel_ground_through", accel_ground_through);
1863
1864                 gyro_zero_roll = j.get_double("gyro_zero_roll", gyro_zero_roll);
1865                 gyro_zero_pitch = j.get_double("gyro_zero_pitch", gyro_zero_pitch);
1866                 gyro_zero_yaw = j.get_double("gyro_zero_yaw", gyro_zero_yaw);
1867
1868                 last_imu_time = j.get_double("last_imu_time", last_imu_time);
1869         }
1870
1871         public static AltosState fromJson(AltosJson j) {
1872                 if (j == null)
1873                         return null;
1874                 if (!j.get_boolean("valid", false))
1875                         return null;
1876                 return new AltosState(j);
1877         }
1878 }