+ }
+
+ 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;
+ }
+ }
+
+ 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) {
+ if (imu.gyro_x != AltosLib.MISSING) {
+ 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 (imu.mag_x != 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)));
+ }
+ }
+ } 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;
+ }
+ }