X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=altoslib%2FAltosCalData.java;h=49b90ce7d5ff9a58f13f7949e2a2f8dd94d0cccb;hp=408d8d7e5f78738e7298fda27c06543f9cdef4a1;hb=32de85691f2e4ed1430a259e05a514ad820b32d9;hpb=355bb4669da05d202054b8265695e9878fd612c2

diff --git a/altoslib/AltosCalData.java b/altoslib/AltosCalData.java
index 408d8d7e..49b90ce7 100644
--- a/altoslib/AltosCalData.java
+++ b/altoslib/AltosCalData.java
@@ -65,8 +65,11 @@ public class AltosCalData {
 	public int		device_type = AltosLib.MISSING;
 
 	public void set_device_type(int device_type) {
-		if (device_type != AltosLib.MISSING)
+		if (device_type != AltosLib.MISSING) {
 			this.device_type = device_type;
+			if (product == null)
+				set_product(AltosLib.product_name(device_type));
+		}
 	}
 
 	public int		config_major = AltosLib.MISSING;
@@ -165,6 +168,7 @@ public class AltosCalData {
 	}
 
 	public int		tick = AltosLib.MISSING;
+	private int		first_tick = AltosLib.MISSING;
 	private int		prev_tick = AltosLib.MISSING;
 
 	public void set_tick(int tick) {
@@ -174,11 +178,25 @@ public class AltosCalData {
 					tick += 65536;
 				}
 			}
+			if (first_tick == AltosLib.MISSING)
+				first_tick = tick;
 			prev_tick = tick;
 			this.tick = tick;
 		}
 	}
 
+	/* Reset all values which change during flight
+	 */
+	public void reset() {
+		state = AltosLib.MISSING;
+		tick = AltosLib.MISSING;
+		prev_tick = AltosLib.MISSING;
+		temp_gps = null;
+		prev_gps = null;
+		temp_gps_sat_tick = AltosLib.MISSING;
+		accel = AltosLib.MISSING;
+	}
+
 	public int		boost_tick = AltosLib.MISSING;
 
 	public void set_boost_tick() {
@@ -194,9 +212,11 @@ public class AltosCalData {
 	public double time() {
 		if (tick == AltosLib.MISSING)
 			return AltosLib.MISSING;
-		if (boost_tick == AltosLib.MISSING)
-			return AltosLib.MISSING;
-		return (tick - boost_tick) / ticks_per_sec;
+		if (boost_tick != AltosLib.MISSING)
+			return (tick - boost_tick) / ticks_per_sec;
+		if (first_tick != AltosLib.MISSING)
+			return (tick - first_tick) / ticks_per_sec;
+		return tick / ticks_per_sec;
 	}
 
 	public double boost_time() {
@@ -293,25 +313,27 @@ public class AltosCalData {
 			gyro_zero_roll = roll;
 			gyro_zero_pitch = pitch;
 			gyro_zero_yaw = yaw;
+			imu_wrap_checked = false;
 		}
 	}
 
 	public double gyro_roll(double counts) {
 		if (gyro_zero_roll == AltosLib.MISSING || counts == AltosLib.MISSING)
 			return AltosLib.MISSING;
-		return AltosIMU.convert_gyro(counts - gyro_zero_roll);
+
+		return AltosIMU.gyro_degrees_per_second(counts, gyro_zero_roll);
 	}
 
 	public double gyro_pitch(double counts) {
 		if (gyro_zero_pitch == AltosLib.MISSING || counts == AltosLib.MISSING)
 			return AltosLib.MISSING;
-		return AltosIMU.convert_gyro(counts - gyro_zero_pitch);
+		return AltosIMU.gyro_degrees_per_second(counts, gyro_zero_pitch);
 	}
 
 	public double gyro_yaw(double counts) {
 		if (gyro_zero_yaw == AltosLib.MISSING || counts == AltosLib.MISSING)
 			return AltosLib.MISSING;
-		return AltosIMU.convert_gyro(counts - gyro_zero_yaw);
+		return AltosIMU.gyro_degrees_per_second(counts, gyro_zero_yaw);
 	}
 
 	private double gyro_zero_overflow(double first) {
@@ -320,13 +342,25 @@ public class AltosCalData {
 			v = Math.ceil(v);
 		else
 			v = Math.floor(v);
+		if (v != 0)
+			System.out.printf("Adjusting gyro axis by %g steps\n", v);
 		return v * 128.0;
 	}
 
+	/* Initial TeleMega log format had only 16 bits for gyro cal, so the top 9 bits got lost as the
+	 * cal data are scaled by 512. Use the first sample to adjust the cal value, assuming that it is
+	 * from a time of fairly low rotation speed. Fixed in later TeleMega firmware by storing 32 bits
+	 * of cal values.
+	 */
+	private boolean imu_wrap_checked = false;
+
 	public void check_imu_wrap(double roll, double pitch, double yaw) {
-		gyro_zero_roll += gyro_zero_overflow(roll);
-		gyro_zero_pitch += gyro_zero_overflow(pitch);
-		gyro_zero_yaw += gyro_zero_overflow(yaw);
+		if (!imu_wrap_checked) {
+			gyro_zero_roll += gyro_zero_overflow(roll);
+			gyro_zero_pitch += gyro_zero_overflow(pitch);
+			gyro_zero_yaw += gyro_zero_overflow(yaw);
+			imu_wrap_checked = true;
+		}
 	}
 
 	public double mag_along(double along) {