X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=altoslib%2FAltosIMU.java;h=8adf710e028a03f8e401ec4c48de5c1e9138a805;hp=8f6731faa5a9bebbbbf1712d0502a4bf4d471042;hb=8d77d5032781c5ef0dbb19de07ea97389b809f08;hpb=bf88c5f829ea5d32043431945e862a9f6c96740a diff --git a/altoslib/AltosIMU.java b/altoslib/AltosIMU.java index 8f6731fa..8adf710e 100644 --- a/altoslib/AltosIMU.java +++ b/altoslib/AltosIMU.java @@ -3,7 +3,8 @@ * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. * * This program is distributed in the hope that it will be useful, but * WITHOUT ANY WARRANTY; without even the implied warranty of @@ -15,15 +16,205 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. */ -package org.altusmetrum.altoslib_1; +package org.altusmetrum.altoslib_13; -public class AltosIMU { - public int accel_x; - public int accel_y; - public int accel_z; +import java.util.concurrent.*; +import java.io.*; - public int gyro_x; - public int gyro_y; - public int gyro_z; +public class AltosIMU implements Cloneable { + public int accel_x = AltosLib.MISSING; + public int accel_y = AltosLib.MISSING; + public int accel_z = AltosLib.MISSING; + + 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; + + public static double convert_accel(double counts) { + return counts / counts_per_g * AltosConvert.gravity; + } + + /* In radians */ + public static final double GYRO_FULLSCALE_DEGREES = 2000.0; + public static final double GYRO_COUNTS = 32767.0; + + 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) { + if (!line.startsWith("Accel:")) + return false; + + String[] items = line.split("\\s+"); + + if (items.length >= 8) { + 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; + } + + public AltosIMU clone() { + AltosIMU n = new AltosIMU(); + + 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; + + return n; + } + + 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_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_x = AltosLib.MISSING; + accel_y = AltosLib.MISSING; + accel_z = AltosLib.MISSING; + + 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 { + this(); + link.printf("I\n"); + for (;;) { + String line = link.get_reply_no_dialog(5000); + if (line == null) { + throw new TimeoutException(); + } + if (parse_string(line)) + break; + } + } } - \ No newline at end of file