altoslib: Compute 'vertical acceleration' for eeprom files

[fw/altos] / altoslib / AltosFlightSeries.java

diff --git a/altoslib/AltosFlightSeries.java b/altoslib/AltosFlightSeries.java
index 3e2a05ec8f9b162f287eb869cc43b7a841f52d10..315e0a44d6fbd39924d5168acd9df53f844dc1d0 100644 (file)
--- a/altoslib/AltosFlightSeries.java
+++ b/altoslib/AltosFlightSeries.java
@@ -159,6 +159,10 @@ public class AltosFlightSeries extends AltosDataListener {
 
        public static final String accel_name = "Accel";
 
+       public AltosTimeSeries  vert_accel_series;
+
+       public static final String vert_accel_name = "Vertical Accel";
+
        public void set_acceleration(double acceleration) {
                if (acceleration == AltosLib.MISSING)
                        return;
@@ -187,11 +191,17 @@ public class AltosFlightSeries extends AltosDataListener {
 
        public static final String rssi_name = "RSSI";
 
+       public AltosTimeSeries status_series;
+
+       public static final String status_name = "Radio Status";
+
        public void set_rssi(int rssi, int status) {
                if (rssi_series == null) {
                        rssi_series = add_series(rssi_name, null);
+                       status_series = add_series(status_name, null);
                }
                rssi_series.add(time(), rssi);
+               status_series.add(time(), status);
        }
 
        public AltosTimeSeries pressure_series;
@@ -231,7 +241,7 @@ public class AltosFlightSeries extends AltosDataListener {
                                height_series.add(alt.time, alt.value - ground_altitude);
                }
 
-               if (gps_height == null && cal_data.gps_pad != null && gps_altitude != null) {
+               if (gps_height == null && cal_data.gps_pad != null && cal_data.gps_pad.alt != AltosLib.MISSING && gps_altitude != null) {
                        double gps_ground_altitude = cal_data.gps_pad.alt;
                        gps_height = add_series(gps_height_name, AltosConvert.height);
                        for (AltosTimeValue gps_alt : gps_altitude)
@@ -258,8 +268,25 @@ public class AltosFlightSeries extends AltosDataListener {
                        temp_series.differentiate(alt_speed_series);
                }
                if (accel_series != null) {
+
+                       if (orient_series != null) {
+                               vert_accel_series = add_series(vert_accel_name, AltosConvert.accel);
+
+                               for (AltosTimeValue a : accel_series) {
+                                       double  orient = orient_series.value(a.time);
+                                       double  a_abs = a.value + AltosConvert.gravity;
+                                       double  v_a = a_abs * Math.cos(AltosConvert.degrees_to_radians(orient)) - AltosConvert.gravity;
+
+                                       vert_accel_series.add(a.time, v_a);
+                               }
+                       }
+
                        AltosTimeSeries temp_series = make_series(speed_name, AltosConvert.speed);
-                       accel_series.integrate(temp_series);
+
+                       if (vert_accel_series != null)
+                               vert_accel_series.integrate(temp_series);
+                       else
+                               accel_series.integrate(temp_series);
 
                        accel_speed_series = make_series(speed_name, AltosConvert.speed);
                        temp_series.filter(accel_speed_series, 0.1);
@@ -298,6 +325,49 @@ public class AltosFlightSeries extends AltosDataListener {
                        add_series(speed_series);
        }
 
+       public AltosTimeSeries orient_series;
+
+       public static final String orient_name = "Tilt Angle";
+
+       private void compute_orient() {
+
+               if (orient_series != null)
+                       return;
+
+               if (accel_ground_across == AltosLib.MISSING)
+                       return;
+
+               if (cal_data.pad_orientation == AltosLib.MISSING)
+                       return;
+
+               if (cal_data.accel_zero_across == AltosLib.MISSING)
+                       return;
+
+               AltosRotation 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);
+               double prev_time = ground_time;
+
+               orient_series = add_series(orient_name, AltosConvert.orient);
+               orient_series.add(ground_time, rotation.tilt());
+
+               for (AltosTimeValue roll_v : gyro_roll) {
+                       double  time = roll_v.time;
+                       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));
+
+                               rotation.rotate(dt, pitch, yaw, roll);
+                               orient_series.add(time, rotation.tilt());
+                       }
+                       prev_time = time;
+               }
+       }
+
        public AltosTimeSeries  kalman_height_series, kalman_speed_series, kalman_accel_series;
 
        public static final String kalman_height_name = "Kalman Height";
@@ -493,7 +563,17 @@ public class AltosFlightSeries extends AltosDataListener {
                accel_through.add(time(), through);
        }
 
+       private double  accel_ground_along = AltosLib.MISSING;
+       private double  accel_ground_across = AltosLib.MISSING;
+       private double  accel_ground_through = AltosLib.MISSING;
+
+       private double          ground_time;
+
        public  void set_accel_ground(double along, double across, double through) {
+               accel_ground_along = along;
+               accel_ground_across = across;
+               accel_ground_through = through;
+               ground_time = time();
        }
 
        public  void set_gyro(double roll, double pitch, double yaw) {
@@ -518,10 +598,6 @@ public class AltosFlightSeries extends AltosDataListener {
                mag_through.add(time(), through);
        }
 
-       public static final String orient_name = "Tilt Angle";
-
-       public AltosTimeSeries orient_series;
-
        public void set_orient(double orient) {
                if (orient_series == null)
                        orient_series = add_series(orient_name, AltosConvert.orient);
@@ -598,6 +674,7 @@ public class AltosFlightSeries extends AltosDataListener {
        }
 
        public void finish() {
+               compute_orient();
                compute_speed();
                compute_accel();
                compute_height();