X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=altoslib%2FAltosIMU.java;h=8adf710e028a03f8e401ec4c48de5c1e9138a805;hp=20a2a413da4f9499a7f1ce052dd52f882440b0c9;hb=8d77d5032781c5ef0dbb19de07ea97389b809f08;hpb=f26cfe417c6977cf1e7e75a4f050e25f64d41859 diff --git a/altoslib/AltosIMU.java b/altoslib/AltosIMU.java index 20a2a413..8adf710e 100644 --- a/altoslib/AltosIMU.java +++ b/altoslib/AltosIMU.java @@ -16,19 +16,23 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. */ -package org.altusmetrum.altoslib_11; +package org.altusmetrum.altoslib_13; import java.util.concurrent.*; import java.io.*; public class AltosIMU implements Cloneable { - public int accel_along; - public int accel_across; - public int accel_through; + public int accel_x = AltosLib.MISSING; + public int accel_y = AltosLib.MISSING; + public int accel_z = AltosLib.MISSING; - public int gyro_roll; - public int gyro_pitch; - public int gyro_yaw; + 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; @@ -36,10 +40,12 @@ public class AltosIMU implements Cloneable { return counts / counts_per_g * AltosConvert.gravity; } - public static final double counts_per_degsec = 16.4; + /* In radians */ + public static final double GYRO_FULLSCALE_DEGREES = 2000.0; + public static final double GYRO_COUNTS = 32767.0; - public static double convert_gyro(double counts) { - return counts / counts_per_degsec; + public static double gyro_degrees_per_second(double counts, double cal) { + return (counts - cal) * GYRO_FULLSCALE_DEGREES / GYRO_COUNTS; } public boolean parse_string(String line) { @@ -49,12 +55,17 @@ public class AltosIMU implements Cloneable { String[] items = line.split("\\s+"); if (items.length >= 8) { - accel_along = Integer.parseInt(items[1]); - accel_across = Integer.parseInt(items[2]); - accel_through = Integer.parseInt(items[3]); - gyro_roll = Integer.parseInt(items[5]); - gyro_pitch = Integer.parseInt(items[6]); - gyro_yaw = Integer.parseInt(items[7]); + 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]); + } + if (items.length >= 12) { + mag_x = Integer.parseInt(items[9]); + mag_y = Integer.parseInt(items[10]); + mag_z = Integer.parseInt(items[11]); } return true; } @@ -62,52 +73,136 @@ public class AltosIMU implements Cloneable { public AltosIMU clone() { AltosIMU n = new AltosIMU(); - n.accel_along = accel_along; - n.accel_across = accel_across; - n.accel_through = accel_through; + n.accel_x = accel_x; + n.accel_y = accel_y; + n.accel_z = accel_z; + + 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; - n.gyro_roll = gyro_roll; - n.gyro_pitch = gyro_pitch; - n.gyro_yaw = gyro_yaw; return n; } - static public void provide_data(AltosDataListener listener, AltosLink link, AltosCalData cal_data) throws InterruptedException { + public static final int orient_telemega = 0; + public static final int orient_easymega_v2 = 1; + + private int accel_across(int orient) { + switch (orient) { + case orient_telemega: + return accel_x; + case orient_easymega_v2: + return -accel_y; + default: + return AltosLib.MISSING; + } + } + + private int accel_along(int orient) { + switch (orient) { + case orient_telemega: + return accel_y; + case orient_easymega_v2: + return accel_x; + default: + return AltosLib.MISSING; + } + } + + private int accel_through(int orient) { + return accel_z; + } + + private int gyro_roll(int orient) { + switch (orient) { + case orient_telemega: + return gyro_y; + case orient_easymega_v2: + return gyro_x; + default: + return AltosLib.MISSING; + } + } + + private int gyro_pitch(int orient) { + switch (orient) { + case orient_telemega: + return gyro_x; + case orient_easymega_v2: + return -gyro_y; + default: + return AltosLib.MISSING; + } + } + + private int gyro_yaw(int orient) { + return gyro_z; + } + + private int mag_across(int orient) { + switch (orient) { + case orient_telemega: + return mag_x; + case orient_easymega_v2: + return -mag_y; + default: + return AltosLib.MISSING; + } + } + + private int mag_along(int orient) { + switch (orient) { + case orient_telemega: + return mag_y; + case orient_easymega_v2: + return mag_x; + default: + return AltosLib.MISSING; + } + } + + private int mag_through(int orient) { + return mag_z; + } + + static public void provide_data(AltosDataListener listener, AltosLink link, int orient) throws InterruptedException { try { AltosIMU imu = new AltosIMU(link); + AltosCalData cal_data = listener.cal_data(); if (imu != null) { - listener.set_accel(cal_data.accel_along(imu.accel_along), - cal_data.accel_across(imu.accel_across), - cal_data.accel_through(imu.accel_through)); - listener.set_gyro(cal_data.gyro_roll(imu.gyro_roll), - cal_data.gyro_pitch(imu.gyro_pitch), - cal_data.gyro_yaw(imu.gyro_yaw)); + listener.set_gyro(cal_data.gyro_roll(imu.gyro_roll(orient)), + cal_data.gyro_pitch(imu.gyro_pitch(orient)), + cal_data.gyro_yaw(imu.gyro_yaw(orient))); + listener.set_accel_ground(imu.accel_along(orient), + imu.accel_across(orient), + imu.accel_through(orient)); + if (imu.mag_x != AltosLib.MISSING) { + listener.set_mag(cal_data.mag_along(imu.mag_along(orient)), + cal_data.mag_across(imu.mag_across(orient)), + cal_data.mag_through(imu.mag_through(orient))); + } } } catch (TimeoutException te) { } } public AltosIMU() { - accel_along = AltosLib.MISSING; - accel_across = AltosLib.MISSING; - accel_through = AltosLib.MISSING; - - gyro_roll = AltosLib.MISSING; - gyro_pitch = AltosLib.MISSING; - gyro_yaw = AltosLib.MISSING; - } - - public AltosIMU(int accel_along, int accel_across, int accel_through, - int gyro_roll, int gyro_pitch, int gyro_yaw) { + accel_x = AltosLib.MISSING; + accel_y = AltosLib.MISSING; + accel_z = AltosLib.MISSING; - this.accel_along = accel_along; - this.accel_across = accel_across; - this.accel_through = accel_through; + gyro_x = AltosLib.MISSING; + gyro_y = AltosLib.MISSING; + gyro_z = AltosLib.MISSING; - this.gyro_roll = gyro_roll; - this.gyro_pitch = gyro_pitch; - this.gyro_yaw = gyro_yaw; + mag_x = AltosLib.MISSING; + mag_y = AltosLib.MISSING; + mag_z = AltosLib.MISSING; } public AltosIMU(AltosLink link) throws InterruptedException, TimeoutException {