altoslib: Parse TeleGPS state value from GPS telemetry packet

[fw/altos] / altoslib / AltosState.java

diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java
index 9e8e22ac8d945168512db1266df11656c63cc9e4..ef3a09766adc68e69c60e09b725f2358aa56ffbc 100644 (file)
--- a/altoslib/AltosState.java
+++ b/altoslib/AltosState.java
@@ -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);
@@ -695,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;
@@ -824,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;
@@ -877,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 &&
@@ -890,6 +918,9 @@ 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) {