altoslib: Keep config data accel cal in antenna up form.

Instead of letting the accel cal data live in whatever form it was
fetched in, keep it in antenna up form and provide it in whatever
orientation is necessary.

This fixes bugs in changing pad configuration where the accel cal values
from the old orientation would get used in the new orientation and
wreck them.

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

1 files changed, 93 insertions, 12 deletions

diff --git a/altoslib/AltosConfigData.java b/altoslib/AltosConfigData.java
index dc036867..35a8602d 100644
--- a/altoslib/AltosConfigData.java
+++ b/altoslib/AltosConfigData.java
@@ -52,7 +52,9 @@ public class AltosConfigData {
 	public int	radio_setting;
 
 	/* HAS_ACCEL */
-	public int	accel_cal_plus, accel_cal_minus;
+	private int	accel_cal_plus_raw, accel_cal_minus_raw;
+	private int	accel_cal_plus_cooked, accel_cal_minus_cooked;
+	private boolean	accel_cal_adjusted;
 	public int	pad_orientation;
 
 	/* HAS_LOG */
@@ -186,6 +188,23 @@ public class AltosConfigData {
 		}
 	}
 
+	public int invert_accel_value(int value) {
+		if (value == AltosLib.MISSING)
+			return AltosLib.MISSING;
+
+		switch (log_format) {
+		case AltosLib.AO_LOG_FORMAT_FULL:
+			return 0x7fff - value;
+		case AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD:
+		case AltosLib.AO_LOG_FORMAT_TELEMETRUM:
+		case AltosLib.AO_LOG_FORMAT_TELEMEGA:
+		case AltosLib.AO_LOG_FORMAT_TELEMEGA_3:
+			return 4095 - value;
+		default:
+			return AltosLib.MISSING;
+		}
+	}
+
 	public boolean has_monitor_battery() {
 		if (product.startsWith("TeleBT"))
 			return true;
@@ -252,9 +271,12 @@ public class AltosConfigData {
 		radio_setting = AltosLib.MISSING;
 		telemetry_rate = AltosLib.MISSING;
 
-		accel_cal_plus = AltosLib.MISSING;
-		accel_cal_minus = AltosLib.MISSING;
+		accel_cal_plus_cooked = AltosLib.MISSING;
+		accel_cal_minus_cooked = AltosLib.MISSING;
+		accel_cal_plus_raw = AltosLib.MISSING;
+		accel_cal_minus_raw = AltosLib.MISSING;
 		pad_orientation = AltosLib.MISSING;
+		accel_cal_adjusted = false;
 
 		flight_log_max = AltosLib.MISSING;
 		log_fixed = AltosLib.MISSING;
@@ -285,6 +307,57 @@ public class AltosConfigData {
 		accel_zero_through = AltosLib.MISSING;
 	}
 
+	/* Return + accel calibration relative to a specific pad orientation */
+	public int accel_cal_plus(int pad_orientation) {
+		switch (pad_orientation) {
+		case AltosLib.AO_PAD_ORIENTATION_ANTENNA_UP:
+			return accel_cal_plus_cooked;
+		case AltosLib.AO_PAD_ORIENTATION_ANTENNA_DOWN:
+			return invert_accel_value(accel_cal_minus_cooked);
+		default:
+			return AltosLib.MISSING;
+		}
+	}
+
+	/* Return - accel calibration relative to a specific pad orientation */
+	public int accel_cal_minus(int pad_orientation) {
+		switch (pad_orientation) {
+		case AltosLib.AO_PAD_ORIENTATION_ANTENNA_UP:
+			return accel_cal_minus_cooked;
+		case AltosLib.AO_PAD_ORIENTATION_ANTENNA_DOWN:
+			return invert_accel_value(accel_cal_plus_cooked);
+		default:
+			return AltosLib.MISSING;
+		}
+	}
+
+	/* Once we have all of the values from the config data, compute the
+	 * accel cal values relative to Antenna Up orientation.
+	 */
+	private void adjust_accel_cal() {
+		if (!accel_cal_adjusted &&
+		    pad_orientation != AltosLib.MISSING &&
+		    accel_cal_plus_raw != AltosLib.MISSING &&
+		    accel_cal_minus_raw != AltosLib.MISSING &&
+		    log_format != AltosLib.AO_LOG_FORMAT_UNKNOWN)
+		{
+			switch (pad_orientation) {
+			case AltosLib.AO_PAD_ORIENTATION_ANTENNA_UP:
+				accel_cal_plus_cooked = accel_cal_plus_raw;
+				accel_cal_minus_cooked = accel_cal_minus_raw;
+				accel_cal_adjusted = true;
+				break;
+			case AltosLib.AO_PAD_ORIENTATION_ANTENNA_DOWN:
+				accel_cal_plus_cooked = invert_accel_value(accel_cal_minus_raw);
+				accel_cal_minus_cooked = invert_accel_value(accel_cal_plus_raw);
+				accel_cal_adjusted = true;
+				break;
+			default:
+				break;
+			}
+		}
+	}
+
 	public void parse_line(String line) {
 
 		/* Version replies */
@@ -349,8 +422,9 @@ public class AltosConfigData {
 			if (line.startsWith("Accel cal")) {
 				String[] bits = line.split("\\s+");
 				if (bits.length >= 6) {
-					accel_cal_plus = Integer.parseInt(bits[3]);
-					accel_cal_minus = Integer.parseInt(bits[5]);
+					accel_cal_plus_raw = Integer.parseInt(bits[3]);
+					accel_cal_minus_raw = Integer.parseInt(bits[5]);
+					accel_cal_adjusted = false;
 				}
 			}
 		} catch (Exception e) {}
@@ -414,6 +488,9 @@ public class AltosConfigData {
 				}
 			}
 		} catch (Exception e) {}
+
+		/* Fix accel cal as soon as all of the necessary values appear */
+		adjust_accel_cal();
 	}
 
 	public AltosConfigData() {
@@ -525,11 +602,11 @@ public class AltosConfigData {
 		if (pad_orientation != AltosLib.MISSING)
 			pad_orientation = source.pad_orientation();
 
-		if (accel_cal_plus != AltosLib.MISSING)
-			accel_cal_plus = source.accel_cal_plus();
+		if (accel_cal_plus_cooked != AltosLib.MISSING)
+			accel_cal_plus_cooked = source.accel_cal_plus();
 
-		if (accel_cal_minus != AltosLib.MISSING)
-			accel_cal_minus = source.accel_cal_minus();
+		if (accel_cal_minus_cooked != AltosLib.MISSING)
+			accel_cal_minus_cooked = source.accel_cal_minus();
 
 		/* HAS_LOG */
 		if (flight_log_max != AltosLib.MISSING)
@@ -598,7 +675,8 @@ public class AltosConfigData {
 		dest.set_flight_log_max(flight_log_max);
 		dest.set_ignite_mode(ignite_mode);
 		dest.set_pad_orientation(pad_orientation);
-		dest.set_accel_cal(accel_cal_plus, accel_cal_minus);
+		dest.set_accel_cal(accel_cal_plus(AltosLib.AO_PAD_ORIENTATION_ANTENNA_UP),
+				   accel_cal_minus(AltosLib.AO_PAD_ORIENTATION_ANTENNA_UP));
 		dest.set_callsign(callsign);
 		if (npyro != AltosLib.MISSING)
 			dest.set_pyros(pyros);
@@ -676,10 +754,13 @@ public class AltosConfigData {
 			link.printf("c e %d\n", radio_enable);
 
 		/* HAS_ACCEL */
+		/* set orientation first so that we know how to set the accel cal */
 		if (pad_orientation != AltosLib.MISSING)
 			link.printf("c o %d\n", pad_orientation);
-		if (accel_cal_plus != AltosLib.MISSING && accel_cal_minus != AltosLib.MISSING)
-			link.printf("c a %d %d\n", accel_cal_plus, accel_cal_minus);
+		int plus = accel_cal_plus(pad_orientation);
+		int minus = accel_cal_minus(pad_orientation);
+		if (plus != AltosLib.MISSING && minus != AltosLib.MISSING)
+			link.printf("c a %d %d\n", plus, minus);
 
 		/* HAS_LOG */
 		if (flight_log_max != 0)