* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
*/
-package org.altusmetrum.altoslib_13;
+package org.altusmetrum.altoslib_14;
import java.util.concurrent.*;
import java.io.*;
public int mag_y = AltosLib.MISSING;
public int mag_z = AltosLib.MISSING;
- public static final double counts_per_g = 2048.0;
+ public static final double counts_per_g_mpu = 2048.0;
+ public static final double counts_per_g_bmx = 2048.0;
+ public static final double counts_per_g_adxl = 20.5;
- public static double convert_accel(double counts) {
- return counts / counts_per_g * AltosConvert.gravity;
+ private static double counts_per_g(int imu_type) {
+ switch (imu_type) {
+ case imu_type_telemega_v1_v2:
+ case imu_type_telemega_v3:
+ case imu_type_easymega_v1:
+ case imu_type_easymega_v2:
+ return counts_per_g_mpu;
+ case imu_type_telemega_v4:
+ case imu_type_easytimer_v1:
+ return counts_per_g_bmx;
+ case imu_type_easymotor_v2:
+ return counts_per_g_adxl;
+ default:
+ return AltosLib.MISSING;
+ }
+ }
+
+ public static double convert_accel(double counts, int imu_type) {
+ double cpg = counts_per_g(imu_type);
+ if (cpg == AltosLib.MISSING)
+ return AltosLib.MISSING;
+ return counts / cpg * AltosConvert.gravity;
}
- /* In radians */
- public static final double GYRO_FULLSCALE_DEGREES = 2000.0;
- public static final double GYRO_COUNTS = 32767.0;
+ public static final double GYRO_FULLSCALE_DEGREES_MPU = 2000.0;
+ public static final double GYRO_COUNTS_MPU = 32767.0;
+ public static final double counts_per_degree_mpu = GYRO_COUNTS_MPU / GYRO_FULLSCALE_DEGREES_MPU;
+ public static final double GYRO_FULLSCALE_DEGREES_BMX = 2000.0;
+ public static final double GYRO_COUNTS_BMX = 32767.0;
+ public static final double counts_per_degree_bmx = GYRO_COUNTS_BMX / GYRO_FULLSCALE_DEGREES_BMX;
+
+ private static double counts_per_degree(int imu_type) {
+ switch (imu_type) {
+ case imu_type_telemega_v1_v2:
+ case imu_type_telemega_v3:
+ case imu_type_easymega_v1:
+ case imu_type_easymega_v2:
+ return counts_per_degree_mpu;
+ case imu_type_telemega_v4:
+ case imu_type_easytimer_v1:
+ return counts_per_degree_bmx;
+ default:
+ return AltosLib.MISSING;
+ }
+ }
- public static double gyro_degrees_per_second(double counts, double cal) {
- return (counts - cal) * GYRO_FULLSCALE_DEGREES / GYRO_COUNTS;
+ public static double gyro_degrees_per_second(double counts, int imu_type) {
+ double cpd = counts_per_degree(imu_type);
+ if (cpd == AltosLib.MISSING)
+ return AltosLib.MISSING;
+ return counts / cpd;
+ }
+
+ public static final int imu_axis_x = 0;
+ public static final int imu_axis_y = 1;
+ public static final int imu_axis_z = 2;
+
+ public static final double MAG_FULLSCALE_GAUSS_MPU = 48.00; /* 4800µT */
+ public static final double MAG_COUNTS_MPU = 32767.0;
+ public static final double counts_per_gauss_mpu = MAG_COUNTS_MPU / MAG_FULLSCALE_GAUSS_MPU;
+
+ public static final double counts_per_gauss_bmx = 100.0; /* BMX driver converts to µT */
+
+ public static double counts_per_gauss(int imu_type, int axis) {
+ switch(imu_type) {
+ case imu_type_telemega_v1_v2:
+ case imu_type_easymega_v1:
+ return AltosMag.counts_per_gauss;
+ case imu_type_telemega_v3:
+ case imu_type_easymega_v2:
+ return counts_per_gauss_mpu;
+ case imu_type_telemega_v4:
+ case imu_type_easytimer_v1:
+ return 100.0;
+ default:
+ return AltosLib.MISSING;
+ }
+ }
+
+ public static double convert_gauss(double counts, int imu_type, int imu_axis) {
+ double cpg = counts_per_gauss(imu_type, imu_axis);
+ if (cpg == AltosLib.MISSING)
+ return AltosLib.MISSING;
+ return counts / cpg;
}
public boolean parse_string(String line) {
return n;
}
- static public void provide_data(AltosDataListener listener, AltosLink link) throws InterruptedException {
+ public static final int imu_type_telemega_v1_v2 = 0; /* MPU6000 */
+ public static final int imu_type_telemega_v3 = 1; /* MPU9250 */
+ public static final int imu_type_telemega_v4 = 2; /* BMX160 */
+
+ public static final int imu_type_easymega_v1 = 3; /* MPU6000 */
+ public static final int imu_type_easymega_v2 = 4; /* MPU9250 */
+
+ public static final int imu_type_easytimer_v1 = 5; /* BMX160 */
+
+ public static final int imu_type_easymotor_v2 = 6; /* ADXL375 (accel only) */
+
+ private int accel_across(int imu_type) {
+ switch (imu_type) {
+ case imu_type_telemega_v1_v2:
+ case imu_type_telemega_v3:
+ case imu_type_easymega_v1:
+ return accel_x;
+ case imu_type_easymega_v2:
+ return -accel_y;
+ case imu_type_telemega_v4:
+ case imu_type_easytimer_v1:
+ return -accel_y;
+ case imu_type_easymotor_v2:
+ return accel_y;
+ default:
+ return AltosLib.MISSING;
+ }
+ }
+
+ private int accel_along(int imu_type) {
+ switch (imu_type) {
+ case imu_type_telemega_v1_v2:
+ case imu_type_telemega_v3:
+ case imu_type_easymega_v1:
+ return accel_y;
+ case imu_type_easymega_v2:
+ case imu_type_telemega_v4:
+ case imu_type_easytimer_v1:
+ return accel_x;
+ case imu_type_easymotor_v2:
+ return -accel_x;
+ default:
+ return AltosLib.MISSING;
+ }
+ }
+
+ private int accel_through(int imu_type) {
+ return accel_z;
+ }
+
+ private int gyro_roll(int imu_type) {
+ switch (imu_type) {
+ case imu_type_telemega_v1_v2:
+ case imu_type_telemega_v3:
+ case imu_type_easymega_v1:
+ return gyro_y;
+ case imu_type_easymega_v2:
+ case imu_type_telemega_v4:
+ case imu_type_easytimer_v1:
+ return gyro_x;
+ default:
+ return AltosLib.MISSING;
+ }
+ }
+
+ private int gyro_pitch(int imu_type) {
+ switch (imu_type) {
+ case imu_type_telemega_v1_v2:
+ case imu_type_telemega_v3:
+ case imu_type_easymega_v1:
+ return gyro_x;
+ case imu_type_easymega_v2:
+ return -gyro_y;
+ case imu_type_telemega_v4:
+ case imu_type_easytimer_v1:
+ return -gyro_y;
+ default:
+ return AltosLib.MISSING;
+ }
+ }
+
+ private int gyro_yaw(int imu_type) {
+ return gyro_z;
+ }
+
+ public static int mag_across_axis(int imu_type) {
+ switch (imu_type) {
+ case imu_type_telemega_v1_v2:
+ case imu_type_telemega_v3:
+ case imu_type_easymega_v1:
+ return imu_axis_x;
+ case imu_type_easymega_v2:
+ case imu_type_telemega_v4:
+ case imu_type_easytimer_v1:
+ return imu_axis_y;
+ default:
+ return AltosLib.MISSING;
+ }
+ }
+
+ private int mag_across(int imu_type) {
+ switch (imu_type) {
+ case imu_type_telemega_v1_v2:
+ case imu_type_telemega_v3:
+ case imu_type_easymega_v1:
+ return mag_x;
+ case imu_type_easymega_v2:
+ return -mag_y;
+ case imu_type_telemega_v4:
+ case imu_type_easytimer_v1:
+ return mag_y;
+ default:
+ return AltosLib.MISSING;
+ }
+ }
+
+ public static int mag_along_axis(int imu_type) {
+ switch (imu_type) {
+ case imu_type_telemega_v1_v2:
+ case imu_type_telemega_v3:
+ case imu_type_easymega_v1:
+ return imu_axis_y;
+ case imu_type_easymega_v2:
+ case imu_type_telemega_v4:
+ case imu_type_easytimer_v1:
+ return imu_axis_x;
+ default:
+ return AltosLib.MISSING;
+ }
+ }
+
+ private int mag_along(int imu_type) {
+ switch (imu_type) {
+ case imu_type_telemega_v1_v2:
+ case imu_type_telemega_v3:
+ case imu_type_easymega_v1:
+ return mag_y;
+ case imu_type_easymega_v2:
+ case imu_type_telemega_v4:
+ case imu_type_easytimer_v1:
+ return mag_x;
+ default:
+ return AltosLib.MISSING;
+ }
+ }
+
+ private static boolean is_primary_accel(int imu_type) {
+ switch (imu_type) {
+ case imu_type_easytimer_v1:
+ return true;
+ default:
+ return false;
+ }
+ }
+
+ public static int mag_through_axis(int imu_type) {
+ return imu_axis_z;
+ }
+
+ private int mag_through(int imu_type) {
+ return mag_z;
+ }
+
+ static public void provide_data(AltosDataListener listener, AltosLink link, int imu_type) throws InterruptedException {
try {
AltosIMU imu = new AltosIMU(link);
AltosCalData cal_data = listener.cal_data();
+ cal_data.set_imu_type(imu_type);
+
if (imu != null) {
- listener.set_gyro(cal_data.gyro_roll(imu.gyro_y),
- cal_data.gyro_pitch(imu.gyro_x),
- cal_data.gyro_yaw(imu.gyro_z));
- listener.set_accel_ground(imu.accel_y,
- imu.accel_x,
- imu.accel_z);
+ if (imu.gyro_x != AltosLib.MISSING) {
+ cal_data.set_gyro_zero(0, 0, 0);
+ listener.set_gyro(cal_data.gyro_roll(imu.gyro_roll(imu_type)),
+ cal_data.gyro_pitch(imu.gyro_pitch(imu_type)),
+ cal_data.gyro_yaw(imu.gyro_yaw(imu_type)));
+ }
+ listener.set_accel_ground(cal_data.accel_along(imu.accel_along(imu_type)),
+ cal_data.accel_across(imu.accel_across(imu_type)),
+ cal_data.accel_through(imu.accel_through(imu_type)));
+ listener.set_accel(cal_data.accel_along(imu.accel_along(imu_type)),
+ cal_data.accel_across(imu.accel_across(imu_type)),
+ cal_data.accel_through(imu.accel_through(imu_type)));
+ if (is_primary_accel(imu_type)) {
+ int accel = imu.accel_along(imu_type);
+ if (!cal_data.adxl375_inverted)
+ accel = -accel;
+ if (cal_data.pad_orientation == 1)
+ accel = -accel;
+ listener.set_acceleration(cal_data.acceleration(accel));
+ }
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));
+ listener.set_mag(cal_data.mag_along(imu.mag_along(imu_type)),
+ cal_data.mag_across(imu.mag_across(imu_type)),
+ cal_data.mag_through(imu.mag_through(imu_type)));
}
}
} catch (TimeoutException te) {