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;
35 public long received_time;
38 public double prev_time;
39 public double time_change;
41 public int boost_tick;
46 public boolean landed;
47 public boolean ascent; /* going up? */
48 public boolean boost; /* under power */
51 public int device_type;
52 public int config_major;
53 public int config_minor;
54 public int apogee_delay;
55 public int main_deploy;
56 public int flight_log_max;
58 public double ground_altitude;
59 public double ground_pressure;
60 public double altitude;
62 public double pressure;
63 public double acceleration;
64 public double battery_voltage;
65 public double pyro_voltage;
66 public double temperature;
67 public double apogee_voltage;
68 public double main_voltage;
70 public double ignitor_voltage[];
72 public double prev_height;
73 public double prev_speed;
74 public double prev_acceleration;
76 public double prev_max_height;
77 public double prev_max_acceleration;
78 public double prev_max_speed;
80 public double max_height;
81 public double max_acceleration;
82 public double max_speed;
84 public double kalman_height, kalman_speed, kalman_acceleration;
87 public AltosGPS temp_gps;
88 public boolean gps_pending;
89 public int gps_sequence;
94 public static final int MIN_PAD_SAMPLES = 10;
97 public int gps_waiting;
98 public boolean gps_ready;
102 public AltosGreatCircle from_pad;
103 public double elevation; /* from pad */
104 public double range; /* total distance */
106 public double gps_height;
108 public double pad_lat, pad_lon, pad_alt;
110 public int speak_tick;
111 public double speak_altitude;
113 public String callsign;
114 public String firmware_version;
116 public double accel_plus_g;
117 public double accel_minus_g;
119 public double ground_accel;
120 public double ground_accel_avg;
122 public int log_format;
124 public AltosMs5607 baro;
126 public AltosRecordCompanion companion;
128 public double speed() {
132 public double max_speed() {
136 public void set_npad(int npad) {
138 gps_waiting = MIN_PAD_SAMPLES - npad;
139 if (this.gps_waiting < 0)
141 gps_ready = gps_waiting == 0;
149 received_time = System.currentTimeMillis();
150 time = AltosRecord.MISSING;
151 time_change = AltosRecord.MISSING;
152 prev_time = AltosRecord.MISSING;
153 tick = AltosRecord.MISSING;
154 boost_tick = AltosRecord.MISSING;
155 state = AltosLib.ao_flight_invalid;
156 flight = AltosRecord.MISSING;
159 rssi = AltosRecord.MISSING;
161 device_type = AltosRecord.MISSING;
162 config_major = AltosRecord.MISSING;
163 config_minor = AltosRecord.MISSING;
164 apogee_delay = AltosRecord.MISSING;
165 main_deploy = AltosRecord.MISSING;
166 flight_log_max = AltosRecord.MISSING;
168 ground_altitude = AltosRecord.MISSING;
169 ground_pressure = AltosRecord.MISSING;
170 altitude = AltosRecord.MISSING;
171 height = AltosRecord.MISSING;
172 pressure = AltosRecord.MISSING;
173 acceleration = AltosRecord.MISSING;
174 temperature = AltosRecord.MISSING;
176 prev_height = AltosRecord.MISSING;
177 prev_speed = AltosRecord.MISSING;
178 prev_acceleration = AltosRecord.MISSING;
182 prev_max_acceleration = 0;
184 battery_voltage = AltosRecord.MISSING;
185 pyro_voltage = AltosRecord.MISSING;
186 apogee_voltage = AltosRecord.MISSING;
187 main_voltage = AltosRecord.MISSING;
188 ignitor_voltage = null;
190 speed = AltosRecord.MISSING;
192 kalman_height = AltosRecord.MISSING;
193 kalman_speed = AltosRecord.MISSING;
194 kalman_acceleration = AltosRecord.MISSING;
198 max_acceleration = 0;
212 elevation = AltosRecord.MISSING;
213 range = AltosRecord.MISSING;
214 gps_height = AltosRecord.MISSING;
216 pad_lat = AltosRecord.MISSING;
217 pad_lon = AltosRecord.MISSING;
218 pad_alt = AltosRecord.MISSING;
220 speak_tick = AltosRecord.MISSING;
221 speak_altitude = AltosRecord.MISSING;
225 accel_plus_g = AltosRecord.MISSING;
226 accel_minus_g = AltosRecord.MISSING;
227 accel = AltosRecord.MISSING;
228 ground_accel = AltosRecord.MISSING;
229 ground_accel_avg = AltosRecord.MISSING;
230 log_format = AltosRecord.MISSING;
231 serial = AltosRecord.MISSING;
237 void copy(AltosState old) {
246 received_time = old.received_time;
250 boost_tick = old.boost_tick;
259 device_type = old.device_type;
260 config_major = old.config_major;
261 config_minor = old.config_minor;
262 apogee_delay = old.apogee_delay;
263 main_deploy = old.main_deploy;
264 flight_log_max = old.flight_log_max;
268 ground_altitude = old.ground_altitude;
269 altitude = old.altitude;
271 pressure = old.pressure;
272 acceleration = old.acceleration;
273 battery_voltage = old.battery_voltage;
274 pyro_voltage = old.pyro_voltage;
275 temperature = old.temperature;
276 apogee_voltage = old.apogee_voltage;
277 main_voltage = old.main_voltage;
278 ignitor_voltage = old.ignitor_voltage;
281 prev_height = old.height;
282 prev_speed = old.speed;
283 prev_acceleration = old.acceleration;
285 prev_max_height = old.max_height;
286 prev_max_speed = old.max_speed;
287 prev_max_acceleration = old.max_acceleration;
288 prev_time = old.time;
290 max_height = old.max_height;
291 max_acceleration = old.max_acceleration;
292 max_speed = old.max_speed;
294 kalman_height = old.kalman_height;
295 kalman_speed = old.kalman_speed;
296 kalman_acceleration = old.kalman_acceleration;
299 gps = old.gps.clone();
302 if (old.temp_gps != null)
303 temp_gps = old.temp_gps.clone();
306 gps_sequence = old.gps_sequence;
307 gps_pending = old.gps_pending;
310 imu = old.imu.clone();
315 mag = old.mag.clone();
320 gps_waiting = old.gps_waiting;
321 gps_ready = old.gps_ready;
324 if (old.from_pad != null)
325 from_pad = old.from_pad.clone();
329 elevation = old.elevation;
332 gps_height = old.gps_height;
333 pad_lat = old.pad_lat;
334 pad_lon = old.pad_lon;
335 pad_alt = old.pad_alt;
337 speak_tick = old.speak_tick;
338 speak_altitude = old.speak_altitude;
340 callsign = old.callsign;
342 accel_plus_g = old.accel_plus_g;
343 accel_minus_g = old.accel_minus_g;
345 ground_accel = old.ground_accel;
346 ground_accel_avg = old.ground_accel_avg;
348 log_format = old.log_format;
352 companion = old.companion;
356 if (altitude != AltosRecord.MISSING)
360 return AltosRecord.MISSING;
363 void update_vertical_pos() {
365 double alt = altitude();
367 if (state == AltosLib.ao_flight_pad && alt != AltosRecord.MISSING && ground_pressure == AltosRecord.MISSING) {
368 if (ground_altitude == AltosRecord.MISSING)
369 ground_altitude = alt;
371 ground_altitude = (ground_altitude * 7 + alt) / 8;
374 if (kalman_height != AltosRecord.MISSING)
375 height = kalman_height;
376 else if (altitude != AltosRecord.MISSING && ground_altitude != AltosRecord.MISSING)
377 height = altitude - ground_altitude;
379 height = AltosRecord.MISSING;
381 if (height != AltosRecord.MISSING && height > prev_max_height)
387 double motion_filter_value() {
388 return 1/ Math.exp(time_change/2.0);
391 void update_speed() {
392 if (kalman_speed != AltosRecord.MISSING)
393 speed = kalman_speed;
394 else if (state != AltosLib.ao_flight_invalid &&
395 time_change != AltosRecord.MISSING)
397 if (ascent && acceleration != AltosRecord.MISSING)
399 if (prev_speed == AltosRecord.MISSING)
400 speed = acceleration * time_change;
402 speed = prev_speed + acceleration * time_change;
404 else if (height != AltosRecord.MISSING &&
405 prev_height != AltosRecord.MISSING &&
408 double new_speed = (height - prev_height) / time_change;
410 if (prev_speed == AltosRecord.MISSING)
413 double filter = motion_filter_value();
415 speed = prev_speed * filter + new_speed * (1-filter);
419 if (acceleration == AltosRecord.MISSING) {
420 if (prev_speed != AltosRecord.MISSING && time_change != 0) {
421 double new_acceleration = (speed - prev_speed) / time_change;
423 if (prev_acceleration == AltosRecord.MISSING)
424 acceleration = new_acceleration;
426 double filter = motion_filter_value();
428 acceleration = prev_acceleration * filter + new_acceleration * (1-filter);
432 if (boost && speed != AltosRecord.MISSING && speed > prev_max_speed)
436 void update_accel() {
437 if (kalman_acceleration != AltosRecord.MISSING) {
438 acceleration = kalman_acceleration;
440 double ground = ground_accel;
442 if (ground == AltosRecord.MISSING)
443 ground = ground_accel_avg;
444 if (accel == AltosRecord.MISSING)
446 if (ground == AltosRecord.MISSING)
448 if (accel_plus_g == AltosRecord.MISSING)
450 if (accel_minus_g == AltosRecord.MISSING)
453 double counts_per_g = (accel_minus_g - accel_plus_g) / 2.0;
454 double counts_per_mss = counts_per_g / 9.80665;
455 acceleration = (ground - accel) / counts_per_mss;
458 /* Only look at accelerometer data under boost */
459 if (boost && acceleration != AltosRecord.MISSING && acceleration > prev_max_acceleration)
460 max_acceleration = acceleration;
465 if (tick != AltosRecord.MISSING) {
467 if (prev_time != AltosRecord.MISSING)
468 time_change = time - prev_time;
480 if (gps.locked && gps.nsat >= 4) {
481 /* Track consecutive 'good' gps reports, waiting for 10 of them */
482 if (state == AltosLib.ao_flight_pad) {
484 if (pad_lat != AltosRecord.MISSING) {
485 pad_lat = (pad_lat * 31 + gps.lat) / 32;
486 pad_lon = (pad_lon * 31 + gps.lon) / 32;
487 pad_alt = (pad_alt * 31 + gps.alt) / 32;
490 if (pad_lat == AltosRecord.MISSING) {
496 if (gps.lat != 0 && gps.lon != 0 &&
497 pad_lat != AltosRecord.MISSING &&
498 pad_lon != AltosRecord.MISSING)
502 if (h == AltosRecord.MISSING)
504 from_pad = new AltosGreatCircle(pad_lat, pad_lon, 0, gps.lat, gps.lon, h);
505 elevation = from_pad.elevation;
506 range = from_pad.range;
507 gps_height = gps.alt - pad_alt;
511 public void set_tick(int tick) {
512 if (tick != AltosRecord.MISSING) {
513 if (this.tick != AltosRecord.MISSING) {
514 while (tick < this.tick)
516 time_change = (tick - this.tick) / 100.0;
524 public void set_boost_tick(int boost_tick) {
525 if (boost_tick != AltosRecord.MISSING)
526 this.boost_tick = boost_tick;
529 public String state_name() {
530 return AltosLib.state_name(state);
533 public void set_state(int state) {
534 if (state != AltosLib.ao_flight_invalid) {
536 ascent = (AltosLib.ao_flight_boost <= state &&
537 state <= AltosLib.ao_flight_coast);
538 boost = (AltosLib.ao_flight_boost == state);
543 public void set_device_type(int device_type) {
544 this.device_type = device_type;
547 public void set_config(int major, int minor, int apogee_delay, int main_deploy, int flight_log_max) {
548 config_major = major;
549 config_minor = minor;
550 this.apogee_delay = apogee_delay;
551 this.main_deploy = main_deploy;
552 this.flight_log_max = flight_log_max;
555 public void set_callsign(String callsign) {
556 this.callsign = callsign;
559 public void set_firmware_version(String version) {
560 firmware_version = version;
563 public void set_flight(int flight) {
565 /* When the flight changes, reset the state */
566 if (flight != AltosRecord.MISSING) {
567 if (this.flight != AltosRecord.MISSING &&
568 this.flight != flight) {
571 this.flight = flight;
575 public void set_serial(int serial) {
576 /* When the serial changes, reset the state */
577 if (serial != AltosRecord.MISSING) {
578 if (this.serial != AltosRecord.MISSING &&
579 this.serial != serial) {
582 this.serial = serial;
587 if (rssi == AltosRecord.MISSING)
592 public void set_rssi(int rssi, int status) {
593 if (rssi != AltosRecord.MISSING) {
595 this.status = status;
599 public void set_received_time(long ms) {
603 public void set_altitude(double altitude) {
604 if (altitude != AltosRecord.MISSING) {
605 this.altitude = altitude;
606 update_vertical_pos();
611 public void set_ground_altitude(double ground_altitude) {
612 if (ground_altitude != AltosRecord.MISSING) {
613 this.ground_altitude = ground_altitude;
614 update_vertical_pos();
618 public void set_ground_pressure (double pressure) {
619 if (pressure != AltosRecord.MISSING) {
620 this.ground_pressure = pressure;
621 set_ground_altitude(AltosConvert.pressure_to_altitude(pressure));
622 update_vertical_pos();
626 public void set_gps(AltosGPS gps, int sequence) {
628 this.gps = gps.clone();
629 gps_sequence = sequence;
631 update_vertical_pos();
636 public void set_kalman(double height, double speed, double acceleration) {
637 if (height != AltosRecord.MISSING) {
638 kalman_height = height;
639 kalman_speed = speed;
640 kalman_acceleration = acceleration;
641 update_vertical_pos();
647 public void set_pressure(double pressure) {
648 if (pressure != AltosRecord.MISSING) {
649 this.pressure = pressure;
650 set_altitude(AltosConvert.pressure_to_altitude(pressure));
654 public void make_baro() {
656 baro = new AltosMs5607();
659 public void set_ms5607(int pres, int temp) {
661 baro.set(pres, temp);
663 set_pressure(baro.pa);
664 set_temperature(baro.cc / 100.0);
668 public void make_companion (int nchannels) {
669 if (companion == null)
670 companion = new AltosRecordCompanion(nchannels);
673 public void set_companion(AltosRecordCompanion companion) {
674 this.companion = companion;
677 public void set_accel_g(double accel_plus_g, double accel_minus_g) {
678 if (accel_plus_g != AltosRecord.MISSING) {
679 this.accel_plus_g = accel_plus_g;
680 this.accel_minus_g = accel_minus_g;
684 public void set_ground_accel(double ground_accel) {
685 if (ground_accel != AltosRecord.MISSING) {
686 this.ground_accel = ground_accel;
691 public void set_accel(double accel) {
692 if (accel != AltosRecord.MISSING) {
694 if (state == AltosLib.ao_flight_pad) {
695 if (ground_accel_avg == AltosRecord.MISSING)
696 ground_accel_avg = accel;
698 ground_accel_avg = (ground_accel_avg * 7 + accel) / 8;
704 public void set_temperature(double temperature) {
705 if (temperature != AltosRecord.MISSING) {
706 this.temperature = temperature;
711 public void set_battery_voltage(double battery_voltage) {
712 if (battery_voltage != AltosRecord.MISSING) {
713 this.battery_voltage = battery_voltage;
718 public void set_pyro_voltage(double pyro_voltage) {
719 if (pyro_voltage != AltosRecord.MISSING) {
720 this.pyro_voltage = pyro_voltage;
725 public void set_apogee_voltage(double apogee_voltage) {
726 if (apogee_voltage != AltosRecord.MISSING) {
727 this.apogee_voltage = apogee_voltage;
732 public void set_main_voltage(double main_voltage) {
733 if (main_voltage != AltosRecord.MISSING) {
734 this.main_voltage = main_voltage;
739 public void set_ignitor_voltage(double[] voltage) {
740 this.ignitor_voltage = voltage;
743 public double time_since_boost() {
744 if (tick == AltosRecord.MISSING)
747 if (boost_tick != AltosRecord.MISSING) {
748 return (tick - boost_tick) / 100.0;
753 public boolean valid() {
754 return tick != AltosRecord.MISSING && serial != AltosRecord.MISSING;
757 public AltosGPS make_temp_gps() {
758 if (temp_gps == null) {
759 temp_gps = new AltosGPS(gps);
760 temp_gps.cc_gps_sat = null;
766 public void set_temp_gps() {
767 set_gps(temp_gps, gps_sequence + 1);
772 public AltosState clone() {
773 AltosState s = new AltosState();
779 public void init (AltosRecord cur, AltosState prev_state) {
781 System.out.printf ("init\n");
783 cur = new AltosRecord();
787 /* Discard previous state if it was for a different board */
788 if (prev_state != null && prev_state.serial != cur.serial)
793 set_ground_altitude(cur.ground_altitude());
794 set_altitude(cur.altitude());
796 set_kalman(cur.kalman_height, cur.kalman_speed, cur.kalman_acceleration);
798 received_time = System.currentTimeMillis();
800 set_temperature(cur.temperature());
801 set_apogee_voltage(cur.drogue_voltage());
802 set_main_voltage(cur.main_voltage());
803 set_battery_voltage(cur.battery_voltage());
805 set_pressure(cur.pressure());
808 set_state(cur.state);
810 set_accel_g (cur.accel_minus_g, cur.accel_plus_g);
811 set_ground_accel(cur.ground_accel);
812 set_accel (cur.accel);
814 if (cur.gps_sequence != gps_sequence)
815 set_gps(cur.gps, cur.gps_sequence);
819 public AltosState(AltosRecord cur) {
823 public AltosState (AltosRecord cur, AltosState prev) {
828 public AltosState () {