projects / fw / altos / blobdiff
? search:
summary | shortlog | log | commit | commitdiff | tree
raw | inline | side by side
altoslib: Reset consecutive GPS count when GPS bad
[fw/altos] / altoslib / AltosState.java
diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java
index 036c1652ed31c0c157f9c5e6b80cf3450fc1e44f..7a64f8a188182704c5aa6fbd01e17fb627eef5e5 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_13;
 
 public class AltosState extends AltosDataListener {
 
@@ -480,7 +480,7 @@ public class AltosState extends AltosDataListener {
        class AltosPressure extends AltosValue {
                void set(double p, double time) {
                        super.set(p, time);
-                       if (state == AltosLib.ao_flight_pad)
+                       if (state() == AltosLib.ao_flight_pad)
                                ground_pressure.set_filtered(p, time);
                        double a = pressure_to_altitude(p);
                        altitude.set_computed(a, time);
@@ -557,7 +557,7 @@ public class AltosState extends AltosDataListener {
        class AltosSpeed extends AltosCValue {
 
                boolean can_max() {
-                       return state < AltosLib.ao_flight_fast || state == AltosLib.ao_flight_stateless;
+                       return state() < AltosLib.ao_flight_fast || state() == AltosLib.ao_flight_stateless;
                }
 
                void set_accel() {
@@ -615,7 +615,7 @@ public class AltosState extends AltosDataListener {
        class AltosAccel extends AltosCValue {
 
                boolean can_max() {
-                       return state < AltosLib.ao_flight_fast || state == AltosLib.ao_flight_stateless;
+                       return state() < AltosLib.ao_flight_fast || state() == AltosLib.ao_flight_stateless;
                }
 
                void set_measured(double a, double time) {
@@ -677,9 +677,6 @@ public class AltosState extends AltosDataListener {
        public AltosGPS gps;
        public boolean  gps_pending;
 
-       public AltosIMU imu;
-       public AltosMag mag;
-
        public static final int MIN_PAD_SAMPLES = 10;
 
        public int      npad;
@@ -690,6 +687,7 @@ public class AltosState extends AltosDataListener {
 
        public AltosGreatCircle from_pad;
        public double   elevation;      /* from pad */
+       public double   distance;       /* distance along ground */
        public double   range;          /* total distance */
 
        public double   gps_height;
@@ -714,11 +712,11 @@ public class AltosState extends AltosDataListener {
        }
 
        public void init() {
+               super.init();
+
                set = 0;
 
                received_time = System.currentTimeMillis();
-               time = AltosLib.MISSING;
-               state = AltosLib.ao_flight_invalid;
                landed = false;
                boost = false;
                rssi = AltosLib.MISSING;
@@ -746,21 +744,31 @@ public class AltosState extends AltosDataListener {
                gps = null;
                gps_pending = false;
 
-               imu = null;
                last_imu_time = AltosLib.MISSING;
                rotation = null;
 
-               mag = null;
-
                accel_ground_along = AltosLib.MISSING;
                accel_ground_across = AltosLib.MISSING;
                accel_ground_through = AltosLib.MISSING;
 
+               accel_along = AltosLib.MISSING;
+               accel_across = AltosLib.MISSING;
+               accel_through = AltosLib.MISSING;
+
+               gyro_roll = AltosLib.MISSING;
+               gyro_pitch = AltosLib.MISSING;
+               gyro_yaw = AltosLib.MISSING;
+
+               mag_along = AltosLib.MISSING;
+               mag_across = AltosLib.MISSING;
+               mag_through = AltosLib.MISSING;
+
                set_npad(0);
                ngps = 0;
 
                from_pad = null;
                elevation = AltosLib.MISSING;
+               distance = AltosLib.MISSING;
                range = AltosLib.MISSING;
                gps_height = AltosLib.MISSING;
 
@@ -803,6 +811,7 @@ public class AltosState extends AltosDataListener {
 
        void update_gps() {
                elevation = AltosLib.MISSING;
+               distance = AltosLib.MISSING;
                range = AltosLib.MISSING;
 
                if (gps == null)
@@ -810,9 +819,9 @@ public class AltosState extends AltosDataListener {
 
                if (gps.locked && gps.nsat >= 4) {
                        /* Track consecutive 'good' gps reports, waiting for 10 of them */
-                       if (state == AltosLib.ao_flight_pad || state == AltosLib.ao_flight_stateless) {
+                       if (state() == AltosLib.ao_flight_pad || state() == AltosLib.ao_flight_stateless) {
                                set_npad(npad+1);
-                               if (pad_lat != AltosLib.MISSING && (npad < 10 || state == AltosLib.ao_flight_pad)) {
+                               if (pad_lat != AltosLib.MISSING && (npad < 10 || state() == AltosLib.ao_flight_pad)) {
                                        pad_lat = (pad_lat * 31 + gps.lat) / 32;
                                        pad_lon = (pad_lon * 31 + gps.lon) / 32;
                                        gps_ground_altitude.set_filtered(gps.alt, time);
@@ -833,6 +842,8 @@ public class AltosState extends AltosDataListener {
                                                        gps.climb_rate * gps.climb_rate), time);
                        if (gps.course != AltosLib.MISSING)
                                gps_course.set(gps.course, time);
+               } else if (state() == AltosLib.ao_flight_pad || state() == AltosLib.ao_flight_stateless) {
+                       set_npad(0);
                }
                if (gps.lat != 0 && gps.lon != 0 &&
                    pad_lat != AltosLib.MISSING &&
@@ -844,29 +855,20 @@ public class AltosState extends AltosDataListener {
                                h = 0;
                        from_pad = new AltosGreatCircle(pad_lat, pad_lon, 0, gps.lat, gps.lon, h);
                        elevation = from_pad.elevation;
+                       distance = from_pad.distance;
                        range = from_pad.range;
                }
        }
 
        public String state_name() {
-               return AltosLib.state_name(state);
+               return AltosLib.state_name(state());
        }
 
        public void set_state(int state) {
-               if (state != AltosLib.ao_flight_invalid) {
-                       this.state = state;
-                       ascent = (AltosLib.ao_flight_boost <= state &&
-                                 state <= AltosLib.ao_flight_coast);
-                       boost = (AltosLib.ao_flight_boost == state);
-               }
-       }
-
-       public int state() {
-               return state;
-       }
-
-       private void re_init() {
-               init();
+               super.set_state(state);
+               ascent = (AltosLib.ao_flight_boost <= state() &&
+                         state() <= AltosLib.ao_flight_coast);
+               boost = (AltosLib.ao_flight_boost == state());
        }
 
        public int rssi() {
@@ -887,6 +889,7 @@ public class AltosState extends AltosDataListener {
        }
 
        public void set_gps(AltosGPS gps) {
+               super.set_gps(gps);
                if (gps != null) {
                        this.gps = gps;
                        update_gps();
@@ -894,17 +897,16 @@ public class AltosState extends AltosDataListener {
                }
        }
 
-
        public AltosRotation    rotation;
 
        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);
                }
        }
@@ -922,12 +924,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);
                        }
                }
@@ -939,7 +941,7 @@ public class AltosState extends AltosDataListener {
        public void set_gyro(double roll, double pitch, double yaw) {
                gyro_roll = roll;
                gyro_pitch = pitch;
-               gyro_roll = yaw;
+               gyro_yaw = yaw;
                update_orient();
        }
 
@@ -989,15 +991,11 @@ public class AltosState extends AltosDataListener {
        }
 
        public double mag_across() {
-               if (mag != null)
-                       return AltosMag.convert_gauss(mag.across);
-               return AltosLib.MISSING;
+               return mag_across;
        }
 
        public double mag_through() {
-               if (mag != null)
-                       return AltosMag.convert_gauss(mag.through);
-               return AltosLib.MISSING;
+               return mag_through;
        }
 
        public void set_companion(AltosCompanion companion) {
@@ -1055,14 +1053,11 @@ public class AltosState extends AltosDataListener {
        }
 
        public AltosState() {
-               Thread.dumpStack();
                init();
        }
 
        public AltosState (AltosCalData cal_data) {
                super(cal_data);
-               if (cal_data == null)
-                       Thread.dumpStack();
                init();
        }
 }
AltOS - the operating system for Altus Metrum products