X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=altoslib%2FAltosCalData.java;h=22d19def1ba9397fbdec3ab510131407afe8bdf8;hp=58d34abeb0e738dca8bcc0964fef69f650f41604;hb=9791ebb256c2892663c072f7989e4b9bd34818a1;hpb=f26cfe417c6977cf1e7e75a4f050e25f64d41859

diff --git a/altoslib/AltosCalData.java b/altoslib/AltosCalData.java
index 58d34abe..22d19def 100644
--- a/altoslib/AltosCalData.java
+++ b/altoslib/AltosCalData.java
@@ -12,7 +12,7 @@
  * General Public License for more details.
  */
 
-package org.altusmetrum.altoslib_11;
+package org.altusmetrum.altoslib_13;
 
 /*
  * Calibration and other data needed to construct 'real' values from various data
@@ -65,8 +65,18 @@ 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		log_format = AltosLib.MISSING;
+
+	public void set_log_format(int log_format) {
+		if (log_format != AltosLib.MISSING)
+			this.log_format = log_format;
 	}
 
 	public int		config_major = AltosLib.MISSING;
@@ -98,6 +108,8 @@ public class AltosCalData {
 
 	public void set_accel_plus_minus(double plus, double minus) {
 		if (plus != AltosLib.MISSING && minus != AltosLib.MISSING) {
+			if (plus == minus)
+				return;
 			accel_plus_g = plus;
 			accel_minus_g = minus;
 		}
@@ -121,6 +133,18 @@ public class AltosCalData {
 		mma655x_inverted = inverted;
 	}
 
+	public boolean adxl375_inverted = false;
+
+	public void set_adxl375_inverted(boolean inverted) {
+		adxl375_inverted = inverted;
+	}
+
+	public int adxl375_axis = AltosLib.MISSING;
+
+	public void set_adxl375_axis(int axis) {
+		adxl375_axis = axis;
+	}
+
 	public int pad_orientation = AltosLib.MISSING;
 
 	public void set_pad_orientation(int orientation) {
@@ -130,7 +154,9 @@ public class AltosCalData {
 
 	/* Compute acceleration */
 	public double acceleration(double sensor) {
-		return AltosConvert.acceleration_from_sensor(sensor, accel_plus_g, accel_minus_g, ground_accel);
+		double accel;
+		accel = AltosConvert.acceleration_from_sensor(sensor, accel_plus_g, accel_minus_g, ground_accel);
+		return accel;
 	}
 
 	public AltosMs5607	ms5607 = null;
@@ -163,6 +189,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) {
@@ -172,28 +199,50 @@ 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;
+		temp_gps_sat_tick = AltosLib.MISSING;
+		accel = AltosLib.MISSING;
+	}
+
 	public int		boost_tick = AltosLib.MISSING;
 
 	public void set_boost_tick() {
 		boost_tick = tick;
 	}
 
+	public double		ticks_per_sec = 100.0;
+
+	public void set_ticks_per_sec(double ticks_per_sec) {
+		this.ticks_per_sec = ticks_per_sec;
+	}
+
 	public double time() {
 		if (tick == AltosLib.MISSING)
 			return AltosLib.MISSING;
-		if (boost_tick == AltosLib.MISSING)
-			return AltosLib.MISSING;
-		return (tick - boost_tick) / 100.0;
+		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() {
 		if (boost_tick == AltosLib.MISSING)
 			return AltosLib.MISSING;
-		return boost_tick / 100.0;
+		return boost_tick / ticks_per_sec;
 	}
 
 	public int		state = AltosLib.MISSING;
@@ -204,12 +253,21 @@ public class AltosCalData {
 		this.state = state;
 	}
 
-	public double		gps_ground_altitude = AltosLib.MISSING;
+	public AltosGPS		gps_pad = null;
 
-	public void set_gps_altitude(double altitude) {
-		if ((state != AltosLib.MISSING && state < AltosLib.ao_flight_boost) ||
-		    gps_ground_altitude == AltosLib.MISSING)
-			gps_ground_altitude = altitude;
+	public AltosGPS		prev_gps = null;
+
+	public double		gps_pad_altitude = AltosLib.MISSING;
+
+	public void set_cal_gps(AltosGPS gps) {
+		if (gps.locked && gps.nsat >= 4) {
+			if ((state != AltosLib.MISSING && state < AltosLib.ao_flight_boost) || gps_pad == null)
+				gps_pad = gps;
+			if (gps_pad_altitude == AltosLib.MISSING && gps.alt != AltosLib.MISSING)
+				gps_pad_altitude = gps.alt;
+		}
+		temp_gps = null;
+		prev_gps = gps;
 	}
 
 	/*
@@ -219,26 +277,22 @@ public class AltosCalData {
 	AltosGPS		temp_gps = null;
 	int			temp_gps_sat_tick = AltosLib.MISSING;
 
-	public AltosGPS temp_gps() {
+	public AltosGPS temp_cal_gps() {
 		return temp_gps;
 	}
 
-	public void reset_temp_gps() {
-		if (temp_gps != null) {
-			if (temp_gps.locked && temp_gps.nsat >= 4)
-				set_gps_altitude(temp_gps.alt);
-		}
-		temp_gps = null;
+	public void reset_temp_cal_gps() {
+		if (temp_gps != null)
+			set_cal_gps(temp_gps);
 	}
 
-	public boolean gps_pending() {
+	public boolean cal_gps_pending() {
 		return temp_gps != null;
 	}
 
-	public AltosGPS make_temp_gps(int tick, boolean sats) {
-		if (temp_gps == null) {
-			temp_gps = new AltosGPS();
-		}
+	public AltosGPS make_temp_cal_gps(int tick, boolean sats) {
+		if (temp_gps == null)
+			temp_gps = new AltosGPS(prev_gps);
 		if (sats) {
 			if (tick != temp_gps_sat_tick)
 				temp_gps.cc_gps_sat = null;
@@ -276,25 +330,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) {
@@ -303,13 +359,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) {
@@ -335,14 +403,29 @@ public class AltosCalData {
 
 	public AltosCalData(AltosConfigData config_data) {
 		set_serial(config_data.serial);
+		set_ticks_per_sec(100.0);
 		set_flight(config_data.flight);
 		set_callsign(config_data.callsign);
-		set_accel_plus_minus(config_data.accel_cal_plus, config_data.accel_cal_minus);
+		set_config(config_data.config_major, config_data.config_minor, config_data.flight_log_max);
+		set_firmware_version(config_data.version);
+		set_flight_params(config_data.apogee_delay / ticks_per_sec, config_data.apogee_lockout / ticks_per_sec);
+		set_pad_orientation(config_data.pad_orientation);
+		set_product(config_data.product);
+		set_accel_plus_minus(config_data.accel_cal_plus(config_data.pad_orientation), config_data.accel_cal_minus(config_data.pad_orientation));
+		set_accel_zero(config_data.accel_zero_along, config_data.accel_zero_across, config_data.accel_zero_through);
 		set_ms5607(config_data.ms5607);
 		try {
 			set_mma655x_inverted(config_data.mma655x_inverted());
 		} catch (AltosUnknownProduct up) {
 		}
+		try {
+			set_adxl375_inverted(config_data.adxl375_inverted());
+		} catch (AltosUnknownProduct up) {
+		}
+		try {
+			set_adxl375_axis(config_data.adxl375_axis());
+		} catch (AltosUnknownProduct up) {
+		}
 		set_pad_orientation(config_data.pad_orientation);
 	}
 }