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_11;
26 public class AltosState implements Cloneable {
28 public static final int set_position = 1;
29 public static final int set_gps = 2;
30 public static final int set_data = 4;
34 static final double filter_len = 2.0;
35 static final double ascent_filter_len = 0.5;
36 static final double descent_filter_len = 5.0;
40 public long received_time;
43 public double prev_time;
44 public double time_change;
46 private int prev_tick;
47 public int boost_tick;
52 private double max_value;
53 private double set_time;
54 private double prev_set_time;
56 boolean can_max() { return true; }
58 void set(double new_value, double time) {
59 if (new_value != AltosLib.MISSING) {
61 if (can_max() && (max_value == AltosLib.MISSING || value > max_value))
67 void set_filtered(double new_value, double time) {
68 if (prev_value != AltosLib.MISSING) {
69 double f = 1/Math.exp((time - prev_set_time) / filter_len);
70 new_value = f * new_value + (1-f) * prev_value;
88 if (value != AltosLib.MISSING && prev_value != AltosLib.MISSING)
89 return value - prev_value;
90 return AltosLib.MISSING;
95 double t = set_time - prev_set_time;
97 if (c != AltosLib.MISSING && t != 0)
99 return AltosLib.MISSING;
103 if (value == AltosLib.MISSING)
104 return AltosLib.MISSING;
105 if (prev_value == AltosLib.MISSING)
106 return AltosLib.MISSING;
108 return (value + prev_value) / 2 * (set_time - prev_set_time);
115 void set_derivative(AltosValue in) {
116 double n = in.rate();
118 if (n == AltosLib.MISSING)
121 double p = prev_value;
122 double pt = prev_set_time;
124 if (p == AltosLib.MISSING) {
126 pt = in.time() - 0.01;
129 /* Clip changes to reduce noise */
130 double ddt = in.time() - pt;
131 double ddv = (n - p) / ddt;
133 final double max = 100000;
136 if (Math.abs(ddv) > max) {
146 filter_len = ascent_filter_len;
148 filter_len = descent_filter_len;
150 double f = 1/Math.exp(ddt/ filter_len);
151 n = p * f + n * (1-f);
156 void set_integral(AltosValue in) {
157 double change = in.integrate();
159 if (change != AltosLib.MISSING) {
160 double prev = prev_value;
161 if (prev == AltosLib.MISSING)
163 set(prev + change, in.time());
167 void copy(AltosValue old) {
169 set_time = old.set_time;
170 prev_value = old.value;
171 prev_set_time = old.set_time;
172 max_value = old.max_value;
175 void finish_update() {
177 prev_set_time = set_time;
181 value = AltosLib.MISSING;
182 prev_value = AltosLib.MISSING;
183 max_value = AltosLib.MISSING;
190 class AltosIValue extends AltosValue {
200 public AltosIValue measured;
201 public AltosIValue computed;
203 boolean can_max() { return true; }
205 boolean c_can_max() { return can_max(); }
208 double v = measured.value();
209 if (v != AltosLib.MISSING)
211 return computed.value();
214 boolean is_measured() {
215 return measured.value() != AltosLib.MISSING;
219 double m = measured.max();
221 if (m != AltosLib.MISSING)
223 return computed.max();
226 double prev_value() {
227 if (measured.value != AltosLib.MISSING && measured.prev_value != AltosLib.MISSING)
228 return measured.prev_value;
229 return computed.prev_value;
232 AltosValue altos_value() {
233 if (measured.value() != AltosLib.MISSING)
239 double c = measured.change();
240 if (c == AltosLib.MISSING)
241 c = computed.change();
246 double r = measured.rate();
247 if (r == AltosLib.MISSING)
252 void set_measured(double new_value, double time) {
253 measured.set(new_value, time);
256 void set_computed(double new_value, double time) {
257 computed.set(new_value, time);
260 void set_derivative(AltosValue in) {
261 computed.set_derivative(in);
264 void set_derivative(AltosCValue in) {
265 set_derivative(in.altos_value());
268 void set_integral(AltosValue in) {
269 computed.set_integral(in);
272 void set_integral(AltosCValue in) {
273 set_integral(in.altos_value());
276 void copy(AltosCValue old) {
277 measured.copy(old.measured);
278 computed.copy(old.computed);
281 void finish_update() {
282 measured.finish_update();
283 computed.finish_update();
286 public AltosCValue() {
287 measured = new AltosIValue();
288 computed = new AltosIValue();
295 public int altitude_32;
296 public int receiver_serial;
297 public boolean landed;
298 public boolean ascent; /* going up? */
299 public boolean boost; /* under power */
302 public int device_type;
303 public int config_major;
304 public int config_minor;
305 public int apogee_delay;
306 public int main_deploy;
307 public int flight_log_max;
309 private double pressure_to_altitude(double p) {
310 if (p == AltosLib.MISSING)
311 return AltosLib.MISSING;
312 return AltosConvert.pressure_to_altitude(p);
315 private AltosCValue ground_altitude;
317 public double ground_altitude() {
318 return ground_altitude.value();
321 public void set_ground_altitude(double a) {
322 ground_altitude.set_measured(a, time);
325 class AltosGpsGroundAltitude extends AltosValue {
326 void set(double a, double t) {
329 gps_altitude.set_gps_height();
332 void set_filtered(double a, double t) {
333 super.set_filtered(a, t);
335 gps_altitude.set_gps_height();
338 AltosGpsGroundAltitude() {
343 private AltosGpsGroundAltitude gps_ground_altitude;
345 public double gps_ground_altitude() {
346 return gps_ground_altitude.value();
349 public void set_gps_ground_altitude(double a) {
350 gps_ground_altitude.set(a, time);
353 class AltosGroundPressure extends AltosCValue {
354 void set_filtered(double p, double time) {
355 computed.set_filtered(p, time);
357 ground_altitude.set_computed(pressure_to_altitude(computed.value()), time);
360 void set_measured(double p, double time) {
361 super.set_measured(p, time);
362 ground_altitude.set_computed(pressure_to_altitude(p), time);
365 AltosGroundPressure () {
370 private AltosGroundPressure ground_pressure;
372 public double ground_pressure() {
373 return ground_pressure.value();
376 public void set_ground_pressure (double pressure) {
377 ground_pressure.set_measured(pressure, time);
380 class AltosAltitude extends AltosCValue {
382 private void set_speed(AltosValue v) {
383 if (!acceleration.is_measured() || !ascent)
384 speed.set_derivative(this);
387 void set_computed(double a, double time) {
388 super.set_computed(a,time);
393 void set_measured(double a, double time) {
394 super.set_measured(a,time);
404 private AltosAltitude altitude;
406 class AltosGpsAltitude extends AltosValue {
408 private void set_gps_height() {
410 double g = gps_ground_altitude.value();
412 if (a != AltosLib.MISSING && g != AltosLib.MISSING)
415 gps_height = AltosLib.MISSING;
418 void set(double a, double t) {
428 private AltosGpsAltitude gps_altitude;
430 private AltosValue gps_ground_speed;
431 private AltosValue gps_ascent_rate;
432 private AltosValue gps_course;
433 private AltosValue gps_speed;
435 public double altitude() {
436 double a = altitude.value();
437 if (a != AltosLib.MISSING)
439 return gps_altitude.value();
442 public double max_altitude() {
443 double a = altitude.max();
444 if (a != AltosLib.MISSING)
446 return gps_altitude.max();
449 public void set_altitude(double new_altitude) {
450 altitude.set_measured(new_altitude, time);
453 public double gps_altitude() {
454 return gps_altitude.value();
457 public double max_gps_altitude() {
458 return gps_altitude.max();
461 public void set_gps_altitude(double new_gps_altitude) {
462 gps_altitude.set(new_gps_altitude, time);
465 public double gps_ground_speed() {
466 return gps_ground_speed.value();
469 public double max_gps_ground_speed() {
470 return gps_ground_speed.max();
473 public double gps_ascent_rate() {
474 return gps_ascent_rate.value();
477 public double max_gps_ascent_rate() {
478 return gps_ascent_rate.max();
481 public double gps_course() {
482 return gps_course.value();
485 public double gps_speed() {
486 return gps_speed.value();
489 public double max_gps_speed() {
490 return gps_speed.max();
493 class AltosPressure extends AltosValue {
494 void set(double p, double time) {
496 if (state == AltosLib.ao_flight_pad)
497 ground_pressure.set_filtered(p, time);
498 double a = pressure_to_altitude(p);
499 altitude.set_computed(a, time);
507 private AltosPressure pressure;
509 public double pressure() {
510 return pressure.value();
513 public void set_pressure(double p) {
514 pressure.set(p, time);
517 public double baro_height() {
518 double a = altitude();
519 double g = ground_altitude();
520 if (a != AltosLib.MISSING && g != AltosLib.MISSING)
522 return AltosLib.MISSING;
525 public double height() {
526 double k = kalman_height.value();
527 if (k != AltosLib.MISSING)
530 double b = baro_height();
531 if (b != AltosLib.MISSING)
537 public double max_height() {
538 double k = kalman_height.max();
539 if (k != AltosLib.MISSING)
542 double a = altitude.max();
543 double g = ground_altitude();
544 if (a != AltosLib.MISSING && g != AltosLib.MISSING)
546 return max_gps_height();
549 public double gps_height() {
550 double a = gps_altitude();
551 double g = gps_ground_altitude();
553 if (a != AltosLib.MISSING && g != AltosLib.MISSING)
555 return AltosLib.MISSING;
558 public double max_gps_height() {
559 double a = gps_altitude.max();
560 double g = gps_ground_altitude();
562 if (a != AltosLib.MISSING && g != AltosLib.MISSING)
564 return AltosLib.MISSING;
567 class AltosSpeed extends AltosCValue {
570 return state < AltosLib.ao_flight_fast || state == AltosLib.ao_flight_stateless;
574 acceleration.set_derivative(this);
577 void set_derivative(AltosCValue in) {
578 super.set_derivative(in);
582 void set_computed(double new_value, double time) {
583 super.set_computed(new_value, time);
587 void set_measured(double new_value, double time) {
588 super.set_measured(new_value, time);
597 private AltosSpeed speed;
599 public double speed() {
600 double v = kalman_speed.value();
601 if (v != AltosLib.MISSING)
604 if (v != AltosLib.MISSING)
607 if (v != AltosLib.MISSING)
609 return AltosLib.MISSING;
612 public double max_speed() {
613 double v = kalman_speed.max();
614 if (v != AltosLib.MISSING)
617 if (v != AltosLib.MISSING)
620 if (v != AltosLib.MISSING)
622 return AltosLib.MISSING;
625 class AltosAccel extends AltosCValue {
628 return state < AltosLib.ao_flight_fast || state == AltosLib.ao_flight_stateless;
631 void set_measured(double a, double time) {
632 super.set_measured(a, time);
634 speed.set_integral(this.measured);
642 AltosAccel acceleration;
644 public double acceleration() {
645 return acceleration.value();
648 public double max_acceleration() {
649 return acceleration.max();
652 public AltosCValue orient;
654 public void set_orient(double new_orient) {
655 orient.set_measured(new_orient, time);
658 public double orient() {
659 return orient.value();
662 public double max_orient() {
666 public AltosValue kalman_height, kalman_speed, kalman_acceleration;
668 public void set_kalman(double height, double speed, double acceleration) {
669 kalman_height.set(height, time);
670 kalman_speed.set(speed, time);
671 kalman_acceleration.set(acceleration, time);
674 public double battery_voltage;
675 public double pyro_voltage;
676 public double temperature;
677 public double apogee_voltage;
678 public double main_voltage;
680 public double ignitor_voltage[];
683 public AltosGPS temp_gps;
684 public int temp_gps_sat_tick;
685 public boolean gps_pending;
686 public int gps_sequence;
691 public static final int MIN_PAD_SAMPLES = 10;
694 public int gps_waiting;
695 public boolean gps_ready;
699 public AltosGreatCircle from_pad;
700 public double elevation; /* from pad */
701 public double range; /* total distance */
703 public double gps_height;
705 public double pad_lat, pad_lon, pad_alt;
707 public int speak_tick;
708 public double speak_altitude;
710 public String callsign;
711 public String firmware_version;
713 public double accel_plus_g;
714 public double accel_minus_g;
716 public double ground_accel;
717 public double ground_accel_avg;
719 public int log_format;
720 public int log_space;
721 public String product;
723 public AltosMs5607 baro;
725 public AltosCompanion companion;
727 public int pyro_fired;
729 public void set_npad(int npad) {
731 gps_waiting = MIN_PAD_SAMPLES - npad;
732 if (this.gps_waiting < 0)
734 gps_ready = gps_waiting == 0;
740 received_time = System.currentTimeMillis();
741 time = AltosLib.MISSING;
742 time_change = AltosLib.MISSING;
743 prev_time = AltosLib.MISSING;
744 tick = AltosLib.MISSING;
745 prev_tick = AltosLib.MISSING;
746 boost_tick = AltosLib.MISSING;
747 state = AltosLib.ao_flight_invalid;
748 flight = AltosLib.MISSING;
751 rssi = AltosLib.MISSING;
753 device_type = AltosLib.MISSING;
754 config_major = AltosLib.MISSING;
755 config_minor = AltosLib.MISSING;
756 apogee_delay = AltosLib.MISSING;
757 main_deploy = AltosLib.MISSING;
758 flight_log_max = AltosLib.MISSING;
760 ground_altitude = new AltosCValue();
761 ground_pressure = new AltosGroundPressure();
762 altitude = new AltosAltitude();
763 pressure = new AltosPressure();
764 speed = new AltosSpeed();
765 acceleration = new AltosAccel();
766 orient = new AltosCValue();
768 temperature = AltosLib.MISSING;
769 battery_voltage = AltosLib.MISSING;
770 pyro_voltage = AltosLib.MISSING;
771 apogee_voltage = AltosLib.MISSING;
772 main_voltage = AltosLib.MISSING;
773 ignitor_voltage = null;
775 kalman_height = new AltosValue();
776 kalman_speed = new AltosValue();
777 kalman_acceleration = new AltosValue();
781 temp_gps_sat_tick = 0;
786 last_imu_time = AltosLib.MISSING;
788 ground_rotation = null;
791 accel_zero_along = AltosLib.MISSING;
792 accel_zero_across = AltosLib.MISSING;
793 accel_zero_through = AltosLib.MISSING;
795 accel_ground_along = AltosLib.MISSING;
796 accel_ground_across = AltosLib.MISSING;
797 accel_ground_through = AltosLib.MISSING;
799 pad_orientation = AltosLib.MISSING;
801 gyro_zero_roll = AltosLib.MISSING;
802 gyro_zero_pitch = AltosLib.MISSING;
803 gyro_zero_yaw = AltosLib.MISSING;
809 elevation = AltosLib.MISSING;
810 range = AltosLib.MISSING;
811 gps_height = AltosLib.MISSING;
813 pad_lat = AltosLib.MISSING;
814 pad_lon = AltosLib.MISSING;
815 pad_alt = AltosLib.MISSING;
817 gps_altitude = new AltosGpsAltitude();
818 gps_ground_altitude = new AltosGpsGroundAltitude();
819 gps_ground_speed = new AltosValue();
820 gps_speed = new AltosValue();
821 gps_ascent_rate = new AltosValue();
822 gps_course = new AltosValue();
824 speak_tick = AltosLib.MISSING;
825 speak_altitude = AltosLib.MISSING;
828 firmware_version = null;
830 accel_plus_g = AltosLib.MISSING;
831 accel_minus_g = AltosLib.MISSING;
832 accel = AltosLib.MISSING;
834 ground_accel = AltosLib.MISSING;
835 ground_accel_avg = AltosLib.MISSING;
837 log_format = AltosLib.MISSING;
838 log_space = AltosLib.MISSING;
840 serial = AltosLib.MISSING;
841 receiver_serial = AltosLib.MISSING;
842 altitude_32 = AltosLib.MISSING;
850 void finish_update() {
853 ground_altitude.finish_update();
854 altitude.finish_update();
855 pressure.finish_update();
856 speed.finish_update();
857 acceleration.finish_update();
858 orient.finish_update();
860 kalman_height.finish_update();
861 kalman_speed.finish_update();
862 kalman_acceleration.finish_update();
865 void copy(AltosState old) {
872 received_time = old.received_time;
874 time_change = old.time_change;
875 prev_time = old.time;
878 prev_tick = old.tick;
879 boost_tick = old.boost_tick;
888 device_type = old.device_type;
889 config_major = old.config_major;
890 config_minor = old.config_minor;
891 apogee_delay = old.apogee_delay;
892 main_deploy = old.main_deploy;
893 flight_log_max = old.flight_log_max;
897 ground_pressure.copy(old.ground_pressure);
898 ground_altitude.copy(old.ground_altitude);
899 altitude.copy(old.altitude);
900 pressure.copy(old.pressure);
901 speed.copy(old.speed);
902 acceleration.copy(old.acceleration);
903 orient.copy(old.orient);
905 battery_voltage = old.battery_voltage;
906 pyro_voltage = old.pyro_voltage;
907 temperature = old.temperature;
908 apogee_voltage = old.apogee_voltage;
909 main_voltage = old.main_voltage;
910 ignitor_voltage = old.ignitor_voltage;
912 kalman_height.copy(old.kalman_height);
913 kalman_speed.copy(old.kalman_speed);
914 kalman_acceleration.copy(old.kalman_acceleration);
917 gps = old.gps.clone();
920 if (old.temp_gps != null)
921 temp_gps = old.temp_gps.clone();
924 temp_gps_sat_tick = old.temp_gps_sat_tick;
925 gps_sequence = old.gps_sequence;
926 gps_pending = old.gps_pending;
929 imu = old.imu.clone();
932 last_imu_time = old.last_imu_time;
934 if (old.rotation != null)
935 rotation = new AltosRotation (old.rotation);
937 if (old.ground_rotation != null) {
938 ground_rotation = new AltosRotation(old.ground_rotation);
941 accel_zero_along = old.accel_zero_along;
942 accel_zero_across = old.accel_zero_across;
943 accel_zero_through = old.accel_zero_through;
945 accel_ground_along = old.accel_ground_along;
946 accel_ground_across = old.accel_ground_across;
947 accel_ground_through = old.accel_ground_through;
948 pad_orientation = old.pad_orientation;
950 gyro_zero_roll = old.gyro_zero_roll;
951 gyro_zero_pitch = old.gyro_zero_pitch;
952 gyro_zero_yaw = old.gyro_zero_yaw;
955 mag = old.mag.clone();
960 gps_waiting = old.gps_waiting;
961 gps_ready = old.gps_ready;
964 if (old.from_pad != null)
965 from_pad = old.from_pad.clone();
969 elevation = old.elevation;
972 gps_height = old.gps_height;
974 gps_altitude.copy(old.gps_altitude);
975 gps_ground_altitude.copy(old.gps_ground_altitude);
976 gps_ground_speed.copy(old.gps_ground_speed);
977 gps_ascent_rate.copy(old.gps_ascent_rate);
978 gps_course.copy(old.gps_course);
979 gps_speed.copy(old.gps_speed);
981 pad_lat = old.pad_lat;
982 pad_lon = old.pad_lon;
983 pad_alt = old.pad_alt;
985 speak_tick = old.speak_tick;
986 speak_altitude = old.speak_altitude;
988 callsign = old.callsign;
989 firmware_version = old.firmware_version;
991 accel_plus_g = old.accel_plus_g;
992 accel_minus_g = old.accel_minus_g;
994 ground_accel = old.ground_accel;
995 ground_accel_avg = old.ground_accel_avg;
997 log_format = old.log_format;
998 log_space = old.log_space;
999 product = old.product;
1000 serial = old.serial;
1001 receiver_serial = old.receiver_serial;
1002 altitude_32 = old.altitude_32;
1005 companion = old.companion;
1007 pyro_fired = old.pyro_fired;
1010 void update_time() {
1014 elevation = AltosLib.MISSING;
1015 range = AltosLib.MISSING;
1020 if (gps.locked && gps.nsat >= 4) {
1021 /* Track consecutive 'good' gps reports, waiting for 10 of them */
1022 if (state == AltosLib.ao_flight_pad || state == AltosLib.ao_flight_stateless) {
1024 if (pad_lat != AltosLib.MISSING && (npad < 10 || state == AltosLib.ao_flight_pad)) {
1025 pad_lat = (pad_lat * 31 + gps.lat) / 32;
1026 pad_lon = (pad_lon * 31 + gps.lon) / 32;
1027 gps_ground_altitude.set_filtered(gps.alt, time);
1030 if (pad_lat == AltosLib.MISSING) {
1033 gps_ground_altitude.set(gps.alt, time);
1035 gps_altitude.set(gps.alt, time);
1036 if (gps.climb_rate != AltosLib.MISSING)
1037 gps_ascent_rate.set(gps.climb_rate, time);
1038 if (gps.ground_speed != AltosLib.MISSING)
1039 gps_ground_speed.set(gps.ground_speed, time);
1040 if (gps.climb_rate != AltosLib.MISSING && gps.ground_speed != AltosLib.MISSING)
1041 gps_speed.set(Math.sqrt(gps.ground_speed * gps.ground_speed +
1042 gps.climb_rate * gps.climb_rate), time);
1043 if (gps.course != AltosLib.MISSING)
1044 gps_course.set(gps.course, time);
1046 if (gps.lat != 0 && gps.lon != 0 &&
1047 pad_lat != AltosLib.MISSING &&
1048 pad_lon != AltosLib.MISSING)
1050 double h = height();
1052 if (h == AltosLib.MISSING)
1054 from_pad = new AltosGreatCircle(pad_lat, pad_lon, 0, gps.lat, gps.lon, h);
1055 elevation = from_pad.elevation;
1056 range = from_pad.range;
1060 public void set_tick(int new_tick) {
1061 if (new_tick != AltosLib.MISSING) {
1062 if (prev_tick != AltosLib.MISSING) {
1063 while (new_tick < prev_tick - 1000) {
1068 time = tick / 100.0;
1069 time_change = time - prev_time;
1073 public void set_boost_tick(int boost_tick) {
1074 if (boost_tick != AltosLib.MISSING)
1075 this.boost_tick = boost_tick;
1078 public String state_name() {
1079 return AltosLib.state_name(state);
1082 public void set_product(String product) {
1083 this.product = product;
1086 public void set_state(int state) {
1087 if (state != AltosLib.ao_flight_invalid) {
1089 ascent = (AltosLib.ao_flight_boost <= state &&
1090 state <= AltosLib.ao_flight_coast);
1091 boost = (AltosLib.ao_flight_boost == state);
1095 public int state() {
1099 public void set_device_type(int device_type) {
1100 this.device_type = device_type;
1101 switch (device_type) {
1102 case AltosLib.product_telegps:
1103 this.state = AltosLib.ao_flight_stateless;
1108 public void set_log_format(int log_format) {
1109 this.log_format = log_format;
1110 switch (log_format) {
1111 case AltosLib.AO_LOG_FORMAT_TELEGPS:
1112 this.state = AltosLib.ao_flight_stateless;
1117 public void set_log_space(int log_space) {
1118 this.log_space = log_space;
1121 public void set_flight_params(int apogee_delay, int main_deploy) {
1122 this.apogee_delay = apogee_delay;
1123 this.main_deploy = main_deploy;
1126 public void set_config(int major, int minor, int flight_log_max) {
1127 config_major = major;
1128 config_minor = minor;
1129 this.flight_log_max = flight_log_max;
1132 public void set_callsign(String callsign) {
1133 this.callsign = callsign;
1136 public void set_firmware_version(String version) {
1137 firmware_version = version;
1140 public int compare_version(String other_version) {
1141 if (firmware_version == null)
1142 return AltosLib.MISSING;
1143 return AltosLib.compare_version(firmware_version, other_version);
1146 private void re_init() {
1147 int bt = boost_tick;
1148 int rs = receiver_serial;
1151 receiver_serial = rs;
1154 public void set_flight(int flight) {
1156 /* When the flight changes, reset the state */
1157 if (flight != AltosLib.MISSING) {
1158 if (this.flight != AltosLib.MISSING &&
1159 this.flight != flight) {
1162 this.flight = flight;
1166 public void set_serial(int serial) {
1167 /* When the serial changes, reset the state */
1168 if (serial != AltosLib.MISSING) {
1169 if (this.serial != AltosLib.MISSING &&
1170 this.serial != serial) {
1173 this.serial = serial;
1177 public void set_receiver_serial(int serial) {
1178 if (serial != AltosLib.MISSING)
1179 receiver_serial = serial;
1182 public boolean altitude_32() {
1183 return altitude_32 == 1;
1186 public void set_altitude_32(int altitude_32) {
1187 if (altitude_32 != AltosLib.MISSING)
1188 this.altitude_32 = altitude_32;
1192 if (rssi == AltosLib.MISSING)
1197 public void set_rssi(int rssi, int status) {
1198 if (rssi != AltosLib.MISSING) {
1200 this.status = status;
1204 public void set_received_time(long ms) {
1208 public void set_gps(AltosGPS gps, int sequence) {
1210 this.gps = gps.clone();
1211 gps_sequence = sequence;
1218 public double accel_zero_along;
1219 public double accel_zero_across;
1220 public double accel_zero_through;
1222 public AltosRotation rotation;
1223 public AltosRotation ground_rotation;
1225 public void set_accel_zero(double zero_along, double zero_across, double zero_through) {
1226 if (zero_along != AltosLib.MISSING) {
1227 accel_zero_along = zero_along;
1228 accel_zero_across = zero_across;
1229 accel_zero_through = zero_through;
1233 public int pad_orientation;
1235 public double accel_ground_along, accel_ground_across, accel_ground_through;
1237 void update_pad_rotation() {
1238 if (pad_orientation != AltosLib.MISSING && accel_ground_along != AltosLib.MISSING) {
1239 rotation = new AltosRotation(AltosIMU.convert_accel(accel_ground_across - accel_zero_across),
1240 AltosIMU.convert_accel(accel_ground_through - accel_zero_through),
1241 AltosIMU.convert_accel(accel_ground_along - accel_zero_along),
1243 ground_rotation = rotation;
1244 orient.set_computed(rotation.tilt(), time);
1248 public void set_accel_ground(double ground_along, double ground_across, double ground_through) {
1249 accel_ground_along = ground_along;
1250 accel_ground_across = ground_across;
1251 accel_ground_through = ground_through;
1252 update_pad_rotation();
1255 public void set_pad_orientation(int pad_orientation) {
1256 this.pad_orientation = pad_orientation;
1257 update_pad_rotation();
1260 public double gyro_zero_roll;
1261 public double gyro_zero_pitch;
1262 public double gyro_zero_yaw;
1264 public void set_gyro_zero(double roll, double pitch, double yaw) {
1265 if (roll != AltosLib.MISSING) {
1266 gyro_zero_roll = roll;
1267 gyro_zero_pitch = pitch;
1268 gyro_zero_yaw = yaw;
1272 public double last_imu_time;
1274 private double radians(double degrees) {
1275 if (degrees == AltosLib.MISSING)
1276 return AltosLib.MISSING;
1277 return degrees * Math.PI / 180.0;
1280 private void update_orient() {
1281 if (last_imu_time != AltosLib.MISSING) {
1282 double t = time - last_imu_time;
1284 double pitch = radians(gyro_pitch());
1285 double yaw = radians(gyro_yaw());
1286 double roll = radians(gyro_roll());
1288 if (t > 0 & pitch != AltosLib.MISSING && rotation != null) {
1289 rotation.rotate(t, pitch, yaw, roll);
1290 orient.set_computed(rotation.tilt(), time);
1293 last_imu_time = time;
1296 public void set_imu(AltosIMU imu) {
1303 private double gyro_zero_overflow(double first) {
1304 double v = first / 128.0;
1312 public void check_imu_wrap(AltosIMU imu) {
1313 if (this.imu == null) {
1314 gyro_zero_roll += gyro_zero_overflow(imu.gyro_roll);
1315 gyro_zero_pitch += gyro_zero_overflow(imu.gyro_pitch);
1316 gyro_zero_yaw += gyro_zero_overflow(imu.gyro_yaw);
1320 public double accel_along() {
1321 if (imu != null && accel_zero_along != AltosLib.MISSING)
1322 return AltosIMU.convert_accel(imu.accel_along - accel_zero_along);
1323 return AltosLib.MISSING;
1326 public double accel_across() {
1327 if (imu != null && accel_zero_across != AltosLib.MISSING)
1328 return AltosIMU.convert_accel(imu.accel_across - accel_zero_across);
1329 return AltosLib.MISSING;
1332 public double accel_through() {
1333 if (imu != null && accel_zero_through != AltosLib.MISSING)
1334 return AltosIMU.convert_accel(imu.accel_through - accel_zero_through);
1335 return AltosLib.MISSING;
1338 public double gyro_roll() {
1339 if (imu != null && gyro_zero_roll != AltosLib.MISSING) {
1340 return AltosIMU.convert_gyro(imu.gyro_roll - gyro_zero_roll);
1342 return AltosLib.MISSING;
1345 public double gyro_pitch() {
1346 if (imu != null && gyro_zero_pitch != AltosLib.MISSING) {
1347 return AltosIMU.convert_gyro(imu.gyro_pitch - gyro_zero_pitch);
1349 return AltosLib.MISSING;
1352 public double gyro_yaw() {
1353 if (imu != null && gyro_zero_yaw != AltosLib.MISSING) {
1354 return AltosIMU.convert_gyro(imu.gyro_yaw - gyro_zero_yaw);
1356 return AltosLib.MISSING;
1359 public void set_mag(AltosMag mag) {
1360 this.mag = mag.clone();
1363 public double mag_along() {
1365 return AltosMag.convert_gauss(mag.along);
1366 return AltosLib.MISSING;
1369 public double mag_across() {
1371 return AltosMag.convert_gauss(mag.across);
1372 return AltosLib.MISSING;
1375 public double mag_through() {
1377 return AltosMag.convert_gauss(mag.through);
1378 return AltosLib.MISSING;
1381 public AltosMs5607 make_baro() {
1383 baro = new AltosMs5607();
1387 public void set_ms5607(AltosMs5607 ms5607) {
1391 set_pressure(baro.pa);
1392 set_temperature(baro.cc / 100.0);
1396 public void set_ms5607(int pres, int temp) {
1398 baro.set(pres, temp);
1400 set_pressure(baro.pa);
1401 set_temperature(baro.cc / 100.0);
1405 public void set_companion(AltosCompanion companion) {
1406 this.companion = companion;
1409 void update_accel() {
1410 if (accel == AltosLib.MISSING)
1412 if (accel_plus_g == AltosLib.MISSING)
1414 if (accel_minus_g == AltosLib.MISSING)
1417 double counts_per_g = (accel_minus_g - accel_plus_g) / 2.0;
1418 double counts_per_mss = counts_per_g / 9.80665;
1419 acceleration.set_measured((accel_plus_g - accel) / counts_per_mss, time);
1422 public void set_accel_g(double accel_plus_g, double accel_minus_g) {
1423 if (accel_plus_g != AltosLib.MISSING) {
1424 this.accel_plus_g = accel_plus_g;
1425 this.accel_minus_g = accel_minus_g;
1430 public void set_ground_accel(double ground_accel) {
1431 if (ground_accel != AltosLib.MISSING)
1432 this.ground_accel = ground_accel;
1435 public void set_accel(double accel) {
1436 if (accel != AltosLib.MISSING) {
1438 if (state == AltosLib.ao_flight_pad) {
1439 if (ground_accel_avg == AltosLib.MISSING)
1440 ground_accel_avg = accel;
1442 ground_accel_avg = (ground_accel_avg * 7 + accel) / 8;
1448 public void set_temperature(double temperature) {
1449 if (temperature != AltosLib.MISSING) {
1450 this.temperature = temperature;
1455 public void set_battery_voltage(double battery_voltage) {
1456 if (battery_voltage != AltosLib.MISSING) {
1457 this.battery_voltage = battery_voltage;
1462 public void set_pyro_voltage(double pyro_voltage) {
1463 if (pyro_voltage != AltosLib.MISSING) {
1464 this.pyro_voltage = pyro_voltage;
1469 public void set_apogee_voltage(double apogee_voltage) {
1470 if (apogee_voltage != AltosLib.MISSING) {
1471 this.apogee_voltage = apogee_voltage;
1476 public void set_main_voltage(double main_voltage) {
1477 if (main_voltage != AltosLib.MISSING) {
1478 this.main_voltage = main_voltage;
1483 public void set_ignitor_voltage(double[] voltage) {
1484 this.ignitor_voltage = voltage;
1487 public void set_pyro_fired(int fired) {
1488 this.pyro_fired = fired;
1491 public double time_since_boost() {
1492 if (tick == AltosLib.MISSING)
1495 if (boost_tick == AltosLib.MISSING)
1496 return tick / 100.0;
1497 return (tick - boost_tick) / 100.0;
1500 public boolean valid() {
1501 return tick != AltosLib.MISSING && serial != AltosLib.MISSING;
1504 public AltosGPS make_temp_gps(boolean sats) {
1505 if (temp_gps == null) {
1506 temp_gps = new AltosGPS(gps);
1510 if (tick != temp_gps_sat_tick)
1511 temp_gps.cc_gps_sat = null;
1512 temp_gps_sat_tick = tick;
1517 public void set_temp_gps() {
1518 set_gps(temp_gps, gps_sequence + 1);
1519 gps_pending = false;
1523 public AltosState clone() {
1524 AltosState s = new AltosState();
1527 /* Code to test state save/restore. Enable only for that purpose
1530 AltosJson json = new AltosJson(this);
1531 String onetrip = json.toPrettyString();
1532 AltosJson back = AltosJson.fromString(onetrip);
1533 AltosState tripstate = (AltosState) back.make(this.getClass());
1534 AltosJson tripjson = new AltosJson(tripstate);
1535 String twotrip = tripjson.toPrettyString();
1537 if (!onetrip.equals(twotrip)) {
1539 FileWriter one_file = new FileWriter("one.json", true);
1540 one_file.write(onetrip);
1542 FileWriter two_file = new FileWriter("two.json", true);
1543 two_file.write(twotrip);
1545 } catch (Exception e) {
1547 System.out.printf("json error\n");
1554 public AltosState () {