X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=altoslib%2FAltosState.java;h=e0c00602c7cdb4827571f4a7df326c59b720a00b;hp=4dbd751bee8d74f78562f4832d2be0fcf656897e;hb=ca1c62ca3ea016a1a5baa4520e484c102280c3e3;hpb=ecebb3902868d1d7485d2bc99ba4140c6b90567e diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java index 4dbd751b..e0c00602 100644 --- a/altoslib/AltosState.java +++ b/altoslib/AltosState.java @@ -19,7 +19,7 @@ * Track flight state from telemetry or eeprom data stream */ -package org.altusmetrum.altoslib_3; +package org.altusmetrum.altoslib_5; public class AltosState implements Cloneable { @@ -389,6 +389,11 @@ public class AltosState implements Cloneable { private AltosGpsAltitude gps_altitude; + private AltosValue gps_ground_speed; + private AltosValue gps_ascent_rate; + private AltosValue gps_course; + private AltosValue gps_speed; + public double altitude() { double a = altitude.value(); if (a != AltosLib.MISSING) @@ -419,6 +424,34 @@ public class AltosState implements Cloneable { gps_altitude.set(new_gps_altitude, time); } + public double gps_ground_speed() { + return gps_ground_speed.value(); + } + + public double max_gps_ground_speed() { + return gps_ground_speed.max(); + } + + public double gps_ascent_rate() { + return gps_ascent_rate.value(); + } + + public double max_gps_ascent_rate() { + return gps_ascent_rate.max(); + } + + public double gps_course() { + return gps_course.value(); + } + + public double gps_speed() { + return gps_speed.value(); + } + + public double max_gps_speed() { + return gps_speed.max(); + } + class AltosPressure extends AltosValue { void set(double p, double time) { super.set(p, time); @@ -448,7 +481,7 @@ public class AltosState implements Cloneable { double g = ground_altitude(); if (a != AltosLib.MISSING && g != AltosLib.MISSING) return a - g; - return AltosLib.MISSING; + return gps_height(); } public double max_height() { @@ -460,7 +493,7 @@ public class AltosState implements Cloneable { double g = ground_altitude(); if (a != AltosLib.MISSING && g != AltosLib.MISSING) return a - g; - return AltosLib.MISSING; + return max_gps_height(); } public double gps_height() { @@ -484,7 +517,7 @@ public class AltosState implements Cloneable { class AltosSpeed extends AltosCValue { boolean can_max() { - return state < AltosLib.ao_flight_fast; + return state < AltosLib.ao_flight_fast || state == AltosLib.ao_flight_stateless; } void set_accel() { @@ -513,20 +546,32 @@ public class AltosState implements Cloneable { double v = kalman_speed.value(); if (v != AltosLib.MISSING) return v; - return speed.value(); + v = speed.value(); + if (v != AltosLib.MISSING) + return v; + v = gps_speed(); + if (v != AltosLib.MISSING) + return v; + return AltosLib.MISSING; } public double max_speed() { double v = kalman_speed.max(); if (v != AltosLib.MISSING) return v; - return speed.max(); + v = speed.max(); + if (v != AltosLib.MISSING) + return v; + v = max_gps_speed(); + if (v != AltosLib.MISSING) + return v; + return AltosLib.MISSING; } class AltosAccel extends AltosCValue { boolean can_max() { - return state < AltosLib.ao_flight_fast; + return state < AltosLib.ao_flight_fast || state == AltosLib.ao_flight_stateless; } void set_measured(double a, double time) { @@ -614,6 +659,7 @@ public class AltosState implements Cloneable { public double ground_accel_avg; public int log_format; + public String product; public AltosMs5607 baro; @@ -694,11 +740,16 @@ public class AltosState implements Cloneable { gps_altitude = new AltosGpsAltitude(); gps_ground_altitude = new AltosGpsGroundAltitude(); + gps_ground_speed = new AltosValue(); + gps_speed = new AltosValue(); + gps_ascent_rate = new AltosValue(); + gps_course = new AltosValue(); speak_tick = AltosLib.MISSING; speak_altitude = AltosLib.MISSING; callsign = null; + firmware_version = null; accel_plus_g = AltosLib.MISSING; accel_minus_g = AltosLib.MISSING; @@ -708,6 +759,7 @@ public class AltosState implements Cloneable { ground_accel_avg = AltosLib.MISSING; log_format = AltosLib.MISSING; + product = null; serial = AltosLib.MISSING; receiver_serial = AltosLib.MISSING; @@ -822,6 +874,10 @@ public class AltosState implements Cloneable { gps_altitude.copy(old.gps_altitude); gps_ground_altitude.copy(old.gps_ground_altitude); + gps_ground_speed.copy(old.gps_ground_speed); + gps_ascent_rate.copy(old.gps_ascent_rate); + gps_course.copy(old.gps_course); + gps_speed.copy(old.gps_speed); pad_lat = old.pad_lat; pad_lon = old.pad_lon; @@ -831,6 +887,7 @@ public class AltosState implements Cloneable { speak_altitude = old.speak_altitude; callsign = old.callsign; + firmware_version = old.firmware_version; accel_plus_g = old.accel_plus_g; accel_minus_g = old.accel_minus_g; @@ -839,6 +896,7 @@ public class AltosState implements Cloneable { ground_accel_avg = old.ground_accel_avg; log_format = old.log_format; + product = old.product; serial = old.serial; receiver_serial = old.receiver_serial; @@ -852,17 +910,17 @@ public class AltosState implements Cloneable { } void update_gps() { - elevation = 0; - range = -1; + elevation = AltosLib.MISSING; + range = AltosLib.MISSING; if (gps == null) return; if (gps.locked && gps.nsat >= 4) { /* Track consecutive 'good' gps reports, waiting for 10 of them */ - if (state == AltosLib.ao_flight_pad) { + if (state == AltosLib.ao_flight_pad || state == AltosLib.ao_flight_stateless) { set_npad(npad+1); - if (pad_lat != AltosLib.MISSING) { + if (pad_lat != AltosLib.MISSING && (npad < 10 || state == AltosLib.ao_flight_pad)) { pad_lat = (pad_lat * 31 + gps.lat) / 32; pad_lon = (pad_lon * 31 + gps.lon) / 32; gps_ground_altitude.set_filtered(gps.alt, time); @@ -874,6 +932,15 @@ public class AltosState implements Cloneable { gps_ground_altitude.set(gps.alt, time); } gps_altitude.set(gps.alt, time); + if (gps.climb_rate != AltosLib.MISSING) + gps_ascent_rate.set(gps.climb_rate, time); + if (gps.ground_speed != AltosLib.MISSING) + gps_ground_speed.set(gps.ground_speed, time); + if (gps.climb_rate != AltosLib.MISSING && gps.ground_speed != AltosLib.MISSING) + gps_speed.set(Math.sqrt(gps.ground_speed * gps.ground_speed + + gps.climb_rate * gps.climb_rate), time); + if (gps.course != AltosLib.MISSING) + gps_course.set(gps.course, time); } if (gps.lat != 0 && gps.lon != 0 && pad_lat != AltosLib.MISSING && @@ -918,18 +985,34 @@ public class AltosState implements Cloneable { state <= AltosLib.ao_flight_coast); boost = (AltosLib.ao_flight_boost == state); } - } public void set_device_type(int device_type) { this.device_type = device_type; + switch (device_type) { + case AltosLib.product_telegps: + this.state = AltosLib.ao_flight_stateless; + break; + } } - public void set_config(int major, int minor, int apogee_delay, int main_deploy, int flight_log_max) { - config_major = major; - config_minor = minor; + public void set_log_format(int log_format) { + this.log_format = log_format; + switch (log_format) { + case AltosLib.AO_LOG_FORMAT_TELEGPS: + this.state = AltosLib.ao_flight_stateless; + break; + } + } + + public void set_flight_params(int apogee_delay, int main_deploy) { this.apogee_delay = apogee_delay; this.main_deploy = main_deploy; + } + + public void set_config(int major, int minor, int flight_log_max) { + config_major = major; + config_minor = minor; this.flight_log_max = flight_log_max; }