- public AltosIMU(int accel_along, int accel_across, int accel_through,
- int gyro_roll, int gyro_pitch, int gyro_yaw) {
+ 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_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_along(orient)),
+ cal_data.mag_across(imu.mag_across(orient)),
+ cal_data.mag_through(imu.mag_through(orient)));
+ }
+ }
+ } catch (TimeoutException te) {
+ }
+ }
+
+ public AltosIMU() {
+ accel_x = AltosLib.MISSING;
+ accel_y = AltosLib.MISSING;
+ accel_z = AltosLib.MISSING;