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; either version 2 of the License, or
7 * (at your option) any later version.
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.
14 * You should have received a copy of the GNU General Public License along
15 * with this program; if not, write to the Free Software Foundation, Inc.,
16 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
20 * Track flight state from telemetry or eeprom data stream
23 package org.altusmetrum.altoslib_11;
27 public class AltosState extends AltosDataListener {
29 public static final int set_position = 1;
30 public static final int set_gps = 2;
31 public static final int set_data = 4;
35 static final double filter_len = 2.0;
36 static final double ascent_filter_len = 0.5;
37 static final double descent_filter_len = 5.0;
41 public long received_time;
49 private double max_value;
50 private double set_time;
51 private double prev_set_time;
53 boolean can_max() { return true; }
55 void set(double new_value, double time) {
56 if (new_value != AltosLib.MISSING) {
58 if (can_max() && (max_value == AltosLib.MISSING || value > max_value))
64 void set_filtered(double new_value, double time) {
65 if (prev_value != AltosLib.MISSING) {
66 double f = 1/Math.exp((time - prev_set_time) / filter_len);
67 new_value = f * new_value + (1-f) * prev_value;
85 if (value != AltosLib.MISSING && prev_value != AltosLib.MISSING)
86 return value - prev_value;
87 return AltosLib.MISSING;
92 double t = set_time - prev_set_time;
94 if (c != AltosLib.MISSING && t != 0)
96 return AltosLib.MISSING;
100 if (value == AltosLib.MISSING)
101 return AltosLib.MISSING;
102 if (prev_value == AltosLib.MISSING)
103 return AltosLib.MISSING;
105 return (value + prev_value) / 2 * (set_time - prev_set_time);
112 void set_derivative(AltosValue in) {
113 double n = in.rate();
115 if (n == AltosLib.MISSING)
118 double p = prev_value;
119 double pt = prev_set_time;
121 if (p == AltosLib.MISSING) {
123 pt = in.time() - 0.01;
126 /* Clip changes to reduce noise */
127 double ddt = in.time() - pt;
128 double ddv = (n - p) / ddt;
130 final double max = 100000;
133 if (Math.abs(ddv) > max) {
143 filter_len = ascent_filter_len;
145 filter_len = descent_filter_len;
147 double f = 1/Math.exp(ddt/ filter_len);
148 n = p * f + n * (1-f);
153 void set_integral(AltosValue in) {
154 double change = in.integrate();
156 if (change != AltosLib.MISSING) {
157 double prev = prev_value;
158 if (prev == AltosLib.MISSING)
160 set(prev + change, in.time());
164 void copy(AltosValue old) {
166 set_time = old.set_time;
167 prev_value = old.value;
168 prev_set_time = old.set_time;
169 max_value = old.max_value;
172 void finish_update() {
174 prev_set_time = set_time;
178 value = AltosLib.MISSING;
179 prev_value = AltosLib.MISSING;
180 max_value = AltosLib.MISSING;
187 class AltosIValue extends AltosValue {
197 public AltosIValue measured;
198 public AltosIValue computed;
200 boolean can_max() { return true; }
202 boolean c_can_max() { return can_max(); }
205 double v = measured.value();
206 if (v != AltosLib.MISSING)
208 return computed.value();
211 boolean is_measured() {
212 return measured.value() != AltosLib.MISSING;
216 double m = measured.max();
218 if (m != AltosLib.MISSING)
220 return computed.max();
223 double prev_value() {
224 if (measured.value != AltosLib.MISSING && measured.prev_value != AltosLib.MISSING)
225 return measured.prev_value;
226 return computed.prev_value;
229 AltosValue altos_value() {
230 if (measured.value() != AltosLib.MISSING)
236 double c = measured.change();
237 if (c == AltosLib.MISSING)
238 c = computed.change();
243 double r = measured.rate();
244 if (r == AltosLib.MISSING)
249 void set_measured(double new_value, double time) {
250 measured.set(new_value, time);
253 void set_computed(double new_value, double time) {
254 computed.set(new_value, time);
257 void set_derivative(AltosValue in) {
258 computed.set_derivative(in);
261 void set_derivative(AltosCValue in) {
262 set_derivative(in.altos_value());
265 void set_integral(AltosValue in) {
266 computed.set_integral(in);
269 void set_integral(AltosCValue in) {
270 set_integral(in.altos_value());
273 void copy(AltosCValue old) {
274 measured.copy(old.measured);
275 computed.copy(old.computed);
278 void finish_update() {
279 measured.finish_update();
280 computed.finish_update();
283 public AltosCValue() {
284 measured = new AltosIValue();
285 computed = new AltosIValue();
289 public boolean landed;
290 public boolean ascent; /* going up? */
291 public boolean boost; /* under power */
293 private double pressure_to_altitude(double p) {
294 if (p == AltosLib.MISSING)
295 return AltosLib.MISSING;
296 return AltosConvert.pressure_to_altitude(p);
299 private AltosCValue ground_altitude;
301 public double ground_altitude() {
302 return ground_altitude.value();
305 public void set_ground_altitude(double a) {
306 ground_altitude.set_measured(a, time);
309 class AltosGpsGroundAltitude extends AltosValue {
310 void set(double a, double t) {
313 gps_altitude.set_gps_height();
316 void set_filtered(double a, double t) {
317 super.set_filtered(a, t);
319 gps_altitude.set_gps_height();
322 AltosGpsGroundAltitude() {
327 private AltosGpsGroundAltitude gps_ground_altitude;
329 public double gps_ground_altitude() {
330 return gps_ground_altitude.value();
333 public void set_gps_ground_altitude(double a) {
334 gps_ground_altitude.set(a, time);
337 class AltosGroundPressure extends AltosCValue {
338 void set_filtered(double p, double time) {
339 computed.set_filtered(p, time);
341 ground_altitude.set_computed(pressure_to_altitude(computed.value()), time);
344 void set_measured(double p, double time) {
345 super.set_measured(p, time);
346 ground_altitude.set_computed(pressure_to_altitude(p), time);
349 AltosGroundPressure () {
354 private AltosGroundPressure ground_pressure;
356 public double ground_pressure() {
357 return ground_pressure.value();
360 public void set_ground_pressure (double pressure) {
361 ground_pressure.set_measured(pressure, time);
364 class AltosAltitude extends AltosCValue {
366 private void set_speed(AltosValue v) {
367 if (!acceleration.is_measured() || !ascent)
368 speed.set_derivative(this);
371 void set_computed(double a, double time) {
372 super.set_computed(a,time);
377 void set_measured(double a, double time) {
378 super.set_measured(a,time);
388 private AltosAltitude altitude;
390 class AltosGpsAltitude extends AltosValue {
392 private void set_gps_height() {
394 double g = gps_ground_altitude.value();
396 if (a != AltosLib.MISSING && g != AltosLib.MISSING)
399 gps_height = AltosLib.MISSING;
402 void set(double a, double t) {
412 private AltosGpsAltitude gps_altitude;
414 private AltosValue gps_ground_speed;
415 private AltosValue gps_ascent_rate;
416 private AltosValue gps_course;
417 private AltosValue gps_speed;
419 public double altitude() {
420 double a = altitude.value();
421 if (a != AltosLib.MISSING)
423 return gps_altitude.value();
426 public double max_altitude() {
427 double a = altitude.max();
428 if (a != AltosLib.MISSING)
430 return gps_altitude.max();
433 public void set_altitude(double new_altitude) {
434 double old_altitude = altitude.value();
435 if (old_altitude != AltosLib.MISSING) {
436 while (old_altitude - new_altitude > 32000)
437 new_altitude += 65536.0;
439 altitude.set_measured(new_altitude, time);
442 public double gps_altitude() {
443 return gps_altitude.value();
446 public double max_gps_altitude() {
447 return gps_altitude.max();
450 public void set_gps_altitude(double new_gps_altitude) {
451 gps_altitude.set(new_gps_altitude, time);
454 public double gps_ground_speed() {
455 return gps_ground_speed.value();
458 public double max_gps_ground_speed() {
459 return gps_ground_speed.max();
462 public double gps_ascent_rate() {
463 return gps_ascent_rate.value();
466 public double max_gps_ascent_rate() {
467 return gps_ascent_rate.max();
470 public double gps_course() {
471 return gps_course.value();
474 public double gps_speed() {
475 return gps_speed.value();
478 public double max_gps_speed() {
479 return gps_speed.max();
482 class AltosPressure extends AltosValue {
483 void set(double p, double time) {
485 if (state == AltosLib.ao_flight_pad)
486 ground_pressure.set_filtered(p, time);
487 double a = pressure_to_altitude(p);
488 altitude.set_computed(a, time);
496 private AltosPressure pressure;
498 public double pressure() {
499 return pressure.value();
502 public void set_pressure(double p) {
503 pressure.set(p, time);
506 public void set_thrust(double N) {
509 public double baro_height() {
510 double a = altitude();
511 double g = ground_altitude();
512 if (a != AltosLib.MISSING && g != AltosLib.MISSING)
514 return AltosLib.MISSING;
517 public double height() {
518 double k = kalman_height.value();
519 if (k != AltosLib.MISSING)
522 double b = baro_height();
523 if (b != AltosLib.MISSING)
529 public double max_height() {
530 double k = kalman_height.max();
531 if (k != AltosLib.MISSING)
534 double a = altitude.max();
535 double g = ground_altitude();
536 if (a != AltosLib.MISSING && g != AltosLib.MISSING)
538 return max_gps_height();
541 public double gps_height() {
542 double a = gps_altitude();
543 double g = gps_ground_altitude();
545 if (a != AltosLib.MISSING && g != AltosLib.MISSING)
547 return AltosLib.MISSING;
550 public double max_gps_height() {
551 double a = gps_altitude.max();
552 double g = gps_ground_altitude();
554 if (a != AltosLib.MISSING && g != AltosLib.MISSING)
556 return AltosLib.MISSING;
559 class AltosSpeed extends AltosCValue {
562 return state < AltosLib.ao_flight_fast || state == AltosLib.ao_flight_stateless;
566 acceleration.set_derivative(this);
569 void set_derivative(AltosCValue in) {
570 super.set_derivative(in);
574 void set_computed(double new_value, double time) {
575 super.set_computed(new_value, time);
579 void set_measured(double new_value, double time) {
580 super.set_measured(new_value, time);
589 private AltosSpeed speed;
591 public double speed() {
592 double v = kalman_speed.value();
593 if (v != AltosLib.MISSING)
596 if (v != AltosLib.MISSING)
599 if (v != AltosLib.MISSING)
601 return AltosLib.MISSING;
604 public double max_speed() {
605 double v = kalman_speed.max();
606 if (v != AltosLib.MISSING)
609 if (v != AltosLib.MISSING)
612 if (v != AltosLib.MISSING)
614 return AltosLib.MISSING;
617 class AltosAccel extends AltosCValue {
620 return state < AltosLib.ao_flight_fast || state == AltosLib.ao_flight_stateless;
623 void set_measured(double a, double time) {
624 super.set_measured(a, time);
626 speed.set_integral(this.measured);
634 AltosAccel acceleration;
636 public double acceleration() {
637 return acceleration.value();
640 public double max_acceleration() {
641 return acceleration.max();
644 public AltosCValue orient;
646 public void set_orient(double new_orient) {
647 orient.set_measured(new_orient, time);
650 public double orient() {
651 return orient.value();
654 public double max_orient() {
658 public AltosValue kalman_height, kalman_speed, kalman_acceleration;
660 public void set_kalman(double height, double speed, double acceleration) {
661 double old_height = kalman_height.value();
662 if (old_height != AltosLib.MISSING) {
663 while (old_height - height > 32000)
666 kalman_height.set(height, time);
667 kalman_speed.set(speed, time);
668 kalman_acceleration.set(acceleration, time);
671 public double battery_voltage;
672 public double pyro_voltage;
673 public double temperature;
674 public double apogee_voltage;
675 public double main_voltage;
677 public double igniter_voltage[];
680 public boolean gps_pending;
685 public static final int MIN_PAD_SAMPLES = 10;
688 public int gps_waiting;
689 public boolean gps_ready;
693 public AltosGreatCircle from_pad;
694 public double elevation; /* from pad */
695 public double range; /* total distance */
697 public double gps_height;
699 public double pad_lat, pad_lon, pad_alt;
701 public int speak_tick;
702 public double speak_altitude;
704 public double ground_accel;
706 public AltosCompanion companion;
708 public int pyro_fired;
710 public void set_npad(int npad) {
712 gps_waiting = MIN_PAD_SAMPLES - npad;
713 if (this.gps_waiting < 0)
715 gps_ready = gps_waiting == 0;
721 received_time = System.currentTimeMillis();
722 time = AltosLib.MISSING;
723 state = AltosLib.ao_flight_invalid;
726 rssi = AltosLib.MISSING;
729 ground_altitude = new AltosCValue();
730 ground_pressure = new AltosGroundPressure();
731 altitude = new AltosAltitude();
732 pressure = new AltosPressure();
733 speed = new AltosSpeed();
734 acceleration = new AltosAccel();
735 orient = new AltosCValue();
737 temperature = AltosLib.MISSING;
738 battery_voltage = AltosLib.MISSING;
739 pyro_voltage = AltosLib.MISSING;
740 apogee_voltage = AltosLib.MISSING;
741 main_voltage = AltosLib.MISSING;
742 igniter_voltage = null;
744 kalman_height = new AltosValue();
745 kalman_speed = new AltosValue();
746 kalman_acceleration = new AltosValue();
752 last_imu_time = AltosLib.MISSING;
754 ground_rotation = null;
758 accel_ground_along = AltosLib.MISSING;
759 accel_ground_across = AltosLib.MISSING;
760 accel_ground_through = AltosLib.MISSING;
762 pad_orientation = AltosLib.MISSING;
768 elevation = AltosLib.MISSING;
769 range = AltosLib.MISSING;
770 gps_height = AltosLib.MISSING;
772 pad_lat = AltosLib.MISSING;
773 pad_lon = AltosLib.MISSING;
774 pad_alt = AltosLib.MISSING;
776 gps_altitude = new AltosGpsAltitude();
777 gps_ground_altitude = new AltosGpsGroundAltitude();
778 gps_ground_speed = new AltosValue();
779 gps_speed = new AltosValue();
780 gps_ascent_rate = new AltosValue();
781 gps_course = new AltosValue();
783 speak_tick = AltosLib.MISSING;
784 speak_altitude = AltosLib.MISSING;
786 ground_accel = AltosLib.MISSING;
793 void finish_update() {
794 ground_altitude.finish_update();
795 altitude.finish_update();
796 pressure.finish_update();
797 speed.finish_update();
798 acceleration.finish_update();
799 orient.finish_update();
801 kalman_height.finish_update();
802 kalman_speed.finish_update();
803 kalman_acceleration.finish_update();
810 elevation = AltosLib.MISSING;
811 range = AltosLib.MISSING;
816 if (gps.locked && gps.nsat >= 4) {
817 /* Track consecutive 'good' gps reports, waiting for 10 of them */
818 if (state == AltosLib.ao_flight_pad || state == AltosLib.ao_flight_stateless) {
820 if (pad_lat != AltosLib.MISSING && (npad < 10 || state == AltosLib.ao_flight_pad)) {
821 pad_lat = (pad_lat * 31 + gps.lat) / 32;
822 pad_lon = (pad_lon * 31 + gps.lon) / 32;
823 gps_ground_altitude.set_filtered(gps.alt, time);
826 if (pad_lat == AltosLib.MISSING) {
829 gps_ground_altitude.set(gps.alt, time);
831 gps_altitude.set(gps.alt, time);
832 if (gps.climb_rate != AltosLib.MISSING)
833 gps_ascent_rate.set(gps.climb_rate, time);
834 if (gps.ground_speed != AltosLib.MISSING)
835 gps_ground_speed.set(gps.ground_speed, time);
836 if (gps.climb_rate != AltosLib.MISSING && gps.ground_speed != AltosLib.MISSING)
837 gps_speed.set(Math.sqrt(gps.ground_speed * gps.ground_speed +
838 gps.climb_rate * gps.climb_rate), time);
839 if (gps.course != AltosLib.MISSING)
840 gps_course.set(gps.course, time);
842 if (gps.lat != 0 && gps.lon != 0 &&
843 pad_lat != AltosLib.MISSING &&
844 pad_lon != AltosLib.MISSING)
848 if (h == AltosLib.MISSING)
850 from_pad = new AltosGreatCircle(pad_lat, pad_lon, 0, gps.lat, gps.lon, h);
851 elevation = from_pad.elevation;
852 range = from_pad.range;
856 public String state_name() {
857 return AltosLib.state_name(state);
860 public void set_state(int state) {
861 if (state != AltosLib.ao_flight_invalid) {
863 ascent = (AltosLib.ao_flight_boost <= state &&
864 state <= AltosLib.ao_flight_coast);
865 boost = (AltosLib.ao_flight_boost == state);
873 private void re_init() {
878 if (rssi == AltosLib.MISSING)
883 public void set_rssi(int rssi, int status) {
884 if (rssi != AltosLib.MISSING) {
886 this.status = status;
890 public void set_received_time(long ms) {
894 public void set_gps(AltosGPS gps) {
903 public AltosRotation rotation;
904 public AltosRotation ground_rotation;
906 public int pad_orientation;
908 public double accel_ground_along, accel_ground_across, accel_ground_through;
910 void update_pad_rotation() {
911 if (pad_orientation != AltosLib.MISSING && accel_ground_along != AltosLib.MISSING) {
912 rotation = new AltosRotation(AltosIMU.convert_accel(accel_ground_across - cal_data.accel_zero_across),
913 AltosIMU.convert_accel(accel_ground_through - cal_data.accel_zero_through),
914 AltosIMU.convert_accel(accel_ground_along - cal_data.accel_zero_along),
916 ground_rotation = rotation;
917 orient.set_computed(rotation.tilt(), time);
921 public void set_accel_ground(double ground_along, double ground_across, double ground_through) {
922 accel_ground_along = ground_along;
923 accel_ground_across = ground_across;
924 accel_ground_through = ground_through;
925 update_pad_rotation();
928 public void set_pad_orientation(int pad_orientation) {
929 this.pad_orientation = pad_orientation;
930 update_pad_rotation();
933 public double last_imu_time;
935 private double radians(double degrees) {
936 if (degrees == AltosLib.MISSING)
937 return AltosLib.MISSING;
938 return degrees * Math.PI / 180.0;
941 private void update_orient() {
942 if (last_imu_time != AltosLib.MISSING) {
943 double t = time - last_imu_time;
945 double pitch = radians(gyro_pitch());
946 double yaw = radians(gyro_yaw());
947 double roll = radians(gyro_roll());
949 if (t > 0 & pitch != AltosLib.MISSING && rotation != null) {
950 rotation.rotate(t, pitch, yaw, roll);
951 orient.set_computed(rotation.tilt(), time);
954 last_imu_time = time;
957 private double gyro_roll, gyro_pitch, gyro_yaw;
959 public void set_gyro(double roll, double pitch, double yaw) {
966 private double accel_along, accel_across, accel_through;
968 public void set_accel(double along, double across, double through) {
970 accel_across = across;
971 accel_through = through;
975 public double accel_along() {
979 public double accel_across() {
983 public double accel_through() {
984 return accel_through;
987 public double gyro_roll() {
991 public double gyro_pitch() {
995 public double gyro_yaw() {
999 private double mag_along, mag_across, mag_through;
1001 public void set_mag(double along, double across, double through) {
1003 mag_across = across;
1004 mag_through = through;
1007 public double mag_along() {
1011 public double mag_across() {
1013 return AltosMag.convert_gauss(mag.across);
1014 return AltosLib.MISSING;
1017 public double mag_through() {
1019 return AltosMag.convert_gauss(mag.through);
1020 return AltosLib.MISSING;
1023 public void set_companion(AltosCompanion companion) {
1024 this.companion = companion;
1027 public void set_acceleration(double acceleration) {
1028 if (acceleration != AltosLib.MISSING) {
1029 this.acceleration.set_measured(acceleration, time);
1034 public void set_temperature(double temperature) {
1035 if (temperature != AltosLib.MISSING) {
1036 this.temperature = temperature;
1041 public void set_battery_voltage(double battery_voltage) {
1042 if (battery_voltage != AltosLib.MISSING) {
1043 this.battery_voltage = battery_voltage;
1048 public void set_pyro_voltage(double pyro_voltage) {
1049 if (pyro_voltage != AltosLib.MISSING) {
1050 this.pyro_voltage = pyro_voltage;
1055 public void set_apogee_voltage(double apogee_voltage) {
1056 if (apogee_voltage != AltosLib.MISSING) {
1057 this.apogee_voltage = apogee_voltage;
1062 public void set_main_voltage(double main_voltage) {
1063 if (main_voltage != AltosLib.MISSING) {
1064 this.main_voltage = main_voltage;
1069 public void set_igniter_voltage(double[] voltage) {
1070 this.igniter_voltage = voltage;
1073 public void set_pyro_fired(int fired) {
1074 this.pyro_fired = fired;
1077 public AltosState() {
1082 public AltosState (AltosCalData cal_data) {
1084 if (cal_data == null)