altoslib: Fix Idle Monitor on TeleMega v3

The new IMU includes a mag sensor, and there's no separate mag sensor.

Add support for reading the mag sensor values from the IMU, separate
out v3 from previous TeleMega versions so that the code can stop
trying to read the mag sensor.

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

diff --git a/altoslib/AltosIMU.java b/altoslib/AltosIMU.java
index ba6f1a82584089c46a643d4e67d03af691ce39aa..df81c9ddea979a3b56fb2ca91760c33aa9204afb 100644 (file)
--- a/altoslib/AltosIMU.java
+++ b/altoslib/AltosIMU.java
@@ -22,13 +22,17 @@ import java.util.concurrent.*;
 import java.io.*;
 
 public class AltosIMU implements Cloneable {
-       public int              accel_x;
-       public int              accel_y;
-       public int              accel_z;
+       public int              accel_x = AltosLib.MISSING;
+       public int              accel_y = AltosLib.MISSING;
+       public int              accel_z = AltosLib.MISSING;
 
-       public int              gyro_x;
-       public int              gyro_y;
-       public int              gyro_z;
+       public int              gyro_x = AltosLib.MISSING;
+       public int              gyro_y = AltosLib.MISSING;
+       public int              gyro_z = AltosLib.MISSING;
+
+       public int              mag_x = AltosLib.MISSING;
+       public int              mag_y = AltosLib.MISSING;
+       public int              mag_z = AltosLib.MISSING;
 
        public static final double      counts_per_g = 2048.0;
 
@@ -58,6 +62,11 @@ public class AltosIMU implements Cloneable {
                        gyro_y = Integer.parseInt(items[6]);
                        gyro_z = Integer.parseInt(items[7]);
                }
+               if (items.length >= 12) {
+                       mag_x = Integer.parseInt(items[9]);
+                       mag_y = Integer.parseInt(items[10]);
+                       mag_z = Integer.parseInt(items[11]);
+               }
                return true;
        }
 
@@ -71,6 +80,11 @@ public class AltosIMU implements Cloneable {
                n.gyro_x = gyro_x;
                n.gyro_y = gyro_y;
                n.gyro_z = gyro_z;
+
+               n.mag_x = mag_x;
+               n.mag_y = mag_y;
+               n.mag_z = mag_z;
+
                return n;
        }
 
@@ -86,6 +100,11 @@ public class AltosIMU implements Cloneable {
                                listener.set_accel_ground(imu.accel_y,
                                                          imu.accel_x,
                                                          imu.accel_z);
+                               if (imu.mag_x != AltosLib.MISSING) {
+                                       listener.set_mag(cal_data.mag_along(imu.mag_y),
+                                                        cal_data.mag_across(imu.mag_x),
+                                                        cal_data.mag_through(imu.mag_z));
+                               }
                        }
                } catch (TimeoutException te) {
                }
@@ -99,6 +118,10 @@ public class AltosIMU implements Cloneable {
                gyro_x = AltosLib.MISSING;
                gyro_y = AltosLib.MISSING;
                gyro_z = AltosLib.MISSING;
+
+               mag_x = AltosLib.MISSING;
+               mag_y = AltosLib.MISSING;
+               mag_z = AltosLib.MISSING;
        }
 
        public AltosIMU(AltosLink link) throws InterruptedException, TimeoutException {

diff --git a/altoslib/AltosIdleFetch.java b/altoslib/AltosIdleFetch.java
index 058df0a1a246db3455e4f4877b05df0f8ff241fa..88a5dab4eafd4885ff36abe7d8914a1e1a80df72 100644 (file)
--- a/altoslib/AltosIdleFetch.java
+++ b/altoslib/AltosIdleFetch.java
@@ -133,12 +133,30 @@ public class AltosIdleFetch implements AltosDataProvider {
                               AltosIdler.idle_ms5607,
                               AltosIdler.idle_sensor_metrum),
 
-               new AltosIdler("TeleMega",
+               new AltosIdler("TeleMega-v0",
                               AltosIdler.idle_gps,
                               AltosIdler.idle_mma655x,
                               AltosIdler.idle_ms5607,
                               AltosIdler.idle_imu, AltosIdler.idle_mag,
                               AltosIdler.idle_sensor_mega),
+               new AltosIdler("TeleMega-v1",
+                              AltosIdler.idle_gps,
+                              AltosIdler.idle_mma655x,
+                              AltosIdler.idle_ms5607,
+                              AltosIdler.idle_imu, AltosIdler.idle_mag,
+                              AltosIdler.idle_sensor_mega),
+               new AltosIdler("TeleMega-v2",
+                              AltosIdler.idle_gps,
+                              AltosIdler.idle_mma655x,
+                              AltosIdler.idle_ms5607,
+                              AltosIdler.idle_imu, AltosIdler.idle_mag,
+                              AltosIdler.idle_sensor_mega),
+               new AltosIdler("TeleMega-v3",
+                              AltosIdler.idle_gps,
+                              AltosIdler.idle_mma655x,
+                              AltosIdler.idle_ms5607,
+                              AltosIdler.idle_imu,
+                              AltosIdler.idle_sensor_mega),
                new AltosIdler("EasyMega",
                               AltosIdler.idle_mma655x,
                               AltosIdler.idle_ms5607,