X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=altoslib%2FAltosTelemetry.java;h=a123d7528145471a9944b1fa95fd6c9f7a611bc3;hp=4d50a05933100b0a7552eb89dca2a0d4fbc0d3e4;hb=8a2be4d36a3b116d82529805430c5fb665688267;hpb=bd440afc2a6e37b74fffcf1b977e149485095316

diff --git a/altoslib/AltosTelemetry.java b/altoslib/AltosTelemetry.java
index 4d50a059..a123d752 100644
--- a/altoslib/AltosTelemetry.java
+++ b/altoslib/AltosTelemetry.java
@@ -114,6 +114,29 @@ public abstract class AltosTelemetry implements AltosStateUpdate {
 		return telem;
 	}
 
+	public static int extend_height(AltosState state, int height_16) {
+		double	compare_height;
+		int	height = height_16;
+
+		if (state.gps != null && state.gps.alt != AltosLib.MISSING) {
+			compare_height = state.gps_height();
+		} else {
+			compare_height = state.height();
+		}
+
+		if (compare_height != AltosLib.MISSING) {
+			int	high_bits = (int) Math.floor (compare_height / 65536.0);
+
+			height = (high_bits << 16) | (height_16 & 0xffff);
+
+			if (Math.abs(height + 65536 - compare_height) < Math.abs(height - compare_height))
+				height += 65536;
+			else if (Math.abs(height - 65536 - compare_height) < Math.abs(height - compare_height))
+				height -= 65536;
+		}
+		return height;
+	}
+
 	public static AltosTelemetry parse(String line) throws ParseException, AltosCRCException {
 		String[] word = line.split("\\s+");
 		int i =0;