return n;
}
- static public void provide_data(AltosDataListener listener, AltosLink link) 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_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);
+ 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_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(orient)),
+ cal_data.mag_across(imu.mag_across(orient)),
+ cal_data.mag_through(imu.mag_through(orient)));
}
}
} catch (TimeoutException te) {
static final int idle_gps = 0;
static final int idle_imu = 1;
- static final int idle_mag = 2;
+ static final int idle_imu_em_v2 = 2;
+ static final int idle_mag = 3;
static final int idle_mma655x = 4;
static final int idle_ms5607 = 5;
static final int idle_adxl375 = 6;
AltosGPS.provide_data(listener, link);
break;
case idle_imu:
- AltosIMU.provide_data(listener, link);
+ AltosIMU.provide_data(listener, link, AltosIMU.orient_telemega);
+ break;
+ case idle_imu_em_v2:
+ AltosIMU.provide_data(listener, link, AltosIMU.orient_easymega_v2);
break;
case idle_mag:
AltosMag.provide_data(listener, link);
new AltosIdler("EasyMega-v2",
AltosIdler.idle_adxl375,
AltosIdler.idle_ms5607,
- AltosIdler.idle_imu,
+ AltosIdler.idle_imu_em_v2,
AltosIdler.idle_sensor_mega),
new AltosIdler("TeleGPS-v1",
AltosIdler.idle_gps,