altoslib: Compute speed at entry to each state

[fw/altos] / altoslib / AltosState.java

diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java
index a75c240c41334d58e4ae4661749a43b9095a4d0c..9ee3d57ddf814fe340ef53df61fa2e31758d5cae 100644 (file)
--- a/altoslib/AltosState.java
+++ b/altoslib/AltosState.java
@@ -20,7 +20,7 @@
  * Track flight state from telemetry or eeprom data stream
  */
 
-package org.altusmetrum.altoslib_11;
+package org.altusmetrum.altoslib_12;
 
 public class AltosState extends AltosDataListener {
 
@@ -909,11 +909,11 @@ public class AltosState extends AltosDataListener {
        public double   accel_ground_along, accel_ground_across, accel_ground_through;
 
        void update_pad_rotation() {
-               if (cal_data.pad_orientation != AltosLib.MISSING && accel_ground_along != AltosLib.MISSING) {
-                       rotation = new AltosRotation(AltosIMU.convert_accel(accel_ground_across - cal_data.accel_zero_across),
-                                                    AltosIMU.convert_accel(accel_ground_through - cal_data.accel_zero_through),
-                                                    AltosIMU.convert_accel(accel_ground_along - cal_data.accel_zero_along),
-                                                    cal_data.pad_orientation);
+               if (cal_data().pad_orientation != AltosLib.MISSING && accel_ground_along != AltosLib.MISSING) {
+                       rotation = new AltosRotation(AltosIMU.convert_accel(accel_ground_across - cal_data().accel_zero_across),
+                                                    AltosIMU.convert_accel(accel_ground_through - cal_data().accel_zero_through),
+                                                    AltosIMU.convert_accel(accel_ground_along - cal_data().accel_zero_along),
+                                                    cal_data().pad_orientation);
                        orient.set_computed(rotation.tilt(), time);
                }
        }
@@ -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);
                        }
                }
@@ -1066,8 +1066,6 @@ public class AltosState extends AltosDataListener {
 
        public AltosState (AltosCalData cal_data) {
                super(cal_data);
-               if (cal_data == null)
-                       Thread.dumpStack();
                init();
        }
 }