Merge branch 'master' of ssh://git.gag.com/scm/git/fw/altos

[fw/altos] / altoslib / AltosIMU.java

diff --git a/altoslib/AltosIMU.java b/altoslib/AltosIMU.java
index 0ff27c75421618a49753afdd7538af45da7bbcd6..b4ba5a563e08c3b3f768dd9cad3b837eae4249db 100644 (file)
--- a/altoslib/AltosIMU.java
+++ b/altoslib/AltosIMU.java
@@ -22,36 +22,37 @@ import java.util.concurrent.*;
 import java.io.*;
 
 public class AltosIMU implements Cloneable {
-       public int              accel_x = AltosLib.MISSING;
-       public int              accel_y = AltosLib.MISSING;
-       public int              accel_z = AltosLib.MISSING;
+       private int             accel_x = AltosLib.MISSING;
+       private int             accel_y = AltosLib.MISSING;
+       private int             accel_z = AltosLib.MISSING;
 
-       public int              accel_along = AltosLib.MISSING;
-       public int              accel_across = AltosLib.MISSING;
-       public int              accel_through = AltosLib.MISSING;
+       private int             accel_along = AltosLib.MISSING;
+       private int             accel_across = AltosLib.MISSING;
+       private int             accel_through = AltosLib.MISSING;
 
-       public int              gyro_x = AltosLib.MISSING;
-       public int              gyro_y = AltosLib.MISSING;
-       public int              gyro_z = AltosLib.MISSING;
+       private int             gyro_x = AltosLib.MISSING;
+       private int             gyro_y = AltosLib.MISSING;
+       private int             gyro_z = AltosLib.MISSING;
 
-       public int              gyro_roll = AltosLib.MISSING;
-       public int              gyro_pitch = AltosLib.MISSING;
-       public int              gyro_yaw = AltosLib.MISSING;
+       private int             gyro_roll = AltosLib.MISSING;
+       private int             gyro_pitch = AltosLib.MISSING;
+       private int             gyro_yaw = AltosLib.MISSING;
 
-       public int              mag_x = AltosLib.MISSING;
-       public int              mag_y = AltosLib.MISSING;
-       public int              mag_z = AltosLib.MISSING;
+       private int             mag_x = AltosLib.MISSING;
+       private int             mag_y = AltosLib.MISSING;
+       private int             mag_z = AltosLib.MISSING;
 
-       public int              mag_along = AltosLib.MISSING;
-       public int              mag_across = AltosLib.MISSING;
-       public int              mag_through = AltosLib.MISSING;
+       private int             mag_along = AltosLib.MISSING;
+       private int             mag_across = AltosLib.MISSING;
+       private int             mag_through = AltosLib.MISSING;
 
-       public int              imu_model = AltosLib.MISSING;
-       public int              mag_model = AltosLib.MISSING;
+       private int             imu_model = AltosLib.MISSING;
+       private int             mag_model = AltosLib.MISSING;
 
-       public static final double      counts_per_g_mpu = 2048.0;
-       public static final double      counts_per_g_bmx = 2048.0;
-       public static final double      counts_per_g_adxl = 20.5;
+       private static final double     counts_per_g_mpu = 2048.0;
+       private static final double     counts_per_g_bmx = 2048.0;
+       private static final double     counts_per_g_adxl = 20.5;
+       private static final double     counts_per_g_bmi088 = 1365.0;
 
        private static double counts_per_g(int imu_type, int imu_model) {
                switch (imu_model) {
@@ -62,6 +63,8 @@ public class AltosIMU implements Cloneable {
                        return counts_per_g_adxl;
                case AltosLib.model_bmx160:
                        return counts_per_g_bmx;
+               case AltosLib.model_bmi088:
+                       return counts_per_g_bmi088;
                }
 
                switch (imu_type) {
@@ -87,12 +90,13 @@ public class AltosIMU implements Cloneable {
                return counts / cpg * AltosConvert.gravity;
        }
 
-       public static final double      GYRO_FULLSCALE_DEGREES_MPU = 2000.0;
-       public static final double      GYRO_COUNTS_MPU = 32767.0;
-       public static final double      counts_per_degree_mpu = GYRO_COUNTS_MPU / GYRO_FULLSCALE_DEGREES_MPU;
-       public static final double      GYRO_FULLSCALE_DEGREES_BMX = 2000.0;
-       public static final double      GYRO_COUNTS_BMX = 32767.0;
-       public static final double      counts_per_degree_bmx = GYRO_COUNTS_BMX / GYRO_FULLSCALE_DEGREES_BMX;
+       private static final double     GYRO_FULLSCALE_DEGREES_MPU = 2000.0;
+       private static final double     GYRO_COUNTS_MPU = 32767.0;
+       private static final double     counts_per_degree_mpu = GYRO_COUNTS_MPU / GYRO_FULLSCALE_DEGREES_MPU;
+       private static final double     GYRO_FULLSCALE_DEGREES_BMX = 2000.0;
+       private static final double     GYRO_COUNTS_BMX = 32767.0;
+       private static final double     counts_per_degree_bmx = GYRO_COUNTS_BMX / GYRO_FULLSCALE_DEGREES_BMX;
+       private static final double     counts_per_degree_bmi088 = 16.384;
 
        private static double counts_per_degree(int imu_type, int imu_model) {
                switch (imu_model) {
@@ -101,6 +105,8 @@ public class AltosIMU implements Cloneable {
                        return counts_per_degree_mpu;
                case AltosLib.model_bmx160:
                        return counts_per_degree_bmx;
+               case AltosLib.model_bmi088:
+                       return counts_per_degree_bmi088;
                }
 
                switch (imu_type) {
@@ -125,11 +131,11 @@ public class AltosIMU implements Cloneable {
                return counts / cpd;
        }
 
-       public static final double MAG_FULLSCALE_GAUSS_MPU = 48.00;     /* 4800µT */
-       public static final double MAG_COUNTS_MPU = 32767.0;
-       public static final double counts_per_gauss_mpu = MAG_COUNTS_MPU / MAG_FULLSCALE_GAUSS_MPU;
+       private static final double MAG_FULLSCALE_GAUSS_MPU = 48.00;    /* 4800µT */
+       private static final double MAG_COUNTS_MPU = 32767.0;
+       private static final double counts_per_gauss_mpu = MAG_COUNTS_MPU / MAG_FULLSCALE_GAUSS_MPU;
 
-       public static final double counts_per_gauss_bmx = 100.0;        /* BMX driver converts to µT */
+       private static final double counts_per_gauss_bmx = 100.0;       /* BMX driver converts to µT */
 
        public static double counts_per_gauss(int imu_type, int imu_model) {
                switch (imu_model) {
@@ -150,7 +156,7 @@ public class AltosIMU implements Cloneable {
                return AltosLib.MISSING;
        }
 
-       public boolean parse_string(String line) {
+       private boolean parse_string(String line) {
                if (line.startsWith("Accel:")) {
 
                        String[] items = line.split("\\s+");
@@ -185,6 +191,21 @@ public class AltosIMU implements Cloneable {
                        }
                        return true;
                }
+               if (line.startsWith("BMI088:")) {
+                       String[] items = line.split("\\s+");
+
+                       imu_model = AltosLib.model_bmi088;
+
+                       if (items.length >= 7) {
+                               accel_along = Integer.parseInt(items[1]);
+                               accel_across = Integer.parseInt(items[2]);
+                               accel_through = Integer.parseInt(items[3]);
+                               gyro_roll = Integer.parseInt(items[4]);
+                               gyro_pitch = Integer.parseInt(items[5]);
+                               gyro_yaw = Integer.parseInt(items[6]);
+                       }
+                       return true;
+               }
 
                return false;
        }