summaryrefslogtreecommitdiff
path: root/altoslib
diff options
context:
space:
mode:
Diffstat (limited to 'altoslib')
-rw-r--r--altoslib/AltosConfigData.java3
-rw-r--r--altoslib/AltosConfigValues.java2
-rw-r--r--altoslib/AltosState.java28
-rw-r--r--altoslib/AltosTelemetry.java29
-rw-r--r--altoslib/AltosTelemetryMegaData.java13
-rw-r--r--altoslib/AltosTelemetryMetrumSensor.java7
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));
generated by cgit v1.2.3 (git 2.39.1) at 2025-10-31 12:01:02 +0000