altoslib: Convert IMU and Mag sensor values to useful units

Convert from raw sensor values to metric units

Signed-off-by: Keith Packard <keithp@keithp.com>

4 files changed, 54 insertions, 36 deletions

diff --git a/altoslib/AltosEepromMega.java b/altoslib/AltosEepromMega.java
index 7a4ee52d..47013b5e 100644
--- a/altoslib/AltosEepromMega.java
+++ b/altoslib/AltosEepromMega.java
@@ -115,19 +115,19 @@ public class AltosEepromMega extends AltosEeprom {
 			state.set_ms5607(pres(), temp());
 
 			AltosIMU imu = new AltosIMU();
-			imu.accel_x = accel_x();
-			imu.accel_y = accel_y();
-			imu.accel_z = accel_z();
+			imu.accel_x = AltosIMU.convert_accel(accel_x());
+			imu.accel_y = AltosIMU.convert_accel(accel_y());
+			imu.accel_z = AltosIMU.convert_accel(accel_z());
 
-			imu.gyro_x = gyro_x();
-			imu.gyro_y = gyro_y();
-			imu.gyro_z = gyro_z();
+			imu.gyro_x = AltosIMU.convert_gyro(gyro_x());
+			imu.gyro_y = AltosIMU.convert_gyro(gyro_y());
+			imu.gyro_z = AltosIMU.convert_gyro(gyro_z());
 			state.imu = imu;
 
 			AltosMag mag = new AltosMag();
-			mag.x = mag_x();
-			mag.y = mag_y();
-			mag.z = mag_z();
+			mag.x = AltosMag.convert_gauss(mag_x());
+			mag.y = AltosMag.convert_gauss(mag_y());
+			mag.z = AltosMag.convert_gauss(mag_z());
 
 			state.mag = mag;
 
diff --git a/altoslib/AltosIMU.java b/altoslib/AltosIMU.java
index 6d88ccae..e6e70401 100644
--- a/altoslib/AltosIMU.java
+++ b/altoslib/AltosIMU.java
@@ -20,13 +20,25 @@ package org.altusmetrum.altoslib_2;
 import java.util.concurrent.*;
 
 public class AltosIMU implements Cloneable {
-	public int		accel_x;
-	public int		accel_y;
-	public int		accel_z;
+	public double		accel_x;
+	public double		accel_y;
+	public double		accel_z;
 
-	public int		gyro_x;
-	public int		gyro_y;
-	public int		gyro_z;
+	public double		gyro_x;
+	public double		gyro_y;
+	public double		gyro_z;
+
+	public static int	counts_per_g = 2048;
+
+	public static double convert_accel(int counts) {
+		return (double) counts / (double) counts_per_g * (-AltosConvert.GRAVITATIONAL_ACCELERATION);
+	}
+
+	public static double	counts_per_degsec = 16.4;
+
+	public static double convert_gyro(int counts) {
+		return (double) counts / counts_per_degsec;
+	}
 
 	public boolean parse_string(String line) {
 		if (!line.startsWith("Accel:"))
@@ -35,12 +47,12 @@ public class AltosIMU implements Cloneable {
 		String[] items = line.split("\\s+");
 
 		if (items.length >= 8) {
-			accel_x = Integer.parseInt(items[1]);
-			accel_y = Integer.parseInt(items[2]);
-			accel_z = Integer.parseInt(items[3]);
-			gyro_x = Integer.parseInt(items[5]);
-			gyro_y = Integer.parseInt(items[6]);
-			gyro_z = Integer.parseInt(items[7]);
+			accel_x = convert_accel(Integer.parseInt(items[1]));
+			accel_y = convert_accel(Integer.parseInt(items[2]));
+			accel_z = convert_accel(Integer.parseInt(items[3]));
+			gyro_x = convert_gyro(Integer.parseInt(items[5]));
+			gyro_y = convert_gyro(Integer.parseInt(items[6]));
+			gyro_z = convert_gyro(Integer.parseInt(items[7]));
 		}
 		return true;
 	}
diff --git a/altoslib/AltosMag.java b/altoslib/AltosMag.java
index 89e72bd6..5136bfd2 100644
--- a/altoslib/AltosMag.java
+++ b/altoslib/AltosMag.java
@@ -20,9 +20,15 @@ package org.altusmetrum.altoslib_2;
 import java.util.concurrent.*;
 
 public class AltosMag implements Cloneable {
-	public int		x;
-	public int		y;
-	public int		z;
+	public double		x;
+	public double		y;
+	public double		z;
+
+	public static double counts_per_gauss = 1090;
+
+	public static double convert_gauss(int counts) {
+		return (double) counts / counts_per_gauss;
+	}
 
 	public boolean parse_string(String line) {
 //		if (line.startsWith("Syntax error")) {
@@ -36,9 +42,9 @@ public class AltosMag implements Cloneable {
 		String[] items = line.split("\\s+");
 
 		if (items.length >= 6) {
-			x = Integer.parseInt(items[1]);
-			y = Integer.parseInt(items[3]);
-			z = Integer.parseInt(items[5]);
+			x = convert_gauss(Integer.parseInt(items[1]));
+			y = convert_gauss(Integer.parseInt(items[3]));
+			z = convert_gauss(Integer.parseInt(items[5]));
 		}
 		return true;
 	}
diff --git a/altoslib/AltosTelemetryMegaSensor.java b/altoslib/AltosTelemetryMegaSensor.java
index 23b67af8..8afabd30 100644
--- a/altoslib/AltosTelemetryMegaSensor.java
+++ b/altoslib/AltosTelemetryMegaSensor.java
@@ -63,21 +63,21 @@ public class AltosTelemetryMegaSensor extends AltosTelemetryStandard {
 
 		AltosIMU imu = new AltosIMU();
 		
-		imu.accel_x = accel_x;
-		imu.accel_y = accel_y;
-		imu.accel_z = accel_z;
+		imu.accel_x = AltosIMU.convert_accel(accel_x);
+		imu.accel_y = AltosIMU.convert_accel(accel_y);
+		imu.accel_z = AltosIMU.convert_accel(accel_z);
 
-		imu.gyro_x = gyro_x;
-		imu.gyro_y = gyro_y;
-		imu.gyro_z = gyro_z;
+		imu.gyro_x = AltosIMU.convert_gyro(gyro_x);
+		imu.gyro_y = AltosIMU.convert_gyro(gyro_y);
+		imu.gyro_z = AltosIMU.convert_gyro(gyro_z);
 
 		state.imu = imu;
 
 		AltosMag mag = new AltosMag();
 
-		mag.x = mag_x;
-		mag.y = mag_y;
-		mag.z = mag_z;
+		mag.x = AltosMag.convert_gauss(mag_x);
+		mag.y = AltosMag.convert_gauss(mag_y);
+		mag.z = AltosMag.convert_gauss(mag_z);
 
 		state.mag = mag;
 	}