From b8fa4e9a077e8e04b922d0c434c139ad0a57ee66 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Thu, 10 Jul 2014 16:15:52 -0700 Subject: [PATCH] altoslib: Clean up GPS DOP support in AltosState Parse out hdop/pdop/vdop from telem and eeprom. Deal with legacy eeprom files that have dop/100 instead of dop/10 values. Clear state DOP values to MISSING at startup Signed-off-by: Keith Packard --- altoslib/AltosEepromGPS.java | 17 +++++++++++-- altoslib/AltosEepromMega.java | 17 +++++++++++-- altoslib/AltosEepromMetrum2.java | 2 ++ altoslib/AltosGPS.java | 36 +++++++++++++++++++--------- altoslib/AltosLib.java | 25 +++++++++++++++++++ altoslib/AltosState.java | 6 +++++ altoslib/AltosTelemetryLegacy.java | 2 ++ altoslib/AltosTelemetryLocation.java | 5 ++-- altoslib/AltosWriter.java | 2 -- 9 files changed, 93 insertions(+), 19 deletions(-) diff --git a/altoslib/AltosEepromGPS.java b/altoslib/AltosEepromGPS.java index 2514b4fc..46a2173d 100644 --- a/altoslib/AltosEepromGPS.java +++ b/altoslib/AltosEepromGPS.java @@ -118,8 +118,21 @@ public class AltosEepromGPS extends AltosEeprom { gps.ground_speed = ground_speed() * 1.0e-2; gps.course = course() * 2; gps.climb_rate = climb_rate() * 1.0e-2; - gps.hdop = hdop(); - gps.vdop = vdop(); + if (state.compare_version("1.4.9") >= 0) { + gps.pdop = pdop() / 10.0; + gps.hdop = hdop() / 10.0; + gps.vdop = vdop() / 10.0; + } else { + gps.pdop = pdop() / 100.0; + if (gps.pdop < 0.8) + gps.pdop += 2.56; + gps.hdop = hdop() / 100.0; + if (gps.hdop < 0.8) + gps.hdop += 2.56; + gps.vdop = vdop() / 100.0; + if (gps.vdop < 0.8) + gps.vdop += 2.56; + } break; } } diff --git a/altoslib/AltosEepromMega.java b/altoslib/AltosEepromMega.java index 5d5f3fef..1616de77 100644 --- a/altoslib/AltosEepromMega.java +++ b/altoslib/AltosEepromMega.java @@ -187,8 +187,21 @@ public class AltosEepromMega extends AltosEeprom { gps.ground_speed = ground_speed() * 1.0e-2; gps.course = course() * 2; gps.climb_rate = climb_rate() * 1.0e-2; - gps.hdop = hdop(); - gps.vdop = vdop(); + if (state.compare_version("1.4.9") >= 0) { + gps.pdop = pdop() / 10.0; + gps.hdop = hdop() / 10.0; + gps.vdop = vdop() / 10.0; + } else { + gps.pdop = pdop() / 100.0; + if (gps.pdop < 0.8) + gps.pdop += 2.56; + gps.hdop = hdop() / 100.0; + if (gps.hdop < 0.8) + gps.hdop += 2.56; + gps.vdop = vdop() / 100.0; + if (gps.vdop < 0.8) + gps.vdop += 2.56; + } break; case AltosLib.AO_LOG_GPS_SAT: state.set_tick(tick); diff --git a/altoslib/AltosEepromMetrum2.java b/altoslib/AltosEepromMetrum2.java index 24277118..2f43879e 100644 --- a/altoslib/AltosEepromMetrum2.java +++ b/altoslib/AltosEepromMetrum2.java @@ -59,6 +59,7 @@ public class AltosEepromMetrum2 extends AltosEeprom { public int year() { return data8(4); } public int month() { return data8(5); } public int day() { return data8(6); } + public int pdop() { return data8(7); } /* AO_LOG_GPS_SAT elements */ public int nsat() { return data8(0); } @@ -136,6 +137,7 @@ public class AltosEepromMetrum2 extends AltosEeprom { gps.year = 2000 + year(); gps.month = month(); gps.day = day(); + gps.pdop = pdop() / 10.0; break; case AltosLib.AO_LOG_GPS_SAT: gps = state.make_temp_gps(true); diff --git a/altoslib/AltosGPS.java b/altoslib/AltosGPS.java index aabcfc5b..0154e95d 100644 --- a/altoslib/AltosGPS.java +++ b/altoslib/AltosGPS.java @@ -40,10 +40,11 @@ public class AltosGPS implements Cloneable { public double ground_speed; /* m/s */ public int course; /* degrees */ public double climb_rate; /* m/s */ + public double pdop; /* unitless */ public double hdop; /* unitless */ public double vdop; /* unitless */ - public int h_error; /* m */ - public int v_error; /* m */ + public double h_error; /* m */ + public double v_error; /* m */ public AltosGPSSat[] cc_gps_sat; /* tracking data */ @@ -95,6 +96,7 @@ public class AltosGPS implements Cloneable { AltosLib.MISSING, 1/100.0); course = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_COURSE, AltosLib.MISSING); + pdop = map.get_double(AltosTelemetryLegacy.AO_TELEM_GPS_PDOP, MISSING, 1.0); hdop = map.get_double(AltosTelemetryLegacy.AO_TELEM_GPS_HDOP, MISSING, 1.0); vdop = map.get_double(AltosTelemetryLegacy.AO_TELEM_GPS_VDOP, MISSING, 1.0); h_error = map.get_int(AltosTelemetryLegacy.AO_TELEM_GPS_HERROR, MISSING); @@ -268,14 +270,26 @@ public class AltosGPS implements Cloneable { cc_gps_sat[cc_gps_sat.length - 1] = sat; } - public AltosGPS() { + private void init() { lat = AltosLib.MISSING; lon = AltosLib.MISSING; alt = AltosLib.MISSING; + ground_speed = AltosLib.MISSING; + course = AltosLib.MISSING; + climb_rate = AltosLib.MISSING; + pdop = AltosLib.MISSING; + hdop = AltosLib.MISSING; + vdop = AltosLib.MISSING; + h_error = AltosLib.MISSING; + v_error = AltosLib.MISSING; ClearGPSTime(); cc_gps_sat = null; } + public AltosGPS() { + init(); + } + public AltosGPS clone() { AltosGPS g = new AltosGPS(); @@ -295,7 +309,9 @@ public class AltosGPS implements Cloneable { g.ground_speed = ground_speed; /* m/s */ g.course = course; /* degrees */ g.climb_rate = climb_rate; /* m/s */ - g.hdop = hdop; /* unitless? */ + g.pdop = pdop; /* unitless */ + g.hdop = hdop; /* unitless */ + g.vdop = vdop; /* unitless */ g.h_error = h_error; /* m */ g.v_error = v_error; /* m */ @@ -327,9 +343,11 @@ public class AltosGPS implements Cloneable { ground_speed = old.ground_speed; /* m/s */ course = old.course; /* degrees */ climb_rate = old.climb_rate; /* m/s */ + pdop = old.pdop; /* unitless? */ hdop = old.hdop; /* unitless? */ - h_error = old.h_error; /* m */ - v_error = old.v_error; /* m */ + vdop = old.vdop; /* unitless? */ + h_error = old.h_error; /* m */ + v_error = old.v_error; /* m */ if (old.cc_gps_sat != null) { cc_gps_sat = new AltosGPSSat[old.cc_gps_sat.length]; @@ -340,11 +358,7 @@ public class AltosGPS implements Cloneable { } } } else { - lat = AltosLib.MISSING; - lon = AltosLib.MISSING; - alt = AltosLib.MISSING; - ClearGPSTime(); - cc_gps_sat = null; + init(); } } diff --git a/altoslib/AltosLib.java b/altoslib/AltosLib.java index c2ec0e3b..eb188d6b 100644 --- a/altoslib/AltosLib.java +++ b/altoslib/AltosLib.java @@ -215,6 +215,31 @@ public class AltosLib { telemetry)); } + private static int[] split_version(String version) { + String[] tokens = version.split("\\."); + int[] ret = new int[tokens.length]; + for (int i = 0; i < tokens.length; i++) + ret[i] = Integer.parseInt(tokens[i]); + return ret; + } + + public static int compare_version(String version_a, String version_b) { + int[] a = split_version(version_a); + int[] b = split_version(version_b); + + for (int i = 0; i < Math.min(a.length, b.length); i++) { + if (a[i] < b[i]) + return -1; + if (a[i] > b[i]) + return 1; + } + if (a.length < b.length) + return -1; + if (a.length > b.length) + return 1; + return 0; + } + private static String[] state_to_string = { "startup", "idle", diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java index 5e7908af..2fc13b44 100644 --- a/altoslib/AltosState.java +++ b/altoslib/AltosState.java @@ -1024,6 +1024,12 @@ public class AltosState implements Cloneable { firmware_version = version; } + public int compare_version(String other_version) { + if (firmware_version == null) + return AltosLib.MISSING; + return AltosLib.compare_version(firmware_version, other_version); + } + private void re_init() { int bt = boost_tick; int rs = receiver_serial; diff --git a/altoslib/AltosTelemetryLegacy.java b/altoslib/AltosTelemetryLegacy.java index b7aae3c4..72a8bc4a 100644 --- a/altoslib/AltosTelemetryLegacy.java +++ b/altoslib/AltosTelemetryLegacy.java @@ -186,6 +186,7 @@ public class AltosTelemetryLegacy extends AltosTelemetry { * g_v GPS vertical speed (integer, cm/sec) * g_s GPS horizontal speed (integer, cm/sec) * g_c GPS course (integer, 0-359) + * g_pd GPS pdop (integer * 10) * g_hd GPS hdop (integer * 10) * g_vd GPS vdop (integer * 10) * g_he GPS h error (integer) @@ -209,6 +210,7 @@ public class AltosTelemetryLegacy extends AltosTelemetry { final static String AO_TELEM_GPS_VERTICAL_SPEED = "g_v"; final static String AO_TELEM_GPS_HORIZONTAL_SPEED = "g_g"; final static String AO_TELEM_GPS_COURSE = "g_c"; + final static String AO_TELEM_GPS_PDOP = "g_pd"; final static String AO_TELEM_GPS_HDOP = "g_hd"; final static String AO_TELEM_GPS_VDOP = "g_vd"; final static String AO_TELEM_GPS_HERROR = "g_he"; diff --git a/altoslib/AltosTelemetryLocation.java b/altoslib/AltosTelemetryLocation.java index 32ca7608..9d50e2fa 100644 --- a/altoslib/AltosTelemetryLocation.java +++ b/altoslib/AltosTelemetryLocation.java @@ -80,8 +80,9 @@ public class AltosTelemetryLocation extends AltosTelemetryStandard { gps.ground_speed = ground_speed * 1.0e-2; gps.course = course * 2; gps.climb_rate = climb_rate * 1.0e-2; - gps.hdop = hdop; - gps.vdop = vdop; + gps.pdop = pdop / 10.0; + gps.hdop = hdop / 10.0; + gps.vdop = vdop / 10.0; } state.set_temp_gps(); } diff --git a/altoslib/AltosWriter.java b/altoslib/AltosWriter.java index 9df52250..fb208b02 100644 --- a/altoslib/AltosWriter.java +++ b/altoslib/AltosWriter.java @@ -19,8 +19,6 @@ package org.altusmetrum.altoslib_5; public interface AltosWriter { - public void write(AltosState state); - public void write(AltosStateIterable states); public void close(); -- 2.30.2