projects
/
fw
/
altos
/ blobdiff
commit
grep
author
committer
pickaxe
?
search:
re
summary
|
shortlog
|
log
|
commit
|
commitdiff
|
tree
raw
|
inline
| side by side
altoslib: Add support for TeleMega v3.0 log files
[fw/altos]
/
altoslib
/
AltosIMU.java
diff --git
a/altoslib/AltosIMU.java
b/altoslib/AltosIMU.java
index a22b3fed29290ca6e59f765734c581f388d1e55c..ba6f1a82584089c46a643d4e67d03af691ce39aa 100644
(file)
--- a/
altoslib/AltosIMU.java
+++ b/
altoslib/AltosIMU.java
@@
-3,7
+3,8
@@
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; version 2 of the License.
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
@@
-15,41
+16,32
@@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
*/
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
*/
-package org.altusmetrum.altoslib_
4
;
+package org.altusmetrum.altoslib_
12
;
import java.util.concurrent.*;
import java.util.concurrent.*;
+import java.io.*;
public class AltosIMU implements Cloneable {
public class AltosIMU implements Cloneable {
- public
double
accel_x;
- public
double
accel_y;
- public
double
accel_z;
+ public
int
accel_x;
+ public
int
accel_y;
+ public
int
accel_z;
- public
double
gyro_x;
- public
double
gyro_y;
- public
double
gyro_z;
+ public
int
gyro_x;
+ public
int
gyro_y;
+ public
int
gyro_z;
-/*
- * XXX use ground measurements to adjust values
-
- public double ground_accel_x;
- public double ground_accel_y;
- public double ground_accel_z;
-
- public double ground_gyro_x;
- public double ground_gyro_y;
- public double ground_gyro_z;
-*/
+ public static final double counts_per_g = 2048.0;
- public static int counts_per_g = 2048;
-
- public static double convert_accel(int counts) {
- return (double) counts / (double) counts_per_g * (-AltosConvert.GRAVITATIONAL_ACCELERATION);
+ public static double convert_accel(double counts) {
+ return counts / counts_per_g * AltosConvert.gravity;
}
}
- public static double counts_per_degsec = 16.4;
+ /* In radians */
+ public static final double GYRO_FULLSCALE_DEGREES = 2000.0;
+ public static final double GYRO_COUNTS = 32767.0;
- public static double
convert_gyro(int counts
) {
- return (
double) counts / counts_per_degsec
;
+ public static double
gyro_degrees_per_second(double counts, double cal
) {
+ return (
counts - cal) * GYRO_FULLSCALE_DEGREES / GYRO_COUNTS
;
}
public boolean parse_string(String line) {
}
public boolean parse_string(String line) {
@@
-59,12
+51,12
@@
public class AltosIMU implements Cloneable {
String[] items = line.split("\\s+");
if (items.length >= 8) {
String[] items = line.split("\\s+");
if (items.length >= 8) {
- accel_x =
convert_accel(Integer.parseInt(items[1])
);
- accel_y =
convert_accel(Integer.parseInt(items[2])
);
- accel_z =
convert_accel(Integer.parseInt(items[3])
);
- gyro_x =
convert_gyro(Integer.parseInt(items[5])
);
- gyro_y =
convert_gyro(Integer.parseInt(items[6])
);
- gyro_z =
convert_gyro(Integer.parseInt(items[7])
);
+ accel_x =
Integer.parseInt(items[1]
);
+ accel_y =
Integer.parseInt(items[2]
);
+ accel_z =
Integer.parseInt(items[3]
);
+ gyro_x =
Integer.parseInt(items[5]
);
+ gyro_y =
Integer.parseInt(items[6]
);
+ gyro_z =
Integer.parseInt(items[7]
);
}
return true;
}
}
return true;
}
@@
-82,12
+74,19
@@
public class AltosIMU implements Cloneable {
return n;
}
return n;
}
- static public void
update_state(AltosState state, AltosLink link, AltosConfigData config_data
) throws InterruptedException {
+ static public void
provide_data(AltosDataListener listener, AltosLink link
) throws InterruptedException {
try {
AltosIMU imu = new AltosIMU(link);
try {
AltosIMU imu = new AltosIMU(link);
-
- if (imu != null)
- state.set_imu(imu);
+ AltosCalData cal_data = listener.cal_data();
+
+ if (imu != null) {
+ listener.set_gyro(cal_data.gyro_roll(imu.gyro_y),
+ cal_data.gyro_pitch(imu.gyro_x),
+ cal_data.gyro_yaw(imu.gyro_z));
+ listener.set_accel_ground(imu.accel_y,
+ imu.accel_x,
+ imu.accel_z);
+ }
} catch (TimeoutException te) {
}
}
} catch (TimeoutException te) {
}
}