X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=altoslib%2FAltosState.java;h=fc9727481201162bf636e6c4324b2d3140458310;hp=cfee819bc76610f81fd2d092a9b7c18c79126368;hb=a6687ce902a3011796aee93f87d08545e5f0cec9;hpb=0da4e201041a4420f273c2e0cda77eea78099518

diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java
index cfee819b..fc972748 100644
--- a/altoslib/AltosState.java
+++ b/altoslib/AltosState.java
@@ -20,7 +20,7 @@
  * Track flight state from telemetry or eeprom data stream
  */
 
-package org.altusmetrum.altoslib_12;
+package org.altusmetrum.altoslib_14;
 
 public class AltosState extends AltosDataListener {
 
@@ -307,13 +307,12 @@ public class AltosState extends AltosDataListener {
 	class AltosGpsGroundAltitude extends AltosValue {
 		void set(double a, double t) {
 			super.set(a, t);
-			pad_alt = value();
+
 			gps_altitude.set_gps_height();
 		}
 
 		void set_filtered(double a, double t) {
 			super.set_filtered(a, t);
-			pad_alt = value();
 			gps_altitude.set_gps_height();
 		}
 
@@ -480,7 +479,7 @@ public class AltosState extends AltosDataListener {
 	class AltosPressure extends AltosValue {
 		void set(double p, double time) {
 			super.set(p, time);
-			if (state == AltosLib.ao_flight_pad)
+			if (state() == AltosLib.ao_flight_pad)
 				ground_pressure.set_filtered(p, time);
 			double a = pressure_to_altitude(p);
 			altitude.set_computed(a, time);
@@ -513,26 +512,27 @@ public class AltosState extends AltosDataListener {
 	}
 
 	public double height() {
-		double k = kalman_height.value();
-		if (k != AltosLib.MISSING)
-			return k;
-
 		double b = baro_height();
 		if (b != AltosLib.MISSING)
 			return b;
 
+		double k = kalman_height.value();
+		if (k != AltosLib.MISSING)
+			return k;
+
 		return gps_height();
 	}
 
 	public double max_height() {
-		double	k = kalman_height.max();
-		if (k != AltosLib.MISSING)
-			return k;
-
 		double a = altitude.max();
 		double g = ground_altitude();
 		if (a != AltosLib.MISSING && g != AltosLib.MISSING)
 			return a - g;
+
+		double	k = kalman_height.max();
+		if (k != AltosLib.MISSING)
+			return k;
+
 		return max_gps_height();
 	}
 
@@ -557,7 +557,7 @@ public class AltosState extends AltosDataListener {
 	class AltosSpeed extends AltosCValue {
 
 		boolean can_max() {
-			return state < AltosLib.ao_flight_fast || state == AltosLib.ao_flight_stateless;
+			return state() < AltosLib.ao_flight_fast || state() == AltosLib.ao_flight_stateless;
 		}
 
 		void set_accel() {
@@ -615,7 +615,7 @@ public class AltosState extends AltosDataListener {
 	class AltosAccel extends AltosCValue {
 
 		boolean can_max() {
-			return state < AltosLib.ao_flight_fast || state == AltosLib.ao_flight_stateless;
+			return state() < AltosLib.ao_flight_fast || state() == AltosLib.ao_flight_stateless;
 		}
 
 		void set_measured(double a, double time) {
@@ -692,7 +692,7 @@ public class AltosState extends AltosDataListener {
 
 	public double	gps_height;
 
-	public double pad_lat, pad_lon, pad_alt;
+	public double pad_lat, pad_lon;
 
 	public int	speak_tick;
 	public double	speak_altitude;
@@ -712,11 +712,11 @@ public class AltosState extends AltosDataListener {
 	}
 
 	public void init() {
+		super.init();
+
 		set = 0;
 
 		received_time = System.currentTimeMillis();
-		time = AltosLib.MISSING;
-		state = AltosLib.ao_flight_invalid;
 		landed = false;
 		boost = false;
 		rssi = AltosLib.MISSING;
@@ -774,7 +774,6 @@ public class AltosState extends AltosDataListener {
 
 		pad_lat = AltosLib.MISSING;
 		pad_lon = AltosLib.MISSING;
-		pad_alt = AltosLib.MISSING;
 
 		gps_altitude = new AltosGpsAltitude();
 		gps_ground_altitude = new AltosGpsGroundAltitude();
@@ -819,9 +818,9 @@ public class AltosState extends AltosDataListener {
 
 		if (gps.locked && gps.nsat >= 4) {
 			/* Track consecutive 'good' gps reports, waiting for 10 of them */
-			if (state == AltosLib.ao_flight_pad || state == AltosLib.ao_flight_stateless) {
+			if (state() == AltosLib.ao_flight_pad || state() == AltosLib.ao_flight_stateless) {
 				set_npad(npad+1);
-				if (pad_lat != AltosLib.MISSING && (npad < 10 || state == AltosLib.ao_flight_pad)) {
+				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);
@@ -842,6 +841,8 @@ public class AltosState extends AltosDataListener {
 							gps.climb_rate * gps.climb_rate), time);
 			if (gps.course != AltosLib.MISSING)
 				gps_course.set(gps.course, time);
+		} else if (state() == AltosLib.ao_flight_pad || state() == AltosLib.ao_flight_stateless) {
+			set_npad(0);
 		}
 		if (gps.lat != 0 && gps.lon != 0 &&
 		    pad_lat != AltosLib.MISSING &&
@@ -858,25 +859,11 @@ public class AltosState extends AltosDataListener {
 		}
 	}
 
-	public String state_name() {
-		return AltosLib.state_name(state);
-	}
-
 	public void set_state(int state) {
-		if (state != AltosLib.ao_flight_invalid) {
-			this.state = state;
-			ascent = (AltosLib.ao_flight_boost <= state &&
-				  state <= AltosLib.ao_flight_coast);
-			boost = (AltosLib.ao_flight_boost == state);
-		}
-	}
-
-	public int state() {
-		return state;
-	}
-
-	private void re_init() {
-		init();
+		super.set_state(state);
+		ascent = (AltosLib.ao_flight_boost <= state() &&
+			  state() <= AltosLib.ao_flight_coast);
+		boost = (AltosLib.ao_flight_boost == state());
 	}
 
 	public int rssi() {
@@ -896,7 +883,8 @@ public class AltosState extends AltosDataListener {
 		received_time = ms;
 	}
 
-	public void set_gps(AltosGPS gps) {
+	public void set_gps(AltosGPS gps, boolean set_location, boolean set_sats) {
+		super.set_gps(gps, set_location, set_sats);
 		if (gps != null) {
 			this.gps = gps;
 			update_gps();
@@ -910,9 +898,9 @@ public class AltosState extends AltosDataListener {
 
 	void update_pad_rotation() {
 		if (cal_data().pad_orientation != AltosLib.MISSING && accel_ground_along != AltosLib.MISSING) {
-			rotation = new AltosRotation(AltosIMU.convert_accel(accel_ground_across - cal_data().accel_zero_across),
-						     AltosIMU.convert_accel(accel_ground_through - cal_data().accel_zero_through),
-						     AltosIMU.convert_accel(accel_ground_along - cal_data().accel_zero_along),
+			rotation = new AltosRotation(accel_ground_across,
+						     accel_ground_through,
+						     accel_ground_along,
 						     cal_data().pad_orientation);
 			orient.set_computed(rotation.tilt(), time);
 		}
@@ -931,12 +919,12 @@ public class AltosState extends AltosDataListener {
 		if (last_imu_time != AltosLib.MISSING) {
 			double	t = time - last_imu_time;
 
-			double	pitch = AltosConvert.degrees_to_radians(gyro_pitch);
-			double	yaw = AltosConvert.degrees_to_radians(gyro_yaw);
-			double	roll = AltosConvert.degrees_to_radians(gyro_roll);
+			if (t > 0 && gyro_pitch != AltosLib.MISSING && rotation != null) {
+				double	pitch = AltosConvert.degrees_to_radians(gyro_pitch) * t;
+				double	yaw = AltosConvert.degrees_to_radians(gyro_yaw) * t;
+				double	roll = AltosConvert.degrees_to_radians(gyro_roll) * t;
 
-			if (t > 0 && pitch != AltosLib.MISSING && rotation != null) {
-				rotation.rotate(t, pitch, yaw, roll);
+				rotation.rotate(pitch, yaw, roll);
 				orient.set_computed(rotation.tilt(), time);
 			}
 		}
@@ -1060,7 +1048,6 @@ public class AltosState extends AltosDataListener {
 	}
 
 	public AltosState() {
-		Thread.dumpStack();
 		init();
 	}