2 * Copyright © 2010 Keith Packard <keithp@keithp.com>
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.
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.
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.
19 * Track flight state from telemetry or eeprom data stream
22 package org.altusmetrum.altoslib_1;
24 public class AltosState implements Cloneable {
25 public AltosRecord record;
27 public static final int set_position = 1;
28 public static final int set_gps = 2;
29 public static final int set_data = 4;
33 static final double filter_len = 0.5;
37 public long received_time;
40 public double prev_time;
41 public double time_change;
43 private int prev_tick;
44 public int boost_tick;
48 private double prev_value;
49 private double max_value;
50 private double set_time;
51 private double prev_set_time;
52 private double max_rate = 1000.0;
54 void set(double new_value, double time) {
55 if (new_value != AltosRecord.MISSING) {
57 if (max_value == AltosRecord.MISSING || value > max_value) {
77 if (value != AltosRecord.MISSING && prev_value != AltosRecord.MISSING)
78 return value - prev_value;
79 return AltosRecord.MISSING;
84 double t = set_time - prev_set_time;
86 if (c != AltosRecord.MISSING && t != 0)
88 return AltosRecord.MISSING;
92 if (value == AltosRecord.MISSING)
93 return AltosRecord.MISSING;
94 if (prev_value == AltosRecord.MISSING)
95 return AltosRecord.MISSING;
97 return (value + prev_value) / 2 * (set_time - prev_set_time);
104 void set_derivative(AltosValue in) {
105 double new_value = in.rate();
107 if (new_value == AltosRecord.MISSING)
110 /* Clip changes to reduce noise */
111 if (prev_value != AltosRecord.MISSING) {
112 double ddt = in.time() - prev_set_time;
113 double ddv = (new_value - prev_value) / ddt;
116 if (Math.abs(ddv) > 1000) {
117 if (new_value > prev_value)
118 new_value = prev_value + ddt * 1000;
120 new_value = prev_value - ddt * 1000;
123 double f = 1/Math.exp(ddt/ filter_len);
124 new_value = prev_value * f + new_value * (1-f);
127 set(new_value, in.time());
130 void set_integral(AltosValue in) {
131 double change = in.integrate();
133 if (change != AltosRecord.MISSING)
134 set(prev_value + change, in.time());
137 void copy(AltosValue old) {
139 set_time = old.set_time;
140 prev_value = old.value;
141 prev_set_time = old.set_time;
142 max_value = old.max_value;
146 value = AltosRecord.MISSING;
147 prev_value = AltosRecord.MISSING;
148 max_value = AltosRecord.MISSING;
157 double v = measured.value();
158 if (v != AltosRecord.MISSING)
160 return computed.value();
163 boolean is_measured() {
164 return measured.value() != AltosRecord.MISSING;
168 double m = measured.max();
170 if (m != AltosRecord.MISSING)
172 return computed.max();
175 double prev_value() {
176 if (measured.value != AltosRecord.MISSING && measured.prev_value != AltosRecord.MISSING)
177 return measured.prev_value;
178 return computed.prev_value;
181 AltosValue altos_value() {
182 if (measured.value() != AltosRecord.MISSING)
188 double c = measured.change();
189 if (c == AltosRecord.MISSING)
190 c = computed.change();
195 double r = measured.rate();
196 if (r == AltosRecord.MISSING)
201 void set_measured(double new_value, double time) {
202 measured.set(new_value, time);
205 void set_computed(double new_value, double time) {
206 computed.set(new_value, time);
209 void set_derivative(AltosValue in) {
210 computed.set_derivative(in);
213 void set_derivative(AltosCValue in) {
214 set_derivative(in.altos_value());
217 void set_integral(AltosValue in) {
218 computed.set_integral(in);
221 void set_integral(AltosCValue in) {
222 set_integral(in.altos_value());
225 void copy(AltosCValue old) {
226 measured.copy(old.measured);
227 computed.copy(old.computed);
231 measured = new AltosValue();
232 computed = new AltosValue();
239 public boolean landed;
240 public boolean ascent; /* going up? */
241 public boolean boost; /* under power */
244 public int device_type;
245 public int config_major;
246 public int config_minor;
247 public int apogee_delay;
248 public int main_deploy;
249 public int flight_log_max;
251 private double pressure_to_altitude(double p) {
252 if (p == AltosRecord.MISSING)
253 return AltosRecord.MISSING;
254 return AltosConvert.pressure_to_altitude(p);
257 private AltosCValue ground_altitude;
259 public double ground_altitude() {
260 return ground_altitude.value();
263 public void set_ground_altitude(double a) {
264 ground_altitude.set_measured(a, time);
267 class AltosGroundPressure extends AltosValue {
268 void set(double p, double time) {
270 ground_altitude.set_computed(pressure_to_altitude(p), time);
274 private AltosGroundPressure ground_pressure;
276 public double ground_pressure() {
277 return ground_pressure.value();
280 public void set_ground_pressure (double pressure) {
281 ground_pressure.set(pressure, time);
284 class AltosAltitude extends AltosCValue {
286 private void set_speed(AltosValue v) {
287 if (!acceleration.is_measured() || !ascent)
288 speed.set_derivative(this);
291 void set_computed(double a, double time) {
292 super.set_computed(a,time);
297 void set_measured(double a, double time) {
298 super.set_measured(a,time);
304 private AltosAltitude altitude;
306 public double altitude() {
307 double a = altitude.value();
308 if (a != AltosRecord.MISSING)
312 return AltosRecord.MISSING;
315 public double max_altitude() {
316 double a = altitude.max();
317 if (a != AltosRecord.MISSING)
319 return AltosRecord.MISSING;
322 public void set_altitude(double new_altitude) {
323 altitude.set_measured(new_altitude, time);
326 class AltosPressure extends AltosValue {
327 void set(double p, double time) {
329 altitude.set_computed(pressure_to_altitude(p), time);
333 private AltosPressure pressure;
335 public double pressure() {
336 return pressure.value();
339 public void set_pressure(double p) {
340 pressure.set(p, time);
343 public double height() {
344 double k = kalman_height.value();
345 if (k != AltosRecord.MISSING)
348 double a = altitude();
349 double g = ground_altitude();
350 if (a != AltosRecord.MISSING && g != AltosRecord.MISSING)
352 return AltosRecord.MISSING;
355 public double max_height() {
356 double k = kalman_height.max();
357 if (k != AltosRecord.MISSING)
360 double a = altitude.max();
361 double g = ground_altitude();
362 if (a != AltosRecord.MISSING && g != AltosRecord.MISSING)
364 return AltosRecord.MISSING;
367 class AltosSpeed extends AltosCValue {
370 acceleration.set_derivative(this);
373 void set_derivative(AltosCValue in) {
374 super.set_derivative(in);
378 void set_computed(double new_value, double time) {
379 super.set_computed(new_value, time);
383 void set_measured(double new_value, double time) {
384 super.set_measured(new_value, time);
389 private AltosSpeed speed;
391 public double speed() {
392 return speed.value();
395 public double max_speed() {
399 class AltosAccel extends AltosCValue {
400 void set_measured(double a, double time) {
401 super.set_measured(a, time);
403 speed.set_integral(this.measured);
407 AltosAccel acceleration;
409 public double acceleration() {
410 return acceleration.value();
413 public double max_acceleration() {
414 return acceleration.max();
417 public AltosValue kalman_height, kalman_speed, kalman_acceleration;
419 public void set_kalman(double height, double speed, double acceleration) {
420 kalman_height.set(height, time);
421 kalman_speed.set(speed, time);
422 kalman_acceleration.set(acceleration, time);
425 public double battery_voltage;
426 public double pyro_voltage;
427 public double temperature;
428 public double apogee_voltage;
429 public double main_voltage;
431 public double ignitor_voltage[];
434 public AltosGPS temp_gps;
435 public boolean temp_gps_clear_sats_pending;
436 public boolean gps_pending;
437 public int gps_sequence;
442 public static final int MIN_PAD_SAMPLES = 10;
445 public int gps_waiting;
446 public boolean gps_ready;
450 public AltosGreatCircle from_pad;
451 public double elevation; /* from pad */
452 public double range; /* total distance */
454 public double gps_height;
456 public double pad_lat, pad_lon, pad_alt;
458 public int speak_tick;
459 public double speak_altitude;
461 public String callsign;
462 public String firmware_version;
464 public double accel_plus_g;
465 public double accel_minus_g;
467 public double ground_accel;
468 public double ground_accel_avg;
470 public int log_format;
472 public AltosMs5607 baro;
474 public AltosRecordCompanion companion;
476 public void set_npad(int npad) {
478 gps_waiting = MIN_PAD_SAMPLES - npad;
479 if (this.gps_waiting < 0)
481 gps_ready = gps_waiting == 0;
489 received_time = System.currentTimeMillis();
490 time = AltosRecord.MISSING;
491 time_change = AltosRecord.MISSING;
492 prev_time = AltosRecord.MISSING;
493 tick = AltosRecord.MISSING;
494 prev_tick = AltosRecord.MISSING;
495 boost_tick = AltosRecord.MISSING;
496 state = AltosLib.ao_flight_invalid;
497 flight = AltosRecord.MISSING;
500 rssi = AltosRecord.MISSING;
502 device_type = AltosRecord.MISSING;
503 config_major = AltosRecord.MISSING;
504 config_minor = AltosRecord.MISSING;
505 apogee_delay = AltosRecord.MISSING;
506 main_deploy = AltosRecord.MISSING;
507 flight_log_max = AltosRecord.MISSING;
509 ground_altitude = new AltosCValue();
510 ground_pressure = new AltosGroundPressure();
511 altitude = new AltosAltitude();
512 pressure = new AltosPressure();
513 speed = new AltosSpeed();
514 acceleration = new AltosAccel();
516 temperature = AltosRecord.MISSING;
517 battery_voltage = AltosRecord.MISSING;
518 pyro_voltage = AltosRecord.MISSING;
519 apogee_voltage = AltosRecord.MISSING;
520 main_voltage = AltosRecord.MISSING;
521 ignitor_voltage = null;
523 kalman_height = new AltosValue();
524 kalman_speed = new AltosValue();
525 kalman_acceleration = new AltosValue();
529 temp_gps_clear_sats_pending = false;
540 elevation = AltosRecord.MISSING;
541 range = AltosRecord.MISSING;
542 gps_height = AltosRecord.MISSING;
544 pad_lat = AltosRecord.MISSING;
545 pad_lon = AltosRecord.MISSING;
546 pad_alt = AltosRecord.MISSING;
548 speak_tick = AltosRecord.MISSING;
549 speak_altitude = AltosRecord.MISSING;
553 accel_plus_g = AltosRecord.MISSING;
554 accel_minus_g = AltosRecord.MISSING;
555 accel = AltosRecord.MISSING;
557 ground_accel = AltosRecord.MISSING;
558 ground_accel_avg = AltosRecord.MISSING;
560 log_format = AltosRecord.MISSING;
561 serial = AltosRecord.MISSING;
567 void copy(AltosState old) {
576 received_time = old.received_time;
580 prev_tick = old.tick;
581 boost_tick = old.boost_tick;
590 device_type = old.device_type;
591 config_major = old.config_major;
592 config_minor = old.config_minor;
593 apogee_delay = old.apogee_delay;
594 main_deploy = old.main_deploy;
595 flight_log_max = old.flight_log_max;
599 ground_altitude.copy(old.ground_altitude);
600 altitude.copy(old.altitude);
601 pressure.copy(old.pressure);
602 speed.copy(old.speed);
603 acceleration.copy(old.acceleration);
605 battery_voltage = old.battery_voltage;
606 pyro_voltage = old.pyro_voltage;
607 temperature = old.temperature;
608 apogee_voltage = old.apogee_voltage;
609 main_voltage = old.main_voltage;
610 ignitor_voltage = old.ignitor_voltage;
612 kalman_height.copy(old.kalman_height);
613 kalman_speed.copy(old.kalman_speed);
614 kalman_acceleration.copy(old.kalman_acceleration);
617 gps = old.gps.clone();
620 if (old.temp_gps != null)
621 temp_gps = old.temp_gps.clone();
624 temp_gps_clear_sats_pending = old.temp_gps_clear_sats_pending;
625 gps_sequence = old.gps_sequence;
626 gps_pending = old.gps_pending;
629 imu = old.imu.clone();
634 mag = old.mag.clone();
639 gps_waiting = old.gps_waiting;
640 gps_ready = old.gps_ready;
643 if (old.from_pad != null)
644 from_pad = old.from_pad.clone();
648 elevation = old.elevation;
651 gps_height = old.gps_height;
652 pad_lat = old.pad_lat;
653 pad_lon = old.pad_lon;
654 pad_alt = old.pad_alt;
656 speak_tick = old.speak_tick;
657 speak_altitude = old.speak_altitude;
659 callsign = old.callsign;
661 accel_plus_g = old.accel_plus_g;
662 accel_minus_g = old.accel_minus_g;
664 ground_accel = old.ground_accel;
665 ground_accel_avg = old.ground_accel_avg;
667 log_format = old.log_format;
671 companion = old.companion;
685 if (gps.locked && gps.nsat >= 4) {
686 /* Track consecutive 'good' gps reports, waiting for 10 of them */
687 if (state == AltosLib.ao_flight_pad) {
689 if (pad_lat != AltosRecord.MISSING) {
690 pad_lat = (pad_lat * 31 + gps.lat) / 32;
691 pad_lon = (pad_lon * 31 + gps.lon) / 32;
692 pad_alt = (pad_alt * 31 + gps.alt) / 32;
695 if (pad_lat == AltosRecord.MISSING) {
701 if (gps.lat != 0 && gps.lon != 0 &&
702 pad_lat != AltosRecord.MISSING &&
703 pad_lon != AltosRecord.MISSING)
707 if (h == AltosRecord.MISSING)
709 from_pad = new AltosGreatCircle(pad_lat, pad_lon, 0, gps.lat, gps.lon, h);
710 elevation = from_pad.elevation;
711 range = from_pad.range;
712 gps_height = gps.alt - pad_alt;
716 public void set_tick(int new_tick) {
717 if (new_tick != AltosRecord.MISSING) {
718 if (prev_tick != AltosRecord.MISSING) {
719 while (new_tick < prev_tick - 32767) {
728 public void set_boost_tick(int boost_tick) {
729 if (boost_tick != AltosRecord.MISSING)
730 this.boost_tick = boost_tick;
733 public String state_name() {
734 return AltosLib.state_name(state);
737 public void set_state(int state) {
738 if (state != AltosLib.ao_flight_invalid) {
740 ascent = (AltosLib.ao_flight_boost <= state &&
741 state <= AltosLib.ao_flight_coast);
742 boost = (AltosLib.ao_flight_boost == state);
747 public void set_device_type(int device_type) {
748 this.device_type = device_type;
751 public void set_config(int major, int minor, int apogee_delay, int main_deploy, int flight_log_max) {
752 config_major = major;
753 config_minor = minor;
754 this.apogee_delay = apogee_delay;
755 this.main_deploy = main_deploy;
756 this.flight_log_max = flight_log_max;
759 public void set_callsign(String callsign) {
760 this.callsign = callsign;
763 public void set_firmware_version(String version) {
764 firmware_version = version;
767 public void set_flight(int flight) {
769 /* When the flight changes, reset the state */
770 if (flight != AltosRecord.MISSING) {
771 if (this.flight != AltosRecord.MISSING &&
772 this.flight != flight) {
775 this.flight = flight;
779 public void set_serial(int serial) {
780 /* When the serial changes, reset the state */
781 if (serial != AltosRecord.MISSING) {
782 if (this.serial != AltosRecord.MISSING &&
783 this.serial != serial) {
786 this.serial = serial;
791 if (rssi == AltosRecord.MISSING)
796 public void set_rssi(int rssi, int status) {
797 if (rssi != AltosRecord.MISSING) {
799 this.status = status;
803 public void set_received_time(long ms) {
807 public void set_gps(AltosGPS gps, int sequence) {
809 this.gps = gps.clone();
810 gps_sequence = sequence;
816 public void make_baro() {
818 baro = new AltosMs5607();
821 public void set_ms5607(int pres, int temp) {
823 baro.set(pres, temp);
825 set_pressure(baro.pa);
826 set_temperature(baro.cc / 100.0);
830 public void make_companion (int nchannels) {
831 if (companion == null)
832 companion = new AltosRecordCompanion(nchannels);
835 public void set_companion(AltosRecordCompanion companion) {
836 this.companion = companion;
839 void update_accel() {
840 double ground = ground_accel;
842 if (ground == AltosRecord.MISSING)
843 ground = ground_accel_avg;
844 if (accel == AltosRecord.MISSING)
846 if (ground == AltosRecord.MISSING)
848 if (accel_plus_g == AltosRecord.MISSING)
850 if (accel_minus_g == AltosRecord.MISSING)
853 double counts_per_g = (accel_minus_g - accel_plus_g) / 2.0;
854 double counts_per_mss = counts_per_g / 9.80665;
855 acceleration.set_computed((ground - accel) / counts_per_mss, time);
858 public void set_accel_g(double accel_plus_g, double accel_minus_g) {
859 if (accel_plus_g != AltosRecord.MISSING) {
860 this.accel_plus_g = accel_plus_g;
861 this.accel_minus_g = accel_minus_g;
866 public void set_ground_accel(double ground_accel) {
867 if (ground_accel != AltosRecord.MISSING) {
868 this.ground_accel = ground_accel;
873 public void set_accel(double accel) {
874 if (accel != AltosRecord.MISSING) {
876 if (state == AltosLib.ao_flight_pad) {
877 if (ground_accel_avg == AltosRecord.MISSING)
878 ground_accel_avg = accel;
880 ground_accel_avg = (ground_accel_avg * 7 + accel) / 8;
886 public void set_temperature(double temperature) {
887 if (temperature != AltosRecord.MISSING) {
888 this.temperature = temperature;
893 public void set_battery_voltage(double battery_voltage) {
894 if (battery_voltage != AltosRecord.MISSING) {
895 this.battery_voltage = battery_voltage;
900 public void set_pyro_voltage(double pyro_voltage) {
901 if (pyro_voltage != AltosRecord.MISSING) {
902 this.pyro_voltage = pyro_voltage;
907 public void set_apogee_voltage(double apogee_voltage) {
908 if (apogee_voltage != AltosRecord.MISSING) {
909 this.apogee_voltage = apogee_voltage;
914 public void set_main_voltage(double main_voltage) {
915 if (main_voltage != AltosRecord.MISSING) {
916 this.main_voltage = main_voltage;
921 public void set_ignitor_voltage(double[] voltage) {
922 this.ignitor_voltage = voltage;
925 public double time_since_boost() {
926 if (tick == AltosRecord.MISSING)
929 if (boost_tick != AltosRecord.MISSING) {
930 return (tick - boost_tick) / 100.0;
935 public boolean valid() {
936 return tick != AltosRecord.MISSING && serial != AltosRecord.MISSING;
939 public AltosGPS make_temp_gps(boolean sats) {
940 if (temp_gps == null) {
941 temp_gps = new AltosGPS(gps);
945 temp_gps_clear_sats_pending = true;
946 else if (temp_gps_clear_sats_pending) {
947 temp_gps.cc_gps_sat = null;
948 temp_gps_clear_sats_pending = false;
953 public void set_temp_gps() {
954 set_gps(temp_gps, gps_sequence + 1);
959 public AltosState clone() {
960 AltosState s = new AltosState();
966 public void init (AltosRecord cur, AltosState prev_state) {
968 System.out.printf ("init\n");
970 cur = new AltosRecord();
974 /* Discard previous state if it was for a different board */
975 if (prev_state != null && prev_state.serial != cur.serial)
980 set_ground_altitude(cur.ground_altitude());
981 set_altitude(cur.altitude());
983 set_kalman(cur.kalman_height, cur.kalman_speed, cur.kalman_acceleration);
985 received_time = System.currentTimeMillis();
987 set_temperature(cur.temperature());
988 set_apogee_voltage(cur.drogue_voltage());
989 set_main_voltage(cur.main_voltage());
990 set_battery_voltage(cur.battery_voltage());
992 set_pressure(cur.pressure());
995 set_state(cur.state);
997 set_accel_g (cur.accel_minus_g, cur.accel_plus_g);
998 set_ground_accel(cur.ground_accel);
999 set_accel (cur.accel);
1001 if (cur.gps_sequence != gps_sequence)
1002 set_gps(cur.gps, cur.gps_sequence);
1006 public AltosState(AltosRecord cur) {
1010 public AltosState (AltosRecord cur, AltosState prev) {
1015 public AltosState () {