+ 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, 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:
+ 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;
+ }
+
+ return AltosLib.MISSING;
+ }
+
+ 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_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, 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:
+ 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, 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 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 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_v3:
+ case imu_type_easymega_v2:
+ return counts_per_gauss_mpu;
+ case imu_type_telemega_v4:
+ case imu_type_easytimer_v1:
+ return counts_per_gauss_bmx;
+ }
+ return AltosLib.MISSING;
+ }