altoslib: Support TeleMega v5.0

[fw/altos] / altoslib / AltosMag.java

diff --git a/altoslib/AltosMag.java b/altoslib/AltosMag.java
index ab5d3f57e38cb5385350f673912b1df35b566c73..c7f1ac1dfabba9bea522fc55d9c577cf0e9f20d3 100644 (file)
--- a/altoslib/AltosMag.java
+++ b/altoslib/AltosMag.java
@@ -22,44 +22,84 @@ import java.util.concurrent.*;
 import java.io.*;
 
 public class AltosMag implements Cloneable {
-       public int              x;
-       public int              z;
-       public int              y;
+       public int              along = AltosLib.MISSING;
+       public int              across = AltosLib.MISSING;
+       public int              through = AltosLib.MISSING;
 
-       public static final double counts_per_gauss = 1090;
+       public static final int model_hmc5883 = 0;
+       public static final int model_mmc5983 = 1;
 
-       public static double convert_gauss(double counts) {
-               return counts / counts_per_gauss;
+       public int mag_model = AltosLib.MISSING;
+
+       public static final double counts_per_gauss_hmc5883 = 1090;
+       public static final double counts_per_gauss_mmc5983 = 4096;
+
+       public static double counts_per_gauss(int imu_type, int mag_model) {
+               switch(mag_model) {
+               case AltosLib.model_hmc5883:
+                       return counts_per_gauss_hmc5883;
+               case AltosLib.model_mmc5983:
+                       return counts_per_gauss_mmc5983;
+               }
+               switch (imu_type) {
+               case AltosIMU.imu_type_telemega_v1_v2:
+               case AltosIMU.imu_type_easymega_v1:
+                       return counts_per_gauss_hmc5883;
+               }
+
+               return AltosIMU.counts_per_gauss(imu_type, mag_model);
+       }
+
+       public static double convert_gauss(double counts, int imu_type, int mag_model) {
+               double cpg = counts_per_gauss(imu_type, mag_model);
+
+               if (cpg == AltosLib.MISSING)
+                       return AltosLib.MISSING;
+
+               return counts / cpg;
        }
 
        public boolean parse_string(String line) {
 
-               if (!line.startsWith("X:"))
-                       return false;
+               if (line.startsWith("X:")) {
 
-               String[] items = line.split("\\s+");
+                       String[] items = line.split("\\s+");
 
-               if (items.length >= 6) {
-                       x = Integer.parseInt(items[1]);
-                       z = Integer.parseInt(items[3]);
-                       y = Integer.parseInt(items[5]);
+                       mag_model = model_hmc5883;
+
+                       if (items.length >= 6) {
+                               across = Integer.parseInt(items[1]);
+                               through = Integer.parseInt(items[3]);
+                               along = Integer.parseInt(items[5]);
+                       }
+                       return true;
+               }
+               if (line.startsWith("MMC5983:")) {
+                       String[] items = line.split("\\s+");
+
+                       mag_model = model_mmc5983;
+
+                       if (items.length >= 4) {
+                               along = Integer.parseInt(items[1]);
+                               across = Integer.parseInt(items[2]);
+                               through = Integer.parseInt(items[3]);
+                       }
+                       return true;
                }
-               return true;
+
+               return false;
        }
 
        public AltosMag clone() {
                AltosMag n = new AltosMag();
 
-               n.x = x;
-               n.z = z;
-               n.y = y;
+               n.along = along;
+               n.across = across;
+               n.through = through;
                return n;
        }
 
        public AltosMag() {
-               x = AltosLib.MISSING;
-               z = AltosLib.MISSING;
-               y = AltosLib.MISSING;
        }
 
        static public void provide_data(AltosDataListener listener, AltosLink link) throws InterruptedException {
@@ -67,10 +107,13 @@ public class AltosMag implements Cloneable {
                        AltosMag        mag = new AltosMag(link);
                        AltosCalData    cal_data = listener.cal_data();
 
-                       if (mag != null)
-                               listener.set_mag(cal_data.mag_along(mag.y),
-                                                cal_data.mag_across(mag.x),
-                                                cal_data.mag_through(mag.z));
+                       if (mag != null) {
+                               if (mag.mag_model != AltosLib.MISSING)
+                                       cal_data.set_mag_model(mag.mag_model);
+                               listener.set_mag(cal_data.mag_along(mag.along),
+                                                cal_data.mag_across(mag.across),
+                                                cal_data.mag_through(mag.through));
+                       }
                } catch (TimeoutException te) {
                }
        }