From 91bcfae2e64ecb2e7de1292b264910382b635aea Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 20 Dec 2013 19:37:08 -0800 Subject: [PATCH] altoslib: Convert IMU and Mag sensor values to useful units Convert from raw sensor values to metric units Signed-off-by: Keith Packard --- altoslib/AltosEepromMega.java | 18 ++++++------- altoslib/AltosIMU.java | 36 +++++++++++++++++--------- altoslib/AltosMag.java | 18 ++++++++----- altoslib/AltosTelemetryMegaSensor.java | 18 ++++++------- 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; } -- 2.30.2