git.gag.com Git - fw/altos/blob - altoslib/AltosFlightSeries.java

   1 /*
   2  * Copyright © 2017 Keith Packard <keithp@keithp.com>
   3  *
   4  * This program is free software; you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 2 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * This program is distributed in the hope that it will be useful, but
  10  * WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  12  * General Public License for more details.
  13  */
  14
  15 package org.altusmetrum.altoslib_11;
  16
  17 import java.util.*;
  18
  19 public class AltosFlightSeries extends AltosDataListener {
  20
  21         public ArrayList<AltosTimeSeries> series;
  22
  23         public int[] indices() {
  24                 int[] indices = new int[series.size()];
  25                 for (int i = 0; i < indices.length; i++)
  26                         indices[i] = 0;
  27                 return indices;
  28         }
  29
  30         private double time(int id, int index) {
  31                 AltosTimeSeries         s = series.get(id);
  32                 if (index < s.values.size())
  33                         return s.values.get(index).time;
  34                 return Double.POSITIVE_INFINITY;
  35         }
  36
  37         public boolean step_indices(int[] indices) {
  38                 double  min_next = time(0, indices[0]+1);
  39
  40                 for (int i = 1; i < indices.length; i++) {
  41                         double next = time(i, indices[i]+1);
  42                         if (next < min_next)
  43                                 min_next = next;
  44                 }
  45
  46                 if (min_next == Double.POSITIVE_INFINITY)
  47                         return false;
  48
  49                 for (int i = 0; i < indices.length; i++) {
  50                         double  t = time(i, indices[i] + 1);
  51
  52                         if (t <= min_next)
  53                                 indices[i]++;
  54                 }
  55                 return true;
  56         }
  57
  58         public double time(int[] indices) {
  59                 double max = time(0, indices[0]);
  60
  61                 for (int i = 1; i < indices.length; i++) {
  62                         double t = time(i, indices[i]);
  63                         if (t >= max)
  64                                 max = t;
  65                 }
  66                 return max;
  67         }
  68
  69         public double value(String name, int[] indices) {
  70                 for (int i = 0; i < indices.length; i++) {
  71                         AltosTimeSeries s = series.get(i);
  72                         if (s.label.equals(name))
  73                                 return s.values.get(indices[i]).value;
  74                 }
  75                 return AltosLib.MISSING;
  76         }
  77
  78         public AltosTimeSeries make_series(String label, AltosUnits units) {
  79                 return new AltosTimeSeries(label, units);
  80         }
  81
  82         public void add_series(AltosTimeSeries s) {
  83                 series.add(s);
  84         }
  85
  86         public AltosTimeSeries add_series(String label, AltosUnits units) {
  87                 AltosTimeSeries s = make_series(label, units);
  88                 add_series(s);
  89                 return s;
  90         }
  91
  92         public void remove_series(AltosTimeSeries s) {
  93                 series.remove(s);
  94         }
  95
  96         public boolean has_series(String label) {
  97                 for (AltosTimeSeries s : series)
  98                         if (s.label.equals(label))
  99                                 return true;
 100                 return false;
 101         }
 102
 103         AltosTimeSeries state_series;
 104
 105         public static final String state_name = "State";
 106
 107         public void set_state(int state) {
 108                 this.state = state;
 109                 if (state_series == null)
 110                         state_series = add_series(state_name, AltosConvert.state_name);
 111                 else if ((int) state_series.get(state_series.size()-1).value == state)
 112                         return;
 113                 state_series.add(time(), state);
 114         }
 115
 116         AltosTimeSeries accel_series;
 117
 118         public static final String accel_name = "Accel";
 119
 120         public void set_acceleration(double acceleration) {
 121                 if (accel_series == null) {
 122                         System.out.printf("set acceleration %g\n", acceleration);
 123                         accel_series = add_series(accel_name, AltosConvert.accel);
 124                 }
 125                 accel_series.add(time(), acceleration);
 126         }
 127
 128         private void compute_accel() {
 129                 if (accel_series != null)
 130                         return;
 131
 132                 if (speed_series != null) {
 133                         AltosTimeSeries temp_series = make_series(accel_name, AltosConvert.accel);
 134                         speed_series.differentiate(temp_series);
 135                         accel_series = add_series(accel_name, AltosConvert.accel);
 136                         temp_series.filter(accel_series, 0.25);
 137                 }
 138         }
 139
 140         public void set_received_time(long received_time) {
 141         }
 142
 143         AltosTimeSeries rssi_series;
 144
 145         public static final String rssi_name = "RSSI";
 146
 147         AltosTimeSeries status_series;
 148
 149         public static final String status_name = "Status";
 150
 151         public void set_rssi(int rssi, int status) {
 152                 if (rssi_series == null)
 153                         rssi_series = add_series(rssi_name, null);
 154                 rssi_series.add(time(), rssi);
 155                 if (status_series == null)
 156                         status_series = add_series(status_name, null);
 157                 status_series.add(time(), status);
 158         }
 159
 160         AltosTimeSeries pressure_series;
 161
 162         public static final String pressure_name = "Pressure";
 163
 164         AltosTimeSeries altitude_series;
 165
 166         public static final String altitude_name = "Altitude";
 167
 168         AltosTimeSeries height_series;
 169
 170         public static final String height_name = "Height";
 171
 172         public  void set_pressure(double pa) {
 173                 if (pressure_series == null)
 174                         pressure_series = add_series(pressure_name, AltosConvert.pressure);
 175                 pressure_series.add(time(), pa);
 176                 if (altitude_series == null)
 177                         altitude_series = add_series(altitude_name, AltosConvert.height);
 178
 179                 double altitude = AltosConvert.pressure_to_altitude(pa);
 180                 altitude_series.add(time(), altitude);
 181         }
 182
 183         private void compute_height(double ground_altitude) {
 184                 if (height_series == null) {
 185                         height_series = add_series(height_name, AltosConvert.height);
 186                         for (AltosTimeValue alt : altitude_series)
 187                                 height_series.add(alt.time, alt.value - ground_altitude);
 188                 }
 189         }
 190
 191         AltosTimeSeries speed_series;
 192
 193         public static final String speed_name = "Speed";
 194
 195         private void compute_speed() {
 196                 if (speed_series != null) {
 197                         System.out.printf("speed series already made\n");
 198                         return;
 199                 }
 200
 201                 AltosTimeSeries alt_speed_series = null;
 202                 AltosTimeSeries accel_speed_series = null;
 203
 204                 if (altitude_series != null) {
 205                         AltosTimeSeries temp_series = make_series(speed_name, AltosConvert.speed);
 206                         altitude_series.differentiate(temp_series);
 207
 208                         alt_speed_series = make_series(speed_name, AltosConvert.speed);
 209                         temp_series.filter(alt_speed_series, 10.0);
 210                 } else {
 211                         System.out.printf("no altitude series\n");
 212                 }
 213                 if (accel_series != null) {
 214                         AltosTimeSeries temp_series = make_series(speed_name, AltosConvert.speed);
 215                         accel_series.integrate(temp_series);
 216
 217                         accel_speed_series = make_series(speed_name, AltosConvert.speed);
 218                         temp_series.filter(accel_speed_series, 0.1);
 219                 } else {
 220                         System.out.printf("no accel series\n");
 221                 }
 222
 223                 if (alt_speed_series != null && accel_speed_series != null) {
 224                         double  apogee_time = AltosLib.MISSING;
 225                         if (state_series != null) {
 226                                 for (AltosTimeValue d : state_series) {
 227                                         if (d.value >= AltosLib.ao_flight_drogue){
 228                                                 apogee_time = d.time;
 229                                                 break;
 230                                         }
 231                                 }
 232                         }
 233                         if (apogee_time == AltosLib.MISSING) {
 234                                 speed_series = alt_speed_series;
 235                         } else {
 236                                 speed_series = make_series(speed_name, AltosConvert.speed);
 237                                 for (AltosTimeValue d : accel_speed_series) {
 238                                         if (d.time <= apogee_time)
 239                                                 speed_series.add(d);
 240                                 }
 241                                 for (AltosTimeValue d : alt_speed_series) {
 242                                         if (d.time > apogee_time)
 243                                                 speed_series.add(d);
 244                                 }
 245
 246                         }
 247                 } else if (alt_speed_series != null) {
 248                         speed_series = alt_speed_series;
 249                 } else if (accel_speed_series != null) {
 250                         speed_series = accel_speed_series;
 251                 }
 252                 if (speed_series != null) {
 253                         add_series(speed_series);
 254                         System.out.printf("speed series for %s set to %s\n", this.toString(), speed_series.toString());
 255                 } else
 256                         System.out.printf("didn't manage to make speed series\n");
 257         }
 258
 259         AltosTimeSeries kalman_height_series, kalman_speed_series, kalman_accel_series;
 260
 261         public static final String kalman_height_name = "Kalman Height";
 262         public static final String kalman_speed_name = "Kalman Speed";
 263         public static final String kalman_accel_name = "Kalman Accel";
 264
 265         public void set_kalman(double height, double speed, double acceleration) {
 266                 if (kalman_height_series == null) {
 267                         kalman_height_series = add_series(kalman_height_name, AltosConvert.height);
 268                         kalman_speed_series = add_series(kalman_speed_name, AltosConvert.speed);
 269                         kalman_accel_series = add_series(kalman_accel_name, AltosConvert.accel);
 270                 }
 271                 kalman_height_series.add(time(), height);
 272                 kalman_speed_series.add(time(), speed);
 273                 kalman_accel_series.add(time(), acceleration);
 274         }
 275
 276         AltosTimeSeries thrust_series;
 277
 278         public static final String thrust_name = "Thrust";
 279
 280         public  void set_thrust(double N) {
 281                 if (thrust_series == null)
 282                         thrust_series = add_series(thrust_name, AltosConvert.force);
 283                 thrust_series.add(time(), N);
 284         }
 285
 286         AltosTimeSeries temperature_series;
 287
 288         public static final String temperature_name = "Temperature";
 289
 290         public  void set_temperature(double deg_c) {
 291                 if (temperature_series == null)
 292                         temperature_series = add_series(temperature_name, AltosConvert.temperature);
 293                 temperature_series.add(time(), deg_c);
 294         }
 295
 296         AltosTimeSeries battery_voltage_series;
 297
 298         public static final String battery_voltage_name = "Battery Voltage";
 299
 300         public void set_battery_voltage(double volts) {
 301                 if (volts == AltosLib.MISSING)
 302                         return;
 303                 if (battery_voltage_series == null)
 304                         battery_voltage_series = add_series(battery_voltage_name, AltosConvert.voltage);
 305                 battery_voltage_series.add(time(), volts);
 306         }
 307
 308         AltosTimeSeries apogee_voltage_series;
 309
 310         public static final String apogee_voltage_name = "Apogee Voltage";
 311
 312         public void set_apogee_voltage(double volts) {
 313                 if (volts == AltosLib.MISSING)
 314                         return;
 315                 if (apogee_voltage_series == null)
 316                         apogee_voltage_series = add_series(apogee_voltage_name, AltosConvert.voltage);
 317                 apogee_voltage_series.add(time(), volts);
 318         }
 319
 320         AltosTimeSeries main_voltage_series;
 321
 322         public static final String main_voltage_name = "Main Voltage";
 323
 324         public void set_main_voltage(double volts) {
 325                 if (volts == AltosLib.MISSING)
 326                         return;
 327                 if (main_voltage_series == null)
 328                         main_voltage_series = add_series(main_voltage_name, AltosConvert.voltage);
 329                 main_voltage_series.add(time(), volts);
 330         }
 331
 332         AltosTimeSeries sats_in_view;
 333         AltosTimeSeries sats_in_soln;
 334         AltosTimeSeries gps_altitude;
 335         AltosTimeSeries gps_height;
 336         AltosTimeSeries gps_ground_speed;
 337         AltosTimeSeries gps_ascent_rate;
 338         AltosTimeSeries gps_course;
 339         AltosTimeSeries gps_speed;
 340
 341         public ArrayList<AltosGPSTimeValue> gps_series;
 342
 343         public static final String sats_in_view_name = "Satellites in view";
 344         public static final String sats_in_soln_name = "Satellites in solution";
 345         public static final String gps_altitude_name = "GPS Altitude";
 346         public static final String gps_height_name = "GPS Height";
 347         public static final String gps_ground_speed_name = "GPS Ground Speed";
 348         public static final String gps_ascent_rate_name = "GPS Ascent Rate";
 349         public static final String gps_course_name = "GPS Course";
 350         public static final String gps_speed_name = "GPS Speed";
 351
 352         public void set_gps(AltosGPS gps) {
 353                 if (gps_series == null)
 354                         gps_series = new ArrayList<AltosGPSTimeValue>();
 355                 gps_series.add(new AltosGPSTimeValue(time(), gps));
 356
 357                 if (sats_in_view == null) {
 358                         sats_in_view = add_series(sats_in_view_name, null);
 359                         sats_in_soln = add_series(sats_in_soln_name, null);
 360                         gps_altitude = add_series(gps_altitude_name, AltosConvert.height);
 361                         gps_height = add_series(gps_height_name, AltosConvert.height);
 362                         gps_ground_speed = add_series(gps_ground_speed_name, AltosConvert.speed);
 363                         gps_ascent_rate = add_series(gps_ascent_rate_name, AltosConvert.speed);
 364                         gps_course = add_series(gps_course_name, null);
 365                         gps_speed = add_series(gps_speed_name, null);
 366                 }
 367                 if (gps.cc_gps_sat != null)
 368                         sats_in_view.add(time(), gps.cc_gps_sat.length);
 369                 if (gps.locked) {
 370                         sats_in_soln.add(time(), gps.nsat);
 371                         if (gps.alt != AltosLib.MISSING) {
 372                                 gps_altitude.add(time(), gps.alt);
 373                                 if (cal_data.gps_ground_altitude != AltosLib.MISSING)
 374                                         gps_height.add(time(), gps.alt - cal_data.gps_ground_altitude);
 375                         }
 376                         if (gps.ground_speed != AltosLib.MISSING)
 377                                 gps_ground_speed.add(time(), gps.ground_speed);
 378                         if (gps.climb_rate != AltosLib.MISSING)
 379                                 gps_ascent_rate.add(time(), gps.climb_rate);
 380                         if (gps.course != AltosLib.MISSING)
 381                                 gps_course.add(time(), gps.course);
 382                         if (gps.ground_speed != AltosLib.MISSING && gps.climb_rate != AltosLib.MISSING)
 383                                 gps_speed.add(time(), Math.sqrt(gps.ground_speed * gps.ground_speed +
 384                                                                 gps.climb_rate * gps.climb_rate));
 385                 }
 386         }
 387
 388         public static final String accel_across_name = "Accel Across";
 389         public static final String accel_along_name = "Accel Along";
 390         public static final String accel_through_name = "Accel Through";
 391
 392         public  void set_accel(double along, double across, double through) {
 393         }
 394
 395         public  void set_accel_ground(double along, double across, double through) {
 396         }
 397
 398         public  void set_gyro(double roll, double pitch, double yaw) {
 399         }
 400
 401         public  void set_mag(double along, double across, double through) {
 402         }
 403
 404         public void set_orient(double new_orient) { }
 405
 406         public  void set_pyro_voltage(double volts) {
 407         }
 408
 409         public  void set_ignitor_voltage(double[] voltage) {
 410         }
 411
 412         public  void set_pyro_fired(int pyro_mask) {
 413         }
 414
 415         public void set_companion(AltosCompanion companion) {
 416         }
 417
 418         public void fill_in() {
 419                 System.out.printf("fill in %s\n", this.toString());
 420                 compute_speed();
 421                 compute_accel();
 422                 if (cal_data.ground_altitude != AltosLib.MISSING)
 423                         compute_height(cal_data.ground_altitude);
 424         }
 425
 426         public void init() {
 427                 time = AltosLib.MISSING;
 428                 series = new ArrayList<AltosTimeSeries>();
 429         }
 430
 431         public AltosTimeSeries[] series() {
 432                 fill_in();
 433                 return series.toArray(new AltosTimeSeries[0]);
 434         }
 435
 436         public AltosFlightSeries(AltosCalData cal_data) {
 437                 super(cal_data);
 438                 System.out.printf("new flight series %s\n", this.toString());
 439                 init();
 440         }
 441 }