X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=altoslib%2FAltosTelemetryMegaSensor.java;h=cda977eb36b3a54f071caa1f550bee33239ad1c0;hp=396bdb16eddfe9aef965d3d6a12b1b23f7446b6f;hb=52149fd81df45f37f5097850795f290f463c43ef;hpb=f26cfe417c6977cf1e7e75a4f050e25f64d41859 diff --git a/altoslib/AltosTelemetryMegaSensor.java b/altoslib/AltosTelemetryMegaSensor.java index 396bdb16..cda977eb 100644 --- a/altoslib/AltosTelemetryMegaSensor.java +++ b/altoslib/AltosTelemetryMegaSensor.java @@ -16,7 +16,7 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. */ -package org.altusmetrum.altoslib_11; +package org.altusmetrum.altoslib_14; public class AltosTelemetryMegaSensor extends AltosTelemetryStandard { int orient() { return int8(5); } @@ -34,21 +34,168 @@ public class AltosTelemetryMegaSensor extends AltosTelemetryStandard { int gyro_z() { return int16(24); } int mag_x() { return int16(26); } - int mag_y() { return int16(28); } - int mag_z() { return int16(30); } + int mag_z() { return int16(28); } + int mag_y() { return int16(30); } - public AltosTelemetryMegaSensor(int[] bytes) throws AltosCRCException { + int imu_type; + + private int accel_across(int imu_type) { + switch (imu_type) { + case AltosIMU.imu_type_telemega_v1_v2: + case AltosIMU.imu_type_telemega_v3: + case AltosIMU.imu_type_easymega_v1: + return accel_x(); + case AltosIMU.imu_type_easymega_v2: + return -accel_y(); + case AltosIMU.imu_type_telemega_v4: + return -accel_y(); + default: + return AltosLib.MISSING; + } + } + + private int accel_along(int imu_type) { + switch (imu_type) { + case AltosIMU.imu_type_telemega_v1_v2: + case AltosIMU.imu_type_telemega_v3: + case AltosIMU.imu_type_easymega_v1: + return accel_y(); + case AltosIMU.imu_type_easymega_v2: + case AltosIMU.imu_type_telemega_v4: + 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 AltosIMU.imu_type_telemega_v1_v2: + case AltosIMU.imu_type_telemega_v3: + case AltosIMU.imu_type_easymega_v1: + return gyro_y(); + case AltosIMU.imu_type_easymega_v2: + case AltosIMU.imu_type_telemega_v4: + return gyro_x(); + default: + return AltosLib.MISSING; + } + } + + private int gyro_pitch(int imu_type) { + switch (imu_type) { + case AltosIMU.imu_type_telemega_v1_v2: + case AltosIMU.imu_type_telemega_v3: + case AltosIMU.imu_type_easymega_v1: + return gyro_x(); + case AltosIMU.imu_type_easymega_v2: + return -gyro_y(); + case AltosIMU.imu_type_telemega_v4: + 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 AltosIMU.imu_type_telemega_v1_v2: + case AltosIMU.imu_type_telemega_v3: + case AltosIMU.imu_type_easymega_v1: + return AltosIMU.imu_axis_x; + case AltosIMU.imu_type_telemega_v4: + case AltosIMU.imu_type_easymega_v2: + return AltosIMU.imu_axis_y; + default: + return AltosLib.MISSING; + } + } + + private int mag_across(int imu_type) { + switch (imu_type) { + case AltosIMU.imu_type_telemega_v1_v2: + case AltosIMU.imu_type_telemega_v3: + case AltosIMU.imu_type_easymega_v1: + return mag_x(); + case AltosIMU.imu_type_telemega_v4: + case AltosIMU.imu_type_easymega_v2: + return -mag_y(); + default: + return AltosLib.MISSING; + } + } + + public static int mag_along_axis(int imu_type) { + switch (imu_type) { + case AltosIMU.imu_type_telemega_v1_v2: + case AltosIMU.imu_type_telemega_v3: + case AltosIMU.imu_type_easymega_v1: + return AltosIMU.imu_axis_y; + case AltosIMU.imu_type_easymega_v2: + case AltosIMU.imu_type_telemega_v4: + return AltosIMU.imu_axis_x; + default: + return AltosLib.MISSING; + } + } + + private int mag_along(int imu_type) { + switch (imu_type) { + case AltosIMU.imu_type_telemega_v1_v2: + case AltosIMU.imu_type_telemega_v3: + case AltosIMU.imu_type_easymega_v1: + return mag_y(); + case AltosIMU.imu_type_easymega_v2: + case AltosIMU.imu_type_telemega_v4: + return mag_x(); + default: + return AltosLib.MISSING; + } + } + + public static int mag_through_axis(int imu_type) { + return AltosIMU.imu_axis_z; + } + + private int mag_through(int imu_type) { + return mag_z(); + } + + public AltosTelemetryMegaSensor(int[] bytes, int imu_type) throws AltosCRCException { super(bytes); + switch (imu_type) { + case AltosIMU.imu_type_telemega_v1_v2: + case AltosIMU.imu_type_telemega_v3: + if (serial() < 3000) + imu_type = AltosIMU.imu_type_telemega_v1_v2; + else + imu_type = AltosIMU.imu_type_telemega_v3; + break; + default: + break; + } + this.imu_type = imu_type; } - public void provide_data(AltosDataListener listener, AltosCalData cal_data) { - super.provide_data(listener, cal_data); + public void provide_data(AltosDataListener listener) { + super.provide_data(listener); + + AltosCalData cal_data = listener.cal_data(); listener.set_acceleration(cal_data.acceleration(accel())); listener.set_pressure(pres()); listener.set_temperature(temp() / 100.0); listener.set_orient(orient()); + cal_data.set_imu_type(imu_type); /* XXX we have no calibration data for these values */ @@ -57,16 +204,17 @@ public class AltosTelemetryMegaSensor extends AltosTelemetryStandard { if (cal_data.gyro_zero_roll == AltosLib.MISSING) cal_data.set_gyro_zero(0, 0, 0); - int accel_along = accel_y(); - int accel_across = accel_x(); - int accel_through = accel_z(); - int gyro_roll = gyro_y(); - int gyro_pitch = gyro_x(); - int gyro_yaw = gyro_z(); + int accel_along = accel_along(imu_type); + int accel_across = accel_across(imu_type); + int accel_through = accel_through(imu_type); + + int gyro_roll = gyro_roll(imu_type); + int gyro_pitch = gyro_pitch(imu_type); + int gyro_yaw = gyro_yaw(imu_type); - int mag_along = mag_x(); - int mag_across = mag_y(); - int mag_through = mag_z(); + int mag_along = mag_along(imu_type); + int mag_across = mag_across(imu_type); + int mag_through = mag_through(imu_type); listener.set_accel(cal_data.accel_along(accel_along), cal_data.accel_across(accel_across),