2 * Copyright © 2012 Keith Packard <keithp@keithp.com>
4 * This program is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation; either version 2 of the License, or
7 * (at your option) any later version.
9 * This program is distributed in the hope that it will be useful, but
10 * WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
12 * General Public License for more details.
14 * You should have received a copy of the GNU General Public License along
15 * with this program; if not, write to the Free Software Foundation, Inc.,
16 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
19 package org.altusmetrum.altoslib_14;
21 import java.util.concurrent.*;
24 public class AltosIMU implements Cloneable {
25 private int accel_x = AltosLib.MISSING;
26 private int accel_y = AltosLib.MISSING;
27 private int accel_z = AltosLib.MISSING;
29 private int accel_along = AltosLib.MISSING;
30 private int accel_across = AltosLib.MISSING;
31 private int accel_through = AltosLib.MISSING;
33 private int gyro_x = AltosLib.MISSING;
34 private int gyro_y = AltosLib.MISSING;
35 private int gyro_z = AltosLib.MISSING;
37 private int gyro_roll = AltosLib.MISSING;
38 private int gyro_pitch = AltosLib.MISSING;
39 private int gyro_yaw = AltosLib.MISSING;
41 private int mag_x = AltosLib.MISSING;
42 private int mag_y = AltosLib.MISSING;
43 private int mag_z = AltosLib.MISSING;
45 private int mag_along = AltosLib.MISSING;
46 private int mag_across = AltosLib.MISSING;
47 private int mag_through = AltosLib.MISSING;
49 private int imu_model = AltosLib.MISSING;
50 private int mag_model = AltosLib.MISSING;
52 private static final double counts_per_g_mpu = 2048.0;
53 private static final double counts_per_g_bmx = 2048.0;
54 private static final double counts_per_g_adxl = 20.5;
55 private static final double counts_per_g_bmi088 = 1365.0;
57 private static double counts_per_g(int imu_type, int imu_model) {
59 case AltosLib.model_mpu6000:
60 case AltosLib.model_mpu9250:
61 return counts_per_g_mpu;
62 case AltosLib.model_adxl375:
63 return counts_per_g_adxl;
64 case AltosLib.model_bmx160:
65 return counts_per_g_bmx;
66 case AltosLib.model_bmi088:
67 return counts_per_g_bmi088;
71 case imu_type_telemega_v1_v2:
72 case imu_type_telemega_v3:
73 case imu_type_easymega_v1:
74 case imu_type_easymega_v2:
75 return counts_per_g_mpu;
76 case imu_type_telemega_v4:
77 case imu_type_easytimer_v1:
78 return counts_per_g_bmx;
79 case imu_type_easymotor_v2:
80 return counts_per_g_adxl;
83 return AltosLib.MISSING;
86 public static double convert_accel(double counts, int imu_type, int imu_model) {
87 double cpg = counts_per_g(imu_type, imu_model);
88 if (cpg == AltosLib.MISSING)
89 return AltosLib.MISSING;
90 return counts / cpg * AltosConvert.gravity;
93 private static final double GYRO_FULLSCALE_DEGREES_MPU = 2000.0;
94 private static final double GYRO_COUNTS_MPU = 32767.0;
95 private static final double counts_per_degree_mpu = GYRO_COUNTS_MPU / GYRO_FULLSCALE_DEGREES_MPU;
96 private static final double GYRO_FULLSCALE_DEGREES_BMX = 2000.0;
97 private static final double GYRO_COUNTS_BMX = 32767.0;
98 private static final double counts_per_degree_bmx = GYRO_COUNTS_BMX / GYRO_FULLSCALE_DEGREES_BMX;
99 private static final double counts_per_degree_bmi088 = 16.384;
101 private static double counts_per_degree(int imu_type, int imu_model) {
103 case AltosLib.model_mpu6000:
104 case AltosLib.model_mpu9250:
105 return counts_per_degree_mpu;
106 case AltosLib.model_bmx160:
107 return counts_per_degree_bmx;
108 case AltosLib.model_bmi088:
109 return counts_per_degree_bmi088;
113 case imu_type_telemega_v1_v2:
114 case imu_type_telemega_v3:
115 case imu_type_easymega_v1:
116 case imu_type_easymega_v2:
117 return counts_per_degree_mpu;
118 case imu_type_telemega_v4:
119 case imu_type_easytimer_v1:
120 return counts_per_degree_bmx;
122 return AltosLib.MISSING;
126 public static double gyro_degrees_per_second(double counts, int imu_type, int imu_model) {
127 double cpd = counts_per_degree(imu_type, imu_model);
129 if (cpd == AltosLib.MISSING)
130 return AltosLib.MISSING;
134 private static final double MAG_FULLSCALE_GAUSS_MPU = 48.00; /* 4800µT */
135 private static final double MAG_COUNTS_MPU = 32767.0;
136 private static final double counts_per_gauss_mpu = MAG_COUNTS_MPU / MAG_FULLSCALE_GAUSS_MPU;
138 private static final double counts_per_gauss_bmx = 100.0; /* BMX driver converts to µT */
140 public static double counts_per_gauss(int imu_type, int imu_model) {
142 case AltosLib.model_mpu9250:
143 return counts_per_gauss_mpu;
144 case AltosLib.model_bmx160:
145 return counts_per_gauss_bmx;
149 case imu_type_telemega_v3:
150 case imu_type_easymega_v2:
151 return counts_per_gauss_mpu;
152 case imu_type_telemega_v4:
153 case imu_type_easytimer_v1:
154 return counts_per_gauss_bmx;
156 return AltosLib.MISSING;
159 private boolean parse_string(String line) {
160 if (line.startsWith("Accel:")) {
162 String[] items = line.split("\\s+");
164 if (items.length >= 8) {
165 accel_x = Integer.parseInt(items[1]);
166 accel_y = Integer.parseInt(items[2]);
167 accel_z = Integer.parseInt(items[3]);
168 gyro_x = Integer.parseInt(items[5]);
169 gyro_y = Integer.parseInt(items[6]);
170 gyro_z = Integer.parseInt(items[7]);
172 if (items.length >= 12) {
173 mag_x = Integer.parseInt(items[9]);
174 mag_y = Integer.parseInt(items[10]);
175 mag_z = Integer.parseInt(items[11]);
179 if (line.startsWith("MPU6000:")) {
180 String[] items = line.split("\\s+");
182 imu_model = AltosLib.model_mpu6000;
184 if (items.length >= 7) {
185 accel_along = Integer.parseInt(items[1]);
186 accel_across = Integer.parseInt(items[2]);
187 accel_through = Integer.parseInt(items[3]);
188 gyro_roll = Integer.parseInt(items[4]);
189 gyro_pitch = Integer.parseInt(items[5]);
190 gyro_yaw = Integer.parseInt(items[6]);
194 if (line.startsWith("BMI088:")) {
195 String[] items = line.split("\\s+");
197 imu_model = AltosLib.model_bmi088;
199 if (items.length >= 7) {
200 accel_along = Integer.parseInt(items[1]);
201 accel_across = Integer.parseInt(items[2]);
202 accel_through = Integer.parseInt(items[3]);
203 gyro_roll = Integer.parseInt(items[4]);
204 gyro_pitch = Integer.parseInt(items[5]);
205 gyro_yaw = Integer.parseInt(items[6]);
213 public AltosIMU clone() {
214 AltosIMU n = new AltosIMU();
220 n.accel_along = accel_along;
221 n.accel_across = accel_across;
222 n.accel_through = accel_through;
228 n.gyro_roll = gyro_roll;
229 n.gyro_pitch = gyro_pitch;
230 n.gyro_yaw = gyro_yaw;
236 n.mag_along = mag_along;
237 n.mag_across = mag_across;
238 n.mag_through = mag_through;
243 public static final int imu_type_telemega_v1_v2 = 0; /* MPU6000 */
244 public static final int imu_type_telemega_v3 = 1; /* MPU9250 */
245 public static final int imu_type_telemega_v4 = 2; /* BMX160 */
247 public static final int imu_type_easymega_v1 = 3; /* MPU6000 */
248 public static final int imu_type_easymega_v2 = 4; /* MPU9250 */
250 public static final int imu_type_easytimer_v1 = 5; /* BMX160 */
252 public static final int imu_type_easymotor_v2 = 6; /* ADXL375 (accel only) */
254 private int accel_across(int imu_type) {
256 if (accel_across != AltosLib.MISSING)
260 case imu_type_telemega_v1_v2:
261 case imu_type_telemega_v3:
262 case imu_type_easymega_v1:
264 case imu_type_easymega_v2:
266 case imu_type_telemega_v4:
267 case imu_type_easytimer_v1:
269 case imu_type_easymotor_v2:
272 return AltosLib.MISSING;
276 private int accel_along(int imu_type) {
277 if (accel_along != AltosLib.MISSING)
281 case imu_type_telemega_v1_v2:
282 case imu_type_telemega_v3:
283 case imu_type_easymega_v1:
285 case imu_type_easymega_v2:
286 case imu_type_telemega_v4:
287 case imu_type_easytimer_v1:
289 case imu_type_easymotor_v2:
292 return AltosLib.MISSING;
296 private int accel_through(int imu_type) {
297 if (accel_through != AltosLib.MISSING)
298 return accel_through;
303 private int gyro_roll(int imu_type) {
304 if (gyro_roll != AltosLib.MISSING)
308 case imu_type_telemega_v1_v2:
309 case imu_type_telemega_v3:
310 case imu_type_easymega_v1:
312 case imu_type_easymega_v2:
313 case imu_type_telemega_v4:
314 case imu_type_easytimer_v1:
317 return AltosLib.MISSING;
321 private int gyro_pitch(int imu_type) {
322 if (gyro_pitch != AltosLib.MISSING)
326 case imu_type_telemega_v1_v2:
327 case imu_type_telemega_v3:
328 case imu_type_easymega_v1:
330 case imu_type_easymega_v2:
332 case imu_type_telemega_v4:
333 case imu_type_easytimer_v1:
336 return AltosLib.MISSING;
340 private int gyro_yaw(int imu_type) {
341 if (gyro_yaw != AltosLib.MISSING)
347 private int mag_across(int imu_type) {
348 if (mag_across != AltosLib.MISSING)
352 case imu_type_telemega_v1_v2:
353 case imu_type_telemega_v3:
354 case imu_type_easymega_v1:
356 case imu_type_easymega_v2:
358 case imu_type_telemega_v4:
359 case imu_type_easytimer_v1:
362 return AltosLib.MISSING;
366 private int mag_along(int imu_type) {
367 if (mag_along != AltosLib.MISSING)
371 case imu_type_telemega_v1_v2:
372 case imu_type_telemega_v3:
373 case imu_type_easymega_v1:
375 case imu_type_easymega_v2:
376 case imu_type_telemega_v4:
377 case imu_type_easytimer_v1:
380 return AltosLib.MISSING;
384 private int mag_through(int imu_type) {
385 if (mag_through != AltosLib.MISSING)
391 private static boolean is_primary_accel(int imu_type) {
393 case imu_type_easytimer_v1:
400 static public void provide_data(AltosDataListener listener, AltosLink link, int imu_type) throws InterruptedException {
402 AltosIMU imu = new AltosIMU(link);
403 AltosCalData cal_data = listener.cal_data();
405 if (imu_type != AltosLib.MISSING)
406 cal_data.set_imu_type(imu_type);
408 if (imu.imu_model != AltosLib.MISSING)
409 cal_data.set_imu_model(imu.imu_model);
410 if (imu.mag_model != AltosLib.MISSING)
411 cal_data.set_mag_model(imu.mag_model);
413 if (imu.gyro_roll(imu_type) != AltosLib.MISSING) {
414 cal_data.set_gyro_zero(0, 0, 0);
415 listener.set_gyro(cal_data.gyro_roll(imu.gyro_roll(imu_type)),
416 cal_data.gyro_pitch(imu.gyro_pitch(imu_type)),
417 cal_data.gyro_yaw(imu.gyro_yaw(imu_type)));
419 listener.set_accel_ground(cal_data.accel_along(imu.accel_along(imu_type)),
420 cal_data.accel_across(imu.accel_across(imu_type)),
421 cal_data.accel_through(imu.accel_through(imu_type)));
422 listener.set_accel(cal_data.accel_along(imu.accel_along(imu_type)),
423 cal_data.accel_across(imu.accel_across(imu_type)),
424 cal_data.accel_through(imu.accel_through(imu_type)));
425 if (is_primary_accel(imu_type)) {
426 int accel = imu.accel_along(imu_type);
427 if (!cal_data.adxl375_inverted)
429 if (cal_data.pad_orientation == 1)
431 listener.set_acceleration(cal_data.acceleration(accel));
433 if (imu.mag_along(imu_type) != AltosLib.MISSING) {
434 listener.set_mag(cal_data.mag_along(imu.mag_along(imu_type)),
435 cal_data.mag_across(imu.mag_across(imu_type)),
436 cal_data.mag_through(imu.mag_through(imu_type)));
439 } catch (TimeoutException te) {
446 public AltosIMU(AltosLink link) throws InterruptedException, TimeoutException {
450 String line = link.get_reply_no_dialog(5000);
452 throw new TimeoutException();
454 if (parse_string(line))