altoslib/altosui: Fixes for state changes
[fw/altos] / 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 record;
26
27         public static final int set_position = 1;
28         public static final int set_gps = 2;
29         public static final int set_data = 4;
30
31         public int set;
32
33         /* derived data */
34
35         public long     report_time;
36
37         public double   time;
38         public double   prev_time;
39         public double   time_change;
40         public int      tick;
41         public int      boost_tick;
42
43         public int      state;
44         public int      flight;
45         public int      serial;
46         public boolean  landed;
47         public boolean  ascent; /* going up? */
48         public boolean  boost;  /* under power */
49         public int      rssi;
50         public int      status;
51
52         public double   ground_altitude;
53         public double   ground_pressure;
54         public double   altitude;
55         public double   height;
56         public double   pressure;
57         public double   acceleration;
58         public double   battery_voltage;
59         public double   pyro_voltage;
60         public double   temperature;
61         public double   apogee_voltage;
62         public double   main_voltage;
63         public double   speed;
64
65         public double   prev_height;
66         public double   prev_speed;
67         public double   prev_acceleration;
68
69         public double   max_height;
70         public double   max_acceleration;
71         public double   max_speed;
72
73         public double   kalman_height, kalman_speed, kalman_acceleration;
74
75         public AltosGPS gps;
76         public AltosGPS temp_gps;
77         public boolean  gps_pending;
78         public int gps_sequence;
79
80         public AltosIMU imu;
81         public AltosMag mag;
82
83         public static final int MIN_PAD_SAMPLES = 10;
84
85         public int      npad;
86         public int      gps_waiting;
87         public boolean  gps_ready;
88
89         public int      ngps;
90
91         public AltosGreatCircle from_pad;
92         public double   elevation;      /* from pad */
93         public double   range;          /* total distance */
94
95         public double   gps_height;
96
97         public double pad_lat, pad_lon, pad_alt;
98
99         public int      speak_tick;
100         public double   speak_altitude;
101
102         public String   callsign;
103         public double   accel_plus_g;
104         public double   accel_minus_g;
105         public double   accel;
106         public double   ground_accel;
107         public double   ground_accel_avg;
108
109         public int      log_format;
110
111         public AltosMs5607      baro;
112
113         public AltosRecordCompanion     companion;
114
115         public double speed() {
116                 return speed;
117         }
118
119         public double max_speed() {
120                 return max_speed;
121         }
122
123         public void set_npad(int npad) {
124                 this.npad = npad;
125                 gps_waiting = MIN_PAD_SAMPLES - npad;
126                 if (this.gps_waiting < 0)
127                         gps_waiting = 0;
128                 gps_ready = gps_waiting == 0;
129         }
130
131         public void init() {
132                 record = null;
133
134                 set = 0;
135
136                 report_time = System.currentTimeMillis();
137                 time = AltosRecord.MISSING;
138                 time_change = AltosRecord.MISSING;
139                 prev_time = AltosRecord.MISSING;
140                 tick = AltosRecord.MISSING;
141                 boost_tick = AltosRecord.MISSING;
142                 state = AltosLib.ao_flight_invalid;
143                 flight = AltosRecord.MISSING;
144                 landed = false;
145                 boost = false;
146                 rssi = AltosRecord.MISSING;
147                 status = 0;
148
149                 ground_altitude = AltosRecord.MISSING;
150                 ground_pressure = AltosRecord.MISSING;
151                 altitude = AltosRecord.MISSING;
152                 height = AltosRecord.MISSING;
153                 pressure = AltosRecord.MISSING;
154                 acceleration = AltosRecord.MISSING;
155                 temperature = AltosRecord.MISSING;
156
157                 prev_height = AltosRecord.MISSING;
158                 prev_speed = AltosRecord.MISSING;
159                 prev_acceleration = AltosRecord.MISSING;
160
161                 battery_voltage = AltosRecord.MISSING;
162                 pyro_voltage = AltosRecord.MISSING;
163                 apogee_voltage = AltosRecord.MISSING;
164                 main_voltage = AltosRecord.MISSING;
165
166                 speed = AltosRecord.MISSING;
167
168                 kalman_height = AltosRecord.MISSING;
169                 kalman_speed = AltosRecord.MISSING;
170                 kalman_acceleration = AltosRecord.MISSING;
171
172                 max_speed = 0;
173                 max_height = 0;
174                 max_acceleration = 0;
175
176                 gps = null;
177                 temp_gps = null;
178                 gps_sequence = 0;
179                 gps_pending = false;
180
181                 imu = null;
182                 mag = null;
183
184                 set_npad(0);
185                 ngps = 0;
186
187                 from_pad = null;
188                 elevation = AltosRecord.MISSING;
189                 range = AltosRecord.MISSING;
190                 gps_height = AltosRecord.MISSING;
191
192                 pad_lat = AltosRecord.MISSING;
193                 pad_lon = AltosRecord.MISSING;
194                 pad_alt = AltosRecord.MISSING;
195
196                 speak_tick = AltosRecord.MISSING;
197                 speak_altitude = AltosRecord.MISSING;
198
199                 callsign = null;
200
201                 accel_plus_g = AltosRecord.MISSING;
202                 accel_minus_g = AltosRecord.MISSING;
203                 accel = AltosRecord.MISSING;
204                 ground_accel = AltosRecord.MISSING;
205                 ground_accel_avg = AltosRecord.MISSING;
206                 log_format = AltosRecord.MISSING;
207                 serial = AltosRecord.MISSING;
208
209                 baro = null;
210                 companion = null;
211         }
212
213         void copy(AltosState old) {
214
215                 record = null;
216
217                 if (old == null) {
218                         init();
219                         return;
220                 }
221
222                 report_time = old.report_time;
223                 time = old.time;
224                 time_change = 0;
225                 tick = old.tick;
226                 boost_tick = old.boost_tick;
227
228                 state = old.state;
229                 flight = old.flight;
230                 landed = old.landed;
231                 ascent = old.ascent;
232                 boost = old.boost;
233                 rssi = old.rssi;
234                 status = old.status;
235                 
236                 set = 0;
237
238                 ground_altitude = old.ground_altitude;
239                 altitude = old.altitude;
240                 height = old.height;
241                 pressure = old.pressure;
242                 acceleration = old.acceleration;
243                 battery_voltage = old.battery_voltage;
244                 pyro_voltage = old.pyro_voltage;
245                 temperature = old.temperature;
246                 apogee_voltage = old.apogee_voltage;
247                 main_voltage = old.main_voltage;
248                 speed = old.speed;
249
250                 prev_height = old.height;
251                 prev_speed = old.speed;
252                 prev_acceleration = old.acceleration;
253                 prev_time = old.time;
254
255                 max_height = old.max_height;
256                 max_acceleration = old.max_acceleration;
257                 max_speed = old.max_speed;
258
259                 kalman_height = old.kalman_height;
260                 kalman_speed = old.kalman_speed;
261                 kalman_acceleration = old.kalman_acceleration;
262
263                 if (old.gps != null)
264                         gps = old.gps.clone();
265                 else
266                         gps = null;
267                 if (old.temp_gps != null)
268                         temp_gps = old.temp_gps.clone();
269                 else
270                         temp_gps = null;
271                 gps_sequence = old.gps_sequence;
272                 gps_pending = old.gps_pending;
273
274                 if (old.imu != null)
275                         imu = old.imu.clone();
276                 else
277                         imu = null;
278
279                 if (old.mag != null)
280                         mag = old.mag.clone();
281                 else
282                         mag = null;
283
284                 npad = old.npad;
285                 gps_waiting = old.gps_waiting;
286                 gps_ready = old.gps_ready;
287                 ngps = old.ngps;
288
289                 if (old.from_pad != null)
290                         from_pad = old.from_pad.clone();
291                 else
292                         from_pad = null;
293
294                 elevation = old.elevation;
295                 range = old.range;
296
297                 gps_height = old.gps_height;
298                 pad_lat = old.pad_lat;
299                 pad_lon = old.pad_lon;
300                 pad_alt = old.pad_alt;
301
302                 speak_tick = old.speak_tick;
303                 speak_altitude = old.speak_altitude;
304
305                 callsign = old.callsign;
306
307                 accel_plus_g = old.accel_plus_g;
308                 accel_minus_g = old.accel_minus_g;
309                 accel = old.accel;
310                 ground_accel = old.ground_accel;
311                 ground_accel_avg = old.ground_accel_avg;
312
313                 log_format = old.log_format;
314                 serial = old.serial;
315
316                 baro = old.baro;
317                 companion = old.companion;
318         }
319
320         double altitude() {
321                 if (altitude != AltosRecord.MISSING)
322                         return altitude;
323                 if (gps != null)
324                         return gps.alt;
325                 return AltosRecord.MISSING;
326         }
327
328         void update_vertical_pos() {
329
330                 double  alt = altitude();
331
332                 if (state == AltosLib.ao_flight_pad && alt != AltosRecord.MISSING && ground_pressure == AltosRecord.MISSING) {
333                         if (ground_altitude == AltosRecord.MISSING)
334                                 ground_altitude = alt;
335                         else
336                                 ground_altitude = (ground_altitude * 7 + alt) / 8;
337                 }
338
339                 if (kalman_height != AltosRecord.MISSING)
340                         height = kalman_height;
341                 else if (altitude != AltosRecord.MISSING && ground_altitude != AltosRecord.MISSING)
342                         height = altitude - ground_altitude;
343                 else
344                         height = AltosRecord.MISSING;
345
346                 if (height != AltosRecord.MISSING && height > max_height)
347                         max_height = height;
348
349                 update_speed();
350         }
351
352         double motion_filter_value() {
353                 return 1/ Math.exp(time_change/2.0);
354         }
355
356         void update_speed() {
357                 if (kalman_speed != AltosRecord.MISSING)
358                         speed = kalman_speed;
359                 else if (state != AltosLib.ao_flight_invalid &&
360                          time_change != AltosRecord.MISSING)
361                 {
362                         if (ascent && acceleration != AltosRecord.MISSING)
363                         {
364                                 if (prev_speed == AltosRecord.MISSING)
365                                         speed = acceleration * time_change;
366                                 else
367                                         speed = prev_speed + acceleration * time_change;
368                         }
369                         else if (height != AltosRecord.MISSING &&
370                                  prev_height != AltosRecord.MISSING &&
371                                  time_change != 0)
372                         {
373                                 double  new_speed = (height - prev_height) / time_change;
374
375                                 if (prev_speed == AltosRecord.MISSING)
376                                         speed = new_speed;
377                                 else {
378                                         double  filter = motion_filter_value();
379
380                                         speed = prev_speed * filter + new_speed * (1-filter);
381                                 }
382                         }
383                 }
384                 if (acceleration == AltosRecord.MISSING) {
385                         if (prev_speed != AltosRecord.MISSING && time_change != 0) {
386                                 double  new_acceleration = (speed - prev_speed) / time_change;
387
388                                 if (prev_acceleration == AltosRecord.MISSING)
389                                         acceleration = new_acceleration;
390                                 else {
391                                         double filter = motion_filter_value();
392
393                                         acceleration = prev_acceleration * filter + new_acceleration * (1-filter);
394                                 }
395                         }
396                 }
397                 if (boost && speed != AltosRecord.MISSING && speed > max_speed)
398                         max_speed = speed;
399         }
400         
401         void update_accel() {
402                 double  ground = ground_accel;
403
404                 if (ground == AltosRecord.MISSING)
405                         ground = ground_accel_avg;
406                 if (accel == AltosRecord.MISSING)
407                         return;
408                 if (ground == AltosRecord.MISSING)
409                         return;
410                 if (accel_plus_g == AltosRecord.MISSING)
411                         return;
412                 if (accel_minus_g == AltosRecord.MISSING)
413                         return;
414
415                 double counts_per_g = (accel_minus_g - accel_plus_g) / 2.0;
416                 double counts_per_mss = counts_per_g / 9.80665;
417
418                 acceleration = (ground - accel) / counts_per_mss;
419
420                 /* Only look at accelerometer data under boost */
421                 if (boost && acceleration != AltosRecord.MISSING && (max_acceleration == AltosRecord.MISSING || acceleration > max_acceleration))
422                         max_acceleration = acceleration;
423                 update_speed();
424         }
425
426         void update_time() {
427                 if (tick != AltosRecord.MISSING) {
428                         time = tick / 100.0;
429                         if (prev_time != AltosRecord.MISSING)
430                                 time_change = time - prev_time;
431                 }
432         }
433
434         void update_gps() {
435                 elevation = 0;
436                 range = -1;
437                 gps_height = 0;
438
439                 if (gps == null)
440                         return;
441
442                 if (gps.locked && gps.nsat >= 4) {
443                         /* Track consecutive 'good' gps reports, waiting for 10 of them */
444                         if (state == AltosLib.ao_flight_pad) {
445                                 set_npad(npad+1);
446                                 if (pad_lat != AltosRecord.MISSING) {
447                                         pad_lat = (pad_lat * 31 + gps.lat) / 32;
448                                         pad_lon = (pad_lon * 31 + gps.lon) / 32;
449                                         pad_alt = (pad_alt * 31 + gps.alt) / 32;
450                                 }
451                         }
452                         if (pad_lat == AltosRecord.MISSING) {
453                                 pad_lat = gps.lat;
454                                 pad_lon = gps.lon;
455                                 pad_alt = gps.alt;
456                         }
457                 }
458                 if (gps.lat != 0 && gps.lon != 0 &&
459                     pad_lat != AltosRecord.MISSING &&
460                     pad_lon != AltosRecord.MISSING)
461                 {
462                         double h = height;
463
464                         if (h == AltosRecord.MISSING)
465                                 h = 0;
466                         from_pad = new AltosGreatCircle(pad_lat, pad_lon, 0, gps.lat, gps.lon, h);
467                         elevation = from_pad.elevation;
468                         range = from_pad.range;
469                         gps_height = gps.alt - pad_alt;
470                 }
471         }
472
473         public void set_tick(int tick) {
474                 if (tick != AltosRecord.MISSING) {
475                         if (this.tick != AltosRecord.MISSING) {
476                                 while (tick < this.tick)
477                                         tick += 65536;
478                                 time_change = (tick - this.tick) / 100.0;
479                         } else
480                                 time_change = 0;
481                         this.tick = tick;
482                         update_time();
483                 }
484         }
485
486         public void set_boost_tick(int boost_tick) {
487                 if (boost_tick != AltosRecord.MISSING)
488                         this.boost_tick = boost_tick;
489         }
490
491         public String state_name() {
492                 return AltosLib.state_name(state);
493         }
494
495         public void set_state(int state) {
496                 if (state != AltosLib.ao_flight_invalid) {
497                         this.state = state;
498                         ascent = (AltosLib.ao_flight_boost <= state &&
499                                   state <= AltosLib.ao_flight_coast);
500                         boost = (AltosLib.ao_flight_boost == state);
501                 }
502
503         }
504
505         public void set_flight(int flight) {
506
507                 /* When the flight changes, reset the state */
508                 if (flight != AltosRecord.MISSING) {
509                         if (this.flight != AltosRecord.MISSING &&
510                             this.flight != flight) {
511                                 init();
512                         }
513                         this.flight = flight;
514                 }
515         }
516
517         public void set_serial(int serial) {
518                 /* When the serial changes, reset the state */
519                 if (serial != AltosRecord.MISSING) {
520                         if (this.serial != AltosRecord.MISSING &&
521                             this.serial != serial) {
522                                 init();
523                         }
524                         this.serial = serial;
525                 }
526         }
527
528         public int rssi() {
529                 if (rssi == AltosRecord.MISSING)
530                         return 0;
531                 return rssi;
532         }
533
534         public void set_rssi(int rssi, int status) {
535                 if (rssi != AltosRecord.MISSING) {
536                         this.rssi = rssi;
537                         this.status = status;
538                 }
539         }
540
541         public void set_altitude(double altitude) {
542                 if (altitude != AltosRecord.MISSING) {
543                         this.altitude = altitude;
544                         update_vertical_pos();
545                         set |= set_position;
546                 }
547         }
548
549         public void set_ground_altitude(double ground_altitude) {
550                 if (ground_altitude != AltosRecord.MISSING) {
551                         this.ground_altitude = ground_altitude;
552                         update_vertical_pos();
553                 }
554         }
555
556         public void set_ground_pressure (double pressure) {
557                 if (pressure != AltosRecord.MISSING) {
558                         this.ground_pressure = pressure;
559                         set_ground_altitude(AltosConvert.pressure_to_altitude(pressure));
560                         update_vertical_pos();
561                 }
562         }
563
564         public void set_gps(AltosGPS gps, int sequence) {
565                 if (gps != null) {
566                         this.gps = gps.clone();
567                         gps_sequence = sequence;
568                         update_gps();
569                         update_vertical_pos();
570                         set |= set_gps;
571                 }
572         }
573
574         public void set_kalman(double height, double speed, double acceleration) {
575                 if (height != AltosRecord.MISSING) {
576                         kalman_height = height;
577                         kalman_speed = speed;
578                         kalman_acceleration = acceleration;
579                         update_vertical_pos();
580                 }
581         }
582
583         public void set_pressure(double pressure) {
584                 if (pressure != AltosRecord.MISSING) {
585                         this.pressure = pressure;
586                         set_altitude(AltosConvert.pressure_to_altitude(pressure));
587                 }
588         }
589
590         public void make_baro() {
591                 if (baro == null)
592                         baro = new AltosMs5607();
593         }
594
595         public void set_ms5607(int pres, int temp) {
596                 if (baro != null) {
597                         baro.set(pres, temp);
598
599                         set_pressure(baro.pa);
600                         set_temperature(baro.cc / 100.0);
601                 }
602         }
603
604         public void make_companion (int nchannels) {
605                 if (companion == null)
606                         companion = new AltosRecordCompanion(nchannels);
607         }
608
609         public void set_companion(AltosRecordCompanion companion) {
610                 this.companion = companion;
611         }
612
613         public void set_accel_g(double accel_plus_g, double accel_minus_g) {
614                 if (accel_plus_g != AltosRecord.MISSING) {
615                         this.accel_plus_g = accel_plus_g;
616                         this.accel_minus_g = accel_minus_g;
617                         update_accel();
618                 }
619         }
620         public void set_ground_accel(double ground_accel) {
621                 if (ground_accel != AltosRecord.MISSING) {
622                         this.ground_accel = ground_accel;
623                         update_accel();
624                 }
625         }
626
627         public void set_accel(double accel) {
628                 if (accel != AltosRecord.MISSING) {
629                         this.accel = accel;
630                         if (state == AltosLib.ao_flight_pad) {
631                                 if (ground_accel_avg == AltosRecord.MISSING)
632                                         ground_accel_avg = accel;
633                                 else
634                                         ground_accel_avg = (ground_accel_avg * 7 + accel) / 8;
635                         }
636                 }
637                 update_accel();
638         }
639
640         public void set_temperature(double temperature) {
641                 if (temperature != AltosRecord.MISSING) {
642                         this.temperature = temperature;
643                         set |= set_data;
644                 }
645         }
646
647         public void set_battery_voltage(double battery_voltage) {
648                 if (battery_voltage != AltosRecord.MISSING) {
649                         this.battery_voltage = battery_voltage;
650                         set |= set_data;
651                 }
652         }
653
654         public void set_pyro_voltage(double pyro_voltage) {
655                 if (pyro_voltage != AltosRecord.MISSING) {
656                         this.pyro_voltage = pyro_voltage;
657                         set |= set_data;
658                 }
659         }
660
661         public void set_apogee_voltage(double apogee_voltage) {
662                 if (apogee_voltage != AltosRecord.MISSING) {
663                         this.apogee_voltage = apogee_voltage;
664                         set |= set_data;
665                 }
666         }
667
668         public void set_main_voltage(double main_voltage) {
669                 if (main_voltage != AltosRecord.MISSING) {
670                         this.main_voltage = main_voltage;
671                         set |= set_data;
672                 }
673         }
674
675
676         public double time_since_boost() {
677                 if (tick == AltosRecord.MISSING)
678                         return 0.0;
679
680                 if (boost_tick != AltosRecord.MISSING) {
681                         return (tick - boost_tick) / 100.0;
682                 }
683                 return tick / 100.0;
684         }
685
686         public boolean valid() {
687                 return tick != AltosRecord.MISSING && serial != AltosRecord.MISSING;
688         }
689
690         public AltosGPS make_temp_gps() {
691                 if (temp_gps == null) {
692                         temp_gps = new AltosGPS(gps);
693                         temp_gps.cc_gps_sat = null;
694                 }
695                 gps_pending = true;
696                 return temp_gps;
697         }
698
699         public void set_temp_gps() {
700                 set_gps(temp_gps, gps_sequence + 1);
701                 gps_pending = false;
702                 temp_gps = null;
703         }
704
705         public void init (AltosRecord cur, AltosState prev_state) {
706
707                 System.out.printf ("init\n");
708                 if (cur == null)
709                         cur = new AltosRecord();
710
711                 record = cur;
712
713                 /* Discard previous state if it was for a different board */
714                 if (prev_state != null && prev_state.serial != cur.serial)
715                         prev_state = null;
716
717                 copy(prev_state);
718
719                 set_ground_altitude(cur.ground_altitude());
720                 set_altitude(cur.altitude());
721
722                 set_kalman(cur.kalman_height, cur.kalman_speed, cur.kalman_acceleration);
723
724                 report_time = System.currentTimeMillis();
725
726                 set_temperature(cur.temperature());
727                 set_apogee_voltage(cur.drogue_voltage());
728                 set_main_voltage(cur.main_voltage());
729                 set_battery_voltage(cur.battery_voltage());
730
731                 set_pressure(cur.pressure());
732
733                 set_tick(cur.tick);
734                 set_state(cur.state);
735
736                 set_accel_g (cur.accel_minus_g, cur.accel_plus_g);
737                 set_ground_accel(cur.ground_accel);
738                 set_accel (cur.accel);
739
740                 if (cur.gps_sequence != gps_sequence)
741                         set_gps(cur.gps, cur.gps_sequence);
742
743         }
744
745         public AltosState clone() {
746                 AltosState s = new AltosState();
747                 s.copy(this);
748                 return s;
749         }
750
751         public AltosState(AltosRecord cur) {
752                 init(cur, null);
753         }
754
755         public AltosState (AltosRecord cur, AltosState prev) {
756                 init(cur, prev);
757         }
758
759         public AltosState () {
760                 init();
761         }
762 }