+ 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;