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 report_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 */
52 public double ground_altitude;
53 public double ground_pressure;
54 public double altitude;
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;
65 public double prev_height;
66 public double prev_speed;
67 public double prev_acceleration;
69 public double max_height;
70 public double max_acceleration;
71 public double max_speed;
73 public double kalman_height, kalman_speed, kalman_acceleration;
76 public AltosGPS temp_gps;
77 public boolean gps_pending;
78 public int gps_sequence;
83 public static final int MIN_PAD_SAMPLES = 10;
86 public int gps_waiting;
87 public boolean gps_ready;
91 public AltosGreatCircle from_pad;
92 public double elevation; /* from pad */
93 public double range; /* total distance */
95 public double gps_height;
97 public double pad_lat, pad_lon, pad_alt;
99 public int speak_tick;
100 public double speak_altitude;
102 public String callsign;
103 public double accel_plus_g;
104 public double accel_minus_g;
106 public double ground_accel;
107 public double ground_accel_avg;
109 public int log_format;
111 public AltosMs5607 baro;
113 public AltosRecordCompanion companion;
115 public double speed() {
119 public double max_speed() {
123 public void set_npad(int npad) {
125 gps_waiting = MIN_PAD_SAMPLES - npad;
126 if (this.gps_waiting < 0)
128 gps_ready = gps_waiting == 0;
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;
146 rssi = AltosRecord.MISSING;
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;
157 prev_height = AltosRecord.MISSING;
158 prev_speed = AltosRecord.MISSING;
159 prev_acceleration = AltosRecord.MISSING;
161 battery_voltage = AltosRecord.MISSING;
162 pyro_voltage = AltosRecord.MISSING;
163 apogee_voltage = AltosRecord.MISSING;
164 main_voltage = AltosRecord.MISSING;
166 speed = AltosRecord.MISSING;
168 kalman_height = AltosRecord.MISSING;
169 kalman_speed = AltosRecord.MISSING;
170 kalman_acceleration = AltosRecord.MISSING;
174 max_acceleration = 0;
188 elevation = AltosRecord.MISSING;
189 range = AltosRecord.MISSING;
190 gps_height = AltosRecord.MISSING;
192 pad_lat = AltosRecord.MISSING;
193 pad_lon = AltosRecord.MISSING;
194 pad_alt = AltosRecord.MISSING;
196 speak_tick = AltosRecord.MISSING;
197 speak_altitude = AltosRecord.MISSING;
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;
213 void copy(AltosState old) {
222 report_time = old.report_time;
226 boost_tick = old.boost_tick;
238 ground_altitude = old.ground_altitude;
239 altitude = old.altitude;
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;
250 prev_height = old.height;
251 prev_speed = old.speed;
252 prev_acceleration = old.acceleration;
253 prev_time = old.time;
255 max_height = old.max_height;
256 max_acceleration = old.max_acceleration;
257 max_speed = old.max_speed;
259 kalman_height = old.kalman_height;
260 kalman_speed = old.kalman_speed;
261 kalman_acceleration = old.kalman_acceleration;
264 gps = old.gps.clone();
267 if (old.temp_gps != null)
268 temp_gps = old.temp_gps.clone();
271 gps_sequence = old.gps_sequence;
272 gps_pending = old.gps_pending;
275 imu = old.imu.clone();
280 mag = old.mag.clone();
285 gps_waiting = old.gps_waiting;
286 gps_ready = old.gps_ready;
289 if (old.from_pad != null)
290 from_pad = old.from_pad.clone();
294 elevation = old.elevation;
297 gps_height = old.gps_height;
298 pad_lat = old.pad_lat;
299 pad_lon = old.pad_lon;
300 pad_alt = old.pad_alt;
302 speak_tick = old.speak_tick;
303 speak_altitude = old.speak_altitude;
305 callsign = old.callsign;
307 accel_plus_g = old.accel_plus_g;
308 accel_minus_g = old.accel_minus_g;
310 ground_accel = old.ground_accel;
311 ground_accel_avg = old.ground_accel_avg;
313 log_format = old.log_format;
317 companion = old.companion;
321 if (altitude != AltosRecord.MISSING)
325 return AltosRecord.MISSING;
328 void update_vertical_pos() {
330 double alt = altitude();
332 if (state == AltosLib.ao_flight_pad && alt != AltosRecord.MISSING && ground_pressure == AltosRecord.MISSING) {
333 if (ground_altitude == AltosRecord.MISSING)
334 ground_altitude = alt;
336 ground_altitude = (ground_altitude * 7 + alt) / 8;
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;
344 height = AltosRecord.MISSING;
346 if (height != AltosRecord.MISSING && height > max_height)
352 double motion_filter_value() {
353 return 1/ Math.exp(time_change/10.0);
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)
362 if (ascent && acceleration != AltosRecord.MISSING)
364 if (prev_speed == AltosRecord.MISSING)
365 speed = acceleration * time_change;
367 speed = prev_speed + acceleration * time_change;
369 else if (height != AltosRecord.MISSING &&
370 prev_height != AltosRecord.MISSING &&
373 double new_speed = (height - prev_height) / time_change;
375 if (prev_speed == AltosRecord.MISSING)
378 double filter = motion_filter_value();
380 speed = prev_speed * filter + new_speed * (1-filter);
384 if (acceleration == AltosRecord.MISSING) {
385 if (prev_speed != AltosRecord.MISSING && time_change != 0) {
386 double new_acceleration = (speed - prev_speed) / time_change;
388 if (prev_acceleration == AltosRecord.MISSING)
389 acceleration = new_acceleration;
391 double filter = motion_filter_value();
393 acceleration = prev_acceleration * filter + new_acceleration * (1-filter);
397 if (boost && speed != AltosRecord.MISSING && speed > max_speed)
401 void update_accel() {
402 double ground = ground_accel;
404 if (ground == AltosRecord.MISSING)
405 ground = ground_accel_avg;
406 if (accel == AltosRecord.MISSING)
408 if (ground == AltosRecord.MISSING)
410 if (accel_plus_g == AltosRecord.MISSING)
412 if (accel_minus_g == AltosRecord.MISSING)
415 double counts_per_g = (accel_minus_g - accel_plus_g) / 2.0;
416 double counts_per_mss = counts_per_g / 9.80665;
418 acceleration = (ground - accel) / counts_per_mss;
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;
427 if (tick != AltosRecord.MISSING) {
429 if (prev_time != AltosRecord.MISSING)
430 time_change = time - prev_time;
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) {
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;
452 if (pad_lat == AltosRecord.MISSING) {
458 if (gps.lat != 0 && gps.lon != 0 &&
459 pad_lat != AltosRecord.MISSING &&
460 pad_lon != AltosRecord.MISSING)
464 if (h == AltosRecord.MISSING)
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;
473 public void set_tick(int tick) {
474 if (tick != AltosRecord.MISSING) {
475 if (this.tick != AltosRecord.MISSING) {
476 while (tick < this.tick)
478 time_change = (tick - this.tick) / 100.0;
486 public void set_boost_tick(int boost_tick) {
487 if (boost_tick != AltosRecord.MISSING)
488 this.boost_tick = boost_tick;
491 public String state_name() {
492 return AltosLib.state_name(state);
495 public void set_state(int state) {
496 if (state != AltosLib.ao_flight_invalid) {
498 ascent = (AltosLib.ao_flight_boost <= state &&
499 state <= AltosLib.ao_flight_coast);
500 boost = (AltosLib.ao_flight_boost == state);
505 public void set_flight(int flight) {
507 /* When the flight changes, reset the state */
508 if (flight != AltosRecord.MISSING) {
509 if (this.flight != AltosRecord.MISSING &&
510 this.flight != flight) {
513 this.flight = flight;
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) {
524 this.serial = serial;
529 if (rssi == AltosRecord.MISSING)
534 public void set_rssi(int rssi, int status) {
535 if (rssi != AltosRecord.MISSING) {
537 this.status = status;
541 public void set_altitude(double altitude) {
542 if (altitude != AltosRecord.MISSING) {
543 this.altitude = altitude;
544 update_vertical_pos();
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();
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();
564 public void set_gps(AltosGPS gps, int sequence) {
566 this.gps = gps.clone();
567 gps_sequence = sequence;
569 update_vertical_pos();
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();
583 public void set_pressure(double pressure) {
584 if (pressure != AltosRecord.MISSING) {
585 this.pressure = pressure;
586 set_altitude(AltosConvert.pressure_to_altitude(pressure));
590 public void make_baro() {
592 baro = new AltosMs5607();
595 public void set_ms5607(int pres, int temp) {
597 baro.set(pres, temp);
599 set_pressure(baro.pa);
600 set_temperature(baro.cc / 100.0);
604 public void make_companion (int nchannels) {
605 if (companion == null)
606 companion = new AltosRecordCompanion(nchannels);
609 public void set_companion(AltosRecordCompanion companion) {
610 this.companion = companion;
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;
620 public void set_ground_accel(double ground_accel) {
621 if (ground_accel != AltosRecord.MISSING) {
622 this.ground_accel = ground_accel;
627 public void set_accel(double accel) {
628 if (accel != AltosRecord.MISSING) {
630 if (state == AltosLib.ao_flight_pad) {
631 if (ground_accel_avg == AltosRecord.MISSING)
632 ground_accel_avg = accel;
634 ground_accel_avg = (ground_accel_avg * 7 + accel) / 8;
640 public void set_temperature(double temperature) {
641 if (temperature != AltosRecord.MISSING) {
642 this.temperature = temperature;
647 public void set_battery_voltage(double battery_voltage) {
648 if (battery_voltage != AltosRecord.MISSING) {
649 this.battery_voltage = battery_voltage;
654 public void set_pyro_voltage(double pyro_voltage) {
655 if (pyro_voltage != AltosRecord.MISSING) {
656 this.pyro_voltage = pyro_voltage;
661 public void set_apogee_voltage(double apogee_voltage) {
662 if (apogee_voltage != AltosRecord.MISSING) {
663 this.apogee_voltage = apogee_voltage;
668 public void set_main_voltage(double main_voltage) {
669 if (main_voltage != AltosRecord.MISSING) {
670 this.main_voltage = main_voltage;
676 public double time_since_boost() {
677 if (tick == AltosRecord.MISSING)
680 if (boost_tick != AltosRecord.MISSING) {
681 return (tick - boost_tick) / 100.0;
686 public boolean valid() {
687 return tick != AltosRecord.MISSING && serial != AltosRecord.MISSING;
690 public AltosGPS make_temp_gps() {
691 if (temp_gps == null) {
692 temp_gps = new AltosGPS(gps);
693 temp_gps.cc_gps_sat = null;
699 public void set_temp_gps() {
700 set_gps(temp_gps, gps_sequence + 1);
705 public void init (AltosRecord cur, AltosState prev_state) {
707 System.out.printf ("init\n");
709 cur = new AltosRecord();
713 /* Discard previous state if it was for a different board */
714 if (prev_state != null && prev_state.serial != cur.serial)
719 set_ground_altitude(cur.ground_altitude());
720 set_altitude(cur.altitude());
722 set_kalman(cur.kalman_height, cur.kalman_speed, cur.kalman_acceleration);
724 report_time = System.currentTimeMillis();
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());
731 set_pressure(cur.pressure());
734 set_state(cur.state);
736 set_accel_g (cur.accel_minus_g, cur.accel_plus_g);
737 set_ground_accel(cur.ground_accel);
738 set_accel (cur.accel);
740 if (cur.gps_sequence != gps_sequence)
741 set_gps(cur.gps, cur.gps_sequence);
745 public AltosState clone() {
746 AltosState s = new AltosState();
751 public AltosState(AltosRecord cur) {
755 public AltosState (AltosRecord cur, AltosState prev) {
759 public AltosState () {