altoslib: Compute orientation from eeprom data files
[fw/altos] / 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 = new ArrayList<AltosTimeSeries>();
22
23         public double   speed_filter_width = 4.0;
24         public double   accel_filter_width = 4.0;
25
26         public int[] indices() {
27                 int[] indices = new int[series.size()];
28                 for (int i = 0; i < indices.length; i++)
29                         indices[i] = -1;
30                 step_indices(indices);
31                 return indices;
32         }
33
34         private double time(int id, int index) {
35                 AltosTimeSeries         s = series.get(id);
36
37                 if (index < 0)
38                         return Double.NEGATIVE_INFINITY;
39
40                 if (index < s.values.size())
41                         return s.values.get(index).time;
42                 return Double.POSITIVE_INFINITY;
43         }
44
45         public boolean step_indices(int[] indices) {
46                 double  min_next = time(0, indices[0]+1);
47
48                 for (int i = 1; i < indices.length; i++) {
49                         double next = time(i, indices[i]+1);
50                         if (next < min_next)
51                                 min_next = next;
52                 }
53
54                 if (min_next == Double.POSITIVE_INFINITY)
55                         return false;
56
57                 for (int i = 0; i < indices.length; i++) {
58                         double  t = time(i, indices[i] + 1);
59
60                         if (t <= min_next)
61                                 indices[i]++;
62                 }
63                 return true;
64         }
65
66         public double time(int[] indices) {
67                 double max = time(0, indices[0]);
68
69                 for (int i = 1; i < indices.length; i++) {
70                         double t = time(i, indices[i]);
71                         if (t >= max)
72                                 max = t;
73                 }
74                 return max;
75         }
76
77         public double value(String name, int[] indices) {
78                 for (int i = 0; i < indices.length; i++) {
79                         AltosTimeSeries s = series.get(i);
80                         if (s.label.equals(name)) {
81                                 int index = indices[i];
82                                 if (index < 0)
83                                         index = 0;
84                                 if (index >= s.values.size())
85                                         index = s.values.size() - 1;
86                                 return s.values.get(index).value;
87                         }
88                 }
89                 return AltosLib.MISSING;
90         }
91
92         public double value(String name, double time) {
93                 for (AltosTimeSeries s : series) {
94                         if (s.label.equals(name))
95                                 return s.value(time);
96                 }
97                 return AltosLib.MISSING;
98         }
99
100         public double value_before(String name, double time) {
101                 for (AltosTimeSeries s : series) {
102                         if (s.label.equals(name))
103                                 return s.value_before(time);
104                 }
105                 return AltosLib.MISSING;
106         }
107
108         public double value_after(String name, double time) {
109                 for (AltosTimeSeries s : series) {
110                         if (s.label.equals(name))
111                                 return s.value_after(time);
112                 }
113                 return AltosLib.MISSING;
114         }
115
116         public AltosTimeSeries make_series(String label, AltosUnits units) {
117                 return new AltosTimeSeries(label, units);
118         }
119
120         public void add_series(AltosTimeSeries s) {
121                 series.add(s);
122         }
123
124         public AltosTimeSeries add_series(String label, AltosUnits units) {
125                 AltosTimeSeries s = make_series(label, units);
126                 add_series(s);
127                 return s;
128         }
129
130         public void remove_series(AltosTimeSeries s) {
131                 series.remove(s);
132         }
133
134         public boolean has_series(String label) {
135                 for (AltosTimeSeries s : series)
136                         if (s.label.equals(label))
137                                 return true;
138                 return false;
139         }
140
141         public AltosTimeSeries state_series;
142
143         public static final String state_name = "State";
144
145         public void set_state(int state) {
146
147                 if (state == AltosLib.ao_flight_pad)
148                         return;
149
150                 if (state_series == null)
151                         state_series = add_series(state_name, AltosConvert.state_name);
152                 else if (this.state == state)
153                         return;
154                 this.state = state;
155                 state_series.add(time(), state);
156         }
157
158         public AltosTimeSeries  accel_series;
159
160         public static final String accel_name = "Accel";
161
162         public void set_acceleration(double acceleration) {
163                 if (acceleration == AltosLib.MISSING)
164                         return;
165                 if (accel_series == null)
166                         accel_series = add_series(accel_name, AltosConvert.accel);
167
168                 accel_series.add(time(), acceleration);
169         }
170
171         private void compute_accel() {
172                 if (accel_series != null)
173                         return;
174
175                 if (speed_series != null) {
176                         AltosTimeSeries temp_series = make_series(speed_name, AltosConvert.speed);
177                         speed_series.filter(temp_series, accel_filter_width);
178                         accel_series = add_series(accel_name, AltosConvert.accel);
179                         temp_series.differentiate(accel_series);
180                 }
181         }
182
183         public void set_received_time(long received_time) {
184         }
185
186         public AltosTimeSeries rssi_series;
187
188         public static final String rssi_name = "RSSI";
189
190         public AltosTimeSeries status_series;
191
192         public static final String status_name = "Radio Status";
193
194         public void set_rssi(int rssi, int status) {
195                 if (rssi_series == null) {
196                         rssi_series = add_series(rssi_name, null);
197                         status_series = add_series(status_name, null);
198                 }
199                 rssi_series.add(time(), rssi);
200                 status_series.add(time(), status);
201         }
202
203         public AltosTimeSeries pressure_series;
204
205         public static final String pressure_name = "Pressure";
206
207         public AltosTimeSeries altitude_series;
208
209         public static final String altitude_name = "Altitude";
210
211         public AltosTimeSeries height_series;
212
213         public static final String height_name = "Height";
214
215         public  void set_pressure(double pa) {
216                 if (pa == AltosLib.MISSING)
217                         return;
218
219                 if (pressure_series == null)
220                         pressure_series = add_series(pressure_name, AltosConvert.pressure);
221                 pressure_series.add(time(), pa);
222                 if (altitude_series == null)
223                         altitude_series = add_series(altitude_name, AltosConvert.height);
224
225                 if (cal_data.ground_pressure == AltosLib.MISSING)
226                         cal_data.set_ground_pressure(pa);
227
228                 double altitude = AltosConvert.pressure_to_altitude(pa);
229                 altitude_series.add(time(), altitude);
230         }
231
232         private void compute_height() {
233                 double ground_altitude = cal_data.ground_altitude;
234                 if (height_series == null && ground_altitude != AltosLib.MISSING && altitude_series != null) {
235                         height_series = add_series(height_name, AltosConvert.height);
236                         for (AltosTimeValue alt : altitude_series)
237                                 height_series.add(alt.time, alt.value - ground_altitude);
238                 }
239
240                 if (gps_height == null && cal_data.gps_pad != null && cal_data.gps_pad.alt != AltosLib.MISSING && gps_altitude != null) {
241                         double gps_ground_altitude = cal_data.gps_pad.alt;
242                         gps_height = add_series(gps_height_name, AltosConvert.height);
243                         for (AltosTimeValue gps_alt : gps_altitude)
244                                 gps_height.add(gps_alt.time, gps_alt.value - gps_ground_altitude);
245                 }
246         }
247
248         public AltosTimeSeries speed_series;
249
250         public static final String speed_name = "Speed";
251
252         private void compute_speed() {
253                 if (speed_series != null)
254                         return;
255
256                 AltosTimeSeries alt_speed_series = null;
257                 AltosTimeSeries accel_speed_series = null;
258
259                 if (altitude_series != null) {
260                         AltosTimeSeries temp_series = make_series(altitude_name, AltosConvert.height);
261                         altitude_series.filter(temp_series, speed_filter_width);
262
263                         alt_speed_series = make_series(speed_name, AltosConvert.speed);
264                         temp_series.differentiate(alt_speed_series);
265                 }
266                 if (accel_series != null) {
267                         AltosTimeSeries temp_series = make_series(speed_name, AltosConvert.speed);
268                         accel_series.integrate(temp_series);
269
270                         accel_speed_series = make_series(speed_name, AltosConvert.speed);
271                         temp_series.filter(accel_speed_series, 0.1);
272                 }
273
274                 if (alt_speed_series != null && accel_speed_series != null) {
275                         double  apogee_time = AltosLib.MISSING;
276                         if (state_series != null) {
277                                 for (AltosTimeValue d : state_series) {
278                                         if (d.value >= AltosLib.ao_flight_drogue){
279                                                 apogee_time = d.time;
280                                                 break;
281                                         }
282                                 }
283                         }
284                         if (apogee_time == AltosLib.MISSING) {
285                                 speed_series = alt_speed_series;
286                         } else {
287                                 speed_series = make_series(speed_name, AltosConvert.speed);
288                                 for (AltosTimeValue d : accel_speed_series) {
289                                         if (d.time <= apogee_time)
290                                                 speed_series.add(d);
291                                 }
292                                 for (AltosTimeValue d : alt_speed_series) {
293                                         if (d.time > apogee_time)
294                                                 speed_series.add(d);
295                                 }
296
297                         }
298                 } else if (alt_speed_series != null) {
299                         speed_series = alt_speed_series;
300                 } else if (accel_speed_series != null) {
301                         speed_series = accel_speed_series;
302                 }
303                 if (speed_series != null)
304                         add_series(speed_series);
305         }
306
307         public AltosTimeSeries orient_series;
308
309         public static final String orient_name = "Tilt Angle";
310
311         private void compute_orient() {
312
313                 if (orient_series != null)
314                         return;
315
316                 if (accel_ground_across == AltosLib.MISSING)
317                         return;
318
319                 if (cal_data.pad_orientation == AltosLib.MISSING)
320                         return;
321
322                 if (cal_data.accel_zero_across == AltosLib.MISSING)
323                         return;
324
325                 AltosRotation rotation = new AltosRotation(AltosIMU.convert_accel(accel_ground_across - cal_data.accel_zero_across),
326                                                            AltosIMU.convert_accel(accel_ground_through - cal_data.accel_zero_through),
327                                                            AltosIMU.convert_accel(accel_ground_along - cal_data.accel_zero_along),
328                                                            cal_data.pad_orientation);
329                 double prev_time = ground_time;
330
331                 orient_series = add_series(orient_name, AltosConvert.orient);
332                 orient_series.add(ground_time, rotation.tilt());
333
334                 for (AltosTimeValue roll_v : gyro_roll) {
335                         double  time = roll_v.time;
336                         double  dt = time - prev_time;
337
338                         if (dt > 0) {
339                                 double  roll = AltosConvert.degrees_to_radians(roll_v.value);
340                                 double  pitch = AltosConvert.degrees_to_radians(gyro_pitch.value(time));
341                                 double  yaw = AltosConvert.degrees_to_radians(gyro_yaw.value(time));
342
343                                 rotation.rotate(dt, pitch, yaw, roll);
344                                 orient_series.add(time, rotation.tilt());
345                         }
346                         prev_time = time;
347                 }
348         }
349
350         public AltosTimeSeries  kalman_height_series, kalman_speed_series, kalman_accel_series;
351
352         public static final String kalman_height_name = "Kalman Height";
353         public static final String kalman_speed_name = "Kalman Speed";
354         public static final String kalman_accel_name = "Kalman Accel";
355
356         public void set_kalman(double height, double speed, double acceleration) {
357                 if (kalman_height_series == null) {
358                         kalman_height_series = add_series(kalman_height_name, AltosConvert.height);
359                         kalman_speed_series = add_series(kalman_speed_name, AltosConvert.speed);
360                         kalman_accel_series = add_series(kalman_accel_name, AltosConvert.accel);
361                 }
362                 kalman_height_series.add(time(), height);
363                 kalman_speed_series.add(time(), speed);
364                 kalman_accel_series.add(time(), acceleration);
365         }
366
367         public AltosTimeSeries thrust_series;
368
369         public static final String thrust_name = "Thrust";
370
371         public  void set_thrust(double N) {
372                 if (thrust_series == null)
373                         thrust_series = add_series(thrust_name, AltosConvert.force);
374                 thrust_series.add(time(), N);
375         }
376
377         public AltosTimeSeries temperature_series;
378
379         public static final String temperature_name = "Temperature";
380
381         public  void set_temperature(double deg_c) {
382                 if (temperature_series == null)
383                         temperature_series = add_series(temperature_name, AltosConvert.temperature);
384                 temperature_series.add(time(), deg_c);
385         }
386
387         public AltosTimeSeries battery_voltage_series;
388
389         public static final String battery_voltage_name = "Battery Voltage";
390
391         public void set_battery_voltage(double volts) {
392                 if (volts == AltosLib.MISSING)
393                         return;
394                 if (battery_voltage_series == null)
395                         battery_voltage_series = add_series(battery_voltage_name, AltosConvert.voltage);
396                 battery_voltage_series.add(time(), volts);
397         }
398
399         public AltosTimeSeries apogee_voltage_series;
400
401         public static final String apogee_voltage_name = "Apogee Voltage";
402
403         public void set_apogee_voltage(double volts) {
404                 if (volts == AltosLib.MISSING)
405                         return;
406                 if (apogee_voltage_series == null)
407                         apogee_voltage_series = add_series(apogee_voltage_name, AltosConvert.voltage);
408                 apogee_voltage_series.add(time(), volts);
409         }
410
411         public AltosTimeSeries main_voltage_series;
412
413         public static final String main_voltage_name = "Main Voltage";
414
415         public void set_main_voltage(double volts) {
416                 if (volts == AltosLib.MISSING)
417                         return;
418                 if (main_voltage_series == null)
419                         main_voltage_series = add_series(main_voltage_name, AltosConvert.voltage);
420                 main_voltage_series.add(time(), volts);
421         }
422
423         public ArrayList<AltosGPSTimeValue> gps_series;
424
425         public AltosGPS gps_before(double time) {
426                 AltosGPS gps = null;
427                 for (AltosGPSTimeValue gtv : gps_series)
428                         if (gtv.time <= time)
429                                 gps = gtv.gps;
430                         else
431                                 break;
432                 return gps;
433         }
434
435         public AltosTimeSeries  sats_in_view;
436         public AltosTimeSeries sats_in_soln;
437         public AltosTimeSeries gps_altitude;
438         public AltosTimeSeries gps_height;
439         public AltosTimeSeries gps_ground_speed;
440         public AltosTimeSeries gps_ascent_rate;
441         public AltosTimeSeries gps_course;
442         public AltosTimeSeries gps_speed;
443         public AltosTimeSeries gps_pdop, gps_vdop, gps_hdop;
444
445         public static final String sats_in_view_name = "Satellites in view";
446         public static final String sats_in_soln_name = "Satellites in solution";
447         public static final String gps_altitude_name = "GPS Altitude";
448         public static final String gps_height_name = "GPS Height";
449         public static final String gps_ground_speed_name = "GPS Ground Speed";
450         public static final String gps_ascent_rate_name = "GPS Ascent Rate";
451         public static final String gps_course_name = "GPS Course";
452         public static final String gps_speed_name = "GPS Speed";
453         public static final String gps_pdop_name = "GPS Dilution of Precision";
454         public static final String gps_vdop_name = "GPS Vertical Dilution of Precision";
455         public static final String gps_hdop_name = "GPS Horizontal Dilution of Precision";
456
457         public void set_gps(AltosGPS gps) {
458                 if (gps_series == null)
459                         gps_series = new ArrayList<AltosGPSTimeValue>();
460                 gps_series.add(new AltosGPSTimeValue(time(), gps));
461
462                 if (sats_in_soln == null) {
463                         sats_in_soln = add_series(sats_in_soln_name, null);
464                 }
465                 sats_in_soln.add(time(), gps.nsat);
466                 if (gps.pdop != AltosLib.MISSING) {
467                         if (gps_pdop == null)
468                                 gps_pdop = add_series(gps_pdop_name, null);
469                         gps_pdop.add(time(), gps.pdop);
470                 }
471                 if (gps.hdop != AltosLib.MISSING) {
472                         if (gps_hdop == null)
473                                 gps_hdop = add_series(gps_hdop_name, null);
474                         gps_hdop.add(time(), gps.hdop);
475                 }
476                 if (gps.vdop != AltosLib.MISSING) {
477                         if (gps_vdop == null)
478                                 gps_vdop = add_series(gps_vdop_name, null);
479                         gps_vdop.add(time(), gps.vdop);
480                 }
481                 if (gps.locked) {
482                         if (gps.alt != AltosLib.MISSING) {
483                                 if (gps_altitude == null)
484                                         gps_altitude = add_series(gps_altitude_name, AltosConvert.height);
485                                 gps_altitude.add(time(), gps.alt);
486                         }
487                         if (gps.ground_speed != AltosLib.MISSING) {
488                                 if (gps_ground_speed == null)
489                                         gps_ground_speed = add_series(gps_ground_speed_name, AltosConvert.speed);
490                                 gps_ground_speed.add(time(), gps.ground_speed);
491                         }
492                         if (gps.climb_rate != AltosLib.MISSING) {
493                                 if (gps_ascent_rate == null)
494                                         gps_ascent_rate = add_series(gps_ascent_rate_name, AltosConvert.speed);
495                                 gps_ascent_rate.add(time(), gps.climb_rate);
496                         }
497                         if (gps.course != AltosLib.MISSING) {
498                                 if (gps_course == null)
499                                         gps_course = add_series(gps_course_name, null);
500                                 gps_course.add(time(), gps.course);
501                         }
502                         if (gps.ground_speed != AltosLib.MISSING && gps.climb_rate != AltosLib.MISSING) {
503                                 if (gps_speed == null)
504                                         gps_speed = add_series(gps_speed_name, null);
505                                 gps_speed.add(time(), Math.sqrt(gps.ground_speed * gps.ground_speed +
506                                                                 gps.climb_rate * gps.climb_rate));
507                         }
508                 }
509                 if (gps.cc_gps_sat != null) {
510                         if (sats_in_view == null)
511                                 sats_in_view = add_series(sats_in_view_name, null);
512                         sats_in_view.add(time(), gps.cc_gps_sat.length);
513                 }
514         }
515
516         public static final String accel_along_name = "Accel Along";
517         public static final String accel_across_name = "Accel Across";
518         public static final String accel_through_name = "Accel Through";
519
520         public AltosTimeSeries accel_along, accel_across, accel_through;
521
522         public static final String gyro_roll_name = "Roll Rate";
523         public static final String gyro_pitch_name = "Pitch Rate";
524         public static final String gyro_yaw_name = "Yaw Rate";
525
526         public AltosTimeSeries gyro_roll, gyro_pitch, gyro_yaw;
527
528         public static final String mag_along_name = "Magnetic Field Along";
529         public static final String mag_across_name = "Magnetic Field Across";
530         public static final String mag_through_name = "Magnetic Field Through";
531
532         public AltosTimeSeries mag_along, mag_across, mag_through;
533
534         public  void set_accel(double along, double across, double through) {
535                 if (accel_along == null) {
536                         accel_along = add_series(accel_along_name, AltosConvert.accel);
537                         accel_across = add_series(accel_across_name, AltosConvert.accel);
538                         accel_through = add_series(accel_through_name, AltosConvert.accel);
539                 }
540                 accel_along.add(time(), along);
541                 accel_across.add(time(), across);
542                 accel_through.add(time(), through);
543         }
544
545         private double  accel_ground_along = AltosLib.MISSING;
546         private double  accel_ground_across = AltosLib.MISSING;
547         private double  accel_ground_through = AltosLib.MISSING;
548
549         private double          ground_time;
550
551         public  void set_accel_ground(double along, double across, double through) {
552                 accel_ground_along = along;
553                 accel_ground_across = across;
554                 accel_ground_through = through;
555                 ground_time = time();
556         }
557
558         public  void set_gyro(double roll, double pitch, double yaw) {
559                 if (gyro_roll == null) {
560                         gyro_roll = add_series(gyro_roll_name, AltosConvert.rotation_rate);
561                         gyro_pitch = add_series(gyro_pitch_name, AltosConvert.rotation_rate);
562                         gyro_yaw = add_series(gyro_yaw_name, AltosConvert.rotation_rate);
563                 }
564                 gyro_roll.add(time(), roll);
565                 gyro_pitch.add(time(), pitch);
566                 gyro_yaw.add(time(), yaw);
567         }
568
569         public  void set_mag(double along, double across, double through) {
570                 if (mag_along == null) {
571                         mag_along = add_series(mag_along_name, AltosConvert.magnetic_field);
572                         mag_across = add_series(mag_across_name, AltosConvert.magnetic_field);
573                         mag_through = add_series(mag_through_name, AltosConvert.magnetic_field);
574                 }
575                 mag_along.add(time(), along);
576                 mag_across.add(time(), across);
577                 mag_through.add(time(), through);
578         }
579
580         public void set_orient(double orient) {
581                 if (orient_series == null)
582                         orient_series = add_series(orient_name, AltosConvert.orient);
583                 orient_series.add(time(), orient);
584         }
585
586         public static final String pyro_voltage_name = "Pyro Voltage";
587
588         public AltosTimeSeries pyro_voltage;
589
590         public  void set_pyro_voltage(double volts) {
591                 if (pyro_voltage == null)
592                         pyro_voltage = add_series(pyro_voltage_name, AltosConvert.voltage);
593                 pyro_voltage.add(time(), volts);
594         }
595
596         private static String[] igniter_voltage_names;
597
598         public String igniter_voltage_name(int channel) {
599                 if (igniter_voltage_names == null || igniter_voltage_names.length <= channel) {
600                         String[] new_igniter_voltage_names = new String[channel + 1];
601                         int     i = 0;
602
603                         if (igniter_voltage_names != null) {
604                                 for (; i < igniter_voltage_names.length; i++)
605                                         new_igniter_voltage_names[i] = igniter_voltage_names[i];
606                         }
607                         for (; i < channel+1; i++)
608                                 new_igniter_voltage_names[i] = AltosLib.igniter_name(i);
609                         igniter_voltage_names = new_igniter_voltage_names;
610                 }
611                 return igniter_voltage_names[channel];
612         }
613
614         public AltosTimeSeries[] igniter_voltage;
615
616         public  void set_igniter_voltage(double[] voltage) {
617                 int channels = voltage.length;
618                 if (igniter_voltage == null || igniter_voltage.length <= channels) {
619                         AltosTimeSeries[]       new_igniter_voltage = new AltosTimeSeries[channels + 1];
620                         int                     i = 0;
621
622                         if (igniter_voltage != null) {
623                                 for (; i < igniter_voltage.length; i++)
624                                         new_igniter_voltage[i] = igniter_voltage[i];
625                         }
626                         for (; i < channels; i++)
627                                 new_igniter_voltage[i] = add_series(igniter_voltage_name(i), AltosConvert.voltage);
628                         igniter_voltage = new_igniter_voltage;
629                 }
630                 for (int channel = 0; channel < voltage.length; channel++)
631                         igniter_voltage[channel].add(time(), voltage[channel]);
632         }
633
634         public static final String pyro_fired_name = "Pyro Channel State";
635
636         public AltosTimeSeries pyro_fired_series;
637
638         int     last_pyro_mask;
639
640         public  void set_pyro_fired(int pyro_mask) {
641                 if (pyro_fired_series == null)
642                         pyro_fired_series = add_series(pyro_fired_name, AltosConvert.pyro_name);
643                 for (int channel = 0; channel < 32; channel++) {
644                         if ((last_pyro_mask & (1 << channel)) == 0 &&
645                             (pyro_mask & (1 << channel)) != 0) {
646                                 pyro_fired_series.add(time(), channel);
647                         }
648                 }
649                 last_pyro_mask = pyro_mask;
650         }
651
652         public void set_companion(AltosCompanion companion) {
653         }
654
655         public void finish() {
656                 compute_orient();
657                 compute_speed();
658                 compute_accel();
659                 compute_height();
660         }
661
662         public AltosTimeSeries[] series() {
663                 finish();
664                 return series.toArray(new AltosTimeSeries[0]);
665         }
666
667         public AltosFlightSeries(AltosCalData cal_data) {
668                 super(cal_data);
669         }
670 }