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

[fw/altos] / altoslib / AltosConfigData.java

diff --git a/altoslib/AltosConfigData.java b/altoslib/AltosConfigData.java
index dc036867047cf278432aa99b7b35be0ccb244d6f..35a8602db4097651b149700ba6dfe863e36eead2 100644 (file)
--- 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)