From: Keith Packard Date: Fri, 11 Jul 2014 00:27:43 +0000 (-0700) Subject: altoslib: Extend telemetry heights from 16 to 32 bits X-Git-Tag: 1.4.9.2~13 X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=commitdiff_plain;h=59702e5ff8d0522b0aa9dcca863309eaafbcda09 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 --- 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 { public int log_format; public int log_space; public String version; + public int altitude_32; /* Strings returned */ public LinkedList lines; @@ -274,6 +275,7 @@ public class AltosConfigData implements Iterable { 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 { 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)); diff --git a/altosui/AltosConfigUI.java b/altosui/AltosConfigUI.java index 043cb876..9fcace61 100644 --- 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);