X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=altoslib%2FAltosTelemetryMegaSensor.java;h=cda977eb36b3a54f071caa1f550bee33239ad1c0;hp=e97e18e2872da723093e5fbf4704d1dd55102e08;hb=76b288f8d02e0a48f04de9455a2dd0b77e48bd63;hpb=297eb795b24ec31f6599f48bc8c3769557a7ec6f

diff --git a/altoslib/AltosTelemetryMegaSensor.java b/altoslib/AltosTelemetryMegaSensor.java
index e97e18e2..cda977eb 100644
--- a/altoslib/AltosTelemetryMegaSensor.java
+++ b/altoslib/AltosTelemetryMegaSensor.java
@@ -16,7 +16,7 @@
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  */
 
-package org.altusmetrum.altoslib_13;
+package org.altusmetrum.altoslib_14;
 
 public class AltosTelemetryMegaSensor extends AltosTelemetryStandard {
 	int	orient() { return int8(5); }
@@ -37,8 +37,152 @@ public class AltosTelemetryMegaSensor extends AltosTelemetryStandard {
 	int	mag_z() { return int16(28); }
 	int	mag_y() { return int16(30); }
 
-	public AltosTelemetryMegaSensor(int[] bytes) throws AltosCRCException {
+	int imu_type;
+
+	private int accel_across(int imu_type) {
+		switch (imu_type) {
+		case AltosIMU.imu_type_telemega_v1_v2:
+		case AltosIMU.imu_type_telemega_v3:
+		case AltosIMU.imu_type_easymega_v1:
+			return accel_x();
+		case AltosIMU.imu_type_easymega_v2:
+			return -accel_y();
+		case  AltosIMU.imu_type_telemega_v4:
+			return -accel_y();
+		default:
+			return AltosLib.MISSING;
+		}
+	}
+
+	private int accel_along(int imu_type) {
+		switch (imu_type) {
+		case AltosIMU.imu_type_telemega_v1_v2:
+		case AltosIMU.imu_type_telemega_v3:
+		case AltosIMU.imu_type_easymega_v1:
+			return accel_y();
+		case AltosIMU.imu_type_easymega_v2:
+		case AltosIMU.imu_type_telemega_v4:
+			return accel_x();
+		default:
+			return AltosLib.MISSING;
+		}
+	}
+
+	private int accel_through(int imu_type) {
+		return accel_z();
+	}
+
+	private int gyro_roll(int imu_type) {
+		switch (imu_type) {
+		case AltosIMU.imu_type_telemega_v1_v2:
+		case AltosIMU.imu_type_telemega_v3:
+		case AltosIMU.imu_type_easymega_v1:
+			return gyro_y();
+		case AltosIMU.imu_type_easymega_v2:
+		case AltosIMU.imu_type_telemega_v4:
+			return gyro_x();
+		default:
+			return AltosLib.MISSING;
+		}
+	}
+
+	private int gyro_pitch(int imu_type) {
+		switch (imu_type) {
+		case AltosIMU.imu_type_telemega_v1_v2:
+		case AltosIMU.imu_type_telemega_v3:
+		case AltosIMU.imu_type_easymega_v1:
+			return gyro_x();
+		case AltosIMU.imu_type_easymega_v2:
+			return -gyro_y();
+		case AltosIMU.imu_type_telemega_v4:
+			return gyro_y();
+		default:
+			return AltosLib.MISSING;
+		}
+	}
+
+	private int gyro_yaw(int imu_type) {
+		return gyro_z();
+	}
+
+	public static int mag_across_axis(int imu_type) {
+		switch (imu_type) {
+		case AltosIMU.imu_type_telemega_v1_v2:
+		case AltosIMU.imu_type_telemega_v3:
+		case AltosIMU.imu_type_easymega_v1:
+			return AltosIMU.imu_axis_x;
+		case AltosIMU.imu_type_telemega_v4:
+		case AltosIMU.imu_type_easymega_v2:
+			return AltosIMU.imu_axis_y;
+		default:
+			return AltosLib.MISSING;
+		}
+	}
+
+	private int mag_across(int imu_type) {
+		switch (imu_type) {
+		case AltosIMU.imu_type_telemega_v1_v2:
+		case AltosIMU.imu_type_telemega_v3:
+		case AltosIMU.imu_type_easymega_v1:
+			return mag_x();
+		case AltosIMU.imu_type_telemega_v4:
+		case AltosIMU.imu_type_easymega_v2:
+			return -mag_y();
+		default:
+			return AltosLib.MISSING;
+		}
+	}
+
+	public static int mag_along_axis(int imu_type) {
+		switch (imu_type) {
+		case AltosIMU.imu_type_telemega_v1_v2:
+		case AltosIMU.imu_type_telemega_v3:
+		case AltosIMU.imu_type_easymega_v1:
+			return AltosIMU.imu_axis_y;
+		case AltosIMU.imu_type_easymega_v2:
+		case AltosIMU.imu_type_telemega_v4:
+			return AltosIMU.imu_axis_x;
+		default:
+			return AltosLib.MISSING;
+		}
+	}
+
+	private int mag_along(int imu_type) {
+		switch (imu_type) {
+		case AltosIMU.imu_type_telemega_v1_v2:
+		case AltosIMU.imu_type_telemega_v3:
+		case AltosIMU.imu_type_easymega_v1:
+			return mag_y();
+		case AltosIMU.imu_type_easymega_v2:
+		case AltosIMU.imu_type_telemega_v4:
+			return mag_x();
+		default:
+			return AltosLib.MISSING;
+		}
+	}
+
+	public static int mag_through_axis(int imu_type) {
+		return AltosIMU.imu_axis_z;
+	}
+
+	private int mag_through(int imu_type) {
+		return mag_z();
+	}
+
+	public AltosTelemetryMegaSensor(int[] bytes, int imu_type) throws AltosCRCException {
 		super(bytes);
+		switch (imu_type) {
+		case AltosIMU.imu_type_telemega_v1_v2:
+		case AltosIMU.imu_type_telemega_v3:
+			if (serial() < 3000)
+				imu_type = AltosIMU.imu_type_telemega_v1_v2;
+			else
+				imu_type = AltosIMU.imu_type_telemega_v3;
+			break;
+		default:
+			break;
+		}
+		this.imu_type = imu_type;
 	}
 
 	public void provide_data(AltosDataListener listener) {
@@ -51,6 +195,7 @@ public class AltosTelemetryMegaSensor extends AltosTelemetryStandard {
 		listener.set_temperature(temp() / 100.0);
 
 		listener.set_orient(orient());
+		cal_data.set_imu_type(imu_type);
 
 		/* XXX we have no calibration data for these values */
 
@@ -59,16 +204,17 @@ public class AltosTelemetryMegaSensor extends AltosTelemetryStandard {
 		if (cal_data.gyro_zero_roll == AltosLib.MISSING)
 			cal_data.set_gyro_zero(0, 0, 0);
 
-		int	accel_along = accel_y();
-		int	accel_across = accel_x();
-		int	accel_through = accel_z();
-		int	gyro_roll = gyro_y();
-		int	gyro_pitch = gyro_x();
-		int	gyro_yaw = gyro_z();
+		int	accel_along = accel_along(imu_type);
+		int	accel_across = accel_across(imu_type);
+		int	accel_through = accel_through(imu_type);
+
+		int	gyro_roll = gyro_roll(imu_type);
+		int	gyro_pitch = gyro_pitch(imu_type);
+		int	gyro_yaw = gyro_yaw(imu_type);
 
-		int	mag_along = mag_y();
-		int	mag_across = mag_x();
-		int	mag_through = mag_z();
+		int	mag_along = mag_along(imu_type);
+		int	mag_across = mag_across(imu_type);
+		int	mag_through = mag_through(imu_type);
 
 		listener.set_accel(cal_data.accel_along(accel_along),
 				   cal_data.accel_across(accel_across),