altoslib: Move computed state from AltosRecord to AltosState

Make AltosRecord simply track the raw data and have AltosState hold
all computed values, including cross-packet averages and computed speeds.

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

10 files changed, 114 insertions, 134 deletions

diff --git a/altoslib/AltosIdleMonitor.java b/altoslib/AltosIdleMonitor.java
index 2c4965ff..07d8930d 100644
--- a/altoslib/AltosIdleMonitor.java
+++ b/altoslib/AltosIdleMonitor.java
@@ -97,7 +97,7 @@ public class AltosIdleMonitor extends Thread {
 			else if (has_sensor_mm(config_data))
 				record = sensor_mm(config_data);
 			else
-				record = new AltosRecord();
+				record = new AltosRecordNone();
 
 			if (has_gps(config_data))
 				gps = new AltosGPSQuery(link, config_data);
diff --git a/altoslib/AltosRecord.java b/altoslib/AltosRecord.java
index 8bab1d0c..09169515 100644
--- a/altoslib/AltosRecord.java
+++ b/altoslib/AltosRecord.java
@@ -17,7 +17,7 @@
 
 package org.altusmetrum.AltosLib;
 
-public class AltosRecord implements Comparable <AltosRecord>, Cloneable {
+public abstract class AltosRecord implements Comparable <AltosRecord>, Cloneable {
 
 	public static final int	seen_flight = 1;
 	public static final int	seen_sensor = 2;
@@ -43,11 +43,6 @@ public class AltosRecord implements Comparable <AltosRecord>, Cloneable {
 	public int	state;
 	public int	tick;
 
-	/* Current flight dynamic state */
-	public double	acceleration;	/* m/s² */
-	public double	speed;		/* m/s */
-	public double	height;		/* m */
-
 	public AltosGPS	gps;
 	public boolean	new_gps;
 
@@ -63,6 +58,11 @@ public class AltosRecord implements Comparable <AltosRecord>, Cloneable {
 
 	public AltosRecordCompanion companion;
 
+	/* Telemetry sources have these values recorded from the flight computer */
+	public double	kalman_height;
+	public double	kalman_speed;
+	public double	kalman_acceleration;
+
 	/*
 	 * Abstract methods that convert record data
 	 * to standard units:
@@ -75,76 +75,48 @@ public class AltosRecord implements Comparable <AltosRecord>, Cloneable {
 	 *	temperature:	°C
 	 */
 
-	public double raw_pressure() { return MISSING; }
-
-	public double filtered_pressure() { return MISSING; }
-
-	public double ground_pressure() { return MISSING; }
-
-	public double battery_voltage() { return MISSING; }
+	abstract public double pressure();
+	abstract public double ground_pressure();
+	abstract public double acceleration();
 
-	public double main_voltage() { return MISSING; }
+	public double altitude() {
+		double	p = pressure();
 
-	public double drogue_voltage() { return MISSING; }
-
-	public double temperature() { return MISSING; }
-	
-	public double acceleration() { return MISSING; }
-
-	public double accel_speed() { return MISSING; }
-
-	public AltosIMU imu() { return null; }
-
-	public AltosMag mag() { return null; }
-
-	/*
-	 * Convert various pressure values to altitude
-	 */
-
-	public double raw_altitude() {
-		double p = raw_pressure();
 		if (p == MISSING)
 			return MISSING;
 		return AltosConvert.pressure_to_altitude(p);
 	}
 
 	public double ground_altitude() {
-		double p = ground_pressure();
+		double	p = ground_pressure();
+
 		if (p == MISSING)
 			return MISSING;
 		return AltosConvert.pressure_to_altitude(p);
 	}
 
-	public double filtered_altitude() {
-		double	ga = ground_altitude();
-		if (height != MISSING && ga != MISSING)
-			return height + ga;
+	public double height() {
+		double	g = ground_altitude();
+		double	a = altitude();
 
-		double	p = filtered_pressure();
-		if (p == MISSING)
-			return raw_altitude();
-		return AltosConvert.pressure_to_altitude(p);
+		if (g == MISSING)
+			return MISSING;
+		if (a == MISSING)
+			return MISSING;
+		return a - g;
 	}
 
-	public double filtered_height() {
-		if (height != MISSING)
-			return height;
+	public double battery_voltage() { return MISSING; }
 
-		double f = filtered_altitude();
-		double g = ground_altitude();
-		if (f == MISSING || g == MISSING)
-			return MISSING;
-		return f - g;
-	}
+	public double main_voltage() { return MISSING; }
 
-	public double raw_height() {
-		double r = raw_altitude();
-		double g = ground_altitude();
+	public double drogue_voltage() { return MISSING; }
 
-		if (r == MISSING || g == MISSING)
-			return height;
-		return r - g;
-	}
+	public double temperature() { return MISSING; }
+	
+	public AltosIMU imu() { return null; }
+
+	public AltosMag mag() { return null; }
 
 	public String state() {
 		return AltosLib.state_name(state);
@@ -164,12 +136,12 @@ public class AltosRecord implements Comparable <AltosRecord>, Cloneable {
 		status = old.status;
 		state = old.state;
 		tick = old.tick;
-		acceleration = old.acceleration;
-		speed = old.speed;
-		height = old.height;
 		gps = new AltosGPS(old.gps);
 		new_gps = old.new_gps;
 		companion = old.companion;
+		kalman_acceleration = old.kalman_acceleration;
+		kalman_speed = old.kalman_speed;
+		kalman_height = old.kalman_height;
 	}
 
 	public AltosRecord clone() {
@@ -192,11 +164,12 @@ public class AltosRecord implements Comparable <AltosRecord>, Cloneable {
 		status = 0;
 		state = AltosLib.ao_flight_startup;
 		tick = 0;
-		acceleration = MISSING;
-		speed = MISSING;
-		height = MISSING;
 		gps = new AltosGPS();
 		new_gps = false;
 		companion = null;
+
+		kalman_acceleration = MISSING;
+		kalman_speed = MISSING;
+		kalman_height = MISSING;
 	}
 }
diff --git a/altoslib/AltosRecordMM.java b/altoslib/AltosRecordMM.java
index 63b37f82..9f529234 100644
--- a/altoslib/AltosRecordMM.java
+++ b/altoslib/AltosRecordMM.java
@@ -19,6 +19,7 @@ package org.altusmetrum.AltosLib;
 
 public class AltosRecordMM extends AltosRecord {
 
+	/* Sensor values */
 	public int	accel;
 	public int	pres;
 	public int	temp;
@@ -45,16 +46,12 @@ public class AltosRecordMM extends AltosRecord {
 		return raw / 4095.0;
 	}
 
-	public double raw_pressure() {
+	public double pressure() {
 		if (pres != MISSING)
 			return pres;
 		return MISSING;
 	}
 
-	public double filtered_pressure() {
-		return raw_pressure();
-	}
-
 	public double ground_pressure() {
 		if (ground_pres != MISSING)
 			return ground_pres;
@@ -98,9 +95,6 @@ public class AltosRecordMM extends AltosRecord {
 	}
 
 	public double acceleration() {
-		if (acceleration != MISSING)
-			return acceleration;
-
 		if (ground_accel == MISSING || accel == MISSING)
 			return MISSING;
 
@@ -110,14 +104,6 @@ public class AltosRecordMM extends AltosRecord {
 		return (ground_accel - accel) / accel_counts_per_mss();
 	}
 
-	public double accel_speed() {
-		if (speed != MISSING)
-			return speed;
-		if (flight_vel == MISSING)
-			return MISSING;
-		return flight_vel / (accel_counts_per_mss() * 100.0);
-	}
-
 	public void copy (AltosRecordMM old) {
 		super.copy(old);
 
diff --git a/altoslib/AltosRecordTM.java b/altoslib/AltosRecordTM.java
index 37accef6..9530be31 100644
--- a/altoslib/AltosRecordTM.java
+++ b/altoslib/AltosRecordTM.java
@@ -18,6 +18,8 @@
 package org.altusmetrum.AltosLib;
 
 public class AltosRecordTM extends AltosRecord {
+
+	/* Sensor values */
 	public int	accel;
 	public int	pres;
 	public int	temp;
@@ -57,18 +59,12 @@ public class AltosRecordTM extends AltosRecord {
 		return ((count / 16.0) / 2047.0 + 0.095) / 0.009 * 1000.0;
 	}
 
-	public double raw_pressure() {
+	public double pressure() {
 		if (pres == MISSING)
 			return MISSING;
 		return barometer_to_pressure(pres);
 	}
 
-	public double filtered_pressure() {
-		if (flight_pres == MISSING)
-			return MISSING;
-		return barometer_to_pressure(flight_pres);
-	}
-
 	public double ground_pressure() {
 		if (ground_pres == MISSING)
 			return MISSING;
@@ -121,22 +117,11 @@ public class AltosRecordTM extends AltosRecord {
 	}
 
 	public double acceleration() {
-		if (acceleration != MISSING)
-			return acceleration;
-
 		if (ground_accel == MISSING || accel == MISSING)
 			return MISSING;
 		return (ground_accel - accel) / accel_counts_per_mss();
 	}
 
-	public double accel_speed() {
-		if (speed != MISSING)
-			return speed;
-		if (flight_vel == MISSING)
-			return MISSING;
-		return flight_vel / (accel_counts_per_mss() * 100.0);
-	}
-
 	public void copy(AltosRecordTM old) {
 		super.copy(old);
 
diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java
index 2e4d8870..f28dd1c6 100644
--- a/altoslib/AltosState.java
+++ b/altoslib/AltosState.java
@@ -40,17 +40,17 @@ public class AltosState {
 	public double	ground_altitude;
 	public double	altitude;
 	public double	height;
-	public double	speed;
 	public double	acceleration;
 	public double	battery;
 	public double	temperature;
 	public double	main_sense;
 	public double	drogue_sense;
+	public double	accel_speed;
 	public double	baro_speed;
 
 	public double	max_height;
 	public double	max_acceleration;
-	public double	max_speed;
+	public double	max_accel_speed;
 	public double	max_baro_speed;
 
 	public AltosGPS	gps;
@@ -76,20 +76,39 @@ public class AltosState {
 	public int	speak_tick;
 	public double	speak_altitude;
 
-	public void init (AltosRecord cur, AltosState prev_state) {
-		//int		i;
-		//AltosRecord prev;
+	public double speed() {
+		if (ascent)
+			return accel_speed;
+		else
+			return baro_speed;
+	}
+
+	public double max_speed() {
+		if (max_accel_speed != 0)
+			return max_accel_speed;
+		return max_baro_speed;
+	}
 
+	public void init (AltosRecord cur, AltosState prev_state) {
 		data = cur;
 
 		ground_altitude = data.ground_altitude();
-		altitude = data.raw_altitude();
-		height = data.filtered_height();
+
+		altitude = data.altitude();
+
+		if (data.kalman_height != AltosRecord.MISSING)
+			height = data.kalman_height;
+		else {
+			if (prev_state != null)
+				height = (prev_state.height * 15 + altitude - ground_altitude) / 16.0;
+		}
 
 		report_time = System.currentTimeMillis();
 
-		acceleration = data.acceleration();
-		speed = data.accel_speed();
+		if (data.kalman_acceleration != AltosRecord.MISSING)
+			acceleration = data.kalman_acceleration;
+		else
+			acceleration = data.acceleration();
 		temperature = data.temperature();
 		drogue_sense = data.drogue_voltage();
 		main_sense = data.main_voltage();
@@ -108,7 +127,7 @@ public class AltosState {
 			pad_alt = prev_state.pad_alt;
 			max_height = prev_state.max_height;
 			max_acceleration = prev_state.max_acceleration;
-			max_speed = prev_state.max_speed;
+			max_accel_speed = prev_state.max_accel_speed;
 			max_baro_speed = prev_state.max_baro_speed;
 			imu = prev_state.imu;
 			mag = prev_state.mag;
@@ -119,23 +138,39 @@ public class AltosState {
 
 			time_change = (tick - prev_state.tick) / 100.0;
 
-			/* compute barometric speed */
+			if (data.kalman_speed != AltosRecord.MISSING) {
+				baro_speed = accel_speed = data.kalman_speed;
+			} else {
+				/* compute barometric speed */
 
-			double height_change = height - prev_state.height;
-			if (data.speed != AltosRecord.MISSING)
-				baro_speed = data.speed;
-			else {
+				double height_change = height - prev_state.height;
 				if (time_change > 0)
 					baro_speed = (prev_state.baro_speed * 3 + (height_change / time_change)) / 4.0;
 				else
 					baro_speed = prev_state.baro_speed;
+
+				if (acceleration == AltosRecord.MISSING) {
+					/* Fill in mising acceleration value */
+					accel_speed = baro_speed;
+					if (time_change > 0)
+						acceleration = (accel_speed - prev_state.accel_speed) / time_change;
+					else
+						acceleration = prev_state.acceleration;
+				} else {
+					/* compute accelerometer speed */
+					accel_speed = prev_state.accel_speed + acceleration * time_change;
+				}
 			}
+
 		} else {
 			npad = 0;
 			ngps = 0;
 			gps = null;
 			baro_speed = 0;
+			accel_speed = 0;
 			time_change = 0;
+			if (acceleration == AltosRecord.MISSING)
+				acceleration = 0;
 		}
 
 		time = tick / 100.0;
@@ -180,8 +215,8 @@ public class AltosState {
 		/* Only look at accelerometer data under boost */
 		if (boost && acceleration > max_acceleration && acceleration != AltosRecord.MISSING)
 			max_acceleration = acceleration;
-		if (boost && speed > max_speed && speed != AltosRecord.MISSING)
-			max_speed = speed;
+		if (boost && accel_speed > max_accel_speed && accel_speed != AltosRecord.MISSING)
+			max_accel_speed = accel_speed;
 		if (boost && baro_speed > max_baro_speed && baro_speed != AltosRecord.MISSING)
 			max_baro_speed = baro_speed;
 
diff --git a/altoslib/AltosTelemetryRecordLegacy.java b/altoslib/AltosTelemetryRecordLegacy.java
index 21176069..43189794 100644
--- a/altoslib/AltosTelemetryRecordLegacy.java
+++ b/altoslib/AltosTelemetryRecordLegacy.java
@@ -257,9 +257,9 @@ public class AltosTelemetryRecordLegacy extends AltosTelemetryRecord {
 		record.accel_minus_g = map.get_int(AO_TELEM_CAL_ACCEL_MINUS, AltosRecord.MISSING);
 
 		/* flight computer values */
-		record.acceleration = map.get_double(AO_TELEM_KALMAN_ACCEL, AltosRecord.MISSING, 1/16.0);
-		record.speed = map.get_double(AO_TELEM_KALMAN_SPEED, AltosRecord.MISSING, 1/16.0);
-		record.height = map.get_int(AO_TELEM_KALMAN_HEIGHT, AltosRecord.MISSING);
+		record.kalman_acceleration = map.get_double(AO_TELEM_KALMAN_ACCEL, AltosRecord.MISSING, 1/16.0);
+		record.kalman_speed = map.get_double(AO_TELEM_KALMAN_SPEED, AltosRecord.MISSING, 1/16.0);
+		record.kalman_height = map.get_int(AO_TELEM_KALMAN_HEIGHT, AltosRecord.MISSING);
 
 		record.flight_accel = map.get_int(AO_TELEM_ADHOC_ACCEL, AltosRecord.MISSING);
 		record.flight_vel = map.get_int(AO_TELEM_ADHOC_SPEED, AltosRecord.MISSING);
@@ -334,9 +334,9 @@ public class AltosTelemetryRecordLegacy extends AltosTelemetryRecord {
 
 		/* Old TeleDongle code with kalman-reporting TeleMetrum code */
 		if ((record.flight_vel & 0xffff0000) == 0x80000000) {
-			record.speed = ((short) record.flight_vel) / 16.0;
-			record.acceleration = record.flight_accel / 16.0;
-			record.height = record.flight_pres;
+			record.kalman_speed = ((short) record.flight_vel) / 16.0;
+			record.kalman_acceleration = record.flight_accel / 16.0;
+			record.kalman_height = record.flight_pres;
 			record.flight_vel = AltosRecord.MISSING;
 			record.flight_pres = AltosRecord.MISSING;
 			record.flight_accel = AltosRecord.MISSING;
@@ -455,9 +455,9 @@ public class AltosTelemetryRecordLegacy extends AltosTelemetryRecord {
 		record.accel_minus_g = int16(19);
 
 		if (uint16(11) == 0x8000) {
-			record.acceleration = int16(5);
-			record.speed = int16(9);
-			record.height = int16(13);
+			record.kalman_acceleration = int16(5);
+			record.kalman_speed = int16(9);
+			record.kalman_height = int16(13);
 			record.flight_accel = AltosRecord.MISSING;
 			record.flight_vel = AltosRecord.MISSING;
 			record.flight_pres = AltosRecord.MISSING;
@@ -465,9 +465,9 @@ public class AltosTelemetryRecordLegacy extends AltosTelemetryRecord {
 			record.flight_accel = int16(5);
 			record.flight_vel = uint32(9);
 			record.flight_pres = int16(13);
-			record.acceleration = AltosRecord.MISSING;
-			record.speed = AltosRecord.MISSING;
-			record.height = AltosRecord.MISSING;
+			record.kalman_acceleration = AltosRecord.MISSING;
+			record.kalman_speed = AltosRecord.MISSING;
+			record.kalman_height = AltosRecord.MISSING;
 		}
 
 		record.gps = null;
diff --git a/altoslib/AltosTelemetryRecordMegaData.java b/altoslib/AltosTelemetryRecordMegaData.java
index 8f55d238..16a7b80c 100644
--- a/altoslib/AltosTelemetryRecordMegaData.java
+++ b/altoslib/AltosTelemetryRecordMegaData.java
@@ -83,9 +83,9 @@ public class AltosTelemetryRecordMegaData extends AltosTelemetryRecordRaw {
 		next.accel_plus_g = accel_plus_g;
 		next.accel_minus_g = accel_minus_g;
 
-		next.acceleration = acceleration / 16.0;
-		next.speed = speed / 16.0;
-		next.height = height;
+		next.kalman_acceleration = acceleration / 16.0;
+		next.kalman_speed = speed / 16.0;
+		next.kalman_height = height;
 
 		next.seen |= AltosRecord.seen_flight | AltosRecord.seen_temp_volt;
 
diff --git a/altoslib/AltosTelemetryRecordRaw.java b/altoslib/AltosTelemetryRecordRaw.java
index fbb373d5..c21da6fc 100644
--- a/altoslib/AltosTelemetryRecordRaw.java
+++ b/altoslib/AltosTelemetryRecordRaw.java
@@ -65,7 +65,7 @@ public class AltosTelemetryRecordRaw extends AltosTelemetryRecord {
 		if (previous != null)
 			next = previous.clone();
 		else
-			next = new AltosRecord();
+			next = new AltosRecordNone();
 		next.serial = serial;
 		next.tick = tick;
 		return next;
diff --git a/altoslib/AltosTelemetryRecordSensor.java b/altoslib/AltosTelemetryRecordSensor.java
index 319a91b3..f1fc156c 100644
--- a/altoslib/AltosTelemetryRecordSensor.java
+++ b/altoslib/AltosTelemetryRecordSensor.java
@@ -86,9 +86,9 @@ public class AltosTelemetryRecordSensor extends AltosTelemetryRecordRaw {
 			next.main = AltosRecord.MISSING;
 		}
 
-		next.acceleration = acceleration / 16.0;
-		next.speed = speed / 16.0;
-		next.height = height;
+		next.kalman_acceleration = acceleration / 16.0;
+		next.kalman_speed = speed / 16.0;
+		next.kalman_height = height;
 
 		next.ground_pres = ground_pres;
 		if (type == packet_type_TM_sensor) {
diff --git a/altoslib/Makefile.am b/altoslib/Makefile.am
index b56d8af1..f04c10c6 100644
--- a/altoslib/Makefile.am
+++ b/altoslib/Makefile.am
@@ -46,6 +46,7 @@ AltosLib_JAVA = \
 	$(SRC)/AltosRecordCompanion.java \
 	$(SRC)/AltosRecordIterable.java \
 	$(SRC)/AltosRecord.java \
+	$(SRC)/AltosRecordNone.java \
 	$(SRC)/AltosRecordTM.java \
 	$(SRC)/AltosRecordMM.java \
 	$(SRC)/AltosReplayReader.java \