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

diff --git a/altoslib/AltosFlightSeries.java b/altoslib/AltosFlightSeries.java
index 0b60fdf5..dad066d7 100644
--- a/altoslib/AltosFlightSeries.java
+++ b/altoslib/AltosFlightSeries.java
@@ -304,6 +304,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";
@@ -499,7 +542,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) {
@@ -524,10 +577,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);
@@ -604,6 +653,7 @@ public class AltosFlightSeries extends AltosDataListener {
 	}
 
 	public void finish() {
+		compute_orient();
 		compute_speed();
 		compute_accel();
 		compute_height();