X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=altoslib%2FAltosFlightSeries.java;h=315e0a44d6fbd39924d5168acd9df53f844dc1d0;hp=3e2a05ec8f9b162f287eb869cc43b7a841f52d10;hb=af6ce9a142d001f3b9b83cf6527f4dc848e94d48;hpb=8ec59ee5a6dda114ff55a4af4669842eed5ef79a

diff --git a/altoslib/AltosFlightSeries.java b/altoslib/AltosFlightSeries.java
index 3e2a05ec..315e0a44 100644
--- 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();