altoslib: Add new 'stateless' flight state for TeleGPS

[fw/altos] / altoslib / AltosState.java

diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java
index 4dbd751bee8d74f78562f4832d2be0fcf656897e..2d75f72e676881b45cbc63b9ec9e13dfb90308b4 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_3;
+package org.altusmetrum.altoslib_4;
 
 public class AltosState implements Cloneable {
 
@@ -389,6 +389,10 @@ public class AltosState implements Cloneable {
 
        private AltosGpsAltitude        gps_altitude;
 
+       private AltosValue              gps_ground_speed;
+       private AltosValue              gps_ascent_rate;
+       private AltosValue              gps_course;
+
        public double altitude() {
                double a = altitude.value();
                if (a != AltosLib.MISSING)
@@ -419,6 +423,18 @@ public class AltosState implements Cloneable {
                gps_altitude.set(new_gps_altitude, time);
        }
 
+       public double gps_ground_speed() {
+               return gps_ground_speed.value();
+       }
+
+       public double gps_ascent_rate() {
+               return gps_ascent_rate.value();
+       }
+
+       public double gps_course() {
+               return gps_course.value();
+       }
+
        class AltosPressure extends AltosValue {
                void set(double p, double time) {
                        super.set(p, time);
@@ -484,7 +500,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() {
@@ -526,7 +542,7 @@ public class AltosState implements Cloneable {
        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) {
@@ -614,6 +630,7 @@ public class AltosState implements Cloneable {
        public double   ground_accel_avg;
 
        public int      log_format;
+       public String   product;
 
        public AltosMs5607      baro;
 
@@ -694,6 +711,9 @@ public class AltosState implements Cloneable {
 
                gps_altitude = new AltosGpsAltitude();
                gps_ground_altitude = new AltosGpsGroundAltitude();
+               gps_ground_speed = new AltosValue();
+               gps_ascent_rate = new AltosValue();
+               gps_course = new AltosValue();
 
                speak_tick = AltosLib.MISSING;
                speak_altitude = AltosLib.MISSING;
@@ -708,6 +728,7 @@ public class AltosState implements Cloneable {
                ground_accel_avg = AltosLib.MISSING;
 
                log_format = AltosLib.MISSING;
+               product = null;
                serial = AltosLib.MISSING;
                receiver_serial = AltosLib.MISSING;
 
@@ -822,6 +843,9 @@ public class AltosState implements Cloneable {
 
                gps_altitude.copy(old.gps_altitude);
                gps_ground_altitude.copy(old.gps_ground_altitude);
+               gps_ground_speed.copy(old.gps_ground_speed);
+               gps_ascent_rate.copy(old.gps_ascent_rate);
+               gps_course.copy(old.gps_course);
 
                pad_lat = old.pad_lat;
                pad_lon = old.pad_lon;
@@ -839,6 +863,7 @@ public class AltosState implements Cloneable {
                ground_accel_avg = old.ground_accel_avg;
 
                log_format = old.log_format;
+               product = old.product;
                serial = old.serial;
                receiver_serial = old.receiver_serial;
 
@@ -860,9 +885,9 @@ public class AltosState implements Cloneable {
 
                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);
@@ -874,6 +899,12 @@ public class AltosState implements Cloneable {
                                gps_ground_altitude.set(gps.alt, time);
                        }
                        gps_altitude.set(gps.alt, time);
+                       if (gps.climb_rate != AltosLib.MISSING)
+                               gps_ascent_rate.set(gps.climb_rate, time);
+                       if (gps.ground_speed != AltosLib.MISSING)
+                               gps_ground_speed.set(gps.ground_speed, time);
+                       if (gps.course != AltosLib.MISSING)
+                               gps_course.set(gps.course, time);
                }
                if (gps.lat != 0 && gps.lon != 0 &&
                    pad_lat != AltosLib.MISSING &&
@@ -918,18 +949,34 @@ 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_config(int major, int minor, int apogee_delay, int main_deploy, int flight_log_max) {
-               config_major = major;
-               config_minor = minor;
+       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) {
                this.apogee_delay = apogee_delay;
                this.main_deploy = main_deploy;
+       }
+
+       public void set_config(int major, int minor, int flight_log_max) {
+               config_major = major;
+               config_minor = minor;
                this.flight_log_max = flight_log_max;
        }