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>

diff --git a/altoslib/AltosEepromMega.java b/altoslib/AltosEepromMega.java
index 7a4ee52d10680e1e61c3289bb029a12e079b444c..47013b5eaab0e8d2ce3701e78d14bc479f8e2485 100644 (file)
--- 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();
                        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();
                        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;
 
 
                        state.mag = mag;
 

diff --git a/altoslib/AltosIMU.java b/altoslib/AltosIMU.java
index 6d88ccae9a45f6a45926444528db9debc04993b4..e6e70401eabbbd53a701ef3e9424a66ab3b3c0fe 100644 (file)
--- 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 {
 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:"))
 
        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) {
                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;
        }
                }
                return true;
        }

diff --git a/altoslib/AltosMag.java b/altoslib/AltosMag.java
index 89e72bd6e5983153dcacb499011a254369aa946f..5136bfd20f13b60312082f27b4d1ce02118b8a9e 100644 (file)
--- 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 {
 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")) {
 
        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) {
                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;
        }
                }
                return true;
        }

diff --git a/altoslib/AltosTelemetryMegaSensor.java b/altoslib/AltosTelemetryMegaSensor.java
index 23b67af8b03e8a37ba82a172b28b0ae54625d874..8afabd3064afc9e8327602a92764eb6fefdc71c7 100644 (file)
--- a/altoslib/AltosTelemetryMegaSensor.java
+++ b/altoslib/AltosTelemetryMegaSensor.java
@@ -63,21 +63,21 @@ public class AltosTelemetryMegaSensor extends AltosTelemetryStandard {
 
                AltosIMU imu = new AltosIMU();
                
 
                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();
 
 
                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;
        }
 
                state.mag = mag;
        }