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_1;
  23
  24 public class AltosState implements Cloneable {
  25         public AltosRecord data;
  26
  27         /* derived data */
  28
  29         public long     report_time;
  30
  31         public double   time;
  32         public double   time_change;
  33         public int      tick;
  34
  35         public int      state;
  36         public boolean  landed;
  37         public boolean  ascent; /* going up? */
  38         public boolean  boost;  /* under power */
  39
  40         public double   ground_altitude;
  41         public double   altitude;
  42         public double   height;
  43         public double   pressure;
  44         public double   acceleration;
  45         public double   battery_voltage;
  46         public double   pyro_voltage;
  47         public double   temperature;
  48         public double   apogee_voltage;
  49         public double   main_voltage;
  50         public double   speed;
  51
  52         public double   prev_height;
  53         public double   prev_speed;
  54         public double   prev_acceleration;
  55
  56         public double   max_height;
  57         public double   max_acceleration;
  58         public double   max_speed;
  59
  60         public double   kalman_height, kalman_speed, kalman_acceleration;
  61
  62         public AltosGPS gps;
  63         public int gps_sequence;
  64
  65         public AltosIMU imu;
  66         public AltosMag mag;
  67
  68         public static final int MIN_PAD_SAMPLES = 10;
  69
  70         public int      npad;
  71         public int      gps_waiting;
  72         public boolean  gps_ready;
  73
  74         public int      ngps;
  75
  76         public AltosGreatCircle from_pad;
  77         public double   elevation;      /* from pad */
  78         public double   range;          /* total distance */
  79
  80         public double   gps_height;
  81
  82         public double pad_lat, pad_lon, pad_alt;
  83
  84         public int      speak_tick;
  85         public double   speak_altitude;
  86
  87         public String   callsign;
  88         public double   accel_plus_g;
  89         public double   accel_minus_g;
  90         public double   accel;
  91         public double   ground_accel;
  92
  93         public int      log_format;
  94         public int      serial;
  95
  96         public AltosMs5607      baro;
  97
  98         public double speed() {
  99                 return speed;
 100         }
 101
 102         public double max_speed() {
 103                 return max_speed;
 104         }
 105
 106         public void set_npad(int npad) {
 107                 this.npad = npad;
 108                 gps_waiting = MIN_PAD_SAMPLES - npad;
 109                 if (this.gps_waiting < 0)
 110                         gps_waiting = 0;
 111                 gps_ready = gps_waiting == 0;
 112         }
 113
 114         public void init() {
 115                 data = new AltosRecord();
 116
 117                 report_time = System.currentTimeMillis();
 118                 time = AltosRecord.MISSING;
 119                 time_change = AltosRecord.MISSING;
 120                 tick = AltosRecord.MISSING;
 121                 state = AltosLib.ao_flight_invalid;
 122                 landed = false;
 123                 boost = false;
 124
 125                 ground_altitude = AltosRecord.MISSING;
 126                 altitude = AltosRecord.MISSING;
 127                 height = AltosRecord.MISSING;
 128                 pressure = AltosRecord.MISSING;
 129                 acceleration = AltosRecord.MISSING;
 130                 temperature = AltosRecord.MISSING;
 131
 132                 prev_height = AltosRecord.MISSING;
 133                 prev_speed = AltosRecord.MISSING;
 134                 prev_acceleration = AltosRecord.MISSING;
 135
 136                 battery_voltage = AltosRecord.MISSING;
 137                 pyro_voltage = AltosRecord.MISSING;
 138                 apogee_voltage = AltosRecord.MISSING;
 139                 main_voltage = AltosRecord.MISSING;
 140
 141
 142                 accel_speed = AltosRecord.MISSING;
 143                 baro_speed = AltosRecord.MISSING;
 144
 145                 kalman_height = AltosRecord.MISSING;
 146                 kalman_speed = AltosRecord.MISSING;
 147                 kalman_acceleration = AltosRecord.MISSING;
 148
 149                 max_baro_speed = 0;
 150                 max_accel_speed = 0;
 151                 max_height = 0;
 152                 max_acceleration = 0;
 153
 154                 gps = null;
 155                 gps_sequence = 0;
 156
 157                 imu = null;
 158                 mag = null;
 159
 160                 set_npad(0);
 161                 ngps = 0;
 162
 163                 from_pad = null;
 164                 elevation = AltosRecord.MISSING;
 165                 range = AltosRecord.MISSING;
 166                 gps_height = AltosRecord.MISSING;
 167
 168                 pat_lat = AltosRecord.MISSING;
 169                 pad_lon = AltosRecord.MISSING;
 170                 pad_alt = AltosRecord.MISSING;
 171
 172                 speak_tick = AltosRecord.MISSING;
 173                 speak_altitude = AltosRecord.MISSING;
 174
 175                 callsign = null;
 176
 177                 accel_plus_g = AltosRecord.MISSING;
 178                 accel_minus_g = AltosRecord.MISSING;
 179                 log_format = AltosRecord.MISSING;
 180                 serial = AltosRecord.MISSING;
 181
 182                 baro = null;
 183         }
 184
 185         void copy(AltosState old) {
 186
 187                 data = null;
 188
 189                 if (old == null) {
 190                         init();
 191                         return;
 192                 }
 193
 194                 report_time = old.report_time;
 195                 time = old.time;
 196                 time_change = old.time_change;
 197                 tick = old.tick;
 198
 199                 state = old.state;
 200                 landed = old.landed;
 201                 ascent = old.ascent;
 202                 boost = old.boost;
 203
 204                 ground_altitude = old.ground_altitude;
 205                 altitude = old.altitude;
 206                 height = old.height;
 207                 pressure = old.pressure;
 208                 acceleration = old.acceleration;
 209                 battery_voltage = old.battery_voltage;
 210                 pyro_voltage = old.pyro_voltage;
 211                 temperature = old.temperature;
 212                 apogee_voltage = old.apogee_voltage;
 213                 main_voltage = old.main_voltage;
 214                 accel_speed = old.accel_speed;
 215                 baro_speed = old.baro_speed;
 216
 217                 prev_height = old.height;
 218                 prev_speed = old.speed;
 219                 prev_acceleration = old.acceleration;
 220
 221                 max_height = old.max_height;
 222                 max_acceleration = old.max_acceleration;
 223                 max_accel_speed = old.max_accel_speed;
 224                 max_baro_speed = old.max_baro_speed;
 225
 226                 kalman_height = old.kalman_height;
 227                 kalman_speed = old.kalman_speed;
 228                 kalman_acceleration = old.kalman_acceleration;
 229
 230                 if (old.gps != null)
 231                         gps = old.gps.clone();
 232                 else
 233                         gps = null;
 234                 gps_sequence = old.gps_sequence;
 235
 236                 if (old.imu != null)
 237                         imu = old.imu.clone();
 238                 else
 239                         imu = null;
 240
 241                 if (old.mag != null)
 242                         mag = old.mag.clone();
 243                 else
 244                         mag = null;
 245
 246                 npad = old.npad;
 247                 gps_waiting = old.gps_waiting;
 248                 gps_ready = old.gps_ready;
 249                 ngps = old.ngps;
 250
 251                 if (old.from_pad != null)
 252                         from_pad = old.from_pad.clone();
 253                 else
 254                         from_pad = null;
 255
 256                 elevation = old.elevation;
 257                 range = old.range;
 258
 259                 gps_height = old.gps_height;
 260                 pad_lat = old.pad_lat;
 261                 pad_lon = old.pad_lon;
 262                 pad_alt = old.pad_alt;
 263
 264                 speak_tick = old.speak_tick;
 265                 speak_altitude = old.speak_altitude;
 266
 267                 callsign = old.callsign;
 268
 269                 accel_plus_g = old.accel_plus_g;
 270                 accel_minus_g = old.accel_minus_g;
 271                 log_format = old.log_format;
 272                 serial = old.serial;
 273
 274                 baro = old.baro;
 275         }
 276
 277         double ground_altitude() {
 278
 279         }
 280
 281         double altitude() {
 282                 if (altitude != AltosRecord.MISSING)
 283                         return altitude;
 284                 if (gps != null)
 285                         return gps.alt;
 286                 return AltosRecord.MISSING;
 287         }
 288
 289         void update_vertical_pos() {
 290
 291                 double  alt = altitude();
 292                 if (state == AltosLib.ao_flight_pad) {
 293
 294                 }
 295
 296                 if (kalman_height != AltosRecord.MISSING)
 297                         height = kalman_height;
 298                 else if (altitude != AltosRecord.MISSING && ground_altitude != AltosRecord.MISSING)
 299                         height = altitude - ground_altitude;
 300                 else
 301                         height = AltosRecord.MISSING;
 302
 303                 update_speed();
 304         }
 305
 306         double motion_filter_value() {
 307                 return 1/ Math.exp(time_change/10.0);
 308         }
 309
 310         void update_speed() {
 311                 if (kalman_speed != AltosRecord.MISSING)
 312                         speed = kalman_speed;
 313                 else if (state != AltosLib.ao_flight_invalid &&
 314                          time_change != AltosRecord.MISSING)
 315                 {
 316                         if (ascent && acceleration != AltosRecord.MISSING)
 317                         {
 318                                 if (prev_speed == AltosRecord.MISSING)
 319                                         speed = acceleration * time_change;
 320                                 else
 321                                         speed = prev_speed + acceleration * time_change;
 322                         }
 323                         else if (height != AltosRecord.MISSING &&
 324                                  prev_height != AltosRecord.MISSING &&
 325                                  time_change != 0)
 326                         {
 327                                 double  new_speed = (height - prev_height) / time_change;
 328
 329                                 if (prev_speed == AltosRecord.MISSING)
 330                                         speed = new_speed;
 331                                 else {
 332                                         double  filter = motion_filter_value();
 333
 334                                         speed = prev_speed * filter + new_speed * (1-filter);
 335                                 }
 336                         }
 337                 }
 338                 if (acceleration == AltosRecord.MISSING) {
 339                         if (prev_speed != AltosRecord.MISSING && time_change != 0) {
 340                                 double  new_acceleration = (speed - prev_speed) / time_change;
 341
 342                                 if (prev_acceleration == AltosRecord.MISSING)
 343                                         acceleration = new_acceleration;
 344                                 else {
 345                                         double filter = motion_filter_value();
 346
 347                                         acceleration = prev_acceleration * filter + new_acceleration * (1-filter);
 348                                 }
 349                         }
 350                 }
 351         }
 352
 353         void update_accel() {
 354                 if (accel == AltosRecord.MISSING)
 355                         return;
 356                 if (ground_Accel == AltosRecord.MISSING)
 357                         return;
 358                 if (accel_plus_g == AltosRecord.MISSING)
 359                         return;
 360                 if (accel_minus_g == AltosRecord.MISSING)
 361                         return;
 362
 363                 double counts_per_g = (accel_minus_g - accel_plus_g) / 2.0;
 364                 double counts_per_mss = counts_per_g / 9.80665;
 365
 366                 acceleration = (ground_accel - accel) / counts_per_mss;
 367                 update_speed();
 368         }
 369
 370         public void set_tick(int tick) {
 371                 if (tick != AltosRecord.MISSING) {
 372                         if (this.tick != AltosRecord.MISSING) {
 373                                 while (tick < this.tick)
 374                                         tick += 65536;
 375                                 time_change = (tick - this.tick) / 100.0;
 376                         } else
 377                                 time_change = 0;
 378                         this.tick = tick;
 379                         update_time();
 380                 }
 381         }
 382
 383         public void set_state(int state) {
 384                 if (state != AltosLib.ao_flight_invalid) {
 385                         this.state = state;
 386                         ascent = (AltosLib.ao_flight_boost <= state &&
 387                                   state <= AltosLib.ao_flight_coast);
 388                         boost = (AltosLib.ao_flight_boost == state);
 389                 }
 390
 391         }
 392
 393         public void set_altitude(double altitude) {
 394                 if (altitude != AltosRecord.MISSING) {
 395                         this.altitude = altitude;
 396                         update_vertical_pos();
 397                 }
 398         }
 399
 400         public void set_ground_altitude(double ground_altitude) {
 401                 if (ground_altitude != AltosRecord.MISSING) {
 402                         this.ground_altitude = ground_altitude;
 403                         update_vertical_pos();
 404                 }
 405         }
 406
 407         public void set_gps(AltosGPS gps, int sequence) {
 408                 if (gps != null) {
 409                         this.gps = gps.clone();
 410                         gps_sequence = sequence;
 411                         update_vertical_pos();
 412                 }
 413         }
 414
 415         public void set_kalman(double height, double speed, double acceleration) {
 416                 if (height != AltosRecord.MISSING) {
 417                         kalman_height = height;
 418                         kalman_speed = speed;
 419                         kalman_acceleration = acceleration;
 420                         baro_speed = accel_speed = speed;
 421                         update_vertical_pos();
 422                 }
 423         }
 424
 425         public void set_pressure(double pressure) {
 426                 if (pressure != AltosRecord.MISSING) {
 427                         this.pressure = pressure;
 428                         set_altitude(AltosConvert.pressure_to_altitude(pressure));
 429                 }
 430         }
 431
 432         public void set_accel_g(double accel_plus_g, double accel_minus_g) {
 433                 if (accel_plus_g != AltosRecord.MISSING) {
 434                         this.accel_plus_g = accel_plus_g;
 435                         this.accel_minus_g = accel_minus_g;
 436                         update_accel();
 437                 }
 438         }
 439         public void set_ground_accel(double ground_accel) {
 440                 if (ground_accel != AltosRecord.MISSING) {
 441                         this.ground_accel = ground_accel;
 442                         update_accel();
 443                 }
 444         }
 445
 446         public void set_accel(double accel) {
 447                 if (accel != AltosRecord.MISSING) {
 448                         this.accel = accel;
 449                 }
 450                 update_accel();
 451         }
 452
 453         public void set_temperature(double temperature) {
 454                 if (temperature != AltosRecord.MISSING)
 455                         this.temperature = temperature;
 456         }
 457
 458         public void set_battery_voltage(double battery_voltage) {
 459                 if (battery_voltage != AltosRecord.MISSING)
 460                         this.battery_voltage = battery_voltage;
 461         }
 462
 463         public void set_pyro_voltage(double pyro_voltage) {
 464                 if (pyro_voltage != AltosRecord.MISSING)
 465                         this.pyro_voltage = pyro_voltage;
 466         }
 467
 468         public void set_apogee_voltage(double apogee_voltage) {
 469                 if (apogee_voltage != AltosRecord.MISSING)
 470                         this.apogee_voltage = apogee_voltage;
 471         }
 472
 473         public void set_main_voltage(double main_voltage) {
 474                 if (main_voltage != AltosRecord.MISSING)
 475                         this.main_voltage = main_voltage;
 476         }
 477
 478         public void init (AltosRecord cur, AltosState prev_state) {
 479
 480                 if (cur == null)
 481                         cur = new AltosRecord();
 482
 483                 data = cur;
 484
 485                 /* Discard previous state if it was for a different board */
 486                 if (prev_state != null && prev_state.serial != cur.serial)
 487                         prev_state = null;
 488
 489                 copy(prev_state);
 490
 491                 set_ground_altitude(data.ground_altitude());
 492                 set_altitude(data.altitude());
 493
 494                 set_kalman(data.kalman_height, data.kalman_speed, data.kalman_acceleration);
 495
 496                 report_time = System.currentTimeMillis();
 497
 498                 set_temperature(data.temperature());
 499                 set_apogee_voltage(data.drogue_voltage());
 500                 set_main_voltage(data.main_voltage());
 501                 set_battery_voltage(data.battery_voltage());
 502
 503                 set_pressure(data.pressure());
 504
 505                 set_tick(data.tick);
 506                 set_state(data.state);
 507
 508                 set_accel_g (data.accel_minus_g, data.accel_plus_g);
 509                 set_ground_accel(data.ground_accel);
 510                 set_accel (data.accel);
 511
 512                 set_gps(data.gps, data.gps_sequence);
 513
 514                 if (prev_state != null) {
 515
 516                         if (data.kalman_speed != AltosRecord.MISSING) {
 517                                 baro_speed = accel_speed = data.kalman_speed;
 518                         } else {
 519                                 /* compute barometric speed */
 520
 521                                 double height_change = height - prev_state.height;
 522
 523                                 double prev_baro_speed = prev_state.baro_speed;
 524                                 if (prev_baro_speed == AltosRecord.MISSING)
 525                                         prev_baro_speed = 0;
 526
 527                                 if (time_change > 0)
 528                                         baro_speed = (prev_baro_speed * 3 + (height_change / time_change)) / 4.0;
 529                                 else
 530                                         baro_speed = prev_state.baro_speed;
 531
 532                                 double prev_accel_speed = prev_state.accel_speed;
 533
 534                                 if (prev_accel_speed == AltosRecord.MISSING)
 535                                         prev_accel_speed = 0;
 536
 537                                 if (acceleration == AltosRecord.MISSING) {
 538                                         /* Fill in mising acceleration value */
 539                                         accel_speed = baro_speed;
 540
 541                                         if (time_change > 0 && accel_speed != AltosRecord.MISSING)
 542                                                 acceleration = (accel_speed - prev_accel_speed) / time_change;
 543                                         else
 544                                                 acceleration = prev_state.acceleration;
 545                                 } else {
 546                                         /* compute accelerometer speed */
 547                                         accel_speed = prev_accel_speed + acceleration * time_change;
 548                                 }
 549                         }
 550                 } else {
 551                         npad = 0;
 552                         ngps = 0;
 553                         gps = null;
 554                         gps_sequence = 0;
 555                         baro_speed = AltosRecord.MISSING;
 556                         accel_speed = AltosRecord.MISSING;
 557                         pad_alt = AltosRecord.MISSING;
 558                         max_baro_speed = 0;
 559                         max_accel_speed = 0;
 560                         max_height = 0;
 561                         max_acceleration = 0;
 562                         time_change = 0;
 563                         baro = new AltosMs5607();
 564                         callsign = "";
 565                         accel_plus_g = AltosRecord.MISSING;
 566                         accel_minus_g = AltosRecord.MISSING;
 567                         log_format = AltosRecord.MISSING;
 568                         serial = AltosRecord.MISSING;
 569                 }
 570
 571                 time = tick / 100.0;
 572
 573                 if (data.gps != null && data.gps_sequence != gps_sequence && (state < AltosLib.ao_flight_boost)) {
 574
 575                         /* Track consecutive 'good' gps reports, waiting for 10 of them */
 576                         if (data.gps != null && data.gps.locked && data.gps.nsat >= 4)
 577                                 set_npad(npad+1);
 578                         else
 579                                 set_npad(0);
 580
 581                         /* Average GPS data while on the pad */
 582                         if (data.gps != null && data.gps.locked && data.gps.nsat >= 4) {
 583                                 if (ngps > 1 && state == AltosLib.ao_flight_pad) {
 584                                         /* filter pad position */
 585                                         pad_lat = (pad_lat * 31.0 + data.gps.lat) / 32.0;
 586                                         pad_lon = (pad_lon * 31.0 + data.gps.lon) / 32.0;
 587                                         pad_alt = (pad_alt * 31.0 + data.gps.alt) / 32.0;
 588                                 } else {
 589                                         pad_lat = data.gps.lat;
 590                                         pad_lon = data.gps.lon;
 591                                         pad_alt = data.gps.alt;
 592                                 }
 593                                 ngps++;
 594                         }
 595                 } else {
 596                         if (ngps == 0 && ground_altitude != AltosRecord.MISSING)
 597                                 pad_alt = ground_altitude;
 598                 }
 599
 600                 gps_sequence = data.gps_sequence;
 601
 602                 /* Only look at accelerometer data under boost */
 603                 if (boost && acceleration != AltosRecord.MISSING && (max_acceleration == AltosRecord.MISSING || acceleration > max_acceleration))
 604                         max_acceleration = acceleration;
 605                 if (boost && accel_speed != AltosRecord.MISSING && accel_speed > max_accel_speed)
 606                         max_accel_speed = accel_speed;
 607                 if (boost && baro_speed != AltosRecord.MISSING && baro_speed > max_baro_speed)
 608                         max_baro_speed = baro_speed;
 609
 610                 if (height != AltosRecord.MISSING && height > max_height)
 611                         max_height = height;
 612                 elevation = 0;
 613                 range = -1;
 614                 gps_height = 0;
 615                 if (data.gps != null) {
 616                         gps = data.gps;
 617                         if (ngps > 0 && gps.locked) {
 618                                 double h = height;
 619
 620                                 if (h == AltosRecord.MISSING) h = 0;
 621                                 from_pad = new AltosGreatCircle(pad_lat, pad_lon, 0, gps.lat, gps.lon, h);
 622                                 elevation = from_pad.elevation;
 623                                 range = from_pad.range;
 624                                 gps_height = gps.alt - pad_alt;
 625                         }
 626                 }
 627         }
 628
 629         public AltosState clone() {
 630                 AltosState s = new AltosState(data, this);
 631                 return s;
 632         }
 633
 634         public AltosState(AltosRecord cur) {
 635                 init(cur, null);
 636         }
 637
 638         public AltosState (AltosRecord cur, AltosState prev) {
 639                 init(cur, prev);
 640         }
 641 }