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>

diff --git a/altoslib/AltosConfigData.java b/altoslib/AltosConfigData.java
index 847436cd0b0abc2d68a2fc0b1e7dec18b6686a91..134a0ed2ce0e1d7b3196794b1a45a57187475df7 100644 (file)
--- 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 987da53be7e0bda21519f033bf7dd00324e25674..3f0a70758bf20ede9539ec3912f968b16079bab2 100644 (file)
--- 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 2fc13b446882c56511450a92da9ee530f30b4e00..5fce15c43865e63b43447db93f59faf725f838f7 100644 (file)
--- 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 4d50a05933100b0a7552eb89dca2a0d4fbc0d3e4..2f15cd896effa8be6ec36b1c809927da97ff168c 100644 (file)
--- 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 93610118311cf841b68bebe6026dc9e2fc14c933..8b1869bb65f3141fdc83a74496b97fdaa3fb0995 100644 (file)
--- 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 3e0abedc086c9eec6634c7b5e8b01879d104b3b0..beab6da99f18e9a3180b321e735be57f1c7d2620 100644 (file)
--- 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));
 

diff --git a/altosui/AltosConfigUI.java b/altosui/AltosConfigUI.java
index 043cb876f4571a993c929f0ee08e2d5adb2ce570..9fcace610a7212c604bbe7785fcf13779838aaa8 100644 (file)
--- a/altosui/AltosConfigUI.java
+++ b/altosui/AltosConfigUI.java
@@ -926,6 +926,9 @@ public class AltosConfigUI
                serial_value.setText(String.format("%d", serial));
        }
 
+       public void set_altitude_32(int altitude_32) {
+       }
+
        public void set_main_deploy(int new_main_deploy) {
                main_deploy_value.setSelectedItem(AltosConvert.height.say(new_main_deploy));
                main_deploy_value.setEnabled(new_main_deploy >= 0);