X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=altoslib%2FAltosState.java;h=9ee3d57ddf814fe340ef53df61fa2e31758d5cae;hp=caa1cb4adeb30d8347ca2368a4c4d69f5ae978a4;hb=dd72c9144b207b12150eb6a7ffb012f217f37374;hpb=32de85691f2e4ed1430a259e05a514ad820b32d9

diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java
index caa1cb4a..9ee3d57d 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_11;
+package org.altusmetrum.altoslib_12;
 
 public class AltosState extends AltosDataListener {
 
@@ -677,9 +677,6 @@ public class AltosState extends AltosDataListener {
 	public AltosGPS	gps;
 	public boolean	gps_pending;
 
-	public AltosIMU	imu;
-	public AltosMag	mag;
-
 	public static final int MIN_PAD_SAMPLES = 10;
 
 	public int	npad;
@@ -690,6 +687,7 @@ public class AltosState extends AltosDataListener {
 
 	public AltosGreatCircle from_pad;
 	public double	elevation;	/* from pad */
+	public double	distance;	/* distance along ground */
 	public double	range;		/* total distance */
 
 	public double	gps_height;
@@ -746,22 +744,31 @@ public class AltosState extends AltosDataListener {
 		gps = null;
 		gps_pending = false;
 
-		imu = null;
 		last_imu_time = AltosLib.MISSING;
 		rotation = null;
-		ground_rotation = null;
-
-		mag = null;
 
 		accel_ground_along = AltosLib.MISSING;
 		accel_ground_across = AltosLib.MISSING;
 		accel_ground_through = AltosLib.MISSING;
 
+		accel_along = AltosLib.MISSING;
+		accel_across = AltosLib.MISSING;
+		accel_through = AltosLib.MISSING;
+
+		gyro_roll = AltosLib.MISSING;
+		gyro_pitch = AltosLib.MISSING;
+		gyro_yaw = AltosLib.MISSING;
+
+		mag_along = AltosLib.MISSING;
+		mag_across = AltosLib.MISSING;
+		mag_through = AltosLib.MISSING;
+
 		set_npad(0);
 		ngps = 0;
 
 		from_pad = null;
 		elevation = AltosLib.MISSING;
+		distance = AltosLib.MISSING;
 		range = AltosLib.MISSING;
 		gps_height = AltosLib.MISSING;
 
@@ -804,6 +811,7 @@ public class AltosState extends AltosDataListener {
 
 	void update_gps() {
 		elevation = AltosLib.MISSING;
+		distance = AltosLib.MISSING;
 		range = AltosLib.MISSING;
 
 		if (gps == null)
@@ -845,6 +853,7 @@ public class AltosState extends AltosDataListener {
 				h = 0;
 			from_pad = new AltosGreatCircle(pad_lat, pad_lon, 0, gps.lat, gps.lon, h);
 			elevation = from_pad.elevation;
+			distance = from_pad.distance;
 			range = from_pad.range;
 		}
 	}
@@ -895,19 +904,16 @@ public class AltosState extends AltosDataListener {
 		}
 	}
 
-
 	public AltosRotation	rotation;
-	public AltosRotation	ground_rotation;
 
 	public double	accel_ground_along, accel_ground_across, accel_ground_through;
 
 	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),
-						     cal_data.pad_orientation);
-			ground_rotation = 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),
+						     cal_data().pad_orientation);
 			orient.set_computed(rotation.tilt(), time);
 		}
 	}
@@ -925,12 +931,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);
 			}
 		}
@@ -942,7 +948,7 @@ public class AltosState extends AltosDataListener {
 	public void set_gyro(double roll, double pitch, double yaw) {
 		gyro_roll = roll;
 		gyro_pitch = pitch;
-		gyro_roll = yaw;
+		gyro_yaw = yaw;
 		update_orient();
 	}
 
@@ -992,15 +998,11 @@ public class AltosState extends AltosDataListener {
 	}
 
 	public double mag_across() {
-		if (mag != null)
-			return AltosMag.convert_gauss(mag.across);
-		return AltosLib.MISSING;
+		return mag_across;
 	}
 
 	public double mag_through() {
-		if (mag != null)
-			return AltosMag.convert_gauss(mag.through);
-		return AltosLib.MISSING;
+		return mag_through;
 	}
 
 	public void set_companion(AltosCompanion companion) {
@@ -1064,8 +1066,6 @@ public class AltosState extends AltosDataListener {
 
 	public AltosState (AltosCalData cal_data) {
 		super(cal_data);
-		if (cal_data == null)
-			Thread.dumpStack();
 		init();
 	}
 }