From 312f303be7f2a641020903ed2daccb8a4d67ec42 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 28 Apr 2018 12:03:10 -0700 Subject: [PATCH] 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 --- altoslib/AltosIMU.java | 35 +++++++++++++++++++++++++++++------ altoslib/AltosIdleFetch.java | 20 +++++++++++++++++++- 2 files changed, 48 insertions(+), 7 deletions(-) diff --git a/altoslib/AltosIMU.java b/altoslib/AltosIMU.java index ba6f1a82..df81c9dd 100644 --- 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 058df0a1..88a5dab4 100644 --- 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, -- 2.30.2