X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=altoslib%2FAltosFlightSeries.java;h=57f1a49163f639879c6d2ce65f7b03d429aecab4;hp=dad066d7ba7853ed36c1d91b7d4049ca6ae7952d;hb=dd72c9144b207b12150eb6a7ffb012f217f37374;hpb=32de85691f2e4ed1430a259e05a514ad820b32d9

diff --git a/altoslib/AltosFlightSeries.java b/altoslib/AltosFlightSeries.java
index dad066d7..57f1a491 100644
--- a/altoslib/AltosFlightSeries.java
+++ b/altoslib/AltosFlightSeries.java
@@ -12,7 +12,7 @@
  * General Public License for more details.
  */
 
-package org.altusmetrum.altoslib_11;
+package org.altusmetrum.altoslib_12;
 
 import java.util.*;
 
@@ -118,6 +118,12 @@ public class AltosFlightSeries extends AltosDataListener {
 	}
 
 	public void add_series(AltosTimeSeries s) {
+		for (int e = 0; e < series.size(); e++) {
+			if (s.compareTo(series.get(e)) < 0){
+				series.add(e, s);
+				return;
+			}
+		}
 		series.add(s);
 	}
 
@@ -159,6 +165,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;
@@ -222,23 +232,23 @@ public class AltosFlightSeries extends AltosDataListener {
 		if (altitude_series == null)
 			altitude_series = add_series(altitude_name, AltosConvert.height);
 
-		if (cal_data.ground_pressure == AltosLib.MISSING)
-			cal_data.set_ground_pressure(pa);
+		if (cal_data().ground_pressure == AltosLib.MISSING)
+			cal_data().set_ground_pressure(pa);
 
 		double altitude = AltosConvert.pressure_to_altitude(pa);
 		altitude_series.add(time(), altitude);
 	}
 
 	private void compute_height() {
-		double ground_altitude = cal_data.ground_altitude;
+		double ground_altitude = cal_data().ground_altitude;
 		if (height_series == null && ground_altitude != AltosLib.MISSING && altitude_series != null) {
 			height_series = add_series(height_name, AltosConvert.height);
 			for (AltosTimeValue alt : altitude_series)
 				height_series.add(alt.time, alt.value - ground_altitude);
 		}
 
-		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;
+		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)
 				gps_height.add(gps_alt.time, gps_alt.value - gps_ground_altitude);
@@ -264,8 +274,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);
@@ -316,16 +343,16 @@ public class AltosFlightSeries extends AltosDataListener {
 		if (accel_ground_across == AltosLib.MISSING)
 			return;
 
-		if (cal_data.pad_orientation == AltosLib.MISSING)
+		if (cal_data().pad_orientation == AltosLib.MISSING)
 			return;
 
-		if (cal_data.accel_zero_across == AltosLib.MISSING)
+		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);
+		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);
@@ -336,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;