altosui: Add imu and mag sensor values to plots

Makes for a lot of potential graph elements.

Signed-off-by: Keith Packard <keithp@keithp.com>

3 files changed, 227 insertions, 0 deletions

diff --git a/altosui/AltosFlightStats.java b/altosui/AltosFlightStats.java
index 552210c3..b3ee14ac 100644
--- a/altosui/AltosFlightStats.java
+++ b/altosui/AltosFlightStats.java
@@ -38,6 +38,9 @@ public class AltosFlightStats {
 	boolean		has_gps;
 	boolean		has_other_adc;
 	boolean		has_rssi;
+	boolean		has_imu;
+	boolean		has_mag;
+	boolean		has_orient;
 
 	double landed_time(AltosStateIterable states) {
 		AltosState state = null;
@@ -108,6 +111,9 @@ public class AltosFlightStats {
 		has_gps = false;
 		has_other_adc = false;
 		has_rssi = false;
+		has_imu = false;
+		has_mag = false;
+		has_orient = false;
 		for (AltosState state : states) {
 			if (serial == AltosLib.MISSING && state.serial != AltosLib.MISSING)
 				serial = state.serial;
@@ -157,6 +163,12 @@ public class AltosFlightStats {
 				lon = state.gps.lon;
 				has_gps = true;
 			}
+			if (state.imu != null)
+				has_imu = true;
+			if (state.mag != null)
+				has_mag = true;
+			if (state.orient() != AltosLib.MISSING)
+				has_orient = true;
 		}
 		for (int s = Altos.ao_flight_startup; s <= Altos.ao_flight_landed; s++) {
 			if (state_count[s] > 0) {
diff --git a/altosui/AltosGraph.java b/altosui/AltosGraph.java
index c505d2d8..fa1a5b72 100644
--- a/altosui/AltosGraph.java
+++ b/altosui/AltosGraph.java
@@ -127,6 +127,75 @@ class AltosDbm extends AltosUnits {
 	}
 }
 
+class AltosGyroUnits extends AltosUnits {
+
+	public double value(double p, boolean imperial_units) {
+		return p;
+	}
+
+	public double inverse(double p, boolean imperial_units) {
+		return p;
+	}
+
+	public String show_units(boolean imperial_units) {
+		return "°/sec";
+	}
+
+	public String say_units(boolean imperial_units) {
+		return "degrees per second";
+	}
+
+	public int show_fraction(int width, boolean imperial_units) {
+		return 1;
+	}
+}
+
+class AltosOrientUnits extends AltosUnits {
+
+	public double value(double p, boolean imperial_units) {
+		return p;
+	}
+
+	public double inverse(double p, boolean imperial_units) {
+		return p;
+	}
+
+	public String show_units(boolean imperial_units) {
+		return "°";
+	}
+
+	public String say_units(boolean imperial_units) {
+		return "degrees";
+	}
+
+	public int show_fraction(int width, boolean imperial_units) {
+		return 1;
+	}
+}
+
+class AltosMagUnits extends AltosUnits {
+
+	public double value(double p, boolean imperial_units) {
+		return p;
+	}
+
+	public double inverse(double p, boolean imperial_units) {
+		return p;
+	}
+
+	public String show_units(boolean imperial_units) {
+		return "Ga";
+	}
+
+	public String say_units(boolean imperial_units) {
+		return "gauss";
+	}
+
+	public int show_fraction(int width, boolean imperial_units) {
+		return 2;
+	}
+}
+
 public class AltosGraph extends AltosUIGraph {
 
 	static final private Color height_color = new Color(194,31,31);
@@ -146,14 +215,28 @@ public class AltosGraph extends AltosUIGraph {
 	static final private Color temperature_color = new Color (31, 194, 194);
 	static final private Color dbm_color = new Color(31, 100, 100);
 	static final private Color state_color = new Color(0,0,0);
+	static final private Color accel_x_color = new Color(255, 0, 0);
+	static final private Color accel_y_color = new Color(0, 255, 0);
+	static final private Color accel_z_color = new Color(0, 0, 255);
+	static final private Color gyro_x_color = new Color(192, 0, 0);
+	static final private Color gyro_y_color = new Color(0, 192, 0);
+	static final private Color gyro_z_color = new Color(0, 0, 192);
+	static final private Color mag_x_color = new Color(128, 0, 0);
+	static final private Color mag_y_color = new Color(0, 128, 0);
+	static final private Color mag_z_color = new Color(0, 0, 128);
+	static final private Color orient_color = new Color(31, 31, 31);
 
 	static AltosVoltage voltage_units = new AltosVoltage();
 	static AltosPressure pressure_units = new AltosPressure();
 	static AltosNsat nsat_units = new AltosNsat();
 	static AltosDbm dbm_units = new AltosDbm();
+	static AltosGyroUnits gyro_units = new AltosGyroUnits();
+	static AltosOrientUnits orient_units = new AltosOrientUnits();
+	static AltosMagUnits mag_units = new AltosMagUnits();
 
 	AltosUIAxis	height_axis, speed_axis, accel_axis, voltage_axis, temperature_axis, nsat_axis, dbm_axis;
 	AltosUIAxis	distance_axis, pressure_axis;
+	AltosUIAxis	gyro_axis, orient_axis, mag_axis;
 
 	public AltosGraph(AltosUIEnable enable, AltosFlightStats stats, AltosGraphDataSet dataSet) {
 		super(enable);
@@ -169,6 +252,10 @@ public class AltosGraph extends AltosUIGraph {
 		dbm_axis = newAxis("Signal Strength", dbm_units, dbm_color, 0);
 		distance_axis = newAxis("Distance", AltosConvert.distance, range_color);
 
+		gyro_axis = newAxis("Rotation Rate", gyro_units, gyro_z_color, 0);
+		orient_axis = newAxis("Tilt Angle", orient_units, orient_color, 0);
+		mag_axis = newAxis("Magnetic Field", mag_units, mag_x_color, 0);
+
 		addMarker("State", AltosGraphDataPoint.data_state, state_color);
 		addSeries("Height",
 			  AltosGraphDataPoint.data_height,
@@ -260,6 +347,72 @@ public class AltosGraph extends AltosUIGraph {
 				  voltage_axis);
 		}
 
+		if (stats.has_imu) {
+			addSeries("Acceleration X",
+				  AltosGraphDataPoint.data_accel_x,
+				  AltosConvert.accel,
+				  accel_x_color,
+				  false,
+				  accel_axis);
+			addSeries("Acceleration Y",
+				  AltosGraphDataPoint.data_accel_y,
+				  AltosConvert.accel,
+				  accel_y_color,
+				  false,
+				  accel_axis);
+			addSeries("Acceleration Z",
+				  AltosGraphDataPoint.data_accel_z,
+				  AltosConvert.accel,
+				  accel_z_color,
+				  false,
+				  accel_axis);
+			addSeries("Rotation Rate X",
+				  AltosGraphDataPoint.data_gyro_x,
+				  gyro_units,
+				  gyro_x_color,
+				  false,
+				  gyro_axis);
+			addSeries("Rotation Rate Y",
+				  AltosGraphDataPoint.data_gyro_y,
+				  gyro_units,
+				  gyro_y_color,
+				  false,
+				  gyro_axis);
+			addSeries("Rotation Rate Z",
+				  AltosGraphDataPoint.data_gyro_z,
+				  gyro_units,
+				  gyro_z_color,
+				  false,
+				  gyro_axis);
+		}
+		if (stats.has_mag) {
+			addSeries("Magnetometer X",
+				  AltosGraphDataPoint.data_mag_x,
+				  mag_units,
+				  mag_x_color,
+				  false,
+				  mag_axis);
+			addSeries("Magnetometer Y",
+				  AltosGraphDataPoint.data_mag_y,
+				  mag_units,
+				  mag_y_color,
+				  false,
+				  mag_axis);
+			addSeries("Magnetometer Z",
+				  AltosGraphDataPoint.data_mag_z,
+				  mag_units,
+				  mag_z_color,
+				  false,
+				  mag_axis);
+		}
+		if (stats.has_orient)
+			addSeries("Tilt Angle",
+				  AltosGraphDataPoint.data_orient,
+				  orient_units,
+				  orient_color,
+				  false,
+				  orient_axis);
+
 		setDataSet(dataSet);
 	}
 }
\ No newline at end of file
diff --git a/altosui/AltosGraphDataPoint.java b/altosui/AltosGraphDataPoint.java
index d8191f5d..b2782459 100644
--- a/altosui/AltosGraphDataPoint.java
+++ b/altosui/AltosGraphDataPoint.java
@@ -40,6 +40,16 @@ public class AltosGraphDataPoint implements AltosUIDataPoint {
 	public static final int data_range = 13;
 	public static final int data_distance = 14;
 	public static final int data_pressure = 15;
+	public static final int data_accel_x = 16;
+	public static final int data_accel_y = 17;
+	public static final int data_accel_z = 18;
+	public static final int data_gyro_x = 19;
+	public static final int data_gyro_y = 20;
+	public static final int data_gyro_z = 21;
+	public static final int data_mag_x = 22;
+	public static final int data_mag_y = 23;
+	public static final int data_mag_z = 24;
+	public static final int data_orient = 25;
 
 	public double x() throws AltosUIDataMissing {
 		double	time = state.time_since_boost();
@@ -99,6 +109,58 @@ public class AltosGraphDataPoint implements AltosUIDataPoint {
 		case data_pressure:
 			y = state.pressure();
 			break;
+			
+		case data_accel_x:
+		case data_accel_y:
+		case data_accel_z:
+		case data_gyro_x:
+		case data_gyro_y:
+		case data_gyro_z:
+			AltosIMU	imu = state.imu;
+			if (imu == null)
+				break;
+			switch (index) {
+			case data_accel_x:
+				y = imu.accel_x;
+				break;
+			case data_accel_y:
+				y = imu.accel_y;
+				break;
+			case data_accel_z:
+				y = imu.accel_z;
+				break;
+			case data_gyro_x:
+				y = imu.gyro_x;
+				break;
+			case data_gyro_y:
+				y = imu.gyro_y;
+				break;
+			case data_gyro_z:
+				y = imu.gyro_z;
+				break;
+			}
+			break;
+		case data_mag_x:
+		case data_mag_y:
+		case data_mag_z:
+			AltosMag	mag = state.mag;
+			if (mag == null)
+				break;
+			switch (index) {
+			case data_mag_x:
+				y = mag.x;
+				break;
+			case data_mag_y:
+				y = mag.y;
+				break;
+			case data_mag_z:
+				y = mag.z;
+				break;
+			}
+			break;
+		case data_orient:
+			y = state.orient();
+			break;
 		}
 		if (y == AltosLib.MISSING)
 			throw new AltosUIDataMissing(index);