altoslib: Update GPS state even if new state is unlocked

[fw/altos] / altoslib / AltosState.java

diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java
index f1bcb1c1a67a7e87ee5a3ef4cc70e1ce29c44351..ccbe498d5ca34dedaa3ee7be58512955f93e16d2 100644 (file)
--- a/altoslib/AltosState.java
+++ b/altoslib/AltosState.java
@@ -248,23 +248,20 @@ public class AltosState {
 
                if (height != AltosRecord.MISSING && height > max_height)
                        max_height = height;
-               if (data.gps != null) {
-                       if (gps == null || !gps.locked || data.gps.locked)
-                               gps = data.gps;
-                       if (ngps > 0 && gps.locked) {
-                               from_pad = new AltosGreatCircle(pad_lat, pad_lon, gps.lat, gps.lon);
-                       }
-               }
                elevation = 0;
                range = -1;
-               if (ngps > 0) {
-                       gps_height = gps.alt - pad_alt;
-                       if (from_pad != null) {
-                               elevation = Math.atan2(height, from_pad.distance) * 180 / Math.PI;
-                               range = Math.sqrt(height * height + from_pad.distance * from_pad.distance);
+               gps_height = 0;
+               if (data.gps != null) {
+                       gps = data.gps;
+                       if (ngps > 0 && gps.locked) {
+                               double h = height;
+
+                               if (h == AltosRecord.MISSING) h = 0;
+                               from_pad = new AltosGreatCircle(pad_lat, pad_lon, 0, gps.lat, gps.lon, h);
+                               elevation = from_pad.elevation;
+                               range = from_pad.range;
+                               gps_height = gps.alt - pad_alt;
                        }
-               } else {
-                       gps_height = 0;
                }
        }