X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=ao-tools%2Faltosui%2FAltosState.java;h=1048bb51b09ca8faee0dccb4c4953850d135c2cf;hp=deeb4c771dcec9fb825d918bea922f75c3ff4024;hb=e66919aa46193bd8c7a1e86fb32a3367dae121f5;hpb=0e917f3ff822616adb147517ac961422e5fedbfd

diff --git a/ao-tools/altosui/AltosState.java b/ao-tools/altosui/AltosState.java
index deeb4c77..1048bb51 100644
--- a/ao-tools/altosui/AltosState.java
+++ b/ao-tools/altosui/AltosState.java
@@ -60,10 +60,13 @@ public class AltosState {
 	static final int MIN_PAD_SAMPLES = 10;
 
 	int	npad;
+	int	ngps;
 	int	gps_waiting;
 	boolean	gps_ready;
 
 	AltosGreatCircle from_pad;
+	double	elevation;	/* from pad */
+	double	range;		/* total distance */
 
 	double	gps_height;
 
@@ -78,7 +81,7 @@ public class AltosState {
 		data = cur;
 
 		ground_altitude = data.ground_altitude();
-		height = data.altitude() - ground_altitude;
+		height = data.filtered_altitude() - ground_altitude;
 
 		report_time = System.currentTimeMillis();
 
@@ -95,6 +98,7 @@ public class AltosState {
 
 			/* Preserve any existing gps data */
 			npad = prev_state.npad;
+			ngps = prev_state.ngps;
 			gps = prev_state.gps;
 			pad_lat = prev_state.pad_lat;
 			pad_lon = prev_state.pad_lon;
@@ -118,20 +122,23 @@ public class AltosState {
 				baro_speed = prev_state.baro_speed;
 		} else {
 			npad = 0;
+			ngps = 0;
 			gps = null;
 			baro_speed = 0;
 			time_change = 0;
 		}
 
 		if (state == Altos.ao_flight_pad) {
-			if (data.gps == null)
-				System.out.printf("on pad, gps null\n");
+
+			/* Track consecutive 'good' gps reports, waiting for 10 of them */
+			if (data.gps != null && data.gps.locked && data.gps.nsat >= 4)
+				npad++;
 			else
-				System.out.printf ("on pad gps lat %f lon %f locked %d nsat %d\n",
-						   data.gps.lat, data.gps.lon, data.gps.locked ? 1 : 0, data.gps.nsat);
+				npad = 0;
+
+			/* Average GPS data while on the pad */
 			if (data.gps != null && data.gps.locked && data.gps.nsat >= 4) {
-				npad++;
-				if (npad > 1) {
+				if (ngps > 1) {
 					/* filter pad position */
 					pad_lat = (pad_lat * 31.0 + data.gps.lat) / 32.0;
 					pad_lon = (pad_lon * 31.0 + data.gps.lon) / 32.0;
@@ -141,8 +148,7 @@ public class AltosState {
 					pad_lon = data.gps.lon;
 					pad_alt = data.gps.alt;
 				}
-			} else {
-				npad = 0;
+				ngps++;
 			}
 		}
 
@@ -166,11 +172,18 @@ public class AltosState {
 		if (data.gps != null) {
 			if (gps == null || !gps.locked || data.gps.locked)
 				gps = data.gps;
-			if (npad > 0 && gps.locked)
+			if (ngps > 0 && gps.locked) {
 				from_pad = new AltosGreatCircle(pad_lat, pad_lon, gps.lat, gps.lon);
+			}
 		}
-		if (npad > 0) {
+		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);
+			}
 		} else {
 			gps_height = 0;
 		}