X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=altoslib%2FAltosState.java;h=ccbe498d5ca34dedaa3ee7be58512955f93e16d2;hp=0645e4487df547fc4ca50c4c3c3d28c8886c0f2f;hb=6a6a5d0afa646564a9277ad3bd80c4225247a27b;hpb=69e6df07976a56b49e07c242cd6e5b2cbd2a578d

diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java
index 0645e448..ccbe498d 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;
+package org.altusmetrum.altoslib_1;
 
 public class AltosState {
 	public AltosRecord data;
@@ -38,18 +38,19 @@ public class AltosState {
 	public boolean boost;	/* under power */
 
 	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;
@@ -70,22 +71,55 @@ public class AltosState {
 
 	public double	gps_height;
 
+	public double pad_lat, pad_lon, pad_alt;
+
 	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;
 
+		/* Discard previous state if it was for a different board */
+		if (prev_state != null && prev_state.data.serial != data.serial)
+			prev_state = null;
 		ground_altitude = data.ground_altitude();
-		height = data.filtered_height();
+
+		altitude = data.altitude();
+		if (altitude == AltosRecord.MISSING && data.gps != null)
+			altitude = data.gps.alt;
+
+		height = AltosRecord.MISSING;
+		if (data.kalman_height != AltosRecord.MISSING)
+			height = data.kalman_height;
+		else {
+			if (altitude != AltosRecord.MISSING && ground_altitude != AltosRecord.MISSING) {
+				double	cur_height = altitude - ground_altitude;
+				if (prev_state == null || prev_state.height == AltosRecord.MISSING)
+					height = cur_height;
+				else
+					height = (prev_state.height * 15 + cur_height) / 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();
@@ -104,7 +138,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;
@@ -115,28 +149,57 @@ 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;
+
+				double prev_baro_speed = prev_state.baro_speed;
+				if (prev_baro_speed == AltosRecord.MISSING)
+					prev_baro_speed = 0;
 
-			double height_change = height - prev_state.height;
-			if (data.speed != AltosRecord.MISSING)
-				baro_speed = data.speed;
-			else {
 				if (time_change > 0)
-					baro_speed = (prev_state.baro_speed * 3 + (height_change / time_change)) / 4.0;
+					baro_speed = (prev_baro_speed * 3 + (height_change / time_change)) / 4.0;
 				else
 					baro_speed = prev_state.baro_speed;
+
+				double prev_accel_speed = prev_state.accel_speed;
+
+				if (prev_accel_speed == AltosRecord.MISSING)
+					prev_accel_speed = 0;
+
+				if (acceleration == AltosRecord.MISSING) {
+					/* Fill in mising acceleration value */
+					accel_speed = baro_speed;
+
+					if (time_change > 0 && accel_speed != AltosRecord.MISSING)
+						acceleration = (accel_speed - prev_accel_speed) / time_change;
+					else
+						acceleration = prev_state.acceleration;
+				} else {
+					/* compute accelerometer speed */
+					accel_speed = prev_accel_speed + acceleration * time_change;
+				}
 			}
 		} else {
 			npad = 0;
 			ngps = 0;
 			gps = null;
-			baro_speed = 0;
+			baro_speed = AltosRecord.MISSING;
+			accel_speed = AltosRecord.MISSING;
+			pad_alt = AltosRecord.MISSING;
+			max_baro_speed = 0;
+			max_accel_speed = 0;
+			max_height = 0;
+			max_acceleration = 0;
 			time_change = 0;
 		}
 
 		time = tick / 100.0;
 
-		if (cur.new_gps && (state == AltosLib.ao_flight_pad || state == AltosLib.ao_flight_idle)) {
+		if (cur.new_gps && (state < AltosLib.ao_flight_boost)) {
 
 			/* Track consecutive 'good' gps reports, waiting for 10 of them */
 			if (data.gps != null && data.gps.locked && data.gps.nsat >= 4)
@@ -146,7 +209,7 @@ public class AltosState {
 
 			/* Average GPS data while on the pad */
 			if (data.gps != null && data.gps.locked && data.gps.nsat >= 4) {
-				if (ngps > 1) {
+				if (ngps > 1 && state == AltosLib.ao_flight_pad) {
 					/* filter pad position */
 					pad_lat = (pad_lat * 31.0 + data.gps.lat) / 32.0;
 					pad_lon = (pad_lon * 31.0 + data.gps.lon) / 32.0;
@@ -158,8 +221,13 @@ public class AltosState {
 				}
 				ngps++;
 			}
+		} else {
+			if (ngps == 0 && ground_altitude != AltosRecord.MISSING)
+				pad_alt = ground_altitude;
 		}
 
+		data.new_gps = false;
+
 		gps_waiting = MIN_PAD_SAMPLES - npad;
 		if (gps_waiting < 0)
 			gps_waiting = 0;
@@ -171,32 +239,29 @@ public class AltosState {
 		boost = (AltosLib.ao_flight_boost == state);
 
 		/* Only look at accelerometer data under boost */
-		if (boost && acceleration > max_acceleration)
+		if (boost && acceleration != AltosRecord.MISSING && (max_acceleration == AltosRecord.MISSING || acceleration > max_acceleration))
 			max_acceleration = acceleration;
-		if (boost && speed > max_speed)
-			max_speed = speed;
-		if (boost && baro_speed > max_baro_speed)
+		if (boost && accel_speed != AltosRecord.MISSING && accel_speed > max_accel_speed)
+			max_accel_speed = accel_speed;
+		if (boost && baro_speed != AltosRecord.MISSING && baro_speed > max_baro_speed)
 			max_baro_speed = baro_speed;
 
-		if (height > max_height)
+		if (height != AltosRecord.MISSING && height > max_height)
 			max_height = height;
-		if (data.gps != null) {
-			if (gps == null || !gps.locked || data.gps.locked)
-				gps = data.gps;
-			if (ngps > 0 && gps.locked) {
-				from_pad = new AltosGreatCircle(pad_lat, pad_lon, gps.lat, gps.lon);
-			}
-		}
 		elevation = 0;
 		range = -1;
-		if (ngps > 0) {
-			gps_height = gps.alt - pad_alt;
-			if (from_pad != null) {
-				elevation = Math.atan2(height, from_pad.distance) * 180 / Math.PI;
-				range = Math.sqrt(height * height + from_pad.distance * from_pad.distance);
+		gps_height = 0;
+		if (data.gps != null) {
+			gps = data.gps;
+			if (ngps > 0 && gps.locked) {
+				double h = height;
+
+				if (h == AltosRecord.MISSING) h = 0;
+				from_pad = new AltosGreatCircle(pad_lat, pad_lon, 0, gps.lat, gps.lon, h);
+				elevation = from_pad.elevation;
+				range = from_pad.range;
+				gps_height = gps.alt - pad_alt;
 			}
-		} else {
-			gps_height = 0;
 		}
 	}