altoslib: Get KML export working again

Even annotate the states with avg speed/accel for fun.

Signed-off-by: Keith Packard <keithp@keithp.com>

10 files changed, 73 insertions, 93 deletions

diff --git a/altoslib/AltosCalData.java b/altoslib/AltosCalData.java
index 3da0e400..fff6ba67 100644
--- a/altoslib/AltosCalData.java
+++ b/altoslib/AltosCalData.java
@@ -206,9 +206,13 @@ public class AltosCalData {
 
 	public AltosGPS		gps_pad = null;
 
+	public double		gps_pad_altitude = AltosLib.MISSING;
+
 	public void set_gps(AltosGPS gps) {
 		if ((state != AltosLib.MISSING && state < AltosLib.ao_flight_boost) || gps_pad == null)
 			gps_pad = gps;
+		if (gps_pad_altitude == AltosLib.MISSING && gps.alt != AltosLib.MISSING)
+			gps_pad_altitude = gps.alt;
 	}
 
 	/*
@@ -226,8 +230,8 @@ public class AltosCalData {
 		if (temp_gps != null) {
 			if (temp_gps.locked && temp_gps.nsat >= 4)
 				set_gps(temp_gps);
+			temp_gps = null;
 		}
-		temp_gps = null;
 	}
 
 	public boolean gps_pending() {
@@ -235,9 +239,8 @@ public class AltosCalData {
 	}
 
 	public AltosGPS make_temp_gps(int tick, boolean sats) {
-		if (temp_gps == null) {
+		if (temp_gps == null)
 			temp_gps = new AltosGPS();
-		}
 		if (sats) {
 			if (tick != temp_gps_sat_tick)
 				temp_gps.cc_gps_sat = null;
diff --git a/altoslib/AltosEepromRecord.java b/altoslib/AltosEepromRecord.java
index 7dd37592..7a0cc8f9 100644
--- a/altoslib/AltosEepromRecord.java
+++ b/altoslib/AltosEepromRecord.java
@@ -87,6 +87,15 @@ public abstract class AltosEepromRecord implements Comparable<AltosEepromRecord>
 		if (cmd() == AltosLib.AO_LOG_FLIGHT)
 			cal_data.set_boost_tick();
 		listener.set_time(cal_data.time());
+
+		/* Flush any pending GPS changes */
+		if (!AltosLib.is_gps_cmd(cmd())) {
+			AltosGPS gps = cal_data.temp_gps();
+			if (gps != null) {
+				listener.set_gps(gps);
+				cal_data.reset_temp_gps();
+			}
+		}
 	}
 
 	public int next_start() {
diff --git a/altoslib/AltosEepromRecordFull.java b/altoslib/AltosEepromRecordFull.java
index b4968220..c29da025 100644
--- a/altoslib/AltosEepromRecordFull.java
+++ b/altoslib/AltosEepromRecordFull.java
@@ -26,22 +26,6 @@ public class AltosEepromRecordFull extends AltosEepromRecord {
 		super.provide_data(listener, cal_data);
 		AltosGPS	gps;
 
-		/* Flush any pending GPS changes */
-		if (cal_data.gps_pending()) {
-			switch (cmd()) {
-			case AltosLib.AO_LOG_GPS_LAT:
-			case AltosLib.AO_LOG_GPS_LON:
-			case AltosLib.AO_LOG_GPS_ALT:
-			case AltosLib.AO_LOG_GPS_SAT:
-			case AltosLib.AO_LOG_GPS_DATE:
-				break;
-			default:
-				listener.set_gps(cal_data.temp_gps());
-				cal_data.reset_temp_gps();
-				break;
-			}
-		}
-
 		switch (cmd()) {
 		case AltosLib.AO_LOG_FLIGHT:
 			listener.set_state(AltosLib.ao_flight_pad);
diff --git a/altoslib/AltosEepromRecordMega.java b/altoslib/AltosEepromRecordMega.java
index cd6916b4..0abc3fe7 100644
--- a/altoslib/AltosEepromRecordMega.java
+++ b/altoslib/AltosEepromRecordMega.java
@@ -114,22 +114,6 @@ public class AltosEepromRecordMega extends AltosEepromRecord {
 
 		AltosGPS	gps;
 
-		/* Flush any pending GPS changes */
-		if (cal_data.gps_pending()) {
-			switch (cmd()) {
-			case AltosLib.AO_LOG_GPS_LAT:
-			case AltosLib.AO_LOG_GPS_LON:
-			case AltosLib.AO_LOG_GPS_ALT:
-			case AltosLib.AO_LOG_GPS_SAT:
-			case AltosLib.AO_LOG_GPS_DATE:
-				break;
-			default:
-				listener.set_gps(cal_data.temp_gps());
-				cal_data.reset_temp_gps();
-				break;
-			}
-		}
-
 		switch (cmd()) {
 		case AltosLib.AO_LOG_FLIGHT:
 			cal_data.set_flight(flight());
diff --git a/altoslib/AltosEepromRecordMetrum.java b/altoslib/AltosEepromRecordMetrum.java
index 97a1103d..248709b3 100644
--- a/altoslib/AltosEepromRecordMetrum.java
+++ b/altoslib/AltosEepromRecordMetrum.java
@@ -70,23 +70,6 @@ public class AltosEepromRecordMetrum extends AltosEepromRecord {
 
 		AltosGPS	gps;
 
-		/* Flush any pending GPS changes */
-		if (cal_data.gps_pending()) {
-			switch (cmd()) {
-			case AltosLib.AO_LOG_GPS_POS:
-			case AltosLib.AO_LOG_GPS_LAT:
-			case AltosLib.AO_LOG_GPS_LON:
-			case AltosLib.AO_LOG_GPS_ALT:
-			case AltosLib.AO_LOG_GPS_SAT:
-			case AltosLib.AO_LOG_GPS_DATE:
-				break;
-			default:
-				listener.set_gps(cal_data.temp_gps());
-				cal_data.reset_temp_gps();
-				break;
-			}
-		}
-
 		switch (cmd()) {
 		case AltosLib.AO_LOG_FLIGHT:
 			cal_data.set_flight(flight());
diff --git a/altoslib/AltosFlightSeries.java b/altoslib/AltosFlightSeries.java
index 6f4f7bb1..944cff31 100644
--- a/altoslib/AltosFlightSeries.java
+++ b/altoslib/AltosFlightSeries.java
@@ -86,6 +86,14 @@ public class AltosFlightSeries extends AltosDataListener {
 		return AltosLib.MISSING;
 	}
 
+	public double value(String name, double time) {
+		for (AltosTimeSeries s : series) {
+			if (s.label.equals(name))
+				return s.value(time);
+		}
+		return AltosLib.MISSING;
+	}
+
 	public double value_before(String name, double time) {
 		for (AltosTimeSeries s : series) {
 			if (s.label.equals(name))
diff --git a/altoslib/AltosFlightStats.java b/altoslib/AltosFlightStats.java
index 32beb8b5..9ebdb9e6 100644
--- a/altoslib/AltosFlightStats.java
+++ b/altoslib/AltosFlightStats.java
@@ -109,7 +109,7 @@ public class AltosFlightStats {
 	}
 
 
-	public AltosFlightStats(AltosFlightSeries series) throws InterruptedException, IOException {
+	public AltosFlightStats(AltosFlightSeries series) {
 		AltosCalData	cal_data = series.cal_data;
 		double		boost_time = boost_time(series);
 		double		end_time = 0;
diff --git a/altoslib/AltosKML.java b/altoslib/AltosKML.java
index 61e83daa..2ab91d3e 100644
--- a/altoslib/AltosKML.java
+++ b/altoslib/AltosKML.java
@@ -37,7 +37,8 @@ public class AltosKML implements AltosWriter {
 	PrintWriter		out;
 	int			flight_state = -1;
 	AltosGPS		prev = null;
-	double			gps_start_altitude;
+	double			gps_start_altitude = AltosLib.MISSING;
+	AltosFlightStats	stats;
 
 	static final String[] kml_state_colors = {
 		"FF000000",	// startup
@@ -101,15 +102,16 @@ public class AltosKML implements AltosWriter {
 		"</Document>\n" +
 		"</kml>\n";
 
-	void start (int state) {
-/*
-		out.printf(kml_header_start, record.flight, record.serial);
+	void start (AltosCalData cal_data) {
+		AltosGPS gps = cal_data.gps_pad;
+
+		gps_start_altitude = cal_data.gps_pad_altitude;
+		out.printf(kml_header_start, cal_data.flight, cal_data.serial);
 		out.printf("Date:   %04d-%02d-%02d\n",
-			   record.gps.year, record.gps.month, record.gps.day);
+			   gps.year, gps.month, gps.day);
 		out.printf("Time:     %2d:%02d:%02d\n",
-			   record.gps.hour, record.gps.minute, record.gps.second);
+			   gps.hour, gps.minute, gps.second);
 		out.printf("%s", kml_header_end);
-*/
 	}
 
 	boolean	started = false;
@@ -118,7 +120,10 @@ public class AltosKML implements AltosWriter {
 		String	state_name = AltosLib.state_name(state);
 		String	state_color = state_color(state);
 		out.printf(kml_style_start, state_name, state_color);
-		out.printf("\tState: %s\n", state_name);
+		out.printf("State: %s\n", state_name);
+		out.printf("Time: %6.2f s\n", stats.state_end[state] - stats.state_start[state]);
+		out.printf("Average speed: %s\n", AltosConvert.speed.show(6, stats.state_speed[state]));
+		out.printf("Average accel: %s\n", AltosConvert.accel.show(6, stats.state_accel[state]));
 		out.printf("%s", kml_style_end);
 		out.printf(kml_placemark_start, state_name, state_name);
 	}
@@ -156,52 +161,37 @@ public class AltosKML implements AltosWriter {
 		}
 	}
 
-	public void write(AltosGPS gps, int state, double height) {
+	public void write(AltosGPSTimeValue gtv, AltosCalData cal_data, int state, double height) {
+		AltosGPS gps = gtv.gps;
 		if (gps.lat == AltosLib.MISSING)
 			return;
 		if (gps.lon == AltosLib.MISSING)
 			return;
-		if (!started) {
-			start(state);
-			started = true;
-			gps_start_altitude = gps.alt;
-		}
 		if (state != flight_state) {
 			flight_state = state;
 			if (prev != null) {
-//				coord(gps, state, height);
+				coord(gtv.time, gps, state, height);
 				state_end();
 			}
 			state_start(state);
 		}
-//		coord(0, gps, state, height);
+		coord(0, gps, state, height);
 		prev = gps;
 	}
 
 	private int state(AltosFlightSeries series, double time) {
-		int s = AltosLib.MISSING;
-		for (AltosTimeValue state : series.state_series) {
-			if (state.time > time)
-				break;
-			s = (int) state.value;
-		}
-		return s;
+		return (int) series.value_before(AltosFlightSeries.state_name, time);
 	}
 
 	private double height(AltosFlightSeries series, double time) {
-		double h = AltosLib.MISSING;
-		for (AltosTimeValue height : series.height_series) {
-			if (height.time > time)
-				break;
-			h = height.value;
-		}
-		return h;
+		return series.value(AltosFlightSeries.height_name, time);
 	}
 
 	public void write(AltosFlightSeries series) {
-		for (AltosGPSTimeValue gps : series.gps_series) {
-			write(gps.gps, state(series, gps.time), height(series, gps.time));
-		}
+		stats = new AltosFlightStats(series);
+		start(series.cal_data);
+		for (AltosGPSTimeValue gtv : series.gps_series)
+			write(gtv, series.cal_data, state(series, gtv.time), height(series, gtv.time));
 	}
 
 	public AltosKML(File in_name) throws FileNotFoundException {
diff --git a/altoslib/AltosLib.java b/altoslib/AltosLib.java
index fb43ea20..355c7a27 100644
--- a/altoslib/AltosLib.java
+++ b/altoslib/AltosLib.java
@@ -38,6 +38,20 @@ public class AltosLib {
 	public static final int AO_LOG_GPS_DATE = 'Y';
 	public static final int AO_LOG_PRESSURE = 'P';
 
+	public static boolean is_gps_cmd(int cmd) {
+		switch (cmd) {
+		case AltosLib.AO_LOG_GPS_POS:
+		case AltosLib.AO_LOG_GPS_TIME:
+		case AltosLib.AO_LOG_GPS_LAT:
+		case AltosLib.AO_LOG_GPS_LON:
+		case AltosLib.AO_LOG_GPS_ALT:
+		case AltosLib.AO_LOG_GPS_SAT:
+		case AltosLib.AO_LOG_GPS_DATE:
+			return true;
+		}
+		return false;
+	}
+
 	/* Added for header fields in eeprom files */
 	public static final int AO_LOG_CONFIG_VERSION = 1000;
 	public static final int AO_LOG_MAIN_DEPLOY = 1001;
diff --git a/altoslib/AltosTimeSeries.java b/altoslib/AltosTimeSeries.java
index 142c30ef..64fb399e 100644
--- a/altoslib/AltosTimeSeries.java
+++ b/altoslib/AltosTimeSeries.java
@@ -38,7 +38,7 @@ public class AltosTimeSeries implements Iterable<AltosTimeValue> {
 		if (v0.time == v1.time)
 			return (v0.value + v1.value) / 2;
 
-		return (v0.value * (v1.time - t) + v1.value * (t - v0.time)) / v1.time - v0.time;
+		return (v0.value * (v1.time - t) + v1.value * (t - v0.time)) / (v1.time - v0.time);
 	}
 
 	private int after_index(double time) {
@@ -59,12 +59,18 @@ public class AltosTimeSeries implements Iterable<AltosTimeValue> {
 	/* Compute a value for an arbitrary time */
 	public double value(double time) {
 		int after = after_index(time);
-		if (after == 0)
-			return values.get(0).value;
-		if (after == values.size())
-			return values.get(after - 1).value;
+		double ret;
 
-		return lerp(values.get(after-1), values.get(after), time);
+		if (after == 0)
+			ret = values.get(0).value;
+		else if (after == values.size())
+			ret = values.get(after - 1).value;
+		else {
+			AltosTimeValue b = values.get(after-1);
+			AltosTimeValue a = values.get(after);
+			ret = lerp(b, a, time);
+		}
+		return ret;
 	}
 
 	/* Find the value just before an arbitrary time */
@@ -182,7 +188,6 @@ public class AltosTimeSeries implements Iterable<AltosTimeValue> {
 			}
 			pvalue = v.value;
 			time = v.time;
-//			System.out.printf("%g %g %g\n", time, v.value, value);
 			integral.add(time, value);
 
 		}