altoslib: Preserve receiver_serial across state reset for new transmitter

[fw/altos] / altoslib / AltosState.java

diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java
index 7871da77b3a73a3bc60edefaa68a6adf1b58c88a..5e7908af135402642abf1d7824a34943ad43954e 100644 (file)
--- a/altoslib/AltosState.java
+++ b/altoslib/AltosState.java
@@ -19,7 +19,7 @@
  * Track flight state from telemetry or eeprom data stream
  */
 
-package org.altusmetrum.altoslib_4;
+package org.altusmetrum.altoslib_5;
 
 public class AltosState implements Cloneable {
 
@@ -392,6 +392,7 @@ public class AltosState implements Cloneable {
        private AltosValue              gps_ground_speed;
        private AltosValue              gps_ascent_rate;
        private AltosValue              gps_course;
+       private AltosValue              gps_speed;
 
        public double altitude() {
                double a = altitude.value();
@@ -427,14 +428,30 @@ public class AltosState implements Cloneable {
                return gps_ground_speed.value();
        }
 
+       public double max_gps_ground_speed() {
+               return gps_ground_speed.max();
+       }
+
        public double gps_ascent_rate() {
                return gps_ascent_rate.value();
        }
 
+       public double max_gps_ascent_rate() {
+               return gps_ascent_rate.max();
+       }
+
        public double gps_course() {
                return gps_course.value();
        }
 
+       public double gps_speed() {
+               return gps_speed.value();
+       }
+
+       public double max_gps_speed() {
+               return gps_speed.max();
+       }
+
        class AltosPressure extends AltosValue {
                void set(double p, double time) {
                        super.set(p, time);
@@ -464,7 +481,7 @@ public class AltosState implements Cloneable {
                double g = ground_altitude();
                if (a != AltosLib.MISSING && g != AltosLib.MISSING)
                        return a - g;
-               return AltosLib.MISSING;
+               return gps_height();
        }
 
        public double max_height() {
@@ -476,7 +493,7 @@ public class AltosState implements Cloneable {
                double g = ground_altitude();
                if (a != AltosLib.MISSING && g != AltosLib.MISSING)
                        return a - g;
-               return AltosLib.MISSING;
+               return max_gps_height();
        }
 
        public double gps_height() {
@@ -500,7 +517,7 @@ public class AltosState implements Cloneable {
        class AltosSpeed extends AltosCValue {
 
                boolean can_max() {
-                       return state < AltosLib.ao_flight_fast;
+                       return state < AltosLib.ao_flight_fast || state == AltosLib.ao_flight_stateless;
                }
 
                void set_accel() {
@@ -529,20 +546,32 @@ public class AltosState implements Cloneable {
                double v = kalman_speed.value();
                if (v != AltosLib.MISSING)
                        return v;
-               return speed.value();
+               v = speed.value();
+               if (v != AltosLib.MISSING)
+                       return v;
+               v = gps_speed();
+               if (v != AltosLib.MISSING)
+                       return v;
+               return AltosLib.MISSING;
        }
 
        public double max_speed() {
                double v = kalman_speed.max();
                if (v != AltosLib.MISSING)
                        return v;
-               return speed.max();
+               v = speed.max();
+               if (v != AltosLib.MISSING)
+                       return v;
+               v = max_gps_speed();
+               if (v != AltosLib.MISSING)
+                       return v;
+               return AltosLib.MISSING;
        }
 
        class AltosAccel extends AltosCValue {
 
                boolean can_max() {
-                       return state < AltosLib.ao_flight_fast;
+                       return state < AltosLib.ao_flight_fast || state == AltosLib.ao_flight_stateless;
                }
 
                void set_measured(double a, double time) {
@@ -712,6 +741,7 @@ public class AltosState implements Cloneable {
                gps_altitude = new AltosGpsAltitude();
                gps_ground_altitude = new AltosGpsGroundAltitude();
                gps_ground_speed = new AltosValue();
+               gps_speed = new AltosValue();
                gps_ascent_rate = new AltosValue();
                gps_course = new AltosValue();
 
@@ -719,6 +749,7 @@ public class AltosState implements Cloneable {
                speak_altitude = AltosLib.MISSING;
 
                callsign = null;
+               firmware_version = null;
 
                accel_plus_g = AltosLib.MISSING;
                accel_minus_g = AltosLib.MISSING;
@@ -846,6 +877,7 @@ public class AltosState implements Cloneable {
                gps_ground_speed.copy(old.gps_ground_speed);
                gps_ascent_rate.copy(old.gps_ascent_rate);
                gps_course.copy(old.gps_course);
+               gps_speed.copy(old.gps_speed);
 
                pad_lat = old.pad_lat;
                pad_lon = old.pad_lon;
@@ -855,6 +887,7 @@ public class AltosState implements Cloneable {
                speak_altitude = old.speak_altitude;
 
                callsign = old.callsign;
+               firmware_version = old.firmware_version;
 
                accel_plus_g = old.accel_plus_g;
                accel_minus_g = old.accel_minus_g;
@@ -877,17 +910,17 @@ public class AltosState implements Cloneable {
        }
 
        void update_gps() {
-               elevation = 0;
-               range = -1;
+               elevation = AltosLib.MISSING;
+               range = AltosLib.MISSING;
 
                if (gps == null)
                        return;
 
                if (gps.locked && gps.nsat >= 4) {
                        /* Track consecutive 'good' gps reports, waiting for 10 of them */
-                       if (state == AltosLib.ao_flight_pad) {
+                       if (state == AltosLib.ao_flight_pad || state == AltosLib.ao_flight_stateless) {
                                set_npad(npad+1);
-                               if (pad_lat != AltosLib.MISSING) {
+                               if (pad_lat != AltosLib.MISSING && (npad < 10 || state == AltosLib.ao_flight_pad)) {
                                        pad_lat = (pad_lat * 31 + gps.lat) / 32;
                                        pad_lon = (pad_lon * 31 + gps.lon) / 32;
                                        gps_ground_altitude.set_filtered(gps.alt, time);
@@ -903,6 +936,9 @@ public class AltosState implements Cloneable {
                                gps_ascent_rate.set(gps.climb_rate, time);
                        if (gps.ground_speed != AltosLib.MISSING)
                                gps_ground_speed.set(gps.ground_speed, time);
+                       if (gps.climb_rate != AltosLib.MISSING && gps.ground_speed != AltosLib.MISSING)
+                               gps_speed.set(Math.sqrt(gps.ground_speed * gps.ground_speed +
+                                                       gps.climb_rate * gps.climb_rate), time);
                        if (gps.course != AltosLib.MISSING)
                                gps_course.set(gps.course, time);
                }
@@ -918,9 +954,6 @@ public class AltosState implements Cloneable {
                        elevation = from_pad.elevation;
                        range = from_pad.range;
                }
-
-               if ((gps.state & AltosGPS.AO_GPS_STATE_VALID) != 0)
-                       set_state (gps.state & ~(AltosGPS.AO_GPS_STATE_VALID));
        }
 
        public void set_tick(int new_tick) {
@@ -952,11 +985,24 @@ public class AltosState implements Cloneable {
                                  state <= AltosLib.ao_flight_coast);
                        boost = (AltosLib.ao_flight_boost == state);
                }
-
        }
 
        public void set_device_type(int device_type) {
                this.device_type = device_type;
+               switch (device_type) {
+               case AltosLib.product_telegps:
+                       this.state = AltosLib.ao_flight_stateless;
+                       break;
+               }
+       }
+
+       public void set_log_format(int log_format) {
+               this.log_format = log_format;
+               switch (log_format) {
+               case AltosLib.AO_LOG_FORMAT_TELEGPS:
+                       this.state = AltosLib.ao_flight_stateless;
+                       break;
+               }
        }
 
        public void set_flight_params(int apogee_delay, int main_deploy) {
@@ -978,15 +1024,21 @@ public class AltosState implements Cloneable {
                firmware_version = version;
        }
 
+       private void re_init() {
+               int bt = boost_tick;
+               int rs = receiver_serial;
+               init();
+               boost_tick = bt;
+               receiver_serial = rs;
+       }
+
        public void set_flight(int flight) {
 
                /* When the flight changes, reset the state */
-               if (flight != AltosLib.MISSING && flight != 0) {
+               if (flight != AltosLib.MISSING) {
                        if (this.flight != AltosLib.MISSING &&
                            this.flight != flight) {
-                               int bt = boost_tick;
-                               init();
-                               boost_tick = bt;
+                               re_init();
                        }
                        this.flight = flight;
                }
@@ -997,9 +1049,7 @@ public class AltosState implements Cloneable {
                if (serial != AltosLib.MISSING) {
                        if (this.serial != AltosLib.MISSING &&
                            this.serial != serial) {
-                               int bt = boost_tick;
-                               init();
-                               boost_tick = bt;
+                               re_init();
                        }
                        this.serial = serial;
                }