Add version numbers to java libraries

[fw/altos] / altoslib / AltosState.java

diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java
index 68c7611f2adab67777f93eb07b18bff56b6e13cb..32d02f21d1923ba934cb756e7d013196aac3ec6d 100644 (file)
--- a/altoslib/AltosState.java
+++ b/altoslib/AltosState.java
@@ -19,7 +19,7 @@
  * Track flight state from telemetry or eeprom data stream
  */
 
-package org.altusmetrum.AltosLib;
+package org.altusmetrum.altoslib_1;
 
 public class AltosState {
        public AltosRecord data;
@@ -38,18 +38,19 @@ public class AltosState {
        public boolean boost;   /* under power */
 
        public double   ground_altitude;
+       public double   altitude;
        public double   height;
-       public double   speed;
        public double   acceleration;
        public double   battery;
        public double   temperature;
        public double   main_sense;
        public double   drogue_sense;
+       public double   accel_speed;
        public double   baro_speed;
 
        public double   max_height;
        public double   max_acceleration;
-       public double   max_speed;
+       public double   max_accel_speed;
        public double   max_baro_speed;
 
        public AltosGPS gps;
@@ -75,19 +76,42 @@ public class AltosState {
        public int      speak_tick;
        public double   speak_altitude;
 
-       public void init (AltosRecord cur, AltosState prev_state) {
-               int             i;
-               AltosRecord prev;
+       public double speed() {
+               if (ascent)
+                       return accel_speed;
+               else
+                       return baro_speed;
+       }
+
+       public double max_speed() {
+               if (max_accel_speed != 0)
+                       return max_accel_speed;
+               return max_baro_speed;
+       }
 
+       public void init (AltosRecord cur, AltosState prev_state) {
                data = cur;
 
+               /* Discard previous state if it was for a different board */
+               if (prev_state != null && prev_state.data.serial != data.serial)
+                       prev_state = null;
                ground_altitude = data.ground_altitude();
-               height = data.filtered_height();
+
+               altitude = data.altitude();
+
+               if (data.kalman_height != AltosRecord.MISSING)
+                       height = data.kalman_height;
+               else {
+                       if (prev_state != null)
+                               height = (prev_state.height * 15 + altitude - ground_altitude) / 16.0;
+               }
 
                report_time = System.currentTimeMillis();
 
-               acceleration = data.acceleration();
-               speed = data.accel_speed();
+               if (data.kalman_acceleration != AltosRecord.MISSING)
+                       acceleration = data.kalman_acceleration;
+               else
+                       acceleration = data.acceleration();
                temperature = data.temperature();
                drogue_sense = data.drogue_voltage();
                main_sense = data.main_voltage();
@@ -106,7 +130,7 @@ public class AltosState {
                        pad_alt = prev_state.pad_alt;
                        max_height = prev_state.max_height;
                        max_acceleration = prev_state.max_acceleration;
-                       max_speed = prev_state.max_speed;
+                       max_accel_speed = prev_state.max_accel_speed;
                        max_baro_speed = prev_state.max_baro_speed;
                        imu = prev_state.imu;
                        mag = prev_state.mag;
@@ -117,23 +141,39 @@ public class AltosState {
 
                        time_change = (tick - prev_state.tick) / 100.0;
 
-                       /* compute barometric speed */
+                       if (data.kalman_speed != AltosRecord.MISSING) {
+                               baro_speed = accel_speed = data.kalman_speed;
+                       } else {
+                               /* compute barometric speed */
 
-                       double height_change = height - prev_state.height;
-                       if (data.speed != AltosRecord.MISSING)
-                               baro_speed = data.speed;
-                       else {
+                               double height_change = height - prev_state.height;
                                if (time_change > 0)
                                        baro_speed = (prev_state.baro_speed * 3 + (height_change / time_change)) / 4.0;
                                else
                                        baro_speed = prev_state.baro_speed;
+
+                               if (acceleration == AltosRecord.MISSING) {
+                                       /* Fill in mising acceleration value */
+                                       accel_speed = baro_speed;
+                                       if (time_change > 0)
+                                               acceleration = (accel_speed - prev_state.accel_speed) / time_change;
+                                       else
+                                               acceleration = prev_state.acceleration;
+                               } else {
+                                       /* compute accelerometer speed */
+                                       accel_speed = prev_state.accel_speed + acceleration * time_change;
+                               }
                        }
+
                } else {
                        npad = 0;
                        ngps = 0;
                        gps = null;
                        baro_speed = 0;
+                       accel_speed = 0;
                        time_change = 0;
+                       if (acceleration == AltosRecord.MISSING)
+                               acceleration = 0;
                }
 
                time = tick / 100.0;
@@ -160,6 +200,9 @@ public class AltosState {
                                }
                                ngps++;
                        }
+               } else {
+                       if (ngps == 0)
+                               pad_alt = ground_altitude;
                }
 
                gps_waiting = MIN_PAD_SAMPLES - npad;
@@ -173,14 +216,14 @@ public class AltosState {
                boost = (AltosLib.ao_flight_boost == state);
 
                /* Only look at accelerometer data under boost */
-               if (boost && acceleration > max_acceleration)
+               if (boost && acceleration > max_acceleration && acceleration != AltosRecord.MISSING)
                        max_acceleration = acceleration;
-               if (boost && speed > max_speed)
-                       max_speed = speed;
-               if (boost && baro_speed > max_baro_speed)
+               if (boost && accel_speed > max_accel_speed && accel_speed != AltosRecord.MISSING)
+                       max_accel_speed = accel_speed;
+               if (boost && baro_speed > max_baro_speed && baro_speed != AltosRecord.MISSING)
                        max_baro_speed = baro_speed;
 
-               if (height > max_height)
+               if (height > max_height && height != AltosRecord.MISSING)
                        max_height = height;
                if (data.gps != null) {
                        if (gps == null || !gps.locked || data.gps.locked)