altoslib: Extend telemetry heights from 16 to 32 bits

Uses the GPS data and/or previous kalman data to compute the upper 16
bits of the truncated telemetry altitude value.

Signed-off-by: Keith Packard <keithp@keithp.com>

6 files changed, 72 insertions, 10 deletions

diff --git a/altoslib/AltosConfigData.java b/altoslib/AltosConfigData.java
index 847436cd..134a0ed2 100644
--- a/altoslib/AltosConfigData.java
+++ b/altoslib/AltosConfigData.java
@@ -31,6 +31,7 @@ public class AltosConfigData implements Iterable<String> {
 	public int	log_format;
 	public int	log_space;
 	public String	version;
+	public int	altitude_32;
 
 	/* Strings returned */
 	public LinkedList<String>	lines;
@@ -274,6 +275,7 @@ public class AltosConfigData implements Iterable<String> {
 		try { flight = get_int(line, "current-flight"); } catch (Exception e) {}
 		try { log_format = get_int(line, "log-format"); } catch (Exception e) {}
 		try { log_space = get_int(line, "log-space"); } catch (Exception e) {}
+		try { altitude_32 = get_int(line, "altitude-32"); } catch (Exception e) {}
 		try { version = get_string(line, "software-version"); } catch (Exception e) {}
 
 		/* Version also contains MS5607 info, which we ignore here */
@@ -484,6 +486,7 @@ public class AltosConfigData implements Iterable<String> {
 		dest.set_serial(serial);
 		dest.set_product(product);
 		dest.set_version(version);
+		dest.set_altitude_32(altitude_32);
 		dest.set_main_deploy(main_deploy);
 		dest.set_apogee_delay(apogee_delay);
 		dest.set_apogee_lockout(apogee_lockout);
diff --git a/altoslib/AltosConfigValues.java b/altoslib/AltosConfigValues.java
index 987da53b..3f0a7075 100644
--- a/altoslib/AltosConfigValues.java
+++ b/altoslib/AltosConfigValues.java
@@ -25,6 +25,8 @@ public interface AltosConfigValues {
 
 	public abstract void set_serial(int serial);
 
+	public abstract void set_altitude_32(int altitude_32);
+
 	public abstract void set_main_deploy(int new_main_deploy);
 
 	public abstract int main_deploy() throws AltosConfigDataException;
diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java
index 2fc13b44..5fce15c4 100644
--- a/altoslib/AltosState.java
+++ b/altoslib/AltosState.java
@@ -271,6 +271,7 @@ public class AltosState implements Cloneable {
 	public int	state;
 	public int	flight;
 	public int	serial;
+	public int	altitude_32;
 	public int	receiver_serial;
 	public boolean	landed;
 	public boolean	ascent;	/* going up? */
@@ -472,15 +473,23 @@ public class AltosState implements Cloneable {
 		pressure.set(p, time);
 	}
 
+	public double baro_height() {
+		double a = altitude();
+		double g = ground_altitude();
+		if (a != AltosLib.MISSING && g != AltosLib.MISSING)
+			return a - g;
+		return AltosLib.MISSING;
+	}
+
 	public double height() {
 		double k = kalman_height.value();
 		if (k != AltosLib.MISSING)
 			return k;
 
-		double a = altitude();
-		double g = ground_altitude();
-		if (a != AltosLib.MISSING && g != AltosLib.MISSING)
-			return a - g;
+		double b = baro_height();
+		if (b != AltosLib.MISSING)
+			return b;
+
 		return gps_height();
 	}
 
@@ -762,6 +771,7 @@ public class AltosState implements Cloneable {
 		product = null;
 		serial = AltosLib.MISSING;
 		receiver_serial = AltosLib.MISSING;
+		altitude_32 = AltosLib.MISSING;
 
 		baro = null;
 		companion = null;
@@ -899,6 +909,7 @@ public class AltosState implements Cloneable {
 		product = old.product;
 		serial = old.serial;
 		receiver_serial = old.receiver_serial;
+		altitude_32 = old.altitude_32;
 
 		baro = old.baro;
 		companion = old.companion;
@@ -1066,6 +1077,15 @@ public class AltosState implements Cloneable {
 			receiver_serial = serial;
 	}
 
+	public boolean altitude_32() {
+		return altitude_32 == 1;
+	}
+
+	public void set_altitude_32(int altitude_32) {
+		if (altitude_32 != AltosLib.MISSING)
+			this.altitude_32 = altitude_32;
+	}
+
 	public int rssi() {
 		if (rssi == AltosLib.MISSING)
 			return 0;
diff --git a/altoslib/AltosTelemetry.java b/altoslib/AltosTelemetry.java
index 4d50a059..2f15cd89 100644
--- a/altoslib/AltosTelemetry.java
+++ b/altoslib/AltosTelemetry.java
@@ -114,6 +114,35 @@ 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;
+
+		System.out.printf("state kalman height %g altitude %g ground_altitude %g gps_height %g\n",
+				  state.kalman_height.value(), state.altitude(), state.ground_altitude(), state.gps_height());
+		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;
+			if (height != height_16) {
+				System.out.printf("Height adjusted from %d to %d with %g\n",
+						  height_16, height, compare_height);
+			}
+		}
+		return height;
+	}
+
 	public static AltosTelemetry parse(String line) throws ParseException, AltosCRCException {
 		String[] word = line.split("\\s+");
 		int i =0;
diff --git a/altoslib/AltosTelemetryMegaData.java b/altoslib/AltosTelemetryMegaData.java
index 93610118..8b1869bb 100644
--- a/altoslib/AltosTelemetryMegaData.java
+++ b/altoslib/AltosTelemetryMegaData.java
@@ -31,7 +31,7 @@ public class AltosTelemetryMegaData extends AltosTelemetryStandard {
 
 	int	acceleration;
 	int	speed;
-	int	height;
+	int	height_16;
 
 	public AltosTelemetryMegaData(int[] bytes) {
 		super(bytes);
@@ -55,7 +55,8 @@ public class AltosTelemetryMegaData extends AltosTelemetryStandard {
 
 		acceleration = int16(26);
 		speed = int16(28);
-		height = int16(30);
+
+		height_16 = int16(30);
 	}
 
 	public void update_state(AltosState state) {
@@ -79,7 +80,13 @@ public class AltosTelemetryMegaData extends AltosTelemetryStandard {
 		state.set_ground_pressure(ground_pres);
 		state.set_accel_g(accel_plus_g, accel_minus_g);
 
-		state.set_kalman(height, speed/16.0, acceleration / 16.0);
+		/* Fill in the high bits of height from recent GPS
+		 * data if available, otherwise guess using the
+		 * previous kalman height
+		 */
+
+		state.set_kalman(extend_height(state, height_16),
+				 speed/16.0, acceleration / 16.0);
 	}
 }
 
diff --git a/altoslib/AltosTelemetryMetrumSensor.java b/altoslib/AltosTelemetryMetrumSensor.java
index 3e0abedc..beab6da9 100644
--- a/altoslib/AltosTelemetryMetrumSensor.java
+++ b/altoslib/AltosTelemetryMetrumSensor.java
@@ -27,7 +27,7 @@ public class AltosTelemetryMetrumSensor extends AltosTelemetryStandard {
 
 	int	acceleration;
 	int	speed;
-	int	height;
+	int	height_16;
 
 	int	v_batt;
 	int	sense_a;
@@ -43,7 +43,7 @@ public class AltosTelemetryMetrumSensor extends AltosTelemetryStandard {
 
 		acceleration  = int16(14);
 		speed         = int16(16);
-		height        = int16(18);
+		height_16     = int16(18);
 
 		v_batt        = int16(20);
 		sense_a       = int16(22);
@@ -59,7 +59,8 @@ public class AltosTelemetryMetrumSensor extends AltosTelemetryStandard {
 		state.set_pressure(pres);
 		state.set_temperature(temp/100.0);
 
-		state.set_kalman(height, speed/16.0, acceleration/16.0);
+		state.set_kalman(extend_height(state, height_16),
+				 speed/16.0, acceleration/16.0);
 
 		state.set_battery_voltage(AltosConvert.mega_battery_voltage(v_batt));