altoslib: Compute speed at entry to each state

[fw/altos] / altoslib / AltosFlightSeries.java

diff --git a/altoslib/AltosFlightSeries.java b/altoslib/AltosFlightSeries.java
index 45d3b4563207a0c5f4f11309c6e41064ca107392..57f1a49163f639879c6d2ce65f7b03d429aecab4 100644 (file)
--- a/altoslib/AltosFlightSeries.java
+++ b/altoslib/AltosFlightSeries.java
@@ -363,11 +363,11 @@ public class AltosFlightSeries extends AltosDataListener {
                        double  dt = time - prev_time;
 
                        if (dt > 0) {
-                               double  roll = AltosConvert.degrees_to_radians(roll_v.value);
-                               double  pitch = AltosConvert.degrees_to_radians(gyro_pitch.value(time));
-                               double  yaw = AltosConvert.degrees_to_radians(gyro_yaw.value(time));
+                               double  roll = AltosConvert.degrees_to_radians(roll_v.value) * dt;
+                               double  pitch = AltosConvert.degrees_to_radians(gyro_pitch.value(time)) * dt;
+                               double  yaw = AltosConvert.degrees_to_radians(gyro_yaw.value(time)) * dt;
 
-                               rotation.rotate(dt, pitch, yaw, roll);
+                               rotation.rotate(pitch, yaw, roll);
                                orient_series.add(time, rotation.tilt());
                        }
                        prev_time = time;