From: Keith Packard Date: Thu, 26 Apr 2018 23:30:54 +0000 (-0700) Subject: altoslib: Keep config data accel cal in antenna up form. X-Git-Tag: 1.8.6~1^2~98 X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=commitdiff_plain;h=eee9f6128da146b0e9c711d49b2a3eb49a95aaf3 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 --- diff --git a/altoslib/AltosAccelCal.java b/altoslib/AltosAccelCal.java index 75592d9e..f98090ef 100644 --- a/altoslib/AltosAccelCal.java +++ b/altoslib/AltosAccelCal.java @@ -174,23 +174,23 @@ public class AltosAccelCal implements Runnable { if (worked) new_config = new AltosConfigData(link); } finally { + int plus = config_data.accel_cal_plus(config_data.pad_orientation); + int minus = config_data.accel_cal_minus(config_data.pad_orientation); System.out.printf("Restore orientation %d +g %d -g %d\n", config_data.pad_orientation, - config_data.accel_cal_plus, - config_data.accel_cal_minus); - if (config_data.pad_orientation != AltosLib.MISSING && config_data.pad_orientation != 0) + plus, minus); + if (config_data.pad_orientation != AltosLib.MISSING) link.printf("c o %d\n", config_data.pad_orientation); - if (config_data.accel_cal_plus != AltosLib.MISSING && config_data.accel_cal_minus != AltosLib.MISSING) - link.printf("c a %d %d\n", - config_data.accel_cal_plus, config_data.accel_cal_minus); + if (plus != AltosLib.MISSING && minus != AltosLib.MISSING) + link.printf("c a %d %d\n", plus, minus); link.flush_output(); stop_link(); } if (new_config != null) { - System.out.printf("*** +1g %d -1g %d\n", - new_config.accel_cal_plus, - new_config.accel_cal_minus); - listener.cal_done(this, new_config.accel_cal_plus, new_config.accel_cal_minus); + int plus = new_config.accel_cal_plus(AltosLib.AO_PAD_ORIENTATION_ANTENNA_UP); + int minus = new_config.accel_cal_minus(AltosLib.AO_PAD_ORIENTATION_ANTENNA_UP); + System.out.printf("*** +1g %d -1g %d\n", plus, minus); + listener.cal_done(this, plus, minus); if (!wait_signal()) throw new InterruptedException("aborted"); } else diff --git a/altoslib/AltosCalData.java b/altoslib/AltosCalData.java index 03e2cbd7..5bc6c4fd 100644 --- a/altoslib/AltosCalData.java +++ b/altoslib/AltosCalData.java @@ -397,7 +397,7 @@ public class AltosCalData { set_flight_params(config_data.apogee_delay / ticks_per_sec, config_data.apogee_lockout / ticks_per_sec); set_pad_orientation(config_data.pad_orientation); set_product(config_data.product); - set_accel_plus_minus(config_data.accel_cal_plus, config_data.accel_cal_minus); + set_accel_plus_minus(config_data.accel_cal_plus(config_data.pad_orientation), config_data.accel_cal_minus(config_data.pad_orientation)); set_accel_zero(config_data.accel_zero_along, config_data.accel_zero_across, config_data.accel_zero_through); set_ms5607(config_data.ms5607); try { 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) diff --git a/altoslib/AltosLib.java b/altoslib/AltosLib.java index c25a6273..7d2b68e0 100644 --- a/altoslib/AltosLib.java +++ b/altoslib/AltosLib.java @@ -348,6 +348,9 @@ public class AltosLib { public static final int AO_GPS_NUM_SAT_SHIFT = 0; public static final int AO_GPS_NUM_SAT_MASK = 0xf; + public static final int AO_PAD_ORIENTATION_ANTENNA_UP = 0; + public static final int AO_PAD_ORIENTATION_ANTENNA_DOWN = 1; + public static final int AO_LOG_FORMAT_UNKNOWN = 0; public static final int AO_LOG_FORMAT_FULL = 1; public static final int AO_LOG_FORMAT_TINY = 2;