return temp_gps;
}
- public int imu_type = AltosLib.MISSING;;
+ public int imu_type = AltosLib.MISSING;
+
+ public int imu_model = AltosLib.MISSING;
+
+ public int mag_model = AltosLib.MISSING;
public void set_imu_type(int imu_type) {
this.imu_type = imu_type;
}
+ public void set_imu_model(int imu_model) {
+ this.imu_model = imu_model;
+ }
+
+ public void set_mag_model(int mag_model) {
+ this.mag_model = mag_model;
+ }
+
public double accel_zero_along, accel_zero_across, accel_zero_through;
public void set_accel_zero(double zero_along, double zero_across, double zero_through) {
}
public double accel_along(double counts) {
- return AltosIMU.convert_accel(counts - accel_zero_along, imu_type);
+ return AltosIMU.convert_accel(counts - accel_zero_along, imu_type, imu_model);
}
public double accel_across(double counts) {
- return AltosIMU.convert_accel(counts - accel_zero_across, imu_type);
+ return AltosIMU.convert_accel(counts - accel_zero_across, imu_type, imu_model);
}
public double accel_through(double counts) {
- return AltosIMU.convert_accel(counts - accel_zero_through, imu_type);
+ return AltosIMU.convert_accel(counts - accel_zero_through, imu_type, imu_model);
}
public double gyro_zero_roll = AltosLib.MISSING;
if (gyro_zero_roll == AltosLib.MISSING || counts == AltosLib.MISSING)
return AltosLib.MISSING;
- return AltosIMU.gyro_degrees_per_second(counts - gyro_zero_roll, imu_type);
+ return AltosIMU.gyro_degrees_per_second(counts - gyro_zero_roll, imu_type, imu_model);
}
public double gyro_pitch(double counts) {
if (gyro_zero_pitch == AltosLib.MISSING || counts == AltosLib.MISSING)
return AltosLib.MISSING;
- return AltosIMU.gyro_degrees_per_second(counts - gyro_zero_pitch, imu_type);
+ return AltosIMU.gyro_degrees_per_second(counts - gyro_zero_pitch, imu_type, imu_model);
}
public double gyro_yaw(double counts) {
if (gyro_zero_yaw == AltosLib.MISSING || counts == AltosLib.MISSING)
return AltosLib.MISSING;
- return AltosIMU.gyro_degrees_per_second(counts - gyro_zero_yaw, imu_type);
+ return AltosIMU.gyro_degrees_per_second(counts - gyro_zero_yaw, imu_type, imu_model);
}
private double gyro_zero_overflow(double first) {
public double mag_along(double along) {
if (along == AltosLib.MISSING)
return AltosLib.MISSING;
- return AltosIMU.convert_gauss(along, imu_type, AltosIMU.mag_along_axis(imu_type));
+ return AltosMag.convert_gauss(along, imu_type, mag_model);
}
public double mag_across(double across) {
if (across == AltosLib.MISSING)
return AltosLib.MISSING;
- return AltosIMU.convert_gauss(across, imu_type, AltosIMU.mag_across_axis(imu_type));
+ return AltosMag.convert_gauss(across, imu_type, mag_model);
}
public double mag_through(double through) {
if (through == AltosLib.MISSING)
return AltosLib.MISSING;
- return AltosIMU.convert_gauss(through, imu_type, AltosIMU.mag_through_axis(imu_type));
+ return AltosMag.convert_gauss(through, imu_type, mag_model);
}
public AltosCalData() {
case AltosLib.AO_LOG_FORMAT_TELEMEGA_3:
case AltosLib.AO_LOG_FORMAT_EASYMEGA_2:
case AltosLib.AO_LOG_FORMAT_TELEMEGA_4:
+ case AltosLib.AO_LOG_FORMAT_TELEMEGA_5:
return data32(16);
case AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD:
return data16(14);
case AltosLib.AO_LOG_FORMAT_TELEMEGA_3:
case AltosLib.AO_LOG_FORMAT_EASYMEGA_2:
case AltosLib.AO_LOG_FORMAT_TELEMEGA_4:
+ case AltosLib.AO_LOG_FORMAT_TELEMEGA_5:
return data32(20);
case AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD:
return data16(16);
case AltosLib.AO_LOG_FORMAT_TELEMEGA_3:
case AltosLib.AO_LOG_FORMAT_EASYMEGA_2:
case AltosLib.AO_LOG_FORMAT_TELEMEGA_4:
+ case AltosLib.AO_LOG_FORMAT_TELEMEGA_5:
return data32(24);
case AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD:
return data16(18);
private int mag_z() { return data16(22); }
private int mag_y() { return data16(24); }
+ /* normalized log data */
+ private int norm_accel_along() { return data16(8); }
+ private int norm_accel_across() { return data16(10); }
+ private int norm_accel_through() { return data16(12); }
+ private int norm_gyro_roll() { return data16(14); }
+ private int norm_gyro_pitch() { return data16(16); }
+ private int norm_gyro_yaw() { return data16(18); }
+ private int norm_mag_along() { return data16(20); }
+ private int norm_mag_across() { return data16(22); }
+ private int norm_mag_through() { return data16(24); }
+
private int imu_type() {
switch (log_format) {
case AltosLib.AO_LOG_FORMAT_TELEMEGA:
}
}
+ private int imu_model() {
+ switch (log_format) {
+ case AltosLib.AO_LOG_FORMAT_TELEMEGA_5:
+ return AltosLib.model_mpu6000;
+ }
+ return AltosLib.MISSING;
+ }
+
+ private boolean sensor_normalized() {
+ switch (log_format) {
+ case AltosLib.AO_LOG_FORMAT_TELEMEGA_5:
+ return true;
+ }
+ return false;
+ }
+
+ private int mag_model() {
+ switch (log_format) {
+ case AltosLib.AO_LOG_FORMAT_TELEMEGA_5:
+ return AltosLib.model_mmc5983;
+ }
+ return AltosLib.MISSING;
+ }
+
private int accel_across() {
+ if (sensor_normalized()) {
+ return norm_accel_across();
+ }
+
switch (log_format) {
case AltosLib.AO_LOG_FORMAT_TELEMEGA:
case AltosLib.AO_LOG_FORMAT_TELEMEGA_3:
}
private int accel_along(){
+ if (sensor_normalized()) {
+ return norm_accel_along();
+ }
+
switch (log_format) {
case AltosLib.AO_LOG_FORMAT_TELEMEGA:
case AltosLib.AO_LOG_FORMAT_TELEMEGA_3:
}
private int accel_through() {
+ if (sensor_normalized()) {
+ return norm_accel_through();
+ }
+
return accel_z();
}
private int gyro_pitch() {
+ if (sensor_normalized()) {
+ return norm_gyro_pitch();
+ }
+
switch (log_format) {
case AltosLib.AO_LOG_FORMAT_TELEMEGA:
case AltosLib.AO_LOG_FORMAT_TELEMEGA_3:
}
private int gyro_roll() {
+ if (sensor_normalized()) {
+ return norm_gyro_roll();
+ }
+
switch (log_format) {
case AltosLib.AO_LOG_FORMAT_TELEMEGA:
case AltosLib.AO_LOG_FORMAT_TELEMEGA_3:
}
private int gyro_yaw() {
+ if (sensor_normalized()) {
+ return norm_gyro_yaw();
+ }
+
return gyro_z();
}
private int mag_across() {
+ if (sensor_normalized()) {
+ return norm_mag_across();
+ }
+
switch (log_format) {
case AltosLib.AO_LOG_FORMAT_TELEMEGA:
case AltosLib.AO_LOG_FORMAT_TELEMEGA_3:
}
private int mag_along() {
+ if (sensor_normalized()) {
+ return norm_mag_along();
+ }
+
switch (log_format) {
case AltosLib.AO_LOG_FORMAT_TELEMEGA:
case AltosLib.AO_LOG_FORMAT_TELEMEGA_3:
}
private int mag_through() {
+ if (sensor_normalized()) {
+ return norm_mag_through();
+ }
+
return mag_z();
}
AltosGPS gps;
cal_data.set_imu_type(imu_type());
+ cal_data.set_imu_model(imu_model());
+ cal_data.set_mag_model(mag_model());
switch (cmd()) {
case AltosLib.AO_LOG_FLIGHT:
public int accel_y = AltosLib.MISSING;
public int accel_z = AltosLib.MISSING;
+ public int accel_along = AltosLib.MISSING;
+ public int accel_across = AltosLib.MISSING;
+ public int accel_through = AltosLib.MISSING;
+
public int gyro_x = AltosLib.MISSING;
public int gyro_y = AltosLib.MISSING;
public int gyro_z = AltosLib.MISSING;
+ public int gyro_roll = AltosLib.MISSING;
+ public int gyro_pitch = AltosLib.MISSING;
+ public int gyro_yaw = AltosLib.MISSING;
+
public int mag_x = AltosLib.MISSING;
public int mag_y = AltosLib.MISSING;
public int mag_z = AltosLib.MISSING;
+ public int mag_along = AltosLib.MISSING;
+ public int mag_across = AltosLib.MISSING;
+ public int mag_through = AltosLib.MISSING;
+
+ public int imu_model = AltosLib.MISSING;
+ public int mag_model = AltosLib.MISSING;
+
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;
- private static double counts_per_g(int imu_type) {
+ private static double counts_per_g(int imu_type, int imu_model) {
+ switch (imu_model) {
+ case AltosLib.model_mpu6000:
+ case AltosLib.model_mpu9250:
+ return counts_per_g_mpu;
+ case AltosLib.model_adxl375:
+ return counts_per_g_adxl;
+ case AltosLib.model_bmx160:
+ return counts_per_g_bmx;
+ }
+
switch (imu_type) {
case imu_type_telemega_v1_v2:
case imu_type_telemega_v3:
return counts_per_g_bmx;
case imu_type_easymotor_v2:
return counts_per_g_adxl;
- default:
- return AltosLib.MISSING;
}
+
+ return AltosLib.MISSING;
}
- public static double convert_accel(double counts, int imu_type) {
- double cpg = counts_per_g(imu_type);
+ public static double convert_accel(double counts, int imu_type, int imu_model) {
+ double cpg = counts_per_g(imu_type, imu_model);
if (cpg == AltosLib.MISSING)
return AltosLib.MISSING;
return counts / cpg * AltosConvert.gravity;
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) {
+ private static double counts_per_degree(int imu_type, int imu_model) {
+ switch (imu_model) {
+ case AltosLib.model_mpu6000:
+ case AltosLib.model_mpu9250:
+ return counts_per_degree_mpu;
+ case AltosLib.model_bmx160:
+ return counts_per_degree_bmx;
+ }
+
switch (imu_type) {
case imu_type_telemega_v1_v2:
case imu_type_telemega_v3:
}
}
- public static double gyro_degrees_per_second(double counts, int imu_type) {
- double cpd = counts_per_degree(imu_type);
+ public static double gyro_degrees_per_second(double counts, int imu_type, int imu_model) {
+ double cpd = counts_per_degree(imu_type, imu_model);
+
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) {
+ public static double counts_per_gauss(int imu_type, int imu_model) {
+ switch (imu_model) {
+ case AltosLib.model_mpu9250:
+ return counts_per_gauss_mpu;
+ case AltosLib.model_bmx160:
+ return counts_per_gauss_bmx;
+ }
+
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;
+ return counts_per_gauss_bmx;
}
- }
-
- 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;
+ return AltosLib.MISSING;
}
public boolean parse_string(String line) {
- if (!line.startsWith("Accel:"))
- return false;
+ if (line.startsWith("Accel:")) {
- String[] items = line.split("\\s+");
+ 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 >= 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;
}
- if (items.length >= 12) {
- mag_x = Integer.parseInt(items[9]);
- mag_y = Integer.parseInt(items[10]);
- mag_z = Integer.parseInt(items[11]);
+ if (line.startsWith("MPU6000:")) {
+ String[] items = line.split("\\s+");
+
+ imu_model = AltosLib.model_mpu6000;
+
+ if (items.length >= 7) {
+ accel_along = Integer.parseInt(items[1]);
+ accel_across = Integer.parseInt(items[2]);
+ accel_through = Integer.parseInt(items[3]);
+ gyro_roll = Integer.parseInt(items[4]);
+ gyro_pitch = Integer.parseInt(items[5]);
+ gyro_yaw = Integer.parseInt(items[6]);
+ }
+ return true;
}
- return true;
+
+ return false;
}
public AltosIMU clone() {
n.accel_y = accel_y;
n.accel_z = accel_z;
+ n.accel_along = accel_along;
+ n.accel_across = accel_across;
+ n.accel_through = accel_through;
+
n.gyro_x = gyro_x;
n.gyro_y = gyro_y;
n.gyro_z = gyro_z;
+ n.gyro_roll = gyro_roll;
+ n.gyro_pitch = gyro_pitch;
+ n.gyro_yaw = gyro_yaw;
+
n.mag_x = mag_x;
n.mag_y = mag_y;
n.mag_z = mag_z;
+ n.mag_along = mag_along;
+ n.mag_across = mag_across;
+ n.mag_through = mag_through;
+
return n;
}
public static final int imu_type_easymotor_v2 = 6; /* ADXL375 (accel only) */
private int accel_across(int imu_type) {
+
+ if (accel_across != AltosLib.MISSING)
+ return accel_across;
+
switch (imu_type) {
case imu_type_telemega_v1_v2:
case imu_type_telemega_v3:
}
private int accel_along(int imu_type) {
+ if (accel_along != AltosLib.MISSING)
+ return accel_along;
+
switch (imu_type) {
case imu_type_telemega_v1_v2:
case imu_type_telemega_v3:
}
private int accel_through(int imu_type) {
+ if (accel_through != AltosLib.MISSING)
+ return accel_through;
+
return accel_z;
}
private int gyro_roll(int imu_type) {
+ if (gyro_roll != AltosLib.MISSING)
+ return gyro_roll;
+
switch (imu_type) {
case imu_type_telemega_v1_v2:
case imu_type_telemega_v3:
}
private int gyro_pitch(int imu_type) {
+ if (gyro_pitch != AltosLib.MISSING)
+ return gyro_pitch;
+
switch (imu_type) {
case imu_type_telemega_v1_v2:
case imu_type_telemega_v3:
}
private int gyro_yaw(int imu_type) {
- return gyro_z;
- }
+ if (gyro_yaw != AltosLib.MISSING)
+ return gyro_yaw;
- 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;
- }
+ return gyro_z;
}
private int mag_across(int imu_type) {
+ if (mag_across != AltosLib.MISSING)
+ return mag_across;
+
switch (imu_type) {
case imu_type_telemega_v1_v2:
case imu_type_telemega_v3:
}
}
- 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) {
+ if (mag_along != AltosLib.MISSING)
+ return mag_along;
+
switch (imu_type) {
case imu_type_telemega_v1_v2:
case imu_type_telemega_v3:
}
}
+ private int mag_through(int imu_type) {
+ if (mag_through != AltosLib.MISSING)
+ return mag_through;
+
+ return mag_z;
+ }
+
private static boolean is_primary_accel(int imu_type) {
switch (imu_type) {
case imu_type_easytimer_v1:
}
}
- 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_type != AltosLib.MISSING)
+ cal_data.set_imu_type(imu_type);
if (imu != null) {
- if (imu.gyro_x != AltosLib.MISSING) {
+ if (imu.imu_model != AltosLib.MISSING)
+ cal_data.set_imu_model(imu.imu_model);
+ if (imu.mag_model != AltosLib.MISSING)
+ cal_data.set_mag_model(imu.mag_model);
+
+ if (imu.gyro_roll(imu_type) != 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)),
accel = -accel;
listener.set_acceleration(cal_data.acceleration(accel));
}
- if (imu.mag_x != AltosLib.MISSING) {
+ if (imu.mag_along(imu_type) != AltosLib.MISSING) {
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)));
}
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 {
static final int idle_ms5607 = 9;
static final int idle_adxl375 = 10;
static final int idle_adxl375_easymotor_v2 = 11;
+ static final int idle_imu = 12;
static final int idle_sensor_tm = 100;
static final int idle_sensor_metrum = 101;
case idle_imu_et_v1:
AltosIMU.provide_data(listener, link, AltosIMU.imu_type_easytimer_v1);
break;
+ case idle_imu:
+ AltosIMU.provide_data(listener, link, AltosLib.MISSING);
+ break;
case idle_mag:
AltosMag.provide_data(listener, link);
break;
AltosIdler.idle_ms5607,
AltosIdler.idle_imu_tm_v4,
AltosIdler.idle_sensor_mega),
+ new AltosIdler("TeleMega-v5",
+ AltosIdler.idle_gps,
+ AltosIdler.idle_adxl375,
+ AltosIdler.idle_ms5607,
+ AltosIdler.idle_imu, AltosIdler.idle_mag,
+ AltosIdler.idle_sensor_mega),
new AltosIdler("EasyMega-v1",
AltosIdler.idle_mma655x,
AltosIdler.idle_ms5607,
public static final int AO_LOG_FORMAT_MICROPEAK2 = 18;
public static final int AO_LOG_FORMAT_TELEMEGA_4 = 19;
public static final int AO_LOG_FORMAT_EASYMOTOR = 20;
+ public static final int AO_LOG_FORMAT_TELEMEGA_5 = 21;
public static final int AO_LOG_FORMAT_NONE = 127;
+ public static final int model_mpu6000 = 0;
+ public static final int model_mpu9250 = 1;
+ public static final int model_adxl375 = 2;
+ public static final int model_bmx160 = 3;
+ public static final int model_hmc5883 = 4;
+ public static final int model_mmc5983 = 5;
+
public static boolean isspace(int c) {
switch (c) {
case ' ':
import java.io.*;
public class AltosMag implements Cloneable {
- public int x;
- public int z;
- public int y;
+ public int along = AltosLib.MISSING;
+ public int across = AltosLib.MISSING;
+ public int through = AltosLib.MISSING;
- public static final double counts_per_gauss = 1090;
+ public static final int model_hmc5883 = 0;
+ public static final int model_mmc5983 = 1;
- public static double convert_gauss(double counts) {
- return counts / counts_per_gauss;
+ public int mag_model = AltosLib.MISSING;
+
+ public static final double counts_per_gauss_hmc5883 = 1090;
+ public static final double counts_per_gauss_mmc5983 = 4096;
+
+ public static double counts_per_gauss(int imu_type, int mag_model) {
+ switch(mag_model) {
+ case AltosLib.model_hmc5883:
+ return counts_per_gauss_hmc5883;
+ case AltosLib.model_mmc5983:
+ return counts_per_gauss_mmc5983;
+ }
+ switch (imu_type) {
+ case AltosIMU.imu_type_telemega_v1_v2:
+ case AltosIMU.imu_type_easymega_v1:
+ return counts_per_gauss_hmc5883;
+ }
+
+ return AltosIMU.counts_per_gauss(imu_type, mag_model);
+ }
+
+ public static double convert_gauss(double counts, int imu_type, int mag_model) {
+ double cpg = counts_per_gauss(imu_type, mag_model);
+
+ if (cpg == AltosLib.MISSING)
+ return AltosLib.MISSING;
+
+ return counts / cpg;
}
public boolean parse_string(String line) {
- if (!line.startsWith("X:"))
- return false;
+ if (line.startsWith("X:")) {
- String[] items = line.split("\\s+");
+ String[] items = line.split("\\s+");
- if (items.length >= 6) {
- x = Integer.parseInt(items[1]);
- z = Integer.parseInt(items[3]);
- y = Integer.parseInt(items[5]);
+ mag_model = model_hmc5883;
+
+ if (items.length >= 6) {
+ across = Integer.parseInt(items[1]);
+ through = Integer.parseInt(items[3]);
+ along = Integer.parseInt(items[5]);
+ }
+ return true;
+ }
+ if (line.startsWith("MMC5983:")) {
+ String[] items = line.split("\\s+");
+
+ mag_model = model_mmc5983;
+
+ if (items.length >= 4) {
+ along = Integer.parseInt(items[1]);
+ across = Integer.parseInt(items[2]);
+ through = Integer.parseInt(items[3]);
+ }
+ return true;
}
- return true;
+
+ return false;
}
public AltosMag clone() {
AltosMag n = new AltosMag();
- n.x = x;
- n.z = z;
- n.y = y;
+ n.along = along;
+ n.across = across;
+ n.through = through;
return n;
}
public AltosMag() {
- x = AltosLib.MISSING;
- z = AltosLib.MISSING;
- y = AltosLib.MISSING;
}
static public void provide_data(AltosDataListener listener, AltosLink link) throws InterruptedException {
AltosMag mag = new AltosMag(link);
AltosCalData cal_data = listener.cal_data();
- if (mag != null)
- listener.set_mag(cal_data.mag_along(mag.y),
- cal_data.mag_across(mag.x),
- cal_data.mag_through(mag.z));
+ if (mag != null) {
+ if (mag.mag_model != AltosLib.MISSING)
+ cal_data.set_mag_model(mag.mag_model);
+ listener.set_mag(cal_data.mag_along(mag.along),
+ cal_data.mag_across(mag.across),
+ cal_data.mag_through(mag.through));
+ }
} catch (TimeoutException te) {
}
}
final static int packet_type_mini2 = 0x10;
final static int packet_type_mini3 = 0x11;
final static int packet_type_mega_sensor_bmx160 = 0x12;
+ final static int packet_type_mega_norm_mpu6000_mmc5983 = 0x13;
static AltosTelemetry parse_hex(String hex) throws ParseException, AltosCRCException {
AltosTelemetry telem = null;
--- /dev/null
+/*
+ * Copyright © 2021 Keith Packard <keithp@keithp.com>
+ *
+ * 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; 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
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+package org.altusmetrum.altoslib_14;
+
+public class AltosTelemetryMegaNorm extends AltosTelemetryStandard {
+ int orient() { return int8(5); }
+
+ int accel() { return int16(6); }
+ int pres() { return int32(8); }
+ int temp() { return int16(12); }
+
+ int accel_along() { return int16(14); }
+ int accel_across() { return int16(16); }
+ int accel_through() { return int16(18); }
+
+ int gyro_roll() { return int16(20); }
+ int gyro_pitch() { return int16(22); }
+ int gyro_yaw() { return int16(24); }
+
+ int mag_along() { return int16(26); }
+ int mag_across() { return int16(28); }
+ int mag_through() { return int16(30); }
+
+ int imu_model, mag_model;
+
+ public AltosTelemetryMegaNorm(int[] bytes, int imu_model, int mag_model) throws AltosCRCException {
+ super(bytes);
+ this.imu_model = imu_model;
+ this.mag_model = mag_model;
+ }
+
+ 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_model(imu_model);
+ cal_data.set_mag_model(mag_model);
+
+ /* XXX we have no calibration data for these values */
+
+ if (cal_data.accel_zero_along == AltosLib.MISSING)
+ cal_data.set_accel_zero(0, 0, 0);
+ if (cal_data.gyro_zero_roll == AltosLib.MISSING)
+ cal_data.set_gyro_zero(0, 0, 0);
+
+ int accel_along = accel_along();
+ int accel_across = accel_across();
+ int accel_through = accel_through();
+
+ int gyro_roll = gyro_roll();
+ int gyro_pitch = gyro_pitch();
+ int gyro_yaw = gyro_yaw();
+
+ int mag_along = mag_along();
+ int mag_across = mag_across();
+ int mag_through = mag_through();
+
+ listener.set_accel(cal_data.accel_along(accel_along),
+ cal_data.accel_across(accel_across),
+ cal_data.accel_through(accel_through));
+ listener.set_gyro(cal_data.gyro_roll(gyro_roll),
+ cal_data.gyro_pitch(gyro_pitch),
+ cal_data.gyro_yaw(gyro_yaw));
+ listener.set_mag(cal_data.mag_along(mag_along),
+ cal_data.mag_across(mag_across),
+ cal_data.mag_through(mag_through));
+ }
+}
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:
}
}
- 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:
}
}
- public static int mag_through_axis(int imu_type) {
- return AltosIMU.imu_axis_z;
- }
-
private int mag_through(int imu_type) {
return mag_z();
}
case packet_type_mini3:
telem = new AltosTelemetryMini3(bytes);
break;
+ case packet_type_mega_norm_mpu6000_mmc5983:
+ telem = new AltosTelemetryMegaNorm(bytes, AltosLib.model_mpu6000, AltosLib.model_mmc5983);
+ break;
default:
telem = new AltosTelemetryRaw(bytes);
break;
AltosTelemetryLocation.java \
AltosTelemetryMap.java \
AltosTelemetryMegaSensor.java \
+ AltosTelemetryMegaNorm.java \
AltosTelemetryMegaData.java \
AltosTelemetryMini2.java \
AltosTelemetryMini3.java \
projects / fw / altos / commitdiff