publish support for TeleGPS v2.0

[fw/altos] / altoslib / AltosState.java

diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java
index cfee819bc76610f81fd2d092a9b7c18c79126368..39ab10da14c9c78bafee2156b7b3097b6ac80a71 100644 (file)
--- a/altoslib/AltosState.java
+++ b/altoslib/AltosState.java
@@ -931,12 +931,12 @@ public class AltosState extends AltosDataListener {
                if (last_imu_time != AltosLib.MISSING) {
                        double  t = time - last_imu_time;
 
-                       double  pitch = AltosConvert.degrees_to_radians(gyro_pitch);
-                       double  yaw = AltosConvert.degrees_to_radians(gyro_yaw);
-                       double  roll = AltosConvert.degrees_to_radians(gyro_roll);
+                       if (t > 0 && gyro_pitch != AltosLib.MISSING && rotation != null) {
+                               double  pitch = AltosConvert.degrees_to_radians(gyro_pitch) * t;
+                               double  yaw = AltosConvert.degrees_to_radians(gyro_yaw) * t;
+                               double  roll = AltosConvert.degrees_to_radians(gyro_roll) * t;
 
-                       if (t > 0 && pitch != AltosLib.MISSING && rotation != null) {
-                               rotation.rotate(t, pitch, yaw, roll);
+                               rotation.rotate(pitch, yaw, roll);
                                orient.set_computed(rotation.tilt(), time);
                        }
                }
@@ -1060,7 +1060,6 @@ public class AltosState extends AltosDataListener {
        }
 
        public AltosState() {
-               Thread.dumpStack();
                init();
        }